diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index 7372a973b..7b298eaa2 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -44,6 +44,10 @@ btRigidBody* d6body0 =0; btHingeConstraint* spDoorHinge = NULL; +static bool s_bTestConeTwistMotor = false; + + + void drawLimit() { btVector3 from = sliderTransform*lowerSliderLimit; @@ -71,6 +75,7 @@ void ConstraintDemo::initPhysics() setShadows(true); setCameraDistance(26.f); + m_Time = 0; m_collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); @@ -97,7 +102,7 @@ void ConstraintDemo::initPhysics() trans.setOrigin(btVector3(0,20,0)); float mass = 1.f; -#if 0 +#if 1 //point to point constraint (ball socket) { btRigidBody* body0 = localCreateRigidBody( mass,trans,shape); @@ -135,7 +140,7 @@ void ConstraintDemo::initPhysics() } #endif -#if 0 +#if 1 //create a slider, using the generic D6 constraint { mass = 1.f; @@ -179,7 +184,7 @@ void ConstraintDemo::initPhysics() } #endif -#if 0 +#if 1 { // create a door using hinge constraint attached to the world btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.push_back(pDoorShape); @@ -201,7 +206,7 @@ void ConstraintDemo::initPhysics() //btRigidBody* pDropBody = localCreateRigidBody( 10.0, doorTrans, shape); } #endif -#if 0 +#if 1 { // create a generic 6DOF constraint btTransform tr; @@ -262,7 +267,7 @@ void ConstraintDemo::initPhysics() pGen6DOF->setDbgDrawSize(btScalar(5.f)); } #endif -#if 0 +#if 1 { // create a ConeTwist constraint btTransform tr; @@ -274,7 +279,7 @@ void ConstraintDemo::initPhysics() pBodyA->setActivationState(DISABLE_DEACTIVATION); tr.setIdentity(); - tr.setOrigin(btVector3(btScalar(-10.), btScalar(0.), btScalar(0.))); + tr.setOrigin(btVector3(btScalar(-10.), btScalar(-5.), btScalar(0.))); tr.getBasis().setEulerZYX(0,0,0); btRigidBody* pBodyB = localCreateRigidBody(0.0, tr, shape); // btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape); @@ -282,19 +287,22 @@ void ConstraintDemo::initPhysics() btTransform frameInA, frameInB; frameInA = btTransform::getIdentity(); frameInA.getBasis().setEulerZYX(0, 0, M_PI_2); - frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-1.), btScalar(0.))); + frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-5.), btScalar(0.))); frameInB = btTransform::getIdentity(); frameInB.getBasis().setEulerZYX(0,0, M_PI_2); - frameInB.setOrigin(btVector3(btScalar(0.), btScalar(4.), btScalar(0.))); + frameInB.setOrigin(btVector3(btScalar(0.), btScalar(5.), btScalar(0.))); - btConeTwistConstraint* pCT = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB); -// pCT->setLimit(btScalar(M_PI_4), btScalar(M_PI_4), btScalar(M_PI) * 0.8f); - pCT->setLimit(btScalar(M_PI_4), btScalar(M_PI_4), btScalar(M_PI) * 0.8f, 1.0f); // soft limit == hard limit - m_dynamicsWorld->addConstraint(pCT, true); - pCT->setDbgDrawSize(btScalar(5.f)); + m_ctc = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB); +// m_ctc->setLimit(btScalar(M_PI_4), btScalar(M_PI_4), btScalar(M_PI) * 0.8f); +// m_ctc->setLimit(btScalar(M_PI_4*0.6f), btScalar(M_PI_4), btScalar(M_PI) * 0.8f, 1.0f); // soft limit == hard limit + m_ctc->setLimit(btScalar(M_PI_4*0.6f), btScalar(M_PI_4), btScalar(M_PI) * 0.8f, 0.5f); + m_dynamicsWorld->addConstraint(m_ctc, true); + m_ctc->setDbgDrawSize(btScalar(5.f)); + // s_bTestConeTwistMotor = true; // use only with old solver for now + s_bTestConeTwistMotor = false; } #endif -#if 0 +#if 1 { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) btTransform tr; tr.setIdentity(); @@ -319,54 +327,95 @@ void ConstraintDemo::initPhysics() // static bodyA (parent) on top: btTransform tr; tr.setIdentity(); - tr.setOrigin(btVector3(btScalar(0.), btScalar(4.), btScalar(0.))); + tr.setOrigin(btVector3(btScalar(20.), btScalar(4.), btScalar(0.))); btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape); pBodyA->setActivationState(DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr.setIdentity(); - tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); + tr.setOrigin(btVector3(btScalar(20.), btScalar(0.), btScalar(0.))); btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape); pBodyB->setActivationState(DISABLE_DEACTIVATION); // add some (arbitrary) data to build constraint frames btVector3 parentAxis(1.f, 0.f, 0.f); btVector3 childAxis(0.f, 0.f, 1.f); - btVector3 anchor(0.f, 2.f, 0.f); - // build frame basis - // 6DOF constraint uses Euler angles and to define limits - // it is assumed that rotational order is : - // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number - // used to prevent constraint from instability on poles; - // new position of X, allowed limits are (-PI,PI); - // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs - // Build the frame in world coordinate system first - btVector3 zAxis = parentAxis.normalize(); - btVector3 yAxis = childAxis.normalize(); - btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system - btTransform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - frameInW.setOrigin(anchor); - // now get constraint frame in local coordinate systems - btTransform frameInA = pBodyA->getCenterOfMassTransform().inverse() * frameInW; - btTransform frameInB = pBodyB->getCenterOfMassTransform().inverse() * frameInW; - // now create the constraint - btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); - // linear limits in our case are allowed offset of origin of frameInB in frameInA, so set them to zero - pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.)); - pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.)); - // set limits for parent (axis z) and child (axis Y) - pGen6DOF->setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI * 0.5f, -SIMD_HALF_PI * 0.5f)); - pGen6DOF->setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f)); + btVector3 anchor(20.f, 2.f, 0.f); + btUniversalConstraint* pUniv = new btUniversalConstraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); + pUniv->setLowerLimit(-SIMD_HALF_PI * 0.5f, -SIMD_HALF_PI * 0.5f); + pUniv->setUpperLimit(SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f); // add constraint to world - m_dynamicsWorld->addConstraint(pGen6DOF, true); + m_dynamicsWorld->addConstraint(pUniv, true); // draw constraint frames and limits for debugging - pGen6DOF->setDbgDrawSize(btScalar(10.f)); + pUniv->setDbgDrawSize(btScalar(5.f)); } #endif +#if 1 + { // create a generic 6DOF constraint with springs + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); + tr.getBasis().setEulerZYX(0,0,0); + btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape); + pBodyA->setActivationState(DISABLE_DEACTIVATION); + + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); + tr.getBasis().setEulerZYX(0,0,0); + btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape); + pBodyB->setActivationState(DISABLE_DEACTIVATION); + + btTransform frameInA, frameInB; + frameInA = btTransform::getIdentity(); + frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.))); + frameInB = btTransform::getIdentity(); + frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); + + btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); + pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.)); + pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.)); + + pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f)); + pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f)); + + m_dynamicsWorld->addConstraint(pGen6DOFSpring, true); + pGen6DOFSpring->setDbgDrawSize(btScalar(5.f)); + + pGen6DOFSpring->enableSpring(0, true); + pGen6DOFSpring->setStiffness(0, 39.478f); + pGen6DOFSpring->enableSpring(5, true); + pGen6DOFSpring->setStiffness(5, 39.478f); + pGen6DOFSpring->setEquilibriumPoint(); + } +#endif +#if 1 + { + // create a Hinge2 joint + // create two rigid bodies + // static bodyA (parent) on top: + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-20.), btScalar(4.), btScalar(0.))); + btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape); + pBodyA->setActivationState(DISABLE_DEACTIVATION); + // dynamic bodyB (child) below it : + tr.setIdentity(); + tr.setOrigin(btVector3(btScalar(-20.), btScalar(0.), btScalar(0.))); + btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape); + pBodyB->setActivationState(DISABLE_DEACTIVATION); + // add some data to build constraint frames + btVector3 parentAxis(0.f, 1.f, 0.f); + btVector3 childAxis(1.f, 0.f, 0.f); + btVector3 anchor(-20.f, 0.f, 0.f); + btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); + pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f); + pHinge2->setUpperLimit( SIMD_HALF_PI * 0.5f); + // add constraint to world + m_dynamicsWorld->addConstraint(pHinge2, true); + // draw constraint frames and limits for debugging + pHinge2->setDbgDrawSize(btScalar(5.f)); + } +#endif } @@ -383,6 +432,7 @@ ConstraintDemo::~ConstraintDemo() m_dynamicsWorld->removeConstraint(constraint); delete constraint; } + m_ctc = NULL; //remove the rigidbodies from the dynamics world and delete them for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) @@ -424,6 +474,7 @@ ConstraintDemo::~ConstraintDemo() } + void ConstraintDemo::clientMoveAndDisplay() { @@ -431,6 +482,28 @@ void ConstraintDemo::clientMoveAndDisplay() float dt = float(getDeltaTimeMicroseconds()) * 0.000001f; //printf("dt = %f: ",dt); + + // drive cone-twist motor + m_Time += 0.03f; + if (s_bTestConeTwistMotor) + { // this works only for obsolete constraint solver for now + // build cone target + btScalar t = 1.25f*m_Time; + btVector3 axis(0,sin(t),cos(t)); + axis.normalize(); + btQuaternion q1(axis, 0.75f*M_PI); + + // build twist target + //btQuaternion q2(0,0,0); + //btQuaternion q2(btVehictor3(1,0,0), -0.3*sin(m_Time)); + btQuaternion q2(btVector3(1,0,0), -1.49*sin(1.5*m_Time)); + + // compose cone + twist and set target + q1 = q1 * q2; + m_ctc->enableMotor(true); + m_ctc->setMotorTargetInConstraintSpace(q1); + } + { //during idle mode, just run 1 simulation step maximum diff --git a/Demos/ConstraintDemo/ConstraintDemo.h b/Demos/ConstraintDemo/ConstraintDemo.h index 8a4a1351b..18d95507d 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.h +++ b/Demos/ConstraintDemo/ConstraintDemo.h @@ -48,7 +48,11 @@ class ConstraintDemo : public GlutDemoApplication demo->myinit(); demo->initPhysics(); return demo; - } + } + + // for cone-twist motor driving + float m_Time; + class btConeTwistConstraint* m_ctc; }; diff --git a/Demos/Gpu2dDemo/BasicDemo.cpp b/Demos/Gpu2dDemo/BasicDemo.cpp index 7c3435a34..2b7d7b1cb 100644 --- a/Demos/Gpu2dDemo/BasicDemo.cpp +++ b/Demos/Gpu2dDemo/BasicDemo.cpp @@ -18,8 +18,6 @@ subject to the following restrictions: #include "BulletMultiThreaded/btGpuUtilsSharedDefs.h" #include "BulletMultiThreaded/btGpuUtilsSharedCode.h" -//---------------------------------------------------------------------------------------- - #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" diff --git a/Demos/Gpu2dDemo/btGpuDemo2dCpuFunc.cpp b/Demos/Gpu2dDemo/btGpuDemo2dCpuFunc.cpp index 0374c4fa5..858ddf6da 100644 --- a/Demos/Gpu2dDemo/btGpuDemo2dCpuFunc.cpp +++ b/Demos/Gpu2dDemo/btGpuDemo2dCpuFunc.cpp @@ -13,21 +13,18 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- + #include "LinearMath/btQuickprof.h" -//---------------------------------------------------------------------------------------- + #include "LinearMath/btScalar.h" #include "btGpuDemo2dSharedTypes.h" -//---------------------------------------------------------------------------------------- + #include "BulletMultiThreaded/btGpuDefines.h" #include "BulletMultiThreaded/btGpuUtilsSharedDefs.h" #include "btGpuDemo2dSharedCode.h" -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- diff --git a/Demos/Gpu2dDemo/btGpuDemo2dSharedCode.h b/Demos/Gpu2dDemo/btGpuDemo2dSharedCode.h index 2e3636640..64d1f2be3 100644 --- a/Demos/Gpu2dDemo/btGpuDemo2dSharedCode.h +++ b/Demos/Gpu2dDemo/btGpuDemo2dSharedCode.h @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- + #define USE_FRICTION 1 #define FRICTION_BOX_GROUND_FACT 0.05f @@ -21,14 +21,7 @@ subject to the following restrictions: #define USE_CENTERS 1 //#include "LinearMath/btMinMax.h" -//------------------------------------------------------------------------------------------------ -//------------------------------------------------------------------------------------------------ -//------------------------------------------------------------------------------------------------ -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- //---------- C o n s t r a i n t s o l v e r d e m o ---------------------------- -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- #define MAX_VTX_PER_OBJ 8 diff --git a/Demos/Gpu2dDemo/btGpuDemo2dSharedDefs.h b/Demos/Gpu2dDemo/btGpuDemo2dSharedDefs.h index ff6e15d73..1a2904811 100644 --- a/Demos/Gpu2dDemo/btGpuDemo2dSharedDefs.h +++ b/Demos/Gpu2dDemo/btGpuDemo2dSharedDefs.h @@ -13,11 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- + + //---------- C o n s t r a i n t s o l v e r d e m o ---------------------------- -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- + + extern "C" @@ -30,4 +30,4 @@ void BT_GPU_PREF(collisionBatchResolutionBox(void* constraints,int *batch,int nu } // extern "C" -//---------------------------------------------------------------------------------------- + diff --git a/Demos/Gpu2dDemo/btGpuDemo2dSharedTypes.h b/Demos/Gpu2dDemo/btGpuDemo2dSharedTypes.h index dc8cbb237..2d4a18baa 100644 --- a/Demos/Gpu2dDemo/btGpuDemo2dSharedTypes.h +++ b/Demos/Gpu2dDemo/btGpuDemo2dSharedTypes.h @@ -13,11 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- //---------- C o n s t r a i n t s o l v e r d e m o ---------------------------- -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- struct btCudaPartProps { @@ -36,4 +32,4 @@ struct btCudaBoxProps float maxZ; }; -//---------------------------------------------------------------------------------------- + diff --git a/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.cpp b/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.cpp index ecc5535a2..f98c731a0 100644 --- a/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.cpp +++ b/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//-------------------------------------------------------------------------- + #include "btGpuDemoDynamicsWorld.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -28,7 +28,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -//-------------------------------------------------------------------------- + #define BT_GPU_PREF(func) btCuda_##func @@ -40,11 +40,11 @@ subject to the following restrictions: #include "btGpuDemo2dSharedDefs.h" #undef BT_GPU_PREF -//-------------------------------------------------------------------------- + btGpuDemoDynamicsWorld* gpCudaDemoDynamicsWorld = NULL; -//-------------------------------------------------------------------------- + void btGpuDemoDynamicsWorld::grabNonContactConstraintData() { @@ -64,9 +64,9 @@ void btGpuDemoDynamicsWorld::grabNonContactConstraintData() break; } } -} // btGpuDemoDynamicsWorld::grabNonContactConstraintData() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::grabContactData() { @@ -137,9 +137,9 @@ void btGpuDemoDynamicsWorld::grabContactData() } m_totalNumConstraints++; } -} // btCudaDemoDynamicsWorld::grabContactData() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::grabP2PConstraintData(btPoint2PointConstraint* ct) { @@ -177,9 +177,9 @@ void btGpuDemoDynamicsWorld::grabP2PConstraintData(btPoint2PointConstraint* ct) } m_totalNumConstraints++; m_numNonContactConstraints++; -} // btGpuDemoDynamicsWorld::grabP2PConstraintData() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::grabData() { @@ -214,9 +214,9 @@ void btGpuDemoDynamicsWorld::grabData() grabContactData(); } grabNonContactConstraintData(); -} // btGpuDemoDynamicsWorld::grabGata() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::createBatches2() { @@ -268,9 +268,9 @@ void btGpuDemoDynamicsWorld::createBatches2() m_numInBatches[stage] = numInBatch; pBatchIds += numInBatch; } -} // btGpuDemoDynamicsWorld::createBatches2() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::writebackData() { @@ -288,9 +288,9 @@ void btGpuDemoDynamicsWorld::writebackData() v[2] = m_hAngVel[i + 1]; rb->setAngularVelocity(v); } -} // btGpuDemoDynamicsWorld::writebackData() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::copyDataToGPU() { @@ -339,9 +339,9 @@ void btGpuDemoDynamicsWorld::copyDataToGPU() } #endif //BT_USE_CUDA -} // btGpuDemoDynamicsWorld::copyDataToGPU() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::setConstraintData(btCudaPartProps& partProps) { @@ -358,9 +358,9 @@ void btGpuDemoDynamicsWorld::setConstraintData(btCudaPartProps& partProps) } #endif //BT_USE_CUDA -} // btGpuDemoDynamicsWorld::setConstraintData() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::copyDataFromGPU() { @@ -369,9 +369,9 @@ void btGpuDemoDynamicsWorld::copyDataFromGPU() btCuda_copyArrayFromDevice(m_hVel, m_dcVel, (m_numObj + 1) * sizeof(float4)); btCuda_copyArrayFromDevice(m_hAngVel, m_dcAngVel, (m_numObj + 1) * sizeof(float)); #endif //BT_USE_CUDA -} // btGpuDemoDynamicsWorld::copyDataFromGPU() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { @@ -384,9 +384,9 @@ void btGpuDemoDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) solveConstraints2(solverInfo); } m_totalNumConstraints = 0; -} // btGpuDemoDynamicsWorld::solveConstraints() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::solveConstraints2(btContactSolverInfo& solverInfo) { @@ -433,9 +433,9 @@ void btGpuDemoDynamicsWorld::solveConstraints2(btContactSolverInfo& solverInfo) writebackData(); m_numSimStep++; #endif //BT_USE_CUDA -} // btGpuDemoDynamicsWorld::solveConstraints2() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::solveConstraintsCPU2(btContactSolverInfo& solverInfo) { @@ -496,9 +496,9 @@ void btGpuDemoDynamicsWorld::solveConstraintsCPU2(btContactSolverInfo& solverInf } writebackData(); m_numSimStep++; -} // btGpuDemoDynamicsWorld::solveConstraintsCPU2() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::debugDrawConstraints(int selectedBatch, const float* pColorTab) { @@ -539,10 +539,9 @@ void btGpuDemoDynamicsWorld::debugDrawConstraints(int selectedBatch, const float pBatchIds += numConstraints; glEnd(); } -} // btGpuDemoDynamicsWorld::debugDrawConstraints() +} + -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::initShapeBuffer(int maxShapeBufferSize) { @@ -557,9 +556,9 @@ void btGpuDemoDynamicsWorld::initShapeBuffer(int maxShapeBufferSize) #endif //BT_USE_CUDA m_copyShapeDataToGPU = true; -} // btGpuDemoDynamicsWorld::initShapeBuffer() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::freeShapeBuffer() { @@ -569,9 +568,9 @@ void btGpuDemoDynamicsWorld::freeShapeBuffer() btCuda_freeArray(m_dShapeBuffer); btCuda_freeArray(m_dShapeIds); #endif //BT_USE_CUDA -} // btGpuDemoDynamicsWorld::freeShapeBuffer() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::addSphere(btVector3& pos, btScalar rad) { @@ -579,18 +578,15 @@ void btGpuDemoDynamicsWorld::addSphere(btVector3& pos, btScalar rad) *pBuf = pos; pBuf->setW(rad); m_firstFreeShapeBufferOffset += sizeof(btVector3); -} // btGpuDemoDynamicsWorld::addSphere() +} + -//-------------------------------------------------------------------------- void btGpuDemoDynamicsWorld::addMultiShereObject(int numSpheres, int objIndex) { m_hShapeIds[objIndex].x = m_firstFreeShapeBufferOffset; m_hShapeIds[objIndex].y = numSpheres; return; -} // btGpuDemoDynamicsWorld::addMultiShereObject() +} -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- diff --git a/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.h b/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.h index 2e98f5b8d..88a77d1c8 100644 --- a/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.h +++ b/Demos/Gpu2dDemo/btGpuDemoDynamicsWorld.h @@ -13,12 +13,12 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- + #ifndef BT_CUDA_DEMO_DYNAMICS_WORLD_H #define BT_CUDA_DEMO_DYNAMICS_WORLD_H -//---------------------------------------------------------------------------------------- + #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" @@ -46,15 +46,9 @@ subject to the following restrictions: #undef BT_GPU_PREF -//---------------------------------------------------------------------------------------- - - - -//---------------------------------------------------------------------------------------- - #include "btGpuDemo2dSharedTypes.h" -//---------------------------------------------------------------------------------------- + #define CUDA_DEMO_DYNAMICS_WORLD_MAX_BATCHES 20 @@ -130,10 +124,10 @@ protected: btVector3 m_worldMin; btVector3 m_worldMax; - //------------------------------- + int* m_hConstraintUsed; - //------------------------------- + // shape buffer int m_maxShapeBufferSize; int m_firstFreeShapeBufferOffset; @@ -146,7 +140,7 @@ protected: void freeShapeBuffer(); void sendShapeDataToGpu(); - //------------------------------- + int m_numNonContactConstraints; void grabNonContactConstraintData(); void grabP2PConstraintData(btPoint2PointConstraint* ct); diff --git a/Demos/Gpu2dDemo/btGpuDemoPairCache.cpp b/Demos/Gpu2dDemo/btGpuDemoPairCache.cpp index 6c9d9fbcc..c258be484 100644 --- a/Demos/Gpu2dDemo/btGpuDemoPairCache.cpp +++ b/Demos/Gpu2dDemo/btGpuDemoPairCache.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//-------------------------------------------------------------------------- + #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -21,7 +21,7 @@ subject to the following restrictions: #include "btGpuDemoDynamicsWorld.h" -//-------------------------------------------------------------------------- + void btGpuDemoPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher) { @@ -41,9 +41,9 @@ void btGpuDemoPairCache::processAllOverlappingPairs(btOverlapCallback* callback, } } gpCudaDemoDynamicsWorld->setTotalNumConstraints(numContConstraints); -} // btGpuDemoPairCache::processAllOverlappingPairs() +} + -//-------------------------------------------------------------------------- // this will be called for each overlapping pair if collision detection uses pairCache other than btGpuDemoPairCache // IMPORTANT : m_numConstraints in gpCudaDemoDynamicsWorld is set to 0 at start of simulation step @@ -74,6 +74,3 @@ void cudaDemoNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher } } // cudaDemoNearCallback() -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- diff --git a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp index 09d81b9e8..2054ab665 100755 --- a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp +++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp @@ -21,7 +21,7 @@ Added support for ODE sover April 24, 2008 */ -//----------------------------------------------------------------------------- + #include "btBulletDynamicsCommon.h" @@ -36,14 +36,14 @@ April 24, 2008 #include "GL_ShapeDrawer.h" #include "GlutStuff.h" -//----------------------------------------------------------------------------- + #define SLIDER_DEMO_USE_ODE_SOLVER 0 #define SLIDER_DEMO_USE_6DOF 0 #define CUBE_HALF_EXTENTS 1.f -//----------------------------------------------------------------------------- + // A couple of sliders #if SLIDER_DEMO_USE_6DOF @@ -55,7 +55,7 @@ April 24, 2008 static btPoint2PointConstraint* spP2PConst; static btHingeConstraint* spHingeConst; -//----------------------------------------------------------------------------- + static void draw_axes(const btRigidBody& rb, const btTransform& frame) { @@ -98,7 +98,7 @@ static void draw_axes(const btRigidBody& rb, const btTransform& frame) glEnd(); } // draw_axes() -//----------------------------------------------------------------------------- + #if SLIDER_DEMO_USE_6DOF static void drawSlider(btGeneric6DofConstraint* pSlider) @@ -133,7 +133,7 @@ static void drawSlider(btSliderConstraint* pSlider) } // drawSlider() #endif -//----------------------------------------------------------------------------- + void SliderConstraintDemo::initPhysics() { @@ -339,7 +339,7 @@ void SliderConstraintDemo::initPhysics() } // SliderConstraintDemo::initPhysics() -//----------------------------------------------------------------------------- + SliderConstraintDemo::~SliderConstraintDemo() { @@ -381,7 +381,7 @@ SliderConstraintDemo::~SliderConstraintDemo() delete m_collisionConfiguration; } // SliderConstraintDemo::~SliderConstraintDemo() -//----------------------------------------------------------------------------- + void SliderConstraintDemo::clientMoveAndDisplay() { @@ -420,7 +420,7 @@ void SliderConstraintDemo::clientMoveAndDisplay() glutSwapBuffers(); } // SliderConstraintDemo::clientMoveAndDisplay() -//----------------------------------------------------------------------------- + void SliderConstraintDemo::displayCallback(void) { @@ -436,4 +436,4 @@ void SliderConstraintDemo::displayCallback(void) glutSwapBuffers(); } // SliderConstraintDemo::displayCallback() -//----------------------------------------------------------------------------- + diff --git a/Demos/SliderConstraintDemo/SliderConstraintDemo.h b/Demos/SliderConstraintDemo/SliderConstraintDemo.h index 5c7d25db7..18fd39975 100755 --- a/Demos/SliderConstraintDemo/SliderConstraintDemo.h +++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.h @@ -13,18 +13,18 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//----------------------------------------------------------------------------- + #ifndef SLIDER_CONSTRAINT_DEMO_H #define SLIDER_CONSTRAINT_DEMO_H -//----------------------------------------------------------------------------- + #include "btBulletDynamicsCommon.h" #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" #include "GlutDemoApplication.h" -//----------------------------------------------------------------------------- + /// SliderConstraintDemo shows how to create a slider constraint diff --git a/Extras/CUDA/btCudaBroadphase.cpp b/Extras/CUDA/btCudaBroadphase.cpp index d69c63809..007c1ae4c 100644 --- a/Extras/CUDA/btCudaBroadphase.cpp +++ b/Extras/CUDA/btCudaBroadphase.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//-------------------------------------------------------------------------- + #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btQuickprof.h" @@ -22,7 +22,7 @@ subject to the following restrictions: #include "btCudaBroadphase.h" #include "radixsort.cuh" -//-------------------------------------------------------------------------- + #define BT_GPU_PREF(func) btCuda_##func #include "../../src/BulletMultiThreaded/btGpuUtilsSharedDefs.h" @@ -31,12 +31,12 @@ subject to the following restrictions: extern "C" void btCuda_setParameters(bt3DGridBroadphaseParams* hostParams); -//-------------------------------------------------------------------------- + #include -//-------------------------------------------------------------------------- + btCudaBroadphase::btCudaBroadphase( btOverlappingPairCache* overlappingPairCache, const btVector3& worldAabbMin,const btVector3& worldAabbMax, @@ -46,18 +46,18 @@ btCudaBroadphase::btCudaBroadphase( btOverlappingPairCache* overlappingPairCache btGpu3DGridBroadphase(overlappingPairCache, worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ, maxSmallProxies, maxLargeProxies, maxPairsPerSmallProxy, maxSmallProxiesPerCell) { _initialize(); -} // btCudaBroadphase::btCudaBroadphase() +} + -//-------------------------------------------------------------------------- btCudaBroadphase::~btCudaBroadphase() { //btSimpleBroadphase will free memory of btSortedOverlappingPairCache, because m_ownsPairCache assert(m_bInitialized); _finalize(); -} // btCudaBroadphase::~btCudaBroadphase() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::_initialize() { @@ -79,9 +79,9 @@ void btCudaBroadphase::_initialize() btCuda_allocateArray((void**)&m_dPairScan, (m_maxHandles + 1) * sizeof(unsigned int)); btCuda_allocateArray((void**)&m_dPairOut, m_maxHandles * m_maxPairsPerBody * sizeof(unsigned int)); -} // btCudaBroadphase::_initialize() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::_finalize() { @@ -94,32 +94,32 @@ void btCudaBroadphase::_finalize() btCuda_freeArray(m_dPairBuff); btCuda_freeArray(m_dPairScan); btCuda_freeArray(m_dPairOut); -} // btCudaBroadphase::_finalize() +} + + -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- // // overrides for CUDA version // -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- + + void btCudaBroadphase::prepareAABB() { btGpu3DGridBroadphase::prepareAABB(); btCuda_copyArrayToDevice(m_dAABB, m_hAABB, sizeof(bt3DGrid3F1U) * 2 * (m_numHandles + m_numLargeHandles)); return; -} // btCudaBroadphase::prepareAABB() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::setParameters(bt3DGridBroadphaseParams* hostParams) { btCuda_setParameters(hostParams); return; -} // btCudaBroadphase::setParameters() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::calcHashAABB() { @@ -127,18 +127,18 @@ void btCudaBroadphase::calcHashAABB() btCuda_calcHashAABB(m_dAABB, m_dBodiesHash[0], m_numHandles); // btCuda_copyArrayFromDevice((void*)m_hBodiesHash, (void*)m_dBodiesHash[0], sizeof(unsigned int) * 2 * m_numHandles); return; -} // btCudaBroadphase::calcHashAABB() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::sortHash() { BT_PROFILE("RadixSort-- CUDA"); RadixSort((KeyValuePair*)m_dBodiesHash[0], (KeyValuePair*)m_dBodiesHash[1], m_numHandles, 32); return; -} // btCudaBroadphase::sortHash() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::findCellStart() { @@ -147,36 +147,36 @@ void btCudaBroadphase::findCellStart() // btCuda_copyArrayFromDevice((void*)m_hBodiesHash, (void*)m_dBodiesHash[0], sizeof(unsigned int) * 2 * m_numHandles); // btCuda_copyArrayFromDevice((void*)m_hCellStart, (void*)m_dCellStart, sizeof(unsigned int) * m_params.m_numCells); return; -} // btCudaBroadphase::findCellStart() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::findOverlappingPairs() { BT_PROFILE("btCuda_findOverlappingPairs"); btCuda_findOverlappingPairs(m_dAABB, m_dBodiesHash[0], m_dCellStart, m_dPairBuff, m_dPairBuffStartCurr, m_numHandles); return; -} // btCudaBroadphase::findOverlappingPairs() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::findPairsLarge() { BT_PROFILE("btCuda_findPairsLarge"); btCuda_findPairsLarge(m_dAABB, m_dBodiesHash[0], m_dCellStart, m_dPairBuff, m_dPairBuffStartCurr, m_numHandles, m_numLargeHandles); return; -} // btCudaBroadphase::findPairsLarge() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::computePairCacheChanges() { BT_PROFILE("btCuda_computePairCacheChanges"); btCuda_computePairCacheChanges(m_dPairBuff, m_dPairBuffStartCurr, m_dPairScan, m_dAABB, m_numHandles); return; -} // btCudaBroadphase::computePairCacheChanges() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::scanOverlappingPairBuff() { @@ -184,9 +184,9 @@ void btCudaBroadphase::scanOverlappingPairBuff() btGpu3DGridBroadphase::scanOverlappingPairBuff(); btCuda_copyArrayToDevice(m_dPairScan, m_hPairScan, sizeof(unsigned int)*(m_numHandles + 1)); return; -} // btCudaBroadphase::scanOverlappingPairBuff() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::squeezeOverlappingPairBuff() { @@ -194,14 +194,14 @@ void btCudaBroadphase::squeezeOverlappingPairBuff() btCuda_squeezeOverlappingPairBuff(m_dPairBuff, m_dPairBuffStartCurr, m_dPairScan, m_dPairOut, m_dAABB, m_numHandles); btCuda_copyArrayFromDevice(m_hPairOut, m_dPairOut, sizeof(unsigned int) * m_hPairScan[m_numHandles]); return; -} // btCudaBroadphase::squeezeOverlappingPairBuff() +} + -//-------------------------------------------------------------------------- void btCudaBroadphase::resetPool(btDispatcher* dispatcher) { btGpu3DGridBroadphase::resetPool(dispatcher); btCuda_copyArrayToDevice(m_dPairBuffStartCurr, m_hPairBuffStartCurr, (m_maxHandles * 2 + 1) * sizeof(unsigned int)); -} // btCudaBroadphase::resetPool() +} + -//-------------------------------------------------------------------------- diff --git a/Extras/CUDA/btCudaBroadphase.cu b/Extras/CUDA/btCudaBroadphase.cu index 7d73dfcde..954395e59 100644 --- a/Extras/CUDA/btCudaBroadphase.cu +++ b/Extras/CUDA/btCudaBroadphase.cu @@ -23,55 +23,52 @@ subject to the following restrictions: #include -//---------------------------------------------------------------------------------------- + #include "btCudaDefines.h" -//---------------------------------------------------------------------------------------- + #include "../../src/BulletMultiThreaded/btGpuUtilsSharedDefs.h" #include "../../src/BulletMultiThreaded/btGpu3DGridBroadphaseSharedDefs.h" -//---------------------------------------------------------------------------------------- + __device__ inline bt3DGrid3F1U tex_fetch3F1U(float4 a) { return *((bt3DGrid3F1U*)(&a)); } -//---------------------------------------------------------------------------------------- + void btCuda_exit(int val); -//---------------------------------------------------------------------------------------- + texture particleHashTex; texture cellStartTex; texture pAABBTex; -//---------------------------------------------------------------------------------------- + __constant__ bt3DGridBroadphaseParams params; -//---------------------------------------------------------------------------------------- + extern "C" { -//---------------------------------------------------------------------------------------- + void btCuda_setParameters(bt3DGridBroadphaseParams* hostParams) { // copy parameters to constant memory BT_GPU_SAFE_CALL(cudaMemcpyToSymbol(params, hostParams, sizeof(bt3DGridBroadphaseParams))); -} // btCuda_setParameters() +} + -//---------------------------------------------------------------------------------------- } // extern "C" -//---------------------------------------------------------------------------------------- + #include "../../src/BulletMultiThreaded/btGpu3DGridBroadphaseSharedCode.h" -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- diff --git a/Extras/CUDA/btCudaBroadphase.h b/Extras/CUDA/btCudaBroadphase.h index 1cdc6284d..9e3955f9f 100644 --- a/Extras/CUDA/btCudaBroadphase.h +++ b/Extras/CUDA/btCudaBroadphase.h @@ -13,19 +13,19 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- + #ifndef CUDA_BROADPHASE_H #define CUDA_BROADPHASE_H -//---------------------------------------------------------------------------------------- + #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "../../src/BulletMultiThreaded/btGpu3DGridBroadphaseSharedTypes.h" #include "../../src/BulletMultiThreaded/btGpu3DGridBroadphase.h" -//---------------------------------------------------------------------------------------- + ///The btCudaBroadphase uses CUDA-capable GPU to compute overlapping pairs diff --git a/Extras/CUDA/btCudaDefines.h b/Extras/CUDA/btCudaDefines.h index e0e09714e..5af101c50 100644 --- a/Extras/CUDA/btCudaDefines.h +++ b/Extras/CUDA/btCudaDefines.h @@ -13,16 +13,16 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- + // Common preprocessor definitions for CUDA compiler -//---------------------------------------------------------------------------------------- + #ifndef BTCUDADEFINES_H #define BTCUDADEFINES_H -//---------------------------------------------------------------------------------------- + #ifdef __DEVICE_EMULATION__ #define B_CUDA_USE_TEX 0 @@ -39,7 +39,7 @@ subject to the following restrictions: #define BT_GPU_FETCH4(t, i) t[i] #endif -//---------------------------------------------------------------------------------------- + #define BT_GPU___device__ __device__ #define BT_GPU___devdata__ __device__ @@ -71,7 +71,7 @@ subject to the following restrictions: #define BT_GPU_UnbindTexture(a) cudaUnbindTexture(a) #define BT_GPU_EXECKERNEL(numb, numt, kfunc, args) kfunc<<>>args -//---------------------------------------------------------------------------------------- + //! Check for CUDA error #define BT_GPU_CHECK_ERROR(errorMessage) \ @@ -94,7 +94,7 @@ subject to the following restrictions: } \ while(0) -//---------------------------------------------------------------------------------------- + #define BT_GPU_SAFE_CALL_NO_SYNC(call) \ do \ @@ -109,7 +109,7 @@ subject to the following restrictions: } \ while(0) -//---------------------------------------------------------------------------------------- + #define BT_GPU_SAFE_CALL(call) \ do \ @@ -124,15 +124,15 @@ subject to the following restrictions: } \ } while (0) -//---------------------------------------------------------------------------------------- + extern "C" void btCuda_exit(int val); -//---------------------------------------------------------------------------------------- + #endif // BTCUDADEFINES_H -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- + + diff --git a/Extras/CUDA/btCudaUtils.cu b/Extras/CUDA/btCudaUtils.cu index f1ff53757..4f6282115 100644 --- a/Extras/CUDA/btCudaUtils.cu +++ b/Extras/CUDA/btCudaUtils.cu @@ -25,17 +25,12 @@ subject to the following restrictions: #include -//---------------------------------------------------------------------------------------- + #include "btCudaDefines.h" #include "../../src/BulletMultiThreaded/btGpuUtilsSharedDefs.h" -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- - - -//---------------------------------------------------------------------------------------- void btCuda_exit(int val) { @@ -82,10 +77,8 @@ void btCuda_unmapGLBufferObject(unsigned int vbo) BT_GPU_SAFE_CALL(cudaGLUnmapBufferObject(vbo)); } -//---------------------------------------------------------------------------------------- + #include "../../src/BulletMultiThreaded/btGpuUtilsSharedCode.h" -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- diff --git a/Extras/CUDA/btGpuDemo2dCudaFunc.cu b/Extras/CUDA/btGpuDemo2dCudaFunc.cu index f7232d959..620602ec5 100644 --- a/Extras/CUDA/btGpuDemo2dCudaFunc.cu +++ b/Extras/CUDA/btGpuDemo2dCudaFunc.cu @@ -22,25 +22,21 @@ subject to the following restrictions: #include -//---------------------------------------------------------------------------------------- + #include "btCudaDefines.h" -//---------------------------------------------------------------------------------------- + #include "../../src/BulletMultiThreaded/btGpuUtilsSharedDefs.h" #include "../../Demos/Gpu2dDemo/btGpuDemo2dSharedTypes.h" #include "../../Demos/Gpu2dDemo/btGpuDemo2dSharedDefs.h" -//---------------------------------------------------------------------------------------- + texture posTex; -//---------------------------------------------------------------------------------------- + #include "../../Demos/Gpu2dDemo/btGpuDemo2dSharedCode.h" -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- - diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 29c8496c3..8270b9ac3 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -22,12 +22,13 @@ Written by: Marcus Hennix #include "LinearMath/btMinMax.h" #include -//----------------------------------------------------------------------------- + +//#define CONETWIST_USE_OBSOLETE_SOLVER true #define CONETWIST_USE_OBSOLETE_SOLVER false #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) -//----------------------------------------------------------------------------- + btConeTwistConstraint::btConeTwistConstraint() :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), @@ -69,7 +70,7 @@ void btConeTwistConstraint::init() } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) { @@ -99,9 +100,9 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) info->nub--; } } -} // btConeTwistConstraint::getInfo1() +} -//----------------------------------------------------------------------------- + void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) { @@ -230,7 +231,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) } } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::buildJacobian() { @@ -239,6 +240,7 @@ void btConeTwistConstraint::buildJacobian() m_appliedImpulse = btScalar(0.); m_accTwistLimitImpulse = btScalar(0.); m_accSwingLimitImpulse = btScalar(0.); + m_accMotorImpulse = btVector3(0.,0.,0.); if (!m_angularOnly) { @@ -277,7 +279,7 @@ void btConeTwistConstraint::buildJacobian() } } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { @@ -406,10 +408,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver } } - else // no motor: do a little damping + else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 angVelA; bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; bodyB.getAngularVelocity(angVelB); btVector3 relVel = angVelB - angVelA; if (relVel.length2() > SIMD_EPSILON) { @@ -490,7 +492,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::updateRHS(btScalar timeStep) { @@ -498,7 +500,7 @@ void btConeTwistConstraint::updateRHS(btScalar timeStep) } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::calcAngleInfo() { @@ -584,12 +586,12 @@ void btConeTwistConstraint::calcAngleInfo() m_twistAxis.normalize(); } } -} // btConeTwistConstraint::calcAngleInfo() +} static btVector3 vTwist(1,0,0); // twist axis in constraint's space -//----------------------------------------------------------------------------- + void btConeTwistConstraint::calcAngleInfo2() { @@ -597,13 +599,34 @@ void btConeTwistConstraint::calcAngleInfo2() m_twistLimitSign = btScalar(0.); m_solveTwistLimit = false; m_solveSwingLimit = false; + // compute rotation of A wrt B (in constraint space) + if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) + { // it is assumed that setMotorTarget() was alredy called + // and motor target m_qTarget is within constraint limits + // TODO : split rotation to pure swing and pure twist + // compute desired transforms in world + btTransform trPose(m_qTarget); + btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame; + btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame; + btTransform trDeltaAB = trB * trPose * trA.inverse(); + btQuaternion qDeltaAB = trDeltaAB.getRotation(); + btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); + m_swingAxis = swingAxis; + m_swingAxis.normalize(); + m_swingCorrection = qDeltaAB.getAngle(); + if(!btFuzzyZero(m_swingCorrection)) + { + m_solveSwingLimit = true; + } + return; + } + { // compute rotation of A wrt B (in constraint space) btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation(); btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation(); btQuaternion qAB = qB.inverse() * qA; - // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); @@ -756,7 +779,7 @@ void btConeTwistConstraint::calcAngleInfo2() m_twistAngle = btScalar(0.f); } } -} // btConeTwistConstraint::calcAngleInfo2() +} @@ -982,8 +1005,5 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion & } -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 84ea9e040..8a893d4fb 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -17,6 +17,22 @@ Written by: Marcus Hennix +/* +Overview: + +btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc). +It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint". +It divides the 3 rotational DOFs into swing (movement within a cone) and twist. +Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape. +(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.) + +In the contraint's frame of reference: +twist is along the x-axis, +and swing 1 and 2 are along the z and y axes respectively. +*/ + + + #ifndef CONETWISTCONSTRAINT_H #define CONETWISTCONSTRAINT_H @@ -141,7 +157,18 @@ public: }; } - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + // setLimit(), a few notes: + // _softness: + // 0->1, recommend ~0.8->1. + // describes % of limits where movement is free. + // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached. + // _biasFactor: + // 0->1?, recommend 0.3 +/-0.3 or so. + // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation. + // __relaxationFactor: + // 0->1, recommend to stay near 1. + // the lower the value, the less the constraint will fight velocities which violate the angular limits. + void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { m_swingSpan1 = _swingSpan1; m_swingSpan2 = _swingSpan2; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 6cbfe61f7..e8bc26182 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -26,7 +26,7 @@ http://gimpact.sf.net #define D6_USE_OBSOLETE_METHOD false -//----------------------------------------------------------------------------- + btGeneric6DofConstraint::btGeneric6DofConstraint() :btTypedConstraint(D6_CONSTRAINT_TYPE), @@ -35,7 +35,7 @@ m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { } -//----------------------------------------------------------------------------- + btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) @@ -46,12 +46,12 @@ m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { } -//----------------------------------------------------------------------------- + #define GENERIC_D6_DISABLE_WARMSTARTING 1 -//----------------------------------------------------------------------------- + btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) @@ -61,7 +61,7 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) return mat[i][j]; } -//----------------------------------------------------------------------------- + ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); @@ -129,7 +129,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) } -//----------------------------------------------------------------------------- + btScalar btRotationalLimitMotor::solveAngularLimits( btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, @@ -249,9 +249,9 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu m_currentLimit[limitIndex] = 0;//Free from violation m_currentLimitError[limitIndex] = btScalar(0.f); return 0; -} // btTranslationalLimitMotor::testLimitValue() +} + -//----------------------------------------------------------------------------- btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, @@ -372,7 +372,7 @@ void btGeneric6DofConstraint::calculateAngleInfo() } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::calculateTransforms() { @@ -382,7 +382,7 @@ void btGeneric6DofConstraint::calculateTransforms() calculateAngleInfo(); } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildLinearJacobian( btJacobianEntry & jacLinear,const btVector3 & normalWorld, @@ -400,7 +400,7 @@ void btGeneric6DofConstraint::buildLinearJacobian( m_rbB.getInvMass()); } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildAngularJacobian( btJacobianEntry & jacAngular,const btVector3 & jointAxisW) @@ -413,7 +413,7 @@ void btGeneric6DofConstraint::buildAngularJacobian( } -//----------------------------------------------------------------------------- + bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { @@ -423,7 +423,7 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) return m_angularLimits[axis_index].needApplyTorques(); } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildJacobian() { @@ -483,7 +483,7 @@ void btGeneric6DofConstraint::buildJacobian() } } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) { @@ -519,7 +519,7 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) } } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) { @@ -528,7 +528,7 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) setAngularLimits(info, row); } -//----------------------------------------------------------------------------- + int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) { @@ -559,7 +559,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) return row; } -//----------------------------------------------------------------------------- + int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset) { @@ -582,7 +582,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o return row; } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { @@ -643,7 +643,7 @@ void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv } } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { @@ -651,21 +651,21 @@ void btGeneric6DofConstraint::updateRHS(btScalar timeStep) } -//----------------------------------------------------------------------------- + btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } -//----------------------------------------------------------------------------- + btScalar btGeneric6DofConstraint::getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::calcAnchorPos(void) { @@ -684,9 +684,9 @@ void btGeneric6DofConstraint::calcAnchorPos(void) const btVector3& pB = m_calculatedTransformB.getOrigin(); m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); return; -} // btGeneric6DofConstraint::calcAnchorPos() +} + -//----------------------------------------------------------------------------- void btGeneric6DofConstraint::calculateLinearInfo() { @@ -696,9 +696,9 @@ void btGeneric6DofConstraint::calculateLinearInfo() { m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); } -} // btGeneric6DofConstraint::calculateLinearInfo() +} + -//----------------------------------------------------------------------------- int btGeneric6DofConstraint::get_limit_motor_info2( btRotationalLimitMotor * limot, @@ -824,6 +824,6 @@ int btGeneric6DofConstraint::get_limit_motor_info2( else return 0; } -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- + + + diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 0ae161d5b..5af866390 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -477,4 +477,5 @@ public: }; + #endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp new file mode 100644 index 000000000..49c9309f9 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -0,0 +1,134 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment 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. +*/ + +#include "btGeneric6DofSpringConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + +btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) + : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) +{ + for(int i = 0; i < 6; i++) + { + m_springEnabled[i] = false; + m_equilibriumPoint[i] = btScalar(0.f); + m_springStiffness[i] = btScalar(0.f); + } +} + + +void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + m_springEnabled[index] = onOff; + if(index < 3) + { + m_linearLimits.m_enableMotor[index] = onOff; + } + else + { + m_angularLimits[index - 3].m_enableMotor = onOff; + } +} + + + +void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) +{ + btAssert((index >= 0) && (index < 6)); + m_springStiffness[index] = stiffness; +} + + +void btGeneric6DofSpringConstraint::setEquilibriumPoint() +{ + calculateTransforms(); + for(int i = 0; i < 3; i++) + { + m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + } + for(int i = 0; i < 3; i++) + { + m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; + } +} + + + +void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) +{ + btAssert((index >= 0) && (index < 6)); + calculateTransforms(); + if(index < 3) + { + m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + } + else + { + m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index]; + } +} + + + +void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info) +{ + // it is assumed that calculateTransforms() have been called before this call + int i; + btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); + for(i = 0; i < 3; i++) + { + if(m_springEnabled[i]) + { + // get current position of constraint + btScalar currPos = m_calculatedLinearDiff[i]; + // calculate difference + btScalar delta = currPos - m_equilibriumPoint[i]; + // spring force is (delta * m_stiffness) according to Hooke's Law + btScalar force = delta * m_springStiffness[i]; + m_linearLimits.m_targetVelocity[i] = force * info->fps; + m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + } + } + for(i = 0; i < 3; i++) + { + if(m_springEnabled[i + 3]) + { + // get current position of constraint + btScalar currPos = m_calculatedAxisAngleDiff[i]; + // calculate difference + btScalar delta = currPos - m_equilibriumPoint[i+3]; + // spring force is (-delta * m_stiffness) according to Hooke's Law + btScalar force = -delta * m_springStiffness[i+3]; + m_angularLimits[i].m_targetVelocity = force * info->fps; + m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; + } + } +} + + +void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) +{ + // this will be called by constraint solver at the constraint setup stage + // set current motor parameters + internalUpdateSprings(info); + // do the rest of job for constraint setup + btGeneric6DofConstraint::getInfo2(info); +} + + + + diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h new file mode 100644 index 000000000..afbbcddfb --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -0,0 +1,52 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment 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. +*/ + +#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H +#define GENERIC_6DOF_SPRING_CONSTRAINT_H + + +#include "LinearMath/btVector3.h" +#include "btTypedConstraint.h" +#include "btGeneric6DofConstraint.h" + + +/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF + +/// DOF index used in enableSpring() and setStiffness() means: +/// 0 : translation X +/// 1 : translation Y +/// 2 : translation Z +/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) +/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) +/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) + +class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint +{ +protected: + bool m_springEnabled[6]; + btScalar m_equilibriumPoint[6]; + btScalar m_springStiffness[6]; + void internalUpdateSprings(btConstraintInfo2* info); +public: + btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + void enableSpring(int index, bool onOff); + void setStiffness(int index, btScalar stiffness); + void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF + void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF + virtual void getInfo2 (btConstraintInfo2* info); +}; + +#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H + diff --git a/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp new file mode 100644 index 000000000..887a85171 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -0,0 +1,65 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment 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. +*/ + + + +#include "btHinge2Constraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + + +// constructor +// anchor, axis1 and axis2 are in world coordinate system +// axis1 must be orthogonal to axis2 +btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) +: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), + m_anchor(anchor), + m_axis1(axis1), + m_axis2(axis2) +{ + // build frame basis + // 6DOF constraint uses Euler angles and to define limits + // it is assumed that rotational order is : + // Z - first, allowed limits are (-PI,PI); + // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number + // used to prevent constraint from instability on poles; + // new position of X, allowed limits are (-PI,PI); + // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs + // Build the frame in world coordinate system first + btVector3 zAxis = axis1.normalize(); + btVector3 xAxis = axis2.normalize(); + btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + frameInW.setOrigin(anchor); + // now get constraint frame in local coordinate systems + m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; + // sei limits + setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); + setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); + // like front wheels of a car + setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); + setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); + // enable suspension + enableSpring(2, true); + setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) + setEquilibriumPoint(); +} + diff --git a/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h new file mode 100644 index 000000000..b589687d7 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -0,0 +1,58 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment 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. +*/ + +#ifndef HINGE2_CONSTRAINT_H +#define HINGE2_CONSTRAINT_H + + + +#include "LinearMath/btVector3.h" +#include "btTypedConstraint.h" +#include "btGeneric6DofSpringConstraint.h" + + + +// Constraint similar to ODE Hinge2 Joint +// has 3 degrees of frredom: +// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2) +// 1 translational (along axis Z) with suspension spring + +class btHinge2Constraint : public btGeneric6DofSpringConstraint +{ +protected: + btVector3 m_anchor; + btVector3 m_axis1; + btVector3 m_axis2; +public: + // constructor + // anchor, axis1 and axis2 are in world coordinate system + // axis1 must be orthogonal to axis2 + btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); + // access + const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } + const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } + const btVector3& getAxis1() { return m_axis1; } + const btVector3& getAxis2() { return m_axis2; } + btScalar getAngle1() { return getAngle(2); } + btScalar getAngle2() { return getAngle(0); } + // limits + void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); } + void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); } +}; + + + +#endif // HINGE2_CONSTRAINT_H + diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index b6b343058..7cd9915dd 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -21,11 +21,11 @@ subject to the following restrictions: #include #include "btSolverBody.h" -//----------------------------------------------------------------------------- + #define HINGE_USE_OBSOLETE_SOLVER false -//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint() @@ -37,7 +37,7 @@ m_useReferenceFrameA(false) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) @@ -88,7 +88,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), @@ -128,7 +128,7 @@ m_useReferenceFrameA(useReferenceFrameA) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) @@ -148,7 +148,7 @@ m_useReferenceFrameA(useReferenceFrameA) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), @@ -171,13 +171,14 @@ m_useReferenceFrameA(useReferenceFrameA) m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } -//----------------------------------------------------------------------------- + void btHingeConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { m_appliedImpulse = btScalar(0.); + m_accMotorImpulse = btScalar(0.); if (!m_angularOnly) { @@ -258,7 +259,7 @@ void btHingeConstraint::buildJacobian() } } -//----------------------------------------------------------------------------- + void btHingeConstraint::getInfo1(btConstraintInfo1* info) { @@ -279,9 +280,9 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info) info->nub--; } } -} // btHingeConstraint::getInfo1 () +} + -//----------------------------------------------------------------------------- void btHingeConstraint::getInfo2 (btConstraintInfo2* info) { @@ -467,7 +468,7 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info) } // if angular limit or powered } -//----------------------------------------------------------------------------- + void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { @@ -601,10 +602,22 @@ void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; - //todo: should clip against accumulated impulse - btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - btVector3 motorImp = clippedMotorImpulse * axisA; + + // accumulated impulse clipping: + btScalar fMaxImpulse = m_maxMotorImpulse; + btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse; + btScalar clippedMotorImpulse = unclippedMotorImpulse; + if (newAccImpulse > fMaxImpulse) + { + newAccImpulse = fMaxImpulse; + clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; + } + else if (newAccImpulse < -fMaxImpulse) + { + newAccImpulse = -fMaxImpulse; + clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; + } + m_accMotorImpulse += clippedMotorImpulse; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); @@ -615,7 +628,7 @@ void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody } -//----------------------------------------------------------------------------- + void btHingeConstraint::updateRHS(btScalar timeStep) { @@ -623,7 +636,7 @@ void btHingeConstraint::updateRHS(btScalar timeStep) } -//----------------------------------------------------------------------------- + btScalar btHingeConstraint::getHingeAngle() { @@ -634,7 +647,7 @@ btScalar btHingeConstraint::getHingeAngle() return m_referenceSign * angle; } -//----------------------------------------------------------------------------- + void btHingeConstraint::testLimit() { @@ -659,8 +672,50 @@ void btHingeConstraint::testLimit() } } return; -} // btHingeConstraint::testLimit() +} + + +static btVector3 vHinge(0, 0, btScalar(1)); + +void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) +{ + // convert target from body to constraint space + btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); + qConstraint.normalize(); + + // extract "pure" hinge component + btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); + btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); + btQuaternion qHinge = qNoHinge.inverse() * qConstraint; + qHinge.normalize(); + + // compute angular target, clamped to limits + btScalar targetAngle = qHinge.getAngle(); + if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. + { + qHinge = operator-(qHinge); + targetAngle = qHinge.getAngle(); + } + if (qHinge.getZ() < 0) + targetAngle = -targetAngle; + + setMotorTarget(targetAngle, dt); +} + +void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) +{ + if (m_lowerLimit < m_upperLimit) + { + if (targetAngle < m_lowerLimit) + targetAngle = m_lowerLimit; + else if (targetAngle > m_upperLimit) + targetAngle = m_upperLimit; + } + + // compute angular velocity + btScalar curAngle = getHingeAngle(); + btScalar dAngle = targetAngle - curAngle; + m_motorTargetVelocity = dAngle / dt; +} + -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index aace8e178..8d464e1fc 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -62,6 +62,8 @@ public: bool m_useSolveConstraintObsolete; bool m_useReferenceFrameA; + btScalar m_accMotorImpulse; + public: @@ -116,6 +118,15 @@ public: m_maxMotorImpulse = maxMotorImpulse; } + // extra motor API, including ability to set a target rotation (as opposed to angular velocity) + // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to + // maintain a given angular target. + void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } + void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } + void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. + void setMotorTarget(btScalar targetAngle, btScalar dt); + + void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { m_lowerLimit = low; diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 271a29ea2..133aed727 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -18,14 +18,14 @@ Added by Roman Ponomarev (rponom@gmail.com) April 04, 2008 */ -//----------------------------------------------------------------------------- + #include "btSliderConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" #include -//----------------------------------------------------------------------------- + void btSliderConstraint::initParams() { @@ -62,9 +62,9 @@ void btSliderConstraint::initParams() m_maxAngMotorForce = btScalar(0.); m_accumulatedAngMotorImpulse = btScalar(0.0); -} // btSliderConstraint::initParams() +} + -//----------------------------------------------------------------------------- btSliderConstraint::btSliderConstraint() :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), @@ -73,9 +73,9 @@ btSliderConstraint::btSliderConstraint() // m_useSolveConstraintObsolete(true) { initParams(); -} // btSliderConstraint::btSliderConstraint() +} + -//----------------------------------------------------------------------------- btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) @@ -86,9 +86,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const // m_useSolveConstraintObsolete(true) { initParams(); -} // btSliderConstraint::btSliderConstraint() +} + -//----------------------------------------------------------------------------- static btRigidBody s_fixed(0, 0, 0); btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, s_fixed, rbB) @@ -102,9 +102,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& fram // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); initParams(); -} // btSliderConstraint::btSliderConstraint() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::buildJacobian() { @@ -120,9 +120,9 @@ void btSliderConstraint::buildJacobian() { buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); } -} // btSliderConstraint::buildJacobian() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) { @@ -175,9 +175,9 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co // clear accumulator for motors m_accumulatedLinMotorImpulse = btScalar(0.0); m_accumulatedAngMotorImpulse = btScalar(0.0); -} // btSliderConstraint::buildJacobianInt() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::getInfo1(btConstraintInfo1* info) { @@ -205,9 +205,9 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info) info->nub--; } } -} // btSliderConstraint::getInfo1() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::getInfo2(btConstraintInfo2* info) { @@ -515,9 +515,9 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) info->m_constraintError[srow] *= getSoftnessLimAng(); } // if(limit) } // if angular limit or powered -} // btSliderConstraint::getInfo2() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { @@ -533,9 +533,9 @@ void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBod solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); } } -} // btSliderConstraint::solveConstraint() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) { @@ -719,11 +719,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); } } -} // btSliderConstraint::solveConstraint() +} + + -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- void btSliderConstraint::calculateTransforms(void){ if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) @@ -756,9 +756,9 @@ void btSliderConstraint::calculateTransforms(void){ normalWorld = m_calculatedTransformA.getBasis().getColumn(i); m_depth[i] = m_delta.dot(normalWorld); } -} // btSliderConstraint::calculateTransforms() +} -//----------------------------------------------------------------------------- + void btSliderConstraint::testLinLimits(void) { @@ -785,9 +785,9 @@ void btSliderConstraint::testLinLimits(void) { m_depth[0] = btScalar(0.); } -} // btSliderConstraint::testLinLimits() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::testAngLimits(void) { @@ -811,9 +811,9 @@ void btSliderConstraint::testAngLimits(void) m_solveAngLim = true; } } -} // btSliderConstraint::testAngLimits() +} -//----------------------------------------------------------------------------- + btVector3 btSliderConstraint::getAncorInA(void) { @@ -821,13 +821,13 @@ btVector3 btSliderConstraint::getAncorInA(void) ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; return ancorInA; -} // btSliderConstraint::getAncorInA() +} + -//----------------------------------------------------------------------------- btVector3 btSliderConstraint::getAncorInB(void) { btVector3 ancorInB; ancorInB = m_frameInB.getOrigin(); return ancorInB; -} // btSliderConstraint::getAncorInB(); +} diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 6b37a4c83..01cef59ed 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -25,23 +25,23 @@ TODO: #ifndef SLIDER_CONSTRAINT_H #define SLIDER_CONSTRAINT_H -//----------------------------------------------------------------------------- + #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" -//----------------------------------------------------------------------------- + class btRigidBody; -//----------------------------------------------------------------------------- + #define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) #define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) #define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) -//----------------------------------------------------------------------------- + class btSliderConstraint : public btTypedConstraint { @@ -224,7 +224,7 @@ public: btVector3 getAncorInB(void); }; -//----------------------------------------------------------------------------- + #endif //SLIDER_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 3fa98ee4c..ed155ba30 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -60,7 +60,7 @@ m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) } -//----------------------------------------------------------------------------- + btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact) { @@ -109,6 +109,6 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal lim_fact = btScalar(0.0f); } return lim_fact; -} // btTypedConstraint::getMotorFactor() +} diff --git a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp new file mode 100644 index 000000000..564e5cbfb --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp @@ -0,0 +1,63 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment 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. +*/ + + + +#include "btUniversalConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + + +#define UNIV_EPS btScalar(0.01f) + + +// constructor +// anchor, axis1 and axis2 are in world coordinate system +// axis1 must be orthogonal to axis2 +btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) +: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), + m_anchor(anchor), + m_axis1(axis1), + m_axis2(axis2) +{ + // build frame basis + // 6DOF constraint uses Euler angles and to define limits + // it is assumed that rotational order is : + // Z - first, allowed limits are (-PI,PI); + // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number + // used to prevent constraint from instability on poles; + // new position of X, allowed limits are (-PI,PI); + // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs + // Build the frame in world coordinate system first + btVector3 zAxis = axis1.normalize(); + btVector3 yAxis = axis2.normalize(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + frameInW.setOrigin(anchor); + // now get constraint frame in local coordinate systems + m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; + // sei limits + setLinearLowerLimit(btVector3(0., 0., 0.)); + setLinearUpperLimit(btVector3(0., 0., 0.)); + setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); + setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); +} + diff --git a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h new file mode 100644 index 000000000..fdde703f7 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h @@ -0,0 +1,60 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment 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. +*/ + +#ifndef UNIVERSAL_CONSTRAINT_H +#define UNIVERSAL_CONSTRAINT_H + + + +#include "LinearMath/btVector3.h" +#include "btTypedConstraint.h" +#include "btGeneric6DofConstraint.h" + + + +/// Constraint similar to ODE Universal Joint +/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1) +/// and Y (axis 2) +/// Description from ODE manual : +/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. +/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal." + +class btUniversalConstraint : public btGeneric6DofConstraint +{ +protected: + btVector3 m_anchor; + btVector3 m_axis1; + btVector3 m_axis2; +public: + // constructor + // anchor, axis1 and axis2 are in world coordinate system + // axis1 must be orthogonal to axis2 + btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); + // access + const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } + const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } + const btVector3& getAxis1() { return m_axis1; } + const btVector3& getAxis2() { return m_axis2; } + btScalar getAngle1() { return getAngle(2); } + btScalar getAngle2() { return getAngle(1); } + // limits + void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); } + void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); } +}; + + + +#endif // UNIVERSAL_CONSTRAINT_H + diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index a8b286d15..e19836280 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -1368,7 +1368,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) break; } return; -} // btDiscreteDynamicsWorld::debugDrawConstraint() +} diff --git a/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp b/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp index 16975b10b..69bf94cc0 100644 --- a/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp +++ b/src/BulletMultiThreaded/btGpu3DGridBroadphase.cpp @@ -13,13 +13,13 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//-------------------------------------------------------------------------- + #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btQuickprof.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -//-------------------------------------------------------------------------- + #include "btGpuDefines.h" #include "btGpuUtilsSharedDefs.h" @@ -28,15 +28,15 @@ subject to the following restrictions: #include "btGpu3DGridBroadphase.h" #include //for memset -//-------------------------------------------------------------------------- + #include -//-------------------------------------------------------------------------- + static bt3DGridBroadphaseParams s3DGridBroadphaseParams; -//-------------------------------------------------------------------------- + btGpu3DGridBroadphase::btGpu3DGridBroadphase( const btVector3& worldAabbMin,const btVector3& worldAabbMax, int gridSizeX, int gridSizeY, int gridSizeZ, @@ -52,9 +52,9 @@ btGpu3DGridBroadphase::btGpu3DGridBroadphase( const btVector3& worldAabbMin,cons _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ, maxSmallProxies, maxLargeProxies, maxPairsPerBody, maxBodiesPerCell, cellFactorAABB); -} // btGpu3DGridBroadphase::btGpu3DGridBroadphase() +} + -//-------------------------------------------------------------------------- btGpu3DGridBroadphase::btGpu3DGridBroadphase( btOverlappingPairCache* overlappingPairCache, const btVector3& worldAabbMin,const btVector3& worldAabbMax, @@ -69,18 +69,18 @@ btGpu3DGridBroadphase::btGpu3DGridBroadphase( btOverlappingPairCache* overlappin _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ, maxSmallProxies, maxLargeProxies, maxPairsPerBody, maxBodiesPerCell, cellFactorAABB); -} // btGpu3DGridBroadphase::btGpu3DGridBroadphase() +} + -//-------------------------------------------------------------------------- btGpu3DGridBroadphase::~btGpu3DGridBroadphase() { //btSimpleBroadphase will free memory of btSortedOverlappingPairCache, because m_ownsPairCache assert(m_bInitialized); _finalize(); -} // btGpu3DGridBroadphase::~btGpu3DGridBroadphase() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::_initialize( const btVector3& worldAabbMin,const btVector3& worldAabbMax, int gridSizeX, int gridSizeY, int gridSizeZ, @@ -164,9 +164,9 @@ void btGpu3DGridBroadphase::_initialize( const btVector3& worldAabbMin,const btV m_numOverflows = 0; m_bInitialized = true; -} // btGpu3DGridBroadphase::_initialize() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::_finalize() { @@ -180,9 +180,9 @@ void btGpu3DGridBroadphase::_finalize() delete [] m_hPairOut; btAlignedFree(m_pLargeHandlesRawPtr); m_bInitialized = false; -} // btGpu3DGridBroadphase::_finalize() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { @@ -214,9 +214,9 @@ void btGpu3DGridBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) // find and add large/large pairs to CPU cache addLarge2LargePairsToCache(dispatcher); return; -} // btGpu3DGridBroadphase::calculateOverlappingPairs() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::addPairsToCache(btDispatcher* dispatcher) { @@ -259,9 +259,9 @@ void btGpu3DGridBroadphase::addPairsToCache(btDispatcher* dispatcher) } } } -} // btGpu3DGridBroadphase::addPairsToCache() +} + -//-------------------------------------------------------------------------- btBroadphaseProxy* btGpu3DGridBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) { @@ -284,9 +284,9 @@ btBroadphaseProxy* btGpu3DGridBroadphase::createProxy( const btVector3& aabbMin proxy = btSimpleBroadphase::createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy); } return proxy; -} // btGpu3DGridBroadphase::createProxy() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) { @@ -303,9 +303,9 @@ void btGpu3DGridBroadphase::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* btSimpleBroadphase::destroyProxy(proxy, dispatcher); } return; -} // btGpu3DGridBroadphase::destroyProxy() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::resetPool(btDispatcher* dispatcher) { @@ -316,9 +316,9 @@ void btGpu3DGridBroadphase::resetPool(btDispatcher* dispatcher) m_hPairBuffStartCurr[i * 2] = m_hPairBuffStartCurr[(i-1) * 2] + m_maxPairsPerBody; m_hPairBuffStartCurr[i * 2 + 1] = 0; } -} // btGpu3DGridBroadphase::resetPool() +} + -//-------------------------------------------------------------------------- bool btGpu3DGridBroadphase::isLargeProxy(const btVector3& aabbMin, const btVector3& aabbMax) { @@ -329,16 +329,16 @@ bool btGpu3DGridBroadphase::isLargeProxy(const btVector3& aabbMin, const btVect radius *= m_cellFactorAABB; // user-defined factor return (radius > m_maxRadius); -} // btGpu3DGridBroadphase::isLargeProxy() +} + -//-------------------------------------------------------------------------- bool btGpu3DGridBroadphase::isLargeProxy(btBroadphaseProxy* proxy) { return (proxy->getUid() >= (m_maxHandles+2)); -} // btGpu3DGridBroadphase::isLargeProxy() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::addLarge2LargePairsToCache(btDispatcher* dispatcher) { @@ -384,9 +384,9 @@ void btGpu3DGridBroadphase::addLarge2LargePairsToCache(btDispatcher* dispatcher) } m_LastLargeHandleIndex = new_largest_index; return; -} // btGpu3DGridBroadphase::addLarge2LargePairsToCache() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback) { @@ -400,15 +400,15 @@ void btGpu3DGridBroadphase::rayTest(const btVector3& rayFrom,const btVector3& ra } rayCallback.process(proxy); } -} // btGpu3DGridBroadphase::rayTest() +} + + -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- // // overrides for CPU version // -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- + + void btGpu3DGridBroadphase::prepareAABB() { @@ -465,26 +465,26 @@ void btGpu3DGridBroadphase::prepareAABB() btAssert(num_small == m_numHandles); btAssert(num_large == m_numLargeHandles); return; -} // btGpu3DGridBroadphase::prepareAABB() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::setParameters(bt3DGridBroadphaseParams* hostParams) { s3DGridBroadphaseParams = *hostParams; return; -} // btGpu3DGridBroadphase::setParameters() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::calcHashAABB() { BT_PROFILE("bt3DGrid_calcHashAABB"); btGpu_calcHashAABB(m_hAABB, m_hBodiesHash, m_numHandles); return; -} // btGpu3DGridBroadphase::calcHashAABB() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::sortHash() { @@ -517,45 +517,45 @@ void btGpu3DGridBroadphase::sortHash() bt3DGridHashKey* pHash = (bt3DGridHashKey*)m_hBodiesHash; pHash->quickSort(pHash, 0, m_numHandles - 1); return; -} // btGpu3DGridBroadphase::sortHash() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::findCellStart() { BT_PROFILE("bt3DGrid_findCellStart"); btGpu_findCellStart(m_hBodiesHash, m_hCellStart, m_numHandles, m_params.m_numCells); return; -} // btGpu3DGridBroadphase::findCellStart() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::findOverlappingPairs() { BT_PROFILE("bt3DGrid_findOverlappingPairs"); btGpu_findOverlappingPairs(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles); return; -} // btGpu3DGridBroadphase::findOverlappingPairs() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::findPairsLarge() { BT_PROFILE("bt3DGrid_findPairsLarge"); btGpu_findPairsLarge(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles, m_numLargeHandles); return; -} // btGpu3DGridBroadphase::findPairsLarge() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::computePairCacheChanges() { BT_PROFILE("bt3DGrid_computePairCacheChanges"); btGpu_computePairCacheChanges(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hAABB, m_numHandles); return; -} // btGpu3DGridBroadphase::computePairCacheChanges() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::scanOverlappingPairBuff() { @@ -567,19 +567,19 @@ void btGpu3DGridBroadphase::scanOverlappingPairBuff() m_hPairScan[i] = m_hPairScan[i-1] + delta; } return; -} // btGpu3DGridBroadphase::scanOverlappingPairBuff() +} + -//-------------------------------------------------------------------------- void btGpu3DGridBroadphase::squeezeOverlappingPairBuff() { BT_PROFILE("bt3DGrid_squeezeOverlappingPairBuff"); btGpu_squeezeOverlappingPairBuff(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hPairOut, m_hAABB, m_numHandles); return; -} // btGpu3DGridBroadphase::squeezeOverlappingPairBuff() +} + -//-------------------------------------------------------------------------- #include "btGpu3DGridBroadphaseSharedCode.h" -//-------------------------------------------------------------------------- + diff --git a/src/BulletMultiThreaded/btGpuDefines.h b/src/BulletMultiThreaded/btGpuDefines.h index c2036a54f..521f539e2 100644 --- a/src/BulletMultiThreaded/btGpuDefines.h +++ b/src/BulletMultiThreaded/btGpuDefines.h @@ -13,11 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//-------------------------------------------------------------------------- + // definitions for "GPU on CPU" code -//-------------------------------------------------------------------------- + #ifndef BT_GPU_DEFINES_H #define BT_GPU_DEFINES_H @@ -207,8 +207,5 @@ static uint2 s_blockIdx, s_blockDim, s_threadIdx; #define BT_GPU_CHECK_ERROR(s) -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- #endif //BT_GPU_DEFINES_H diff --git a/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h b/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h index d1b857af5..92cb251fe 100644 --- a/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h +++ b/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//---------------------------------------------------------------------------------------- + // Shared definitions for GPU-based utilities @@ -23,17 +23,17 @@ subject to the following restrictions: // file with definitions of BT_GPU_xxx should be included first //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -//---------------------------------------------------------------------------------------- + #ifndef BTGPUUTILSDHAREDDEFS_H #define BTGPUUTILSDHAREDDEFS_H -//---------------------------------------------------------------------------------------- + extern "C" { -//---------------------------------------------------------------------------------------- + //Round a / b to nearest higher integer value int BT_GPU_PREF(iDivUp)(int a, int b); @@ -47,16 +47,13 @@ void BT_GPU_PREF(copyArrayFromDevice)(void* host, const void* device, unsigned i void BT_GPU_PREF(copyArrayToDevice)(void* device, const void* host, unsigned int size); -//---------------------------------------------------------------------------------------- + } // extern "C" -//---------------------------------------------------------------------------------------- + #endif // BTGPUUTILSDHAREDDEFS_H -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- -//---------------------------------------------------------------------------------------- diff --git a/src/btBulletDynamicsCommon.h b/src/btBulletDynamicsCommon.h index b95972cd1..db8b37989 100644 --- a/src/btBulletDynamicsCommon.h +++ b/src/btBulletDynamicsCommon.h @@ -30,6 +30,9 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h" +#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"