diff --git a/demo/gpudemo/rigidbody/Bullet2GpuDemo.cpp b/demo/gpudemo/rigidbody/Bullet2GpuDemo.cpp new file mode 100644 index 000000000..c2c054af8 --- /dev/null +++ b/demo/gpudemo/rigidbody/Bullet2GpuDemo.cpp @@ -0,0 +1,28 @@ +#include "Bullet2GpuDemo.h" +#include "../btGpuDynamicsWorld.h" +#include "GpuRigidBodyDemoInternalData.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "gpu_rigidbody/host/btRigidBody.h" + +void Bullet2GpuDemo::setupScene(const ConstructionInfo& ci) +{ + +// m_data->m_np = np; +// m_data->m_bp = bp; +// m_data->m_rigidBodyPipeline + m_gpuDynamicsWorld = new btGpuDynamicsWorld(m_data->m_bp,m_data->m_np,m_data->m_rigidBodyPipeline); + + btVector3 halfExtents(100,1,100); + btBoxShape* boxShape = new btBoxShape(halfExtents); + btVector3 localInertia; + btScalar mass=1.f; + boxShape->calculateLocalInertia(mass,localInertia); + btRigidBody* body = new btRigidBody(mass,0,boxShape,localInertia); + m_gpuDynamicsWorld->addRigidBody(body); +} + +void Bullet2GpuDemo::destroyScene() +{ + delete m_gpuDynamicsWorld; + m_gpuDynamicsWorld = 0; +} \ No newline at end of file diff --git a/demo/gpudemo/rigidbody/Bullet2GpuDemo.h b/demo/gpudemo/rigidbody/Bullet2GpuDemo.h new file mode 100644 index 000000000..7e9158cf2 --- /dev/null +++ b/demo/gpudemo/rigidbody/Bullet2GpuDemo.h @@ -0,0 +1,31 @@ +#ifndef BULLET2_GPU_DEMO_H +#define BULLET2_GPU_DEMO_H + +#include "GpuRigidBodyDemo.h" + +class Bullet2GpuDemo : public GpuRigidBodyDemo +{ +protected: + + class btGpuDynamicsWorld* m_gpuDynamicsWorld; + +public: + + Bullet2GpuDemo(){} + virtual ~Bullet2GpuDemo(){} + virtual const char* getName() + { + return "Bullet2Gpu"; + } + + static GpuDemo* MyCreateFunc() + { + GpuDemo* demo = new Bullet2GpuDemo; + return demo; + } + + virtual void setupScene(const ConstructionInfo& ci); + virtual void destroyScene(); +}; +#endif //BULLET2_GPU_DEMO_H + diff --git a/demo/gpudemo/rigidbody/GpuConvexScene.cpp b/demo/gpudemo/rigidbody/GpuConvexScene.cpp index 246de37d2..16fd83d2b 100644 --- a/demo/gpudemo/rigidbody/GpuConvexScene.cpp +++ b/demo/gpudemo/rigidbody/GpuConvexScene.cpp @@ -17,7 +17,7 @@ #include "gpu_rigidbody/host/b3Config.h" #include "GpuRigidBodyDemoInternalData.h" #include "../gwenUserInterface.h" - +#include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h" void GpuConvexScene::setupScene(const ConstructionInfo& ci) { @@ -83,6 +83,9 @@ int GpuConvexScene::createDynamicsObjects2(const ConstructionInfo& ci, const flo int curColor = 0; float scaling[4] = {1,1,1,1}; + int prevBody = -1; + int insta = 0; + int colIndex = m_data->m_np->registerConvexHullShape(&vertices[0],strideInBytes,numVertices, scaling); //int colIndex = m_data->m_np->registerSphereShape(1); for (int i=0;iregisterGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); + + if (prevBody>=0) + { + b3Point2PointConstraint* p2p = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0)); +// m_data->m_rigidBodyPipeline->addConstraint(p2p);//,false); + } + prevBody = pid; + index++; } } diff --git a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp index f13610ae7..cf77e4e71 100644 --- a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp +++ b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp @@ -15,8 +15,8 @@ //#define TEST_OTHER_GPU_SOLVER -bool useDbvt = true; -bool useBullet2CpuSolver = false; +bool useDbvt = false; +bool useBullet2CpuSolver = true;//false; bool dumpContactStats = false; #ifdef TEST_OTHER_GPU_SOLVER @@ -94,6 +94,11 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() delete m_data; } + +void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint) +{ + m_data->m_joints.push_back(constraint); +} void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) { @@ -183,30 +188,37 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) //convert contact points to contact constraints //solve constraints - + + btOpenCLArray gpuBodies(m_data->m_context,m_data->m_queue,0,true); + gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumBodiesGpu()); + btOpenCLArray gpuInertias(m_data->m_context,m_data->m_queue,0,true); + gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumBodiesGpu()); + btOpenCLArray gpuContacts(m_data->m_context,m_data->m_queue,0,true); + gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); + + if (useBullet2CpuSolver) + { + + b3AlignedObjectArray hostBodies; + gpuBodies.copyToHost(hostBodies); + b3AlignedObjectArray hostInertias; + gpuInertias.copyToHost(hostInertias); + b3AlignedObjectArray hostContacts; + gpuContacts.copyToHost(hostContacts); + { + int numJoints = m_data->m_joints.size(); + b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 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->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints); + + } + gpuBodies.copyFromHost(hostBodies); + } + if (numContacts) { - btOpenCLArray gpuBodies(m_data->m_context,m_data->m_queue,0,true); - gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumBodiesGpu()); - btOpenCLArray gpuInertias(m_data->m_context,m_data->m_queue,0,true); - gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumBodiesGpu()); - btOpenCLArray gpuContacts(m_data->m_context,m_data->m_queue,0,true); - gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); - - if (useBullet2CpuSolver) - { - b3AlignedObjectArray hostBodies; - gpuBodies.copyToHost(hostBodies); - b3AlignedObjectArray hostInertias; - gpuInertias.copyToHost(hostInertias); - b3AlignedObjectArray hostContacts; - gpuContacts.copyToHost(hostContacts); - { - m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]); - } - gpuBodies.copyFromHost(hostBodies); - } else #ifdef TEST_OTHER_GPU_SOLVER if (useJacobi) { diff --git a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h index 7541619b8..c27b34f9f 100644 --- a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h +++ b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h @@ -34,6 +34,8 @@ public: //if you passed "writeInstanceToGpu" false in the registerPhysicsInstance method (for performance) you need to call writeAllInstancesToGpu after all instances are registered void writeAllInstancesToGpu(); + void addConstraint(class b3TypedConstraint* constraint); + cl_mem getBodyBuffer(); int getNumBodies() const; diff --git a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h index 17d377003..fac30328f 100644 --- a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h +++ b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h @@ -8,7 +8,7 @@ #include "../../gpu_narrowphase/host/b3Collidable.h" #include "gpu_broadphase/host/b3SapAabb.h" - +#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" @@ -35,7 +35,7 @@ struct b3GpuRigidBodyPipelineInternalData b3AlignedObjectArray m_allAabbsCPU; btOpenCLArray* m_overlappingPairsGPU; - + b3AlignedObjectArray m_joints; class b3GpuNarrowPhase* m_narrowphase; }; diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp index 12e7ad4e3..cbc5e60d5 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: //#define COMPUTE_IMPULSE_DENOM 1 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. -#define DISABLE_JOINTS +//#define DISABLE_JOINTS #include "b3PgsJacobiSolver.h" #include "Bullet3Common/b3MinMax.h" @@ -154,7 +154,7 @@ b3PgsJacobiSolver::~b3PgsJacobiSolver() { } -void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts) +void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints) { b3ContactSolverInfo infoGlobal; infoGlobal.m_splitImpulse = false; @@ -168,7 +168,7 @@ void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btIn //if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - solveGroup(bodies,inertias,numBodies,contacts,numContacts,0,0,infoGlobal); + solveGroup(bodies,inertias,numBodies,contacts,numContacts,constraints,numConstraints,infoGlobal); if (!numContacts) return; @@ -1088,6 +1088,21 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, int totalBodies = 0; + 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 + btAssert(0); + } + + } for (int i=0;igetRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); + b3RigidBodyCL& rbA = bodies[ constraint->getRigidBodyA()]; + //btRigidBody& rbA = constraint->getRigidBodyA(); + // btRigidBody& rbB = constraint->getRigidBodyB(); + b3RigidBodyCL& rbB = bodies[ constraint->getRigidBodyB()]; - int solverBodyIdA = getOrInitSolverBody(rbA,); - int solverBodyIdB = getOrInitSolverBody(rbB); + int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias); + int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(),bodies,inertias); b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; @@ -1238,7 +1255,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2); + constraints[i]->getInfo2(&info2,bodies); ///finalize the constraint setup for ( j=0;jgetRigidBodyA()].m_invInertiaWorld; + { - { + //btVector3 angularFactorA(1,1,1); const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); + solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; } + + b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld; { + const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); + solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint->getRigidBodyB().getAngularFactor(); } { b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); - b3Vector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; + b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal; b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? - b3Vector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; + b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal; b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); @@ -1286,8 +1309,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, ///todo: add force/torque accelerators { b3Scalar rel_vel; - b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); - b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); + 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; @@ -1406,20 +1429,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint if (iteration< infoGlobal.m_numIterations) { -#ifndef DISABLE_JOINTS - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - b3SolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - b3SolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } -#endif - ///solve all contact constraints using SIMD, if available if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) @@ -1539,19 +1548,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint if (iteration< infoGlobal.m_numIterations) { -#ifndef DISABLE_JOINTS - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - b3SolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - b3SolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } -#endif //DISABLE_JOINTS ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h index 1fc7a0eba..02f072bf5 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h @@ -116,8 +116,9 @@ public: b3PgsJacobiSolver(); virtual ~b3PgsJacobiSolver(); - void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts); - +// void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts); + void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); + b3Scalar solveGroup(b3RigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); ///clear internal cached data and reset random seed diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp new file mode 100644 index 000000000..25e10e732 --- /dev/null +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -0,0 +1,225 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + + +#include "b3Point2PointConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" + +#include + + + + + +b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB) +:b3TypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_flags(0), +m_useSolveConstraintObsolete(false) +{ + +} + +/* +b3Point2PointConstraint::b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA) +:b3TypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_flags(0), +m_useSolveConstraintObsolete(false) +{ + +} +*/ + +void b3Point2PointConstraint::buildJacobian() +{ + + ///we need it for both methods + { + m_appliedImpulse = b3Scalar(0.); + } + +} + +void b3Point2PointConstraint::getInfo1 (btConstraintInfo1* info) +{ + getInfo1NonVirtual(info); +} + +void b3Point2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + info->m_numConstraintRows = 3; + info->nub = 3; + } +} + + + + +void b3Point2PointConstraint::getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies) +{ + b3Transform trA; + trA.setIdentity(); + trA.setOrigin(bodies[m_rbA].m_pos); + trA.setRotation(bodies[m_rbB].m_quat); + + b3Transform trB; + trB.setIdentity(); + trB.setOrigin(bodies[m_rbB].m_pos); + trB.setRotation(bodies[m_rbB].m_quat); + + getInfo2NonVirtual(info, trA,trB); +} + +void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) +{ + btAssert(!m_useSolveConstraintObsolete); + + //retrieve matrices + + // 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 = body0_trans.getBasis()*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 = body1_trans.getBasis()*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 & BT_P2P_FLAGS_ERP) ? m_erp : info->erp; + b3Scalar k = info->fps * currERP; + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + if(m_flags & BT_P2P_FLAGS_CFM) + { + for (j=0; j<3; j++) + { + info->cfm[j*info->rowskip] = m_cfm; + } + } + + 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; + +} + + + +void b3Point2PointConstraint::updateRHS(b3Scalar timeStep) +{ + (void)timeStep; + +} + +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. +void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis) +{ + if(axis != -1) + { + btAssertConstrParams(0); + } + else + { + switch(num) + { + case BT_CONSTRAINT_ERP : + case BT_CONSTRAINT_STOP_ERP : + m_erp = value; + m_flags |= BT_P2P_FLAGS_ERP; + break; + case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_STOP_CFM : + m_cfm = value; + m_flags |= BT_P2P_FLAGS_CFM; + break; + default: + btAssertConstrParams(0); + } + } +} + +///return the local value of parameter +b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const +{ + b3Scalar retVal(SIMD_INFINITY); + if(axis != -1) + { + btAssertConstrParams(0); + } + else + { + switch(num) + { + case BT_CONSTRAINT_ERP : + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP); + retVal = m_erp; + break; + case BT_CONSTRAINT_CFM : + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM); + retVal = m_cfm; + break; + default: + btAssertConstrParams(0); + } + } + return retVal; +} + diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h new file mode 100644 index 000000000..d041dc6db --- /dev/null +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -0,0 +1,163 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +#ifndef BT_POINT2POINTCONSTRAINT_H +#define BT_POINT2POINTCONSTRAINT_H + +#include "Bullet3Common/b3Vector3.h" +//#include "b3JacobianEntry.h" +#include "b3TypedConstraint.h" + +class btRigidBody; + + +#ifdef BT_USE_DOUBLE_PRECISION +#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData +#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData" +#else +#define btPoint2PointConstraintData btPoint2PointConstraintFloatData +#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION + +struct btConstraintSetting +{ + btConstraintSetting() : + m_tau(b3Scalar(0.3)), + m_damping(b3Scalar(1.)), + m_impulseClamp(b3Scalar(0.)) + { + } + b3Scalar m_tau; + b3Scalar m_damping; + b3Scalar m_impulseClamp; +}; + +enum btPoint2PointFlags +{ + BT_P2P_FLAGS_ERP = 1, + BT_P2P_FLAGS_CFM = 2 +}; + +/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space +ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint +{ +#ifdef IN_PARALLELL_SOLVER +public: +#endif + + b3Vector3 m_pivotInA; + b3Vector3 m_pivotInB; + + int m_flags; + b3Scalar m_erp; + b3Scalar m_cfm; + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + + btConstraintSetting m_setting; + + b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB); + + b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA); + + + virtual void buildJacobian(); + + virtual void getInfo1 (btConstraintInfo1* info); + + void getInfo1NonVirtual (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies); + + void getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans); + + void updateRHS(b3Scalar timeStep); + + void setPivotA(const b3Vector3& pivotA) + { + m_pivotInA = pivotA; + } + + void setPivotB(const b3Vector3& pivotB) + { + m_pivotInB = pivotB; + } + + const b3Vector3& getPivotInA() const + { + return m_pivotInA; + } + + const b3Vector3& getPivotInB() const + { + return m_pivotInB; + } + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, b3Scalar value, int axis = -1); + ///return the local value of parameter + virtual b3Scalar getParam(int num, int axis = -1) const; + +// virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +// virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btPoint2PointConstraintFloatData +{ + btTypedConstraintData m_typeConstraintData; + btVector3FloatData m_pivotInA; + btVector3FloatData m_pivotInB; +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btPoint2PointConstraintDoubleData +{ + btTypedConstraintData m_typeConstraintData; + btVector3DoubleData m_pivotInA; + btVector3DoubleData m_pivotInB; +}; + +/* +SIMD_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btPoint2PointConstraintData); + +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer; + + b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); + m_pivotInA.serialize(p2pData->m_pivotInA); + m_pivotInB.serialize(p2pData->m_pivotInB); + + return btPoint2PointConstraintDataName; +} +*/ + +#endif //BT_POINT2POINTCONSTRAINT_H diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h index 7175f9e95..a8519e7c1 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h @@ -259,7 +259,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody void writebackVelocity() { - if (m_originalBody) + //if (m_originalBody>=0) { m_linearVelocity +=m_deltaLinearVelocity; m_angularVelocity += m_deltaAngularVelocity; diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h index 079bdd33d..983dfd127 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -60,6 +60,8 @@ ATTRIBUTE_ALIGNED16(struct) btJointFeedback b3Vector3 m_appliedTorqueBodyB; }; +struct b3RigidBodyCL; + ///TypedConstraint is the baseclass for Bullet constraints and vehicles ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public btTypedObject @@ -171,7 +173,7 @@ public: virtual void getInfo1 (btConstraintInfo1* info)=0; ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (btConstraintInfo2* info)=0; + virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies)=0; ///internal method used by the constraint solver, don't use them directly void internalSetAppliedImpulse(b3Scalar appliedImpulse) @@ -321,10 +323,10 @@ public: ///return the local value of parameter virtual b3Scalar getParam(int num, int axis = -1) const = 0; - virtual int calculateSerializeBufferSize() const; +// virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + //virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; @@ -377,11 +379,11 @@ struct btTypedConstraintData }; -SIMD_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const +/*SIMD_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const { return sizeof(btTypedConstraintData); } - +*/ class btAngularLimit diff --git a/src/Bullet3Dynamics/Dynamics/btGpuDynamicsWorld.cpp b/src/Bullet3Dynamics/Dynamics/btGpuDynamicsWorld.cpp new file mode 100644 index 000000000..70597578c --- /dev/null +++ b/src/Bullet3Dynamics/Dynamics/btGpuDynamicsWorld.cpp @@ -0,0 +1,277 @@ +#include "btGpuDynamicsWorld.h" +#include "gpu_rigidbody/host/btRigidBody.h" + + +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" + +#include "BulletCommon/btQuickprof.h" + +#include "gpu_rigidbody/host/btGpuRigidBodyPipeline.h" + +#include "gpu_rigidbody/host/btGpuRigidBodyPipeline.h" +#include "gpu_rigidbody/host/btGpuNarrowPhase.h" + +#ifdef _WIN32 + #include +#endif + + + +btGpuDynamicsWorld::btGpuDynamicsWorld(btGpuSapBroadphase*bp, btGpuNarrowPhase* np, btGpuRigidBodyPipeline* rb) +:m_gravity(0,-10,0), +m_once(true) +{ + m_bp = bp; + m_np=np; + m_rigidBodyPipeline = rb; + +} + +btGpuDynamicsWorld::~btGpuDynamicsWorld() +{ +} + + + +int btGpuDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +{ +#ifndef BT_NO_PROFILE +// CProfileManager::Reset(); +#endif //BT_NO_PROFILE + + BT_PROFILE("stepSimulation"); + + m_rigidBodyPipeline->stepSimulation(timeStep); + + { + BT_PROFILE("scatter transforms into rigidbody (CPU)"); + for (int i=0;im_collisionObjects.size();i++) + { + btVector3 pos; + btQuaternion orn; + cl_mem gpuBodies = m_rigidBodyPipeline->getBodyBuffer(); + btTransform newTrans; + newTrans.setOrigin(pos); + newTrans.setRotation(orn); + this->m_collisionObjects[i]->setWorldTransform(newTrans); + } + } + + +#ifndef BT_NO_PROFILE + //CProfileManager::Increment_Frame_Counter(); +#endif //BT_NO_PROFILE + + + return 1; +} + + +void btGpuDynamicsWorld::setGravity(const btVector3& gravity) +{ +} + +int btGpuDynamicsWorld::findOrRegisterCollisionShape(const btCollisionShape* colShape) +{ + int index = m_uniqueShapes.findLinearSearch(colShape); + if (index==m_uniqueShapes.size()) + { + if (colShape->isPolyhedral()) + { + m_uniqueShapes.push_back(colShape); + + btPolyhedralConvexShape* convex = (btPolyhedralConvexShape*)colShape; + int numVertices=convex->getNumVertices(); + + int strideInBytes=sizeof(btVector3); + btAlignedObjectArray tmpVertices; + tmpVertices.resize(numVertices); + for (int i=0;igetVertex(i,tmpVertices[i]); + const float scaling[4]={1,1,1,1}; + bool noHeightField=true; + + int gpuShapeIndex = m_np->registerConvexHullShape(&tmpVertices[0].getX(), strideInBytes, numVertices, scaling); + m_uniqueShapeMapping.push_back(gpuShapeIndex); + } else + { + if (colShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + m_uniqueShapes.push_back(colShape); + + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) colShape; + btStridingMeshInterface* meshInterface = trimesh->getMeshInterface(); + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + + btVector3 trimeshScaling(1,1,1); + for (int partId=0;partIdgetNumSubParts();partId++) + { + + const unsigned char *vertexbase = 0; + int numverts = 0; + PHY_ScalarType type = PHY_INTEGER; + int stride = 0; + const unsigned char *indexbase = 0; + int indexstride = 0; + int numfaces = 0; + PHY_ScalarType indicestype = PHY_INTEGER; + //PHY_ScalarType indexType=0; + + btVector3 triangleVerts[3]; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); + btVector3 aabbMin,aabbMax; + + for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + { + unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + + for (int j=2;j>=0;j--) + { + + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + if (type == PHY_FLOAT) + { + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = btVector3( + graphicsbase[0]*trimeshScaling.getX(), + graphicsbase[1]*trimeshScaling.getY(), + graphicsbase[2]*trimeshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*trimeshScaling.getX()), + btScalar(graphicsbase[1]*trimeshScaling.getY()), + btScalar(graphicsbase[2]*trimeshScaling.getZ())); + } + } + vertices.push_back(triangleVerts[0]); + vertices.push_back(triangleVerts[1]); + vertices.push_back(triangleVerts[2]); + indices.push_back(indices.size()); + indices.push_back(indices.size()); + indices.push_back(indices.size()); + } + } + //GraphicsShape* gfxShape = 0;//btBulletDataExtractor::createGraphicsShapeFromWavefrontObj(objData); + + //GraphicsShape* gfxShape = btBulletDataExtractor::createGraphicsShapeFromConvexHull(&sUnitSpherePoints[0],MY_UNITSPHERE_POINTS); + float meshScaling[4] = {1,1,1,1}; + //int shapeIndex = renderer.registerShape(gfxShape->m_vertices,gfxShape->m_numvertices,gfxShape->m_indices,gfxShape->m_numIndices); + float groundPos[4] = {0,0,0,0}; + + //renderer.registerGraphicsInstance(shapeIndex,groundPos,rotOrn,color,meshScaling); + if (vertices.size() && indices.size()) + { + int gpuShapeIndex = m_np->registerConcaveMesh(&vertices,&indices, meshScaling); + m_uniqueShapeMapping.push_back(gpuShapeIndex); + } else + { + printf("Error: no vertices in mesh in btGpuDynamicsWorld::addRigidBody\n"); + index = -1; + btAssert(0); + } + + + } else + { + if (colShape->getShapeType()==COMPOUND_SHAPE_PROXYTYPE) + { + + btCompoundShape* compound = (btCompoundShape*) colShape; + btAlignedObjectArray childShapes; + + for (int i=0;igetNumChildShapes();i++) + { + //for now, only support polyhedral child shapes + btAssert(compound->getChildShape(i)->isPolyhedral()); + btGpuChildShape child; + child.m_shapeIndex = findOrRegisterCollisionShape(compound->getChildShape(i)); + btVector3 pos = compound->getChildTransform(i).getOrigin(); + btQuaternion orn = compound->getChildTransform(i).getRotation(); + for (int v=0;v<4;v++) + { + child.m_childPosition[v] = pos[v]; + child.m_childOrientation[v] = orn[v]; + } + childShapes.push_back(child); + } + index = m_uniqueShapes.size(); + m_uniqueShapes.push_back(colShape); + + int gpuShapeIndex = m_np->registerCompoundShape(&childShapes); + m_uniqueShapeMapping.push_back(gpuShapeIndex); + + + + + /*printf("Error: unsupported compound type (%d) in btGpuDynamicsWorld::addRigidBody\n",colShape->getShapeType()); + index = -1; + btAssert(0); + */ + } else + { + if (colShape->getShapeType()==SPHERE_SHAPE_PROXYTYPE) + { + m_uniqueShapes.push_back(colShape); + btSphereShape* sphere = (btSphereShape*)colShape; + + int gpuShapeIndex = m_np->registerSphereShape(sphere->getRadius()); + m_uniqueShapeMapping.push_back(gpuShapeIndex); + } else + { + if (colShape->getShapeType()==STATIC_PLANE_PROXYTYPE) + { + m_uniqueShapes.push_back(colShape); + btStaticPlaneShape* plane = (btStaticPlaneShape*)colShape; + + int gpuShapeIndex = m_np->registerPlaneShape(plane->getPlaneNormal(),plane->getPlaneConstant()); + m_uniqueShapeMapping.push_back(gpuShapeIndex); + } else + { + printf("Error: unsupported shape type (%d) in btGpuDynamicsWorld::addRigidBody\n",colShape->getShapeType()); + index = -1; + btAssert(0); + } + } + } + } + } + + } + + return index; +} + +void btGpuDynamicsWorld::addRigidBody(btRigidBody* body) +{ + + body->setMotionState(0); + + + int index = findOrRegisterCollisionShape(body->getCollisionShape()); + + if (index>=0) + { + int gpuShapeIndex= m_uniqueShapeMapping[index]; + float mass = body->getInvMass() ? 1.f/body->getInvMass() : 0.f; + btVector3 pos = body->getWorldTransform().getOrigin(); + btQuaternion orn = body->getWorldTransform().getRotation(); + + m_rigidBodyPipeline->registerPhysicsInstance(mass,&pos.getX(),&orn.getX(),gpuShapeIndex,m_collisionObjects.size()); + + m_collisionObjects.push_back(body); + } +} + +void btGpuDynamicsWorld::removeCollisionObject(btCollisionObject* colObj) +{ + btAssert(0); +} + + diff --git a/src/Bullet3Dynamics/Dynamics/btGpuDynamicsWorld.h b/src/Bullet3Dynamics/Dynamics/btGpuDynamicsWorld.h new file mode 100644 index 000000000..8fd86156f --- /dev/null +++ b/src/Bullet3Dynamics/Dynamics/btGpuDynamicsWorld.h @@ -0,0 +1,89 @@ +#ifndef BT_GPU_DYNAMICS_WORLD_H +#define BT_GPU_DYNAMICS_WORLD_H + +class btVector3; +class btRigidBody; +class btCollisionObject; +struct btGpuInternalData;//use this struct to avoid 'leaking' all OpenCL headers into clients code base + +#include "BulletCommon/btAlignedObjectArray.h" +//#include "BulletDynamics/Dynamics/btDynamicsWorld.h" +#include "BulletCommon/btTransform.h" + +class btGpuDynamicsWorld +{ + + btAlignedObjectArray m_uniqueShapes; + btAlignedObjectArray m_uniqueShapeMapping; + + btAlignedObjectArray m_collisionObjects; + + class btGpuSapBroadphase* m_bp; + class btGpuNarrowPhase* m_np; + class btGpuRigidBodyPipeline* m_rigidBodyPipeline; + + + btVector3 m_gravity; + bool m_once; + + bool initOpenCL(int preferredDeviceIndex, int preferredPlatformIndex, bool useInterop); + void exitOpenCL(); + + int findOrRegisterCollisionShape(const btCollisionShape* colShape); + + +public: + btGpuDynamicsWorld(btGpuSapBroadphase*bp, btGpuNarrowPhase* np, btGpuRigidBodyPipeline* rb); + + virtual ~btGpuDynamicsWorld(); + + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + + virtual void synchronizeMotionStates() + { + btAssert(0); + } + + void debugDrawWorld() {} + + void setGravity(const btVector3& gravity); + + void addRigidBody(btRigidBody* body); + + void removeCollisionObject(btCollisionObject* colObj); + + + + btAlignedObjectArray& getCollisionObjectArray(); + + const btAlignedObjectArray& getCollisionObjectArray() const; + + + + btVector3 getGravity () const + { + return m_gravity; + } + + virtual void addRigidBody(btRigidBody* body, short group, short mask) + { + addRigidBody(body); + } + + virtual void removeRigidBody(btRigidBody* body) + { + btAssert(0); + } + + + virtual void clearForces() + { + btAssert(0); + } + + + +}; + + +#endif //BT_GPU_DYNAMICS_WORLD_H