From c4375a09e44f2f7e31efc4260e49b7d2a339b252 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 9 Jul 2013 10:46:47 -0700 Subject: [PATCH] added GPU joint solver for non-contact constraints. Only point 2 point version for now, will add some other constraints soon (changes are very local) --- .../GpuDemos/constraints/ConstraintsDemo.cpp | 47 +- Demos3/GpuDemos/main_opengl3core.cpp | 2 + src/Bullet3Common/b3Logging.cpp | 17 +- .../ConstraintSolver/b3PgsJacobiSolver.cpp | 11 +- .../RigidBody/b3GpuGenericConstraint.cpp | 137 +++ .../RigidBody/b3GpuGenericConstraint.h | 130 +++ .../RigidBody/b3GpuPgsJacobiSolver.cpp | 896 ++++++++++++------ .../RigidBody/b3GpuPgsJacobiSolver.h | 63 +- .../RigidBody/b3GpuRigidBodyPipeline.cpp | 66 +- .../RigidBody/b3GpuRigidBodyPipeline.h | 18 + .../b3GpuRigidBodyPipelineInternalData.h | 23 + src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h | 228 +++++ .../RigidBody/kernels/integrateKernel.cl | 14 + .../RigidBody/kernels/integrateKernel.h | 14 + .../RigidBody/kernels/jointSolver.cl | 598 +++++++++++- .../RigidBody/kernels/jointSolver.h | 596 +++++++++++- .../RigidBody/kernels/solverUtils.cl | 4 +- .../RigidBody/kernels/solverUtils.h | 2 +- 18 files changed, 2502 insertions(+), 364 deletions(-) create mode 100644 src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp create mode 100644 src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h create mode 100644 src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h diff --git a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp b/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp index 6f6d88e5f..1a005133a 100644 --- a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp +++ b/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp @@ -1,3 +1,18 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + #include "ConstraintsDemo.h" #include "OpenGLWindow/ShapeData.h" @@ -37,12 +52,12 @@ void GpuConstraintsDemo::setupScene(const ConstructionInfo& ci) index+=createDynamicsObjects(ci); m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); - m_data->m_rigidBodyPipeline->setGravity(b3Vector3(4,-10,0)); +// m_data->m_rigidBodyPipeline->setGravity(b3Vector3(4,-10,0)); float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0}; //float camPos[4]={1,12.5,1.5,0}; m_instancingRenderer->setCameraTargetPosition(camPos); - m_instancingRenderer->setCameraDistance(100); + m_instancingRenderer->setCameraDistance(30); m_instancingRenderer->updateCamera(); @@ -129,7 +144,7 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const int constraintType=0; for (int i=0;im_np->registerConvexHullShape(utilPtr); float mass = 1.f; - if (j==0)//ci.arraySizeY-1) + if (j==0 || j==ci.arraySizeY-1) { - //mass=0.f; + mass=0.f; } //b3Vector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2); - b3Vector3 position((-ci.arraySizeX/2*ci.gapX)+i*ci.gapX,1+j*2.,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ); + //b3Vector3 position((-ci.arraySizeX/2*ci.gapX)+i*ci.gapX,1+j*2.,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ); + b3Vector3 position(1+j*2.,10+i*ci.gapX,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ); b3Quaternion orn(0,0,0,1); @@ -162,7 +178,7 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); - + bool useGpu = false; b3TypedConstraint* c = 0; if (prevBody>=0) @@ -170,10 +186,18 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const switch (constraintType) { case 0: - c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0)); + { + + //c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(-1.1,0,0),b3Vector3(1.1,0,0)); +// c->setBreakingImpulseThreshold(14); + b3Vector3 pivotInA(-1.1,0,0); + b3Vector3 pivotInB (1.1,0,0); + int cid = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(pid,prevBody,pivotInA,pivotInB); break; + } case 1: { + /* b3Transform frameInA,frameInB; frameInA.setIdentity(); frameInB.setIdentity(); @@ -182,11 +206,13 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB); //c->setBreakingImpulseThreshold(37.1); + */ break; } case 2: { + /* b3Transform frameInA,frameInB; frameInA.setIdentity(); frameInB.setIdentity(); @@ -197,6 +223,7 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const for (int i=0;i<6;i++) dof6->setLimit(i,0,0); c=dof6; + */ break; } default: @@ -207,9 +234,9 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const }; if (c) { - m_data->m_rigidBodyPipeline->addConstraint(c);//,false); + m_data->m_rigidBodyPipeline->addConstraint(c); } - + } diff --git a/Demos3/GpuDemos/main_opengl3core.cpp b/Demos3/GpuDemos/main_opengl3core.cpp index ae1c582c9..13eff96b6 100644 --- a/Demos3/GpuDemos/main_opengl3core.cpp +++ b/Demos3/GpuDemos/main_opengl3core.cpp @@ -89,6 +89,8 @@ GpuDemo::CreateFunc* allDemos[]= // ConcaveSphereScene::MyCreateFunc, + + GpuBoxPlaneScene::MyCreateFunc, GpuConvexPlaneScene::MyCreateFunc, diff --git a/src/Bullet3Common/b3Logging.cpp b/src/Bullet3Common/b3Logging.cpp index b13728e4e..ae9353075 100644 --- a/src/Bullet3Common/b3Logging.cpp +++ b/src/Bullet3Common/b3Logging.cpp @@ -1,3 +1,18 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + #include "b3Logging.h" #include @@ -39,7 +54,7 @@ void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc) } //#define B3_MAX_DEBUG_STRING_LENGTH 2048 -#define B3_MAX_DEBUG_STRING_LENGTH 8192 +#define B3_MAX_DEBUG_STRING_LENGTH 32768 void b3OutputPrintfVarArgsInternal(const char *str, ...) diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp index 162c9b2c9..9b146f13b 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -36,7 +36,7 @@ subject to the following restrictions: #include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" -b3Transform getWorldTransform(b3RigidBodyCL* rb) +static b3Transform getWorldTransform(b3RigidBodyCL* rb) { b3Transform newTrans; newTrans.setOrigin(rb->m_pos); @@ -44,24 +44,24 @@ b3Transform getWorldTransform(b3RigidBodyCL* rb) return newTrans; } -const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia) +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia) { return inertia->m_invInertiaWorld; } -const b3Vector3& getLinearVelocity(b3RigidBodyCL* rb) +static const b3Vector3& getLinearVelocity(b3RigidBodyCL* rb) { return rb->m_linVel; } -const b3Vector3& getAngularVelocity(b3RigidBodyCL* rb) +static const b3Vector3& getAngularVelocity(b3RigidBodyCL* rb) { return rb->m_angVel; } -b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos) +static b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos) { //we also calculate lin/ang velocity for kinematic objects return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); @@ -1704,6 +1704,7 @@ void b3PgsJacobiSolver::averageVelocities() b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal) { + B3_PROFILE("solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int i,j; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp new file mode 100644 index 000000000..f42fd9ba4 --- /dev/null +++ b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp @@ -0,0 +1,137 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#include "b3GpuGenericConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" + +#include +#include "Bullet3Common/b3Transform.h" + +void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyCL* bodies) +{ + switch (m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + *info = 3; + break; + }; + default: + { + b3Assert(0); + } + }; +} + +void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyCL* bodies) +{ + b3Transform trA; + trA.setIdentity(); + trA.setOrigin(bodies[constraint->m_rbA].m_pos); + trA.setRotation(bodies[constraint->m_rbA].m_quat); + + b3Transform trB; + trB.setIdentity(); + trB.setOrigin(bodies[constraint->m_rbB].m_pos); + trB.setRotation(bodies[constraint->m_rbB].m_quat); + + // anchor points in global coordinates with respect to body PORs. + + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + b3Vector3 a1 = trA.getBasis()*constraint->getPivotInA(); + b3Vector3 a1a = b3QuatRotate(trA.getRotation(),constraint->getPivotInA()); + + { + b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + b3Vector3 a2 = trB.getBasis()*constraint->getPivotInB(); + + { + // b3Vector3 a2n = -a2; + b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + + + // set right hand side +// b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; + b3Scalar currERP = info->erp; + + b3Scalar k = info->fps * currERP; + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (a2[j] + trB.getOrigin()[j] - a1[j] - trA.getOrigin()[j]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } +#if 0 + if(m_flags & B3_P2P_FLAGS_CFM) + { + for (j=0; j<3; j++) + { + info->cfm[j*info->rowskip] = m_cfm; + } + } +#endif + +#if 0 + b3Scalar impulseClamp = m_setting.m_impulseClamp;// + for (j=0; j<3; j++) + { + if (m_setting.m_impulseClamp > 0) + { + info->m_lowerLimit[j*info->rowskip] = -impulseClamp; + info->m_upperLimit[j*info->rowskip] = impulseClamp; + } + } + info->m_damping = m_setting.m_damping; +#endif + +} + +void b3GpuGenericConstraint::getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyCL* bodies) +{ + switch (m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + getInfo2Point2Point(this,info,bodies); + break; + }; + default: + { + b3Assert(0); + } + }; +} diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h new file mode 100644 index 000000000..a3ac9fa84 --- /dev/null +++ b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h @@ -0,0 +1,130 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_GENERIC_CONSTRAINT_H +#define B3_GPU_GENERIC_CONSTRAINT_H + +#include "Bullet3Common/b3Quaternion.h" +struct b3RigidBodyCL; +enum B3_CONSTRAINT_FLAGS +{ + B3_CONSTRAINT_FLAG_ENABLED=1, +}; + +enum b3GpuGenericConstraintType +{ + B3_GPU_POINT2POINT_CONSTRAINT_TYPE=3, +// B3_HINGE_CONSTRAINT_TYPE, +// B3_CONETWIST_CONSTRAINT_TYPE, +// B3_D6_CONSTRAINT_TYPE, +// B3_SLIDER_CONSTRAINT_TYPE, +// B3_CONTACT_CONSTRAINT_TYPE, +// B3_D6_SPRING_CONSTRAINT_TYPE, +// B3_GEAR_CONSTRAINT_TYPE, +// B3_FIXED_CONSTRAINT_TYPE, + B3_GPU_MAX_CONSTRAINT_TYPE +}; + + + +struct b3GpuConstraintInfo2 +{ + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + b3Scalar fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + b3Scalar *m_constraintError,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + b3Scalar *m_lowerLimit,*m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + b3Scalar m_damping; +}; + + +B3_ATTRIBUTE_ALIGNED16(struct) b3GpuGenericConstraint +{ + int m_constraintType; + int m_rbA; + int m_rbB; + float m_breakingImpulseThreshold; + + b3Vector3 m_pivotInA; + b3Vector3 m_pivotInB; + b3Quaternion m_relTargetAB; + + int m_flags; + int m_padding[3]; + + int getRigidBodyA() const + { + return m_rbA; + } + int getRigidBodyB() const + { + return m_rbB; + } + + const b3Vector3& getPivotInA() const + { + return m_pivotInA; + } + + const b3Vector3& getPivotInB() const + { + return m_pivotInB; + } + + int isEnabled() const + { + return m_flags & B3_CONSTRAINT_FLAG_ENABLED; + } + + float getBreakingImpulseThreshold() const + { + return m_breakingImpulseThreshold; + } + + ///internal method used by the constraint solver, don't use them directly + void getInfo1 (unsigned int* info,const b3RigidBodyCL* bodies); + + ///internal method used by the constraint solver, don't use them directly + void getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyCL* bodies); + + +}; + +#endif //B3_GPU_GENERIC_CONSTRAINT_H \ No newline at end of file diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.cpp index 76a759f70..cc3cfbf8e 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.cpp @@ -1,3 +1,19 @@ + +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + #include "b3GpuPgsJacobiSolver.h" #include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" @@ -10,7 +26,7 @@ #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" - +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" #include "Bullet3OpenCL/RigidBody/kernels/jointSolver.h" //solveConstraintRowsCL #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" @@ -24,28 +40,74 @@ struct b3GpuPgsJacobiSolverInternalData cl_context m_context; cl_device_id m_device; cl_command_queue m_queue; + + b3PrefixScanCL* m_prefixScan; + cl_kernel m_solveJointConstraintRowsKernels; - b3OpenCLArray* m_gpuSolverConstraintRows; - b3OpenCLArray* m_gpuSolverBodies; + cl_kernel m_initSolverBodiesKernel; + cl_kernel m_getInfo1Kernel; + cl_kernel m_initBatchConstraintsKernel; + cl_kernel m_getInfo2Kernel; + cl_kernel m_writeBackVelocitiesKernel; + + b3OpenCLArray* m_gpuSolverBodies; b3OpenCLArray* m_gpuBatchConstraints; b3OpenCLArray* m_gpuConstraintRows; + b3OpenCLArray* m_gpuConstraintInfo1; }; -b3GpuPgsJacobiSolver::b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs) - :b3PgsJacobiSolver (usePgs) + + + +static b3Transform getWorldTransform(b3RigidBodyCL* rb) { + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +} + +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia) +{ + return inertia->m_invInertiaWorld; +} + + + +static const b3Vector3& getLinearVelocity(b3RigidBodyCL* rb) +{ + return rb->m_linVel; +} + +static const b3Vector3& getAngularVelocity(b3RigidBodyCL* rb) +{ + return rb->m_angVel; +} + +b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos) +{ + //we also calculate lin/ang velocity for kinematic objects + return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); + +} + + + +b3GpuPgsJacobiSolver::b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs) +{ + m_usePgs = usePgs; m_gpuData = new b3GpuPgsJacobiSolverInternalData(); m_gpuData->m_context = ctx; m_gpuData->m_device = device; m_gpuData->m_queue = queue; - m_gpuData->m_gpuSolverConstraintRows = new b3OpenCLArray(ctx,queue); - m_gpuData->m_gpuSolverBodies = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); + m_gpuData->m_prefixScan = new b3PrefixScanCL(ctx,device,queue); + + m_gpuData->m_gpuSolverBodies = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); m_gpuData->m_gpuBatchConstraints = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); m_gpuData->m_gpuConstraintRows = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); - - + m_gpuData->m_gpuConstraintInfo1 = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); cl_int errNum=0; { @@ -53,6 +115,20 @@ b3GpuPgsJacobiSolver::b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, b3Assert(errNum==CL_SUCCESS); m_gpuData->m_solveJointConstraintRowsKernels = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device,solveConstraintRowsCL, "solveJointConstraintRows",&errNum,prog); b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_initSolverBodiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"initSolverBodies",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_getInfo1Kernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"getInfo1Kernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_initBatchConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"initBatchConstraintsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_getInfo2Kernel= b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"getInfo2Kernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_writeBackVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"writeBackVelocitiesKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + + + + clReleaseProgram(prog); } @@ -62,11 +138,17 @@ b3GpuPgsJacobiSolver::b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, b3GpuPgsJacobiSolver::~b3GpuPgsJacobiSolver () { clReleaseKernel(m_gpuData->m_solveJointConstraintRowsKernels); + clReleaseKernel(m_gpuData->m_initSolverBodiesKernel); + clReleaseKernel(m_gpuData->m_getInfo1Kernel); + clReleaseKernel(m_gpuData->m_initBatchConstraintsKernel); + clReleaseKernel(m_gpuData->m_getInfo2Kernel); + clReleaseKernel(m_gpuData->m_writeBackVelocitiesKernel); - delete m_gpuData->m_gpuSolverConstraintRows; + delete m_gpuData->m_prefixScan; delete m_gpuData->m_gpuSolverBodies; delete m_gpuData->m_gpuBatchConstraints; delete m_gpuData->m_gpuConstraintRows; + delete m_gpuData->m_gpuConstraintInfo1; delete m_gpuData; } @@ -93,299 +175,382 @@ static b3AlignedObjectArray batches; -b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies, b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("GPU solveGroupCacheFriendlySetup"); batchConstraints.resize(numConstraints); + m_gpuData->m_gpuBatchConstraints->resize(numConstraints); m_staticIdx = -1; m_maxOverrideNumSolverIterations = 0; + +/* m_gpuData->m_gpuBodies->resize(numBodies); + m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies); + + b3OpenCLArray gpuInertias(m_gpuData->m_context,m_gpuData->m_queue); + gpuInertias.resize(numBodies); + gpuInertias.copyFromHostPointer(inertias,numBodies); + */ + + m_gpuData->m_gpuSolverBodies->resize(numBodies); - m_tmpSolverBodyPool.resize(0); - - - m_bodyCount.resize(0); - m_bodyCount.resize(numBodies,0); - m_bodyCountCheck.resize(0); - m_bodyCountCheck.resize(numBodies,0); - - m_deltaLinearVelocities.resize(0); - m_deltaLinearVelocities.resize(numBodies,b3Vector3(0,0,0)); - m_deltaAngularVelocities.resize(0); - m_deltaAngularVelocities.resize(numBodies,b3Vector3(0,0,0)); + m_tmpSolverBodyPool.resize(numBodies); + { + bool useGpu = true; + if (useGpu) + { + B3_PROFILE("m_initSolverBodiesKernel"); + + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initSolverBodiesKernel); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_gpuData->m_queue); + +// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + } else + { + /* + for (int i=0;igetRigidBodyA(); - int bodyIndexB = constraints[i]->getRigidBodyB(); - if (m_usePgs) - { - m_bodyCount[bodyIndexA]=-1; - m_bodyCount[bodyIndexB]=-1; - } else - { - //didn't implement joints with Jacobi version yet - b3Assert(0); - } - - } - for (int i=0;iinternalSetAppliedImpulse(0.0f); - } - } - + int totalNumRows = 0; //b3RigidBody* rb0=0,*rb1=0; //if (1) { { - int totalNumRows = 0; - int i; + +// int i; m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); - //calculate the total number of contraint rows - for (i=0;i gpuConstraints(m_gpuData->m_context,m_gpuData->m_queue); + + + bool useGpu = true; + if (useGpu) { - b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + B3_PROFILE("info1 and init batchConstraint"); + + if (1) + { + m_gpuData->m_gpuConstraintInfo1->resize(numConstraints); +// gpuConstraints.resize(numConstraints); + // gpuConstraints.copyFromHostPointer(gpuConstraints,numConstraints); + // m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); - b3JointFeedback* fb = constraints[i]->getJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA.setZero(); - fb->m_appliedTorqueBodyA.setZero(); - fb->m_appliedForceBodyB.setZero(); - fb->m_appliedTorqueBodyB.setZero(); } + if (1) + { + B3_PROFILE("getInfo1Kernel"); - if (constraints[i]->isEnabled()) - { + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo1Kernel); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); } - if (constraints[i]->isEnabled()) + clFinish(m_gpuData->m_queue); + if (batches.size()==0) + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + + if (1) { - constraints[i]->getInfo1(&info1,bodies); - } else - { - info1.m_numConstraintRows = 0; - info1.nub = 0; - } + m_gpuData->m_gpuConstraintInfo1->copyToHost(m_tmpConstraintSizesPool); + b3OpenCLArray dst(m_gpuData->m_context,m_gpuData->m_queue); + dst.resize(numConstraints); + unsigned int total=0; + m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1,dst,numConstraints,&total); + unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints-1); + b3AlignedObjectArray dstHost; + dst.copyToHost(dstHost); + totalNumRows = total+lastElem; + + { + B3_PROFILE("init batch constraints"); + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initBatchConstraintsKernel); + launcher.setBuffer(dst.getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); + } + if (batches.size()==0) + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); - batchConstraints[i].m_numConstraintRows = info1.m_numConstraintRows; - batchConstraints[i].m_constraintRowOffset = totalNumRows; - totalNumRows += info1.m_numConstraintRows; + } + } + else + { +#if 0 + totalNumRows = 0; + //calculate the total number of contraint rows + for (i=0;im_gpuConstraintRows->resize(totalNumRows); + bool useGpuInfo2= true; +// b3ConstraintArray verify; - -#ifndef DISABLE_JOINTS - ///setup the b3SolverConstraints - int currentRow = 0; - - for (i=0;im_queue,m_gpuData->m_getInfo2Kernel); + launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setBuffer(gpuInertias->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setConst(infoGlobal.m_timeStep); + launcher.setConst(infoGlobal.m_erp); + launcher.setConst(infoGlobal.m_globalCfm); + launcher.setConst(infoGlobal.m_damping); + launcher.setConst(infoGlobal.m_numIterations); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); - b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; - b3TypedConstraint* constraint = constraints[i]; + if (batches.size()==0) + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + //m_gpuData->m_gpuConstraintRows->copyToHost(verify); + m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); - b3RigidBodyCL& rbA = bodies[ constraint->getRigidBodyA()]; - //b3RigidBody& rbA = constraint->getRigidBodyA(); - // b3RigidBody& rbB = constraint->getRigidBodyB(); - b3RigidBodyCL& rbB = bodies[ constraint->getRigidBodyB()]; - - - - int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias); - int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(),bodies,inertias); - - b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; - - if (rbA.getInvMass()) - { - batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; - } else - { - if (!solverBodyIdA) - m_staticIdx = 0; - batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; - } - - if (rbB.getInvMass()) - { - batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; - } else - { - if (!solverBodyIdB) - m_staticIdx = 0; - batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; - } - - - int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) - m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - - - int j; - for ( j=0;jinternalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - - - b3TypedConstraint::b3ConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; - info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; - info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = 0; - info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(b3SolverConstraint)/sizeof(b3Scalar);//check this - ///the size of b3SolverConstraint needs be a multiple of b3Scalar - b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint)); - info2.m_constraintError = ¤tConstraintRow->m_rhs; - currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; - info2.m_damping = infoGlobal.m_damping; - info2.cfm = ¤tConstraintRow->m_cfm; - info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; - info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; - info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2,bodies); - - ///finalize the constraint setup - for ( j=0;j=constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); - } - - if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); - } - - solverConstraint.m_originalContactPoint = constraint; - - b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld; - { - - //b3Vector3 angularFactorA(1,1,1); - const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; - } - b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld; - { - const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint->getRigidBodyB().getAngularFactor(); + } + } + else + { +#if 0 + ///setup the b3SolverConstraints + + for (i=0;i B3_EPSILON); - solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f; + batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; + } else + { + if (!solverBodyIdB) + m_staticIdx = 0; + batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; } - ///fix rhs - ///todo: add force/torque accelerators + int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + + int j; + for ( j=0;jinternalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + + b3GpuConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = 0; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(b3SolverConstraint)/sizeof(b3Scalar);//check this + ///the size of b3SolverConstraint needs be a multiple of b3Scalar + b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; + info2.m_damping = infoGlobal.m_damping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; + constraints[i].getInfo2(&info2,bodies); + + ///finalize the constraint setup + for ( j=0;j=constraints[i].getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = constraints[i].getBreakingImpulseThreshold(); + } + + if (solverConstraint.m_lowerLimit<=-constraints[i].getBreakingImpulseThreshold()) + { + solverConstraint.m_lowerLimit = -constraints[i].getBreakingImpulseThreshold(); + } + + // solverConstraint.m_originalContactPoint = constraint; + + b3Matrix3x3& invInertiaWorldA= inertias[constraint.getRigidBodyA()].m_invInertiaWorld; + { + + //b3Vector3 angularFactorA(1,1,1); + const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; + solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; + } + + b3Matrix3x3& invInertiaWorldB= inertias[constraint.getRigidBodyB()].m_invInertiaWorld; + { + + const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint.getRigidBodyB().getAngularFactor(); + } + + { + //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal + //because it gets multiplied iMJlB + b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); + b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal; + b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? + b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal; + + b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + b3Scalar fsum = b3Fabs(sum); + b3Assert(fsum > B3_EPSILON); + solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + b3Scalar rel_vel; + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel); + + rel_vel = vel1Dotn+vel2Dotn; + + b3Scalar restitution = 0.f; + b3Scalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + b3Scalar velocityError = restitution - rel_vel * info2.m_damping; + b3Scalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + b3Scalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + + } + } + } } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; } -#endif //DISABLE_JOINTS +#endif + } - +#ifdef B3_SUPPORT_CONTACT_CONSTRAINTS { int i; @@ -395,6 +560,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodie convertContact(bodies,inertias,&manifold,infoGlobal); } } +#endif //B3_SUPPORT_CONTACT_CONSTRAINTS } // b3ContactSolverInfo info = infoGlobal; @@ -404,29 +570,6 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodie int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); - else - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); - - m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); - { - int i; - for (i=0;im_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor; body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor); } -void resolveSingleConstraintRowGeneric2( b3SolverBody* body1, b3SolverBody* body2, b3SolverConstraint* c) +void resolveSingleConstraintRowGeneric2( b3GpuSolverBody* body1, b3GpuSolverBody* body2, b3SolverConstraint* c) { float deltaImpulse = c->m_rhs-c->m_appliedImpulse.x*c->m_cfm; float deltaVel1Dotn = b3Dot(c->m_contactNormal,body1->m_deltaLinearVelocity) + b3Dot(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity); @@ -474,14 +617,35 @@ void resolveSingleConstraintRowGeneric2( b3SolverBody* body1, b3SolverBody* bod -b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** cpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +void b3GpuPgsJacobiSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyCL* rb) { - bool useb3PgsJacobiSolver = false; - bool createBatches = batches.size()==0; - if (useb3PgsJacobiSolver) - { - return b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(cpuConstraints,numConstraints,infoGlobal); - } else + + solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + b3Assert(rb); +// solverBody->m_worldTransform = getWorldTransform(rb); + solverBody->internalSetInvMass(b3Vector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())); + solverBody->m_originalBodyIndex = bodyIndex; + solverBody->m_angularFactor = b3Vector3(1,1,1); + solverBody->m_linearFactor = b3Vector3(1,1,1); + solverBody->m_linearVelocity = getLinearVelocity(rb); + solverBody->m_angularVelocity = getAngularVelocity(rb); +} + + +void b3GpuPgsJacobiSolver::averageVelocities() +{ +} + + +b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +{ + //only create the batches once. + //@todo: incrementally update batches when constraints are added/activated and/or removed/deactivated + bool createBatches = true;//batches.size()==0; { B3_PROFILE("GpuSolveGroupCacheFriendlyIterations"); if (createBatches) @@ -490,27 +654,27 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstrai batches.resize(0); { + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + B3_PROFILE("batch joints"); b3Assert(batchConstraints.size()==numConstraints); int simdWidth =numConstraints; int numBodies = m_tmpSolverBodyPool.size(); sortConstraintByBatch3( &batchConstraints[0], numConstraints, simdWidth , m_staticIdx, numBodies); + + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + } + } else + { + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); } int maxIterations = infoGlobal.m_numIterations; bool useBatching = true; - bool useGpu=false; + bool useGpu=true; if (useBatching ) { - if (useGpu) - { - B3_PROFILE("copy from host"); - m_gpuData->m_gpuSolverConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); - m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); - m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); - m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); - } @@ -527,7 +691,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstrai if (useGpu) { - B3_PROFILE("b3LauncherCL"); + B3_PROFILE("solveJointConstraintRowsKernels"); b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_solveJointConstraintRowsKernels); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); @@ -541,10 +705,19 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstrai } else { + B3_PROFILE("copy from host"); + // m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); + // m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + // m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); + for (int b=0;bm_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); } - //int sz = sizeof(b3SolverBody); - //printf("cpu sizeof(b3SolverBody)=%d\n",sz); + //int sz = sizeof(b3GpuSolverBody); + //printf("cpu sizeof(b3GpuSolverBody)=%d\n",sz); @@ -585,7 +758,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstrai for (int j=0;j* gpuBodies, b3OpenCLArray* gpuInertias, + int numBodies, b3OpenCLArray* gpuConstraints,int numConstraints, const b3ContactSolverInfo& infoGlobal) +{ + + B3_PROFILE("solveJoints"); + //you need to provide at least some bodies + + solveGroupCacheFriendlySetup( gpuBodies, gpuInertias,numBodies,gpuConstraints, numConstraints,infoGlobal); + + solveGroupCacheFriendlyIterations(m_gpuData->m_gpuConstraintRows, numConstraints,infoGlobal); + + solveGroupCacheFriendlyFinish(gpuBodies, gpuInertias,numBodies, infoGlobal); + + return 0.f; +} + +void b3GpuPgsJacobiSolver::solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, + int numConstraints, b3OpenCLArray* gpuConstraints) +{ + b3ContactSolverInfo infoGlobal; + infoGlobal.m_splitImpulse = false; + infoGlobal.m_timeStep = 1.f/60.f; + infoGlobal.m_numIterations = 4;//4; +// infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; + //infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; + infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS; + + //if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + //if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + + + solveGroup(gpuBodies,gpuInertias,numBodies,gpuConstraints,numConstraints,infoGlobal); + +} + +//b3AlignedObjectArray testBodies; + + +b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,const b3ContactSolverInfo& infoGlobal) +{ + B3_PROFILE("solveGroupCacheFriendlyFinish"); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); +// int i,j; + + + { + bool useGpu = true; + if (useGpu) + { + B3_PROFILE("GPU write back velocities and transforms"); + + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_writeBackVelocitiesKernel); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_gpuData->m_queue); +// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); +// m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies); + //m_gpuData->m_gpuBodies->copyToHost(testBodies); + + } + else + { +#if 0 + B3_PROFILE("CPU write back velocities and transforms"); + for ( i=0;igetInvMass()) + { + if (infoGlobal.m_splitImpulse) + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + else + m_tmpSolverBodyPool[i].writebackVelocity(); + + if (m_usePgs) + { + body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; + body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; + } else + { + b3Assert(0); + } + /* + if (infoGlobal.m_splitImpulse) + { + body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); + b3Quaternion orn; + orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation(); + body->m_quat = orn; + } + */ + } + } +#endif + } + } + + + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + return 0.f; +} diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.h b/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.h index 5ef255b64..25c4240b9 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.h @@ -1,23 +1,76 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + #ifndef B3_GPU_PGS_JACOBI_SOLVER_H #define B3_GPU_PGS_JACOBI_SOLVER_H -#include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h" +struct b3Contact4; +struct b3ContactPoint; + + +class b3Dispatcher; + +#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" +#include "Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h" +#include "b3GpuSolverBody.h" +#include "Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +struct b3RigidBodyCL; +struct b3InertiaCL; + #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "b3GpuGenericConstraint.h" - -class b3GpuPgsJacobiSolver : public b3PgsJacobiSolver +class b3GpuPgsJacobiSolver { +protected: int m_staticIdx; struct b3GpuPgsJacobiSolverInternalData* m_gpuData; + protected: + b3AlignedObjectArray m_tmpSolverBodyPool; + b3ConstraintArray m_tmpSolverContactConstraintPool; + b3ConstraintArray m_tmpSolverNonContactConstraintPool; + b3ConstraintArray m_tmpSolverContactFrictionConstraintPool; + b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + + b3AlignedObjectArray m_tmpConstraintSizesPool; + + + bool m_usePgs; + void averageVelocities(); + + int m_maxOverrideNumSolverIterations; + + int m_numSplitImpulseRecoveries; + +// int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias); + void initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyCL* rb); public: b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs); virtual~b3GpuPgsJacobiSolver (); - virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveGroup(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + void solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, + int numConstraints, b3OpenCLArray* gpuConstraints); + int sortConstraintByBatch3( struct b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies); }; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index 38eb0ab3c..3e14cc3ba 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -1,3 +1,18 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + #include "b3GpuRigidBodyPipeline.h" #include "b3GpuRigidBodyPipelineInternalData.h" #include "kernels/integrateKernel.h" @@ -46,11 +61,13 @@ b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id devic m_data->m_device = device; m_data->m_queue = q; - m_data->m_solver = new b3GpuPgsJacobiSolver(ctx,device,q,true);//new b3PgsJacobiSolver(true); + m_data->m_solver = new b3PgsJacobiSolver(true);//new b3PgsJacobiSolver(true); + m_data->m_gpuSolver = new b3GpuPgsJacobiSolver(ctx,device,q,true);//new b3PgsJacobiSolver(true); m_data->m_allAabbsGPU = new b3OpenCLArray(ctx,q,config.m_maxConvexBodies); m_data->m_overlappingPairsGPU = new b3OpenCLArray(ctx,q,config.m_maxBroadphasePairs); + m_data->m_gpuConstraints = new b3OpenCLArray(ctx,q); #ifdef TEST_OTHER_GPU_SOLVER m_data->m_solver3 = new b3GpuJacobiSolver(ctx,device,q,config.m_maxBroadphasePairs); #endif // TEST_OTHER_GPU_SOLVER @@ -92,6 +109,7 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() delete m_data->m_raycaster; delete m_data->m_solver; delete m_data->m_allAabbsGPU; + delete m_data->m_gpuConstraints; delete m_data->m_overlappingPairsGPU; #ifdef TEST_OTHER_GPU_SOLVER @@ -106,9 +124,10 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() void b3GpuRigidBodyPipeline::reset() { + m_data->m_gpuConstraints->resize(0); + m_data->m_cpuConstraints.resize(0); m_data->m_allAabbsGPU->resize(0); m_data->m_allAabbsCPU.resize(0); - m_data->m_joints.resize(0); } void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint) @@ -121,6 +140,23 @@ void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint) m_data->m_joints.remove(constraint); } +int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB) +{ + b3GpuGenericConstraint c; + c.m_flags = B3_CONSTRAINT_FLAG_ENABLED; + c.m_rbA = bodyA; + c.m_rbB = bodyB; + c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); + c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); + c.m_breakingImpulseThreshold = 1e30f; + c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE; + m_data->m_cpuConstraints.push_back(c); + return 0; +} +int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB) +{ + return 0; +} void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) @@ -220,25 +256,32 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) b3OpenCLArray gpuContacts(m_data->m_context,m_data->m_queue,0,true); gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); - int numJoints = m_data->m_joints.size(); + int numJoints = m_data->m_cpuConstraints.size(); if (useBullet2CpuSolver && numJoints) { - b3AlignedObjectArray hostBodies; - gpuBodies.copyToHost(hostBodies); - b3AlignedObjectArray hostInertias; - gpuInertias.copyToHost(hostInertias); // b3AlignedObjectArray hostContacts; //gpuContacts.copyToHost(hostContacts); { - b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0; + bool useGpu = m_data->m_joints.size()==0; + // b3Contact4* contacts = numContacts? &hostContacts[0]: 0; //m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,contacts,numJoints, joints); - m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints); + if (useGpu) + { + m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(),&gpuBodies,&gpuInertias,numJoints, m_data->m_gpuConstraints); + } else + { + b3AlignedObjectArray hostBodies; + gpuBodies.copyToHost(hostBodies); + b3AlignedObjectArray hostInertias; + gpuInertias.copyToHost(hostInertias); + b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0; + m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints); + gpuBodies.copyFromHost(hostBodies); + } } - gpuBodies.copyFromHost(hostBodies); - } if (numContacts) @@ -387,6 +430,7 @@ void b3GpuRigidBodyPipeline::setGravity(const float* grav) void b3GpuRigidBodyPipeline::writeAllInstancesToGpu() { m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); } diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h index bc205e017..480fec0ae 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h @@ -1,3 +1,18 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + #ifndef B3_GPU_RIGIDBODY_PIPELINE_H #define B3_GPU_RIGIDBODY_PIPELINE_H @@ -40,6 +55,9 @@ public: void setGravity(const float* grav); void reset(); + int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB); + int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB); + void addConstraint(class b3TypedConstraint* constraint); void removeConstraint(b3TypedConstraint* constraint); diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h index 7358ea7c5..e604488b6 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h @@ -1,3 +1,18 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + #ifndef B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H #define B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H @@ -14,6 +29,7 @@ #include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h" +#include "Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h" struct b3GpuRigidBodyPipelineInternalData { @@ -26,6 +42,9 @@ struct b3GpuRigidBodyPipelineInternalData cl_kernel m_updateAabbsKernel; class b3PgsJacobiSolver* m_solver; + + class b3GpuPgsJacobiSolver* m_gpuSolver; + class b3GpuBatchingPgsSolver* m_solver2; class b3GpuJacobiSolver* m_solver3; class b3GpuRaycast* m_raycaster; @@ -37,7 +56,11 @@ struct b3GpuRigidBodyPipelineInternalData b3AlignedObjectArray m_allAabbsCPU; b3OpenCLArray* m_overlappingPairsGPU; + b3OpenCLArray* m_gpuConstraints; + b3AlignedObjectArray m_cpuConstraints; + b3AlignedObjectArray m_joints; + class b3GpuNarrowPhase* m_narrowphase; b3Vector3 m_gravity; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h b/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h new file mode 100644 index 000000000..89074b2e4 --- /dev/null +++ b/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h @@ -0,0 +1,228 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + + +#ifndef B3_GPU_SOLVER_BODY_H +#define B3_GPU_SOLVER_BODY_H + +class b3RigidBody; +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" + +#include "Bullet3Common/b3AlignedAllocator.h" +#include "Bullet3Common/b3TransformUtil.h" + +///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision +#ifdef B3_USE_SSE +#define USE_SIMD 1 +#endif // + + + +///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. +B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverBody +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); +// b3Transform m_worldTransformUnused; + b3Vector3 m_deltaLinearVelocity; + b3Vector3 m_deltaAngularVelocity; + b3Vector3 m_angularFactor; + b3Vector3 m_linearFactor; + b3Vector3 m_invMass; + b3Vector3 m_pushVelocity; + b3Vector3 m_turnVelocity; + b3Vector3 m_linearVelocity; + b3Vector3 m_angularVelocity; + + union + { + void* m_originalBody; + int m_originalBodyIndex; + }; + + int padding[3]; + + /* + void setWorldTransform(const b3Transform& worldTransform) + { + m_worldTransform = worldTransform; + } + + const b3Transform& getWorldTransform() const + { + return m_worldTransform; + } + */ + B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + if (m_originalBody) + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + else + velocity.setValue(0,0,0); + } + + B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const + { + if (m_originalBody) + angVel =m_angularVelocity+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + { + if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude) + { + if (m_originalBody) + { + m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + const b3Vector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const b3Vector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const b3Vector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const b3Vector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + b3Vector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + b3Vector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const b3Vector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const b3Vector3& internalGetInvMass() const + { + return m_invMass; + } + + void internalSetInvMass(const b3Vector3& invMass) + { + m_invMass = invMass; + } + + b3Vector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + b3Vector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + } + + B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const + { + angVel = m_angularVelocity+m_deltaAngularVelocity; + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + { + //if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + + void writebackVelocity() + { + //if (m_originalBody>=0) + { + m_linearVelocity +=m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //m_originalBody->setCompanionId(-1); + } + } + + + void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) + { + (void) timeStep; + if (m_originalBody) + { + m_linearVelocity += m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //correct the position/orientation based on push/turn recovery + b3Transform newTransform; + if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0) + { + // b3Quaternion orn = m_worldTransform.getRotation(); +// b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); +// m_worldTransform = newTransform; + } + //m_worldTransform.setRotation(orn); + //m_originalBody->setCompanionId(-1); + } + } + + + +}; + +#endif //B3_SOLVER_BODY_H + + diff --git a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl index 96f5e4d64..b431ba511 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl +++ b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl @@ -1,3 +1,17 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans float4 quatMult(float4 q1, float4 q2) { diff --git a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h index 296234c81..43d7eae85 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h +++ b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h @@ -1,5 +1,19 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project static const char* integrateKernelCL= \ +"/*\n" +"Copyright (c) 2013 Advanced Micro Devices, Inc. \n" +"\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"\n" +"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Erwin Coumans\n" "\n" "float4 quatMult(float4 q1, float4 q2)\n" "{\n" diff --git a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl index eb3f6040b..9925b2ee4 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl +++ b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl @@ -1,19 +1,84 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + + +#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3 +#define B3_INFINITY 1e30f + +#define mymake_float4 (float4) + + +__inline float dot3F4(float4 a, float4 b) +{ + float4 a1 = mymake_float4(a.xyz,0.f); + float4 b1 = mymake_float4(b.xyz,0.f); + return dot(a1, b1); +} typedef float4 Quaternion; + typedef struct { float4 m_row[3]; }Matrix3x3; +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + typedef struct { - Matrix3x3 m_invInertia; + Matrix3x3 m_invInertiaWorld; Matrix3x3 m_initInvInertia; -} Shape; +} BodyInertia; + typedef struct { @@ -23,7 +88,7 @@ typedef struct typedef struct { - b3Transform m_worldTransform; +// b3Transform m_worldTransformUnused; float4 m_deltaLinearVelocity; float4 m_deltaAngularVelocity; float4 m_angularFactor; @@ -41,9 +106,20 @@ typedef struct }; int padding[3]; -} b3SolverBody; +} b3GpuSolverBody; +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + unsigned int m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} b3RigidBodyCL; typedef struct { @@ -92,24 +168,109 @@ typedef struct } b3BatchConstraint; -#define mymake_float4 (float4) -__inline float dot3F4(float4 a, float4 b) + + +typedef struct { - float4 a1 = mymake_float4(a.xyz,0.f); - float4 b1 = mymake_float4(b.xyz,0.f); - return dot(a1, b1); + int m_constraintType; + int m_rbA; + int m_rbB; + float m_breakingImpulseThreshold; + + float4 m_pivotInA; + float4 m_pivotInB; + Quaternion m_relTargetAB; + + int m_flags; + int m_padding[3]; +} b3GpuGenericConstraint; + + +/*b3Transform getWorldTransform(b3RigidBodyCL* rb) +{ + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +}*/ + + + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); } -__inline void internalApplyImpulse(__global b3SolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude) +__inline +float4 fastNormalize4(float4 v) +{ + v = mymake_float4(v.xyz,0.f); + return fast_normalize(v); +} + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + + +__inline void internalApplyImpulse(__global b3GpuSolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude) { body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor; body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor); } -void resolveSingleConstraintRowGeneric(__global b3SolverBody* body1, __global b3SolverBody* body2, __global b3SolverConstraint* c) +void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c) { float deltaImpulse = c->m_rhs-c->m_appliedImpulse.x*c->m_cfm; float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity); @@ -140,7 +301,7 @@ void resolveSingleConstraintRowGeneric(__global b3SolverBody* body1, __global b3 } __kernel -void solveJointConstraintRows(__global b3SolverBody* solverBodies, +void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies, __global b3BatchConstraint* batchConstraints, __global b3SolverConstraint* rows, int batchOffset, @@ -158,4 +319,415 @@ void solveJointConstraintRows(__global b3SolverBody* solverBodies, __global b3SolverConstraint* constraint = &rows[c->m_constraintRowOffset+jj]; resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint); } -}; \ No newline at end of file +}; + +__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies) +{ + int i = get_global_id(0); + if (i>=numBodies) + return; + + __global b3GpuSolverBody* solverBody = &solverBodies[i]; + __global b3RigidBodyCL* bodyCL = &bodiesCL[i]; + + solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f); + solverBody->m_originalBodyIndex = i; + solverBody->m_angularFactor = (float4)(1,1,1,0); + solverBody->m_linearFactor = (float4) (1,1,1,0); + solverBody->m_linearVelocity = bodyCL->m_linVel; + solverBody->m_angularVelocity = bodyCL->m_angVel; +} + +__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, __global b3BatchConstraint* batchConstraints, int numConstraints) +{ + int i = get_global_id(0); + if (i>=numConstraints) + return; + + __global b3GpuGenericConstraint* constraint = &constraints[i]; + + switch (constraint->m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + infos[i] = 3; + batchConstraints[i].m_numConstraintRows = 3; + break; + } + default: + { + } + } +} + +__kernel void initBatchConstraintsKernel(__global unsigned int* rowOffsets, __global b3BatchConstraint* batchConstraints, int numConstraints) +{ + int i = get_global_id(0); + if (i>=numConstraints) + return; + + batchConstraints[i].m_constraintRowOffset = rowOffsets[i]; +} + + + + +typedef struct +{ + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + float fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + union + { + __global float4* m_J1linearAxisFloat4; + __global float* m_J1linearAxis; + }; + union + { + __global float4* m_J1angularAxisFloat4; + __global float* m_J1angularAxis; + + }; + union + { + __global float4* m_J2linearAxisFloat4; + __global float* m_J2linearAxis; + }; + union + { + __global float4* m_J2angularAxisFloat4; + __global float* m_J2angularAxis; + }; + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + __global float* m_constraintError; + __global float* cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + __global float* m_lowerLimit; + __global float* m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + __global int *findex; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + float m_damping; +} b3GpuConstraintInfo2; + + +void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2) +{ + *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f); + *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f); + *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f); +} + + +void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies) +{ + float4 posA = bodies[constraint->m_rbA].m_pos; + Quaternion rotA = bodies[constraint->m_rbA].m_quat; + + float4 posB = bodies[constraint->m_rbB].m_pos; + Quaternion rotB = bodies[constraint->m_rbB].m_quat; + + + + // anchor points in global coordinates with respect to body PORs. + + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + float4 a1 = qtRotate(rotA,constraint->m_pivotInA); + + { + __global float4* angular0 = (__global float4*)(info->m_J1angularAxis); + __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip); + __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip); + float4 a1neg = -a1; + getSkewSymmetricMatrix(a1neg,angular0,angular1,angular2); + } + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + float4 a2 = qtRotate(rotB,constraint->m_pivotInB); + + { + // float4 a2n = -a2; + __global float4* angular0 = (__global float4*)(info->m_J2angularAxis); + __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip); + __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip); + getSkewSymmetricMatrix(a2,angular0,angular1,angular2); + } + + // set right hand side +// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; + float currERP = info->erp; + + float k = info->fps * currERP; + int j; + float4 result = a2 + posB - a1 - posA; + float* resultPtr = &result; + + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]); + } +} + + +__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies) +{ + int i = get_global_id(0); + if (i>=numBodies) + return; + + if (bodies[i].m_invMass) + { +// solverBodies[i].m_linearVelocity += solverBodies[i].m_deltaLinearVelocity; +// solverBodies[i].m_angularVelocity += solverBodies[i].m_deltaAngularVelocity; + bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity; + bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity; + } +} + + +__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, + __global unsigned int* infos, + __global b3GpuGenericConstraint* constraints, + __global b3BatchConstraint* batchConstraints, + __global b3RigidBodyCL* bodies, + __global BodyInertia* inertias, + __global b3GpuSolverBody* solverBodies, + float timeStep, + float globalErp, + float globalCfm, + float globalDamping, + int globalNumIterations, + int numConstraints) +{ + + int i = get_global_id(0); + if (i>=numConstraints) + return; + + int info1 = infos[i]; + + if (info1) + { + __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[batchConstraints[i].m_constraintRowOffset]; + __global b3GpuGenericConstraint* constraint = &constraints[i]; + + __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA]; + __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB]; + + int solverBodyIdA = constraint->m_rbA; + int solverBodyIdB = constraint->m_rbB; + + __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA]; + __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB]; + + if (rbA->m_invMass) + { + batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; + } else + { +// if (!solverBodyIdA) +// m_staticIdx = 0; + batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; + } + + if (rbB->m_invMass) + { + batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; + } else + { +// if (!solverBodyIdB) +// m_staticIdx = 0; + batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; + } + + int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; +// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + // m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + + int j; + for ( j=0;jm_deltaLinearVelocity = (float4)(0,0,0,0); + bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0); + bodyAPtr->m_pushVelocity = (float4)(0,0,0,0); + bodyAPtr->m_turnVelocity = (float4)(0,0,0,0); + bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0); + bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0); + bodyBPtr->m_pushVelocity = (float4)(0,0,0,0); + bodyBPtr->m_turnVelocity = (float4)(0,0,0,0); + + int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this + + + + + b3GpuConstraintInfo2 info2; + info2.fps = 1.f/timeStep; + info2.erp = globalErp; + info2.m_J1linearAxisFloat4 = ¤tConstraintRow->m_contactNormal; + info2.m_J1angularAxisFloat4 = ¤tConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxisFloat4 = 0; + info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this + + ///the size of b3SolverConstraint needs be a multiple of float +// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = globalCfm; + info2.m_damping = globalDamping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = globalNumIterations; + + switch (constraint->m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + getInfo2Point2Point(constraint,&info2,bodies); + break; + } + default: + { + } + } + + ///finalize the constraint setup + for ( j=0;jm_upperLimit>=constraint->m_breakingImpulseThreshold) + { + solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold; + } + + if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold) + { + solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold; + } + +// solverConstraint->m_originalContactPoint = constraint; + + Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld; + { + + //float4 angularFactorA(1,1,1); + float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal; + solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA; + } + + Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld; + { + + float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal; + solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor(); + } + + { + //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal + //because it gets multiplied iMJlB + float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass; + float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA); + float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal? + float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB); + + float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal); + sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal); + sum += dot3F4(iMJlB,solverConstraint->m_contactNormal); + sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal); + float fsum = fabs(sum); + if (fsum>FLT_EPSILON) + { + solverConstraint->m_jacDiagABInv = 1.f/sum; + } else + { + solverConstraint->m_jacDiagABInv = 0.f; + } + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + float rel_vel; + float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel); + float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel); + + rel_vel = vel1Dotn+vel2Dotn; + + float restitution = 0.f; + float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2 + float velocityError = restitution - rel_vel * info2.m_damping; + float penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv; + float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv; + solverConstraint->m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint->m_appliedImpulse = 0.f; + + } + } + } +} \ No newline at end of file diff --git a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h index d43d2979b..4e0072975 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h +++ b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h @@ -1,21 +1,86 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project static const char* solveConstraintRowsCL= \ +"/*\n" +"Copyright (c) 2013 Advanced Micro Devices, Inc. \n" +"\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"\n" +"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Erwin Coumans\n" +"\n" +"\n" +"#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n" +"#define B3_INFINITY 1e30f\n" +"\n" +"#define mymake_float4 (float4)\n" +"\n" +"\n" +"__inline float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = mymake_float4(a.xyz,0.f);\n" +" float4 b1 = mymake_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" "\n" "\n" "typedef float4 Quaternion;\n" "\n" +"\n" "typedef struct\n" "{\n" " float4 m_row[3];\n" "}Matrix3x3;\n" "\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b);\n" +"\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b);\n" +"\n" +"\n" +"\n" +"\n" +"\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b)\n" +"{\n" +" float4 ans;\n" +" ans.x = dot3F4( a.m_row[0], b );\n" +" ans.y = dot3F4( a.m_row[1], b );\n" +" ans.z = dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b)\n" +"{\n" +" float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +"\n" +" float4 ans;\n" +" ans.x = dot3F4( a, colx );\n" +" ans.y = dot3F4( a, coly );\n" +" ans.z = dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"\n" "\n" "\n" "typedef struct\n" "{\n" -" Matrix3x3 m_invInertia;\n" +" Matrix3x3 m_invInertiaWorld;\n" " Matrix3x3 m_initInvInertia;\n" -"} Shape;\n" +"} BodyInertia;\n" +"\n" "\n" "typedef struct\n" "{\n" @@ -25,7 +90,7 @@ static const char* solveConstraintRowsCL= \ "\n" "typedef struct\n" "{\n" -" b3Transform m_worldTransform;\n" +"// b3Transform m_worldTransformUnused;\n" " float4 m_deltaLinearVelocity;\n" " float4 m_deltaAngularVelocity;\n" " float4 m_angularFactor;\n" @@ -43,9 +108,20 @@ static const char* solveConstraintRowsCL= \ " };\n" " int padding[3];\n" "\n" -"} b3SolverBody;\n" +"} b3GpuSolverBody;\n" "\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" Quaternion m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" "\n" +" unsigned int m_shapeIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} b3RigidBodyCL;\n" "\n" "typedef struct\n" "{\n" @@ -94,24 +170,109 @@ static const char* solveConstraintRowsCL= \ "\n" "} b3BatchConstraint;\n" "\n" -"#define mymake_float4 (float4)\n" "\n" "\n" -"__inline float dot3F4(float4 a, float4 b)\n" +"\n" +"\n" +"typedef struct \n" "{\n" -" float4 a1 = mymake_float4(a.xyz,0.f);\n" -" float4 b1 = mymake_float4(b.xyz,0.f);\n" -" return dot(a1, b1);\n" +" int m_constraintType;\n" +" int m_rbA;\n" +" int m_rbB;\n" +" float m_breakingImpulseThreshold;\n" +"\n" +" float4 m_pivotInA;\n" +" float4 m_pivotInB;\n" +" Quaternion m_relTargetAB;\n" +"\n" +" int m_flags;\n" +" int m_padding[3];\n" +"} b3GpuGenericConstraint;\n" +"\n" +"\n" +"/*b3Transform getWorldTransform(b3RigidBodyCL* rb)\n" +"{\n" +" b3Transform newTrans;\n" +" newTrans.setOrigin(rb->m_pos);\n" +" newTrans.setRotation(rb->m_quat);\n" +" return newTrans;\n" +"}*/\n" +"\n" +"\n" +"\n" +"\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" "}\n" "\n" -"__inline void internalApplyImpulse(__global b3SolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" v = mymake_float4(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +"\n" +"\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"\n" +"\n" +"\n" +"\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"\n" +"\n" +"__inline void internalApplyImpulse(__global b3GpuSolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)\n" "{\n" " body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor;\n" " body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor);\n" "}\n" "\n" "\n" -"void resolveSingleConstraintRowGeneric(__global b3SolverBody* body1, __global b3SolverBody* body2, __global b3SolverConstraint* c)\n" +"void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)\n" "{\n" " float deltaImpulse = c->m_rhs-c->m_appliedImpulse.x*c->m_cfm;\n" " float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);\n" @@ -142,7 +303,7 @@ static const char* solveConstraintRowsCL= \ "}\n" "\n" "__kernel\n" -"void solveJointConstraintRows(__global b3SolverBody* solverBodies,\n" +"void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n" " __global b3BatchConstraint* batchConstraints,\n" " __global b3SolverConstraint* rows,\n" " int batchOffset,\n" @@ -161,4 +322,415 @@ static const char* solveConstraintRowsCL= \ " resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n" " }\n" "};\n" +"\n" +"__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numBodies)\n" +" return;\n" +"\n" +" __global b3GpuSolverBody* solverBody = &solverBodies[i];\n" +" __global b3RigidBodyCL* bodyCL = &bodiesCL[i];\n" +"\n" +" solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f);\n" +" solverBody->m_originalBodyIndex = i;\n" +" solverBody->m_angularFactor = (float4)(1,1,1,0);\n" +" solverBody->m_linearFactor = (float4) (1,1,1,0);\n" +" solverBody->m_linearVelocity = bodyCL->m_linVel;\n" +" solverBody->m_angularVelocity = bodyCL->m_angVel;\n" +"}\n" +"\n" +"__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, __global b3BatchConstraint* batchConstraints, int numConstraints)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numConstraints)\n" +" return;\n" +"\n" +" __global b3GpuGenericConstraint* constraint = &constraints[i];\n" +"\n" +" switch (constraint->m_constraintType)\n" +" {\n" +" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" +" {\n" +" infos[i] = 3;\n" +" batchConstraints[i].m_numConstraintRows = 3;\n" +" break;\n" +" }\n" +" default:\n" +" {\n" +" }\n" +" }\n" +"}\n" +"\n" +"__kernel void initBatchConstraintsKernel(__global unsigned int* rowOffsets, __global b3BatchConstraint* batchConstraints, int numConstraints)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numConstraints)\n" +" return;\n" +"\n" +" batchConstraints[i].m_constraintRowOffset = rowOffsets[i];\n" +"}\n" +"\n" +"\n" +"\n" +"\n" +"typedef struct\n" +"{\n" +" // integrator parameters: frames per second (1/stepsize), default error\n" +" // reduction parameter (0..1).\n" +" float fps,erp;\n" +"\n" +" // for the first and second body, pointers to two (linear and angular)\n" +" // n*3 jacobian sub matrices, stored by rows. these matrices will have\n" +" // been initialized to 0 on entry. if the second body is zero then the\n" +" // J2xx pointers may be 0.\n" +" union \n" +" {\n" +" __global float4* m_J1linearAxisFloat4;\n" +" __global float* m_J1linearAxis;\n" +" };\n" +" union\n" +" {\n" +" __global float4* m_J1angularAxisFloat4;\n" +" __global float* m_J1angularAxis;\n" +"\n" +" };\n" +" union\n" +" {\n" +" __global float4* m_J2linearAxisFloat4;\n" +" __global float* m_J2linearAxis;\n" +" };\n" +" union\n" +" {\n" +" __global float4* m_J2angularAxisFloat4;\n" +" __global float* m_J2angularAxis;\n" +" };\n" +" // elements to jump from one row to the next in J's\n" +" int rowskip;\n" +"\n" +" // right hand sides of the equation J*v = c + cfm * lambda. cfm is the\n" +" // \"constraint force mixing\" vector. c is set to zero on entry, cfm is\n" +" // set to a constant value (typically very small or zero) value on entry.\n" +" __global float* m_constraintError;\n" +" __global float* cfm;\n" +"\n" +" // lo and hi limits for variables (set to -/+ infinity on entry).\n" +" __global float* m_lowerLimit;\n" +" __global float* m_upperLimit;\n" +"\n" +" // findex vector for variables. see the LCP solver interface for a\n" +" // description of what this does. this is set to -1 on entry.\n" +" // note that the returned indexes are relative to the first index of\n" +" // the constraint.\n" +" __global int *findex;\n" +" // number of solver iterations\n" +" int m_numIterations;\n" +"\n" +" //damping of the velocity\n" +" float m_damping;\n" +"} b3GpuConstraintInfo2;\n" +"\n" +"\n" +"void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2)\n" +"{\n" +" *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f);\n" +" *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f);\n" +" *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f);\n" +"}\n" +"\n" +"\n" +"void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies)\n" +"{\n" +" float4 posA = bodies[constraint->m_rbA].m_pos;\n" +" Quaternion rotA = bodies[constraint->m_rbA].m_quat;\n" +"\n" +" float4 posB = bodies[constraint->m_rbB].m_pos;\n" +" Quaternion rotB = bodies[constraint->m_rbB].m_quat;\n" +"\n" +"\n" +"\n" +" // anchor points in global coordinates with respect to body PORs.\n" +" \n" +" // set jacobian\n" +" info->m_J1linearAxis[0] = 1;\n" +" info->m_J1linearAxis[info->rowskip+1] = 1;\n" +" info->m_J1linearAxis[2*info->rowskip+2] = 1;\n" +"\n" +" float4 a1 = qtRotate(rotA,constraint->m_pivotInA);\n" +"\n" +" {\n" +" __global float4* angular0 = (__global float4*)(info->m_J1angularAxis);\n" +" __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip);\n" +" __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip);\n" +" float4 a1neg = -a1;\n" +" getSkewSymmetricMatrix(a1neg,angular0,angular1,angular2);\n" +" }\n" +" if (info->m_J2linearAxis)\n" +" {\n" +" info->m_J2linearAxis[0] = -1;\n" +" info->m_J2linearAxis[info->rowskip+1] = -1;\n" +" info->m_J2linearAxis[2*info->rowskip+2] = -1;\n" +" }\n" +" \n" +" float4 a2 = qtRotate(rotB,constraint->m_pivotInB);\n" +" \n" +" {\n" +" // float4 a2n = -a2;\n" +" __global float4* angular0 = (__global float4*)(info->m_J2angularAxis);\n" +" __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip);\n" +" __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip);\n" +" getSkewSymmetricMatrix(a2,angular0,angular1,angular2);\n" +" }\n" +" \n" +" // set right hand side\n" +"// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;\n" +" float currERP = info->erp;\n" +"\n" +" float k = info->fps * currERP;\n" +" int j;\n" +" float4 result = a2 + posB - a1 - posA;\n" +" float* resultPtr = &result;\n" +"\n" +" for (j=0; j<3; j++)\n" +" {\n" +" info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]);\n" +" }\n" +"}\n" +"\n" +"\n" +"__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numBodies)\n" +" return;\n" +"\n" +" if (bodies[i].m_invMass)\n" +" {\n" +"// solverBodies[i].m_linearVelocity += solverBodies[i].m_deltaLinearVelocity;\n" +"// solverBodies[i].m_angularVelocity += solverBodies[i].m_deltaAngularVelocity;\n" +" bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;\n" +" bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;\n" +" }\n" +"}\n" +"\n" +"\n" +"__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n" +" __global unsigned int* infos, \n" +" __global b3GpuGenericConstraint* constraints, \n" +" __global b3BatchConstraint* batchConstraints, \n" +" __global b3RigidBodyCL* bodies,\n" +" __global BodyInertia* inertias,\n" +" __global b3GpuSolverBody* solverBodies,\n" +" float timeStep,\n" +" float globalErp,\n" +" float globalCfm,\n" +" float globalDamping,\n" +" int globalNumIterations,\n" +" int numConstraints)\n" +"{\n" +"\n" +" int i = get_global_id(0);\n" +" if (i>=numConstraints)\n" +" return;\n" +" \n" +" int info1 = infos[i];\n" +" \n" +" if (info1)\n" +" {\n" +" __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[batchConstraints[i].m_constraintRowOffset];\n" +" __global b3GpuGenericConstraint* constraint = &constraints[i];\n" +"\n" +" __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n" +" __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n" +"\n" +" int solverBodyIdA = constraint->m_rbA;\n" +" int solverBodyIdB = constraint->m_rbB;\n" +"\n" +" __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n" +" __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n" +"\n" +" if (rbA->m_invMass)\n" +" {\n" +" batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n" +" } else\n" +" {\n" +"// if (!solverBodyIdA)\n" +"// m_staticIdx = 0;\n" +" batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n" +" }\n" +"\n" +" if (rbB->m_invMass)\n" +" {\n" +" batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n" +" } else\n" +" {\n" +"// if (!solverBodyIdB)\n" +"// m_staticIdx = 0;\n" +" batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n" +" }\n" +"\n" +" int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n" +"// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n" +" // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n" +"\n" +"\n" +" int j;\n" +" for ( j=0;jm_deltaLinearVelocity = (float4)(0,0,0,0);\n" +" bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" +" bodyAPtr->m_pushVelocity = (float4)(0,0,0,0);\n" +" bodyAPtr->m_turnVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_pushVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_turnVelocity = (float4)(0,0,0,0);\n" +"\n" +" int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" +"\n" +" \n" +"\n" +"\n" +" b3GpuConstraintInfo2 info2;\n" +" info2.fps = 1.f/timeStep;\n" +" info2.erp = globalErp;\n" +" info2.m_J1linearAxisFloat4 = ¤tConstraintRow->m_contactNormal;\n" +" info2.m_J1angularAxisFloat4 = ¤tConstraintRow->m_relpos1CrossNormal;\n" +" info2.m_J2linearAxisFloat4 = 0;\n" +" info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal;\n" +" info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" +"\n" +" ///the size of b3SolverConstraint needs be a multiple of float\n" +"// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint));\n" +" info2.m_constraintError = ¤tConstraintRow->m_rhs;\n" +" currentConstraintRow->m_cfm = globalCfm;\n" +" info2.m_damping = globalDamping;\n" +" info2.cfm = ¤tConstraintRow->m_cfm;\n" +" info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;\n" +" info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;\n" +" info2.m_numIterations = globalNumIterations;\n" +"\n" +" switch (constraint->m_constraintType)\n" +" {\n" +" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" +" {\n" +" getInfo2Point2Point(constraint,&info2,bodies);\n" +" break;\n" +" }\n" +" default:\n" +" {\n" +" }\n" +" }\n" +"\n" +" ///finalize the constraint setup\n" +" for ( j=0;jm_upperLimit>=constraint->m_breakingImpulseThreshold)\n" +" {\n" +" solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold;\n" +" }\n" +"\n" +" if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold)\n" +" {\n" +" solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold;\n" +" }\n" +"\n" +"// solverConstraint->m_originalContactPoint = constraint;\n" +" \n" +" Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld;\n" +" {\n" +"\n" +" //float4 angularFactorA(1,1,1);\n" +" float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal;\n" +" solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA;\n" +" }\n" +" \n" +" Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld;\n" +" {\n" +"\n" +" float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal;\n" +" solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor();\n" +" }\n" +"\n" +" {\n" +" //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal\n" +" //because it gets multiplied iMJlB\n" +" float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass;\n" +" float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA);\n" +" float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal?\n" +" float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB);\n" +"\n" +" float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal);\n" +" sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal);\n" +" sum += dot3F4(iMJlB,solverConstraint->m_contactNormal);\n" +" sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal);\n" +" float fsum = fabs(sum);\n" +" if (fsum>FLT_EPSILON)\n" +" {\n" +" solverConstraint->m_jacDiagABInv = 1.f/sum;\n" +" } else\n" +" {\n" +" solverConstraint->m_jacDiagABInv = 0.f;\n" +" }\n" +" }\n" +"\n" +"\n" +" ///fix rhs\n" +" ///todo: add force/torque accelerators\n" +" {\n" +" float rel_vel;\n" +" float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel);\n" +" float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel);\n" +"\n" +" rel_vel = vel1Dotn+vel2Dotn;\n" +"\n" +" float restitution = 0.f;\n" +" float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2\n" +" float velocityError = restitution - rel_vel * info2.m_damping;\n" +" float penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv;\n" +" float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv;\n" +" solverConstraint->m_rhs = penetrationImpulse+velocityImpulse;\n" +" solverConstraint->m_appliedImpulse = 0.f;\n" +"\n" +" }\n" +" }\n" +" }\n" +"}\n" ; diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl index 0c82d70ae..933f39e88 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl +++ b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl @@ -1,5 +1,5 @@ /* -Copyright (c) 2012 Advanced Micro Devices, Inc. +Copyright (c) 2013 Advanced Micro Devices, Inc. This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -11,7 +11,7 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ - +//Originally written by Erwin Coumans #pragma OPENCL EXTENSION cl_amd_printf : enable #pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h index 91726f36e..cdd3b66d4 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h +++ b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h @@ -13,7 +13,7 @@ static const char* solverUtilsCL= \ "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" "3. This notice may not be removed or altered from any source distribution.\n" "*/\n" -"\n" +"//Originally written by Erwin Coumans\n" "\n" "#pragma OPENCL EXTENSION cl_amd_printf : enable\n" "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"