From 56e135874bd697e33f23e98b2aad9af24f87a9b8 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Fri, 10 Nov 2006 04:00:16 +0000 Subject: [PATCH] added angular limits to the Generic D6 constraint. Works for small angles. Will add a check for different combinations, and use different extraction of ordering of rotation from the diff quaternion. Improved vehicle interpolation of wheels, and added Z-up axis option for the Demo made 'getWorldTransform' const method in btMotionState added future 'deactivationCallback'(not used yet) --- Demos/ConstraintDemo/ConstraintDemo.cpp | 36 +++++-- Demos/VehicleDemo/VehicleDemo.cpp | 18 +++- .../btGeneric6DofConstraint.cpp | 87 +++++++++++++++++ .../Dynamics/btDiscreteDynamicsWorld.cpp | 94 +++++++++++-------- .../Vehicle/btRaycastVehicle.cpp | 58 +++++++++--- src/BulletDynamics/Vehicle/btRaycastVehicle.h | 9 +- src/LinearMath/btDefaultMotionState.h | 11 ++- src/LinearMath/btMotionState.h | 9 +- src/LinearMath/btTransformUtil.h | 20 ++-- 9 files changed, 254 insertions(+), 88 deletions(-) diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index 6c34f3acc..2ff91c6fd 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -47,8 +47,10 @@ int main(int argc,char** argv) } btTransform sliderTransform; -btVector3 lowerSliderLimit = btVector3(-20,0,0); -btVector3 hiSliderLimit = btVector3(10,0,0); +btVector3 lowerSliderLimit = btVector3(0,0,0); +btVector3 hiSliderLimit = btVector3(0,0,0); + +btRigidBody* d6body0 =0; void drawLimit() { @@ -59,9 +61,18 @@ void drawLimit() glColor3f(color.getX(), color.getY(), color.getZ()); glVertex3d(from.getX(), from.getY(), from.getZ()); glVertex3d(to.getX(), to.getY(), to.getZ()); + if (d6body0) + { + from = d6body0->getWorldTransform().getOrigin(); + to = from + d6body0->getWorldTransform().getBasis() * btVector3(0,0,10); + glVertex3d(from.getX(), from.getY(), from.getZ()); + glVertex3d(to.getX(), to.getY(), to.getZ()); + } glEnd(); } + + void ConstraintDemo::initPhysics() { //ConstraintSolver* solver = new btSequentialImpulseConstraintSolver; @@ -108,26 +119,31 @@ void ConstraintDemo::initPhysics() { mass = 1.f; btVector3 sliderWorldPos(0,10,0); - btVector3 sliderAxis(0,0,1); - btScalar angle=SIMD_RADS_PER_DEG * 10.f; + btVector3 sliderAxis(1,0,0); + btScalar angle=0.f;//SIMD_RADS_PER_DEG * 10.f; btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis ,angle)); + trans.setIdentity(); trans.setOrigin(sliderWorldPos); - trans.setBasis(sliderOrientation); + //trans.setBasis(sliderOrientation); sliderTransform = trans; - btRigidBody* body0 = localCreateRigidBody( mass,trans,shape); - body0->setActivationState(DISABLE_DEACTIVATION); + d6body0 = localCreateRigidBody( mass,trans,shape); + d6body0->setActivationState(DISABLE_DEACTIVATION); btRigidBody* fixedBody1 = localCreateRigidBody(0,trans,0); btTransform frameInA, frameInB; frameInA = btTransform::getIdentity(); frameInB = btTransform::getIdentity(); - btGeneric6DofConstraint* slider = new btGeneric6DofConstraint(*body0,*fixedBody1,frameInA,frameInB); + btGeneric6DofConstraint* slider = new btGeneric6DofConstraint(*d6body0,*fixedBody1,frameInA,frameInB); slider->setLinearLowerLimit(lowerSliderLimit); slider->setLinearUpperLimit(hiSliderLimit); - slider->setAngularLowerLimit(btVector3(1e30,0,0)); - slider->setAngularUpperLimit(btVector3(-1e30,0,0)); +// slider->setAngularLowerLimit(btVector3(-3.1415*0.5f,0,0)); +// slider->setAngularUpperLimit(btVector3(3.1415*0.5f,0,0)); + + //range should be small, otherwise singularities will 'explode' the constraint + slider->setAngularLowerLimit(btVector3(0,-3.1415*0.25f,-3.1415*0.25f)); + slider->setAngularUpperLimit(btVector3(0,3.1415*0.25f,3.1415*0.25f)); m_dynamicsWorld->addConstraint(slider); diff --git a/Demos/VehicleDemo/VehicleDemo.cpp b/Demos/VehicleDemo/VehicleDemo.cpp index 575755029..f2e7f6983 100644 --- a/Demos/VehicleDemo/VehicleDemo.cpp +++ b/Demos/VehicleDemo/VehicleDemo.cpp @@ -301,7 +301,7 @@ void VehicleDemo::renderme() for (i=0;igetNumWheels();i++) { //synchronize the wheels with the (interpolated) chassis worldtransform - m_vehicle->updateWheelTransform(i); + m_vehicle->updateWheelTransform(i,true); //draw wheels (cylinders) m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m); GL_ShapeDrawer::drawOpenGL(m,&wheelShape,wheelColor,getDebugMode()); @@ -323,11 +323,14 @@ void VehicleDemo::clientMoveAndDisplay() if (m_dynamicsWorld) { //during idle mode, just run 1 simulation step maximum - int maxSimSubSteps = m_idle ? 1 : 1; + int maxSimSubSteps = m_idle ? 1 : 2; if (m_idle) dt = 1.0/420.f; int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps); + +#define VERBOSE_FEEDBACK +#ifdef VERBOSE_FEEDBACK if (!numSimSteps) printf("Interpolated transforms\n"); else @@ -341,6 +344,7 @@ void VehicleDemo::clientMoveAndDisplay() printf("Simulated (%i) steps\n",numSimSteps); } } +#endif //VERBOSE_FEEDBACK } @@ -384,16 +388,20 @@ void VehicleDemo::clientMoveAndDisplay() void VehicleDemo::displayCallback(void) { + clientMoveAndDisplay(); + return; + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - m_dynamicsWorld->updateAabbs(); + //m_dynamicsWorld->updateAabbs(); + //draw contactpoints //m_physicsEnvironmentPtr->CallbackTriggers(); - renderme(); + //renderme(); glFlush(); @@ -416,7 +424,7 @@ void VehicleDemo::clientResetScene() for (int i=0;igetNumWheels();i++) { //synchronize the wheels with the (interpolated) chassis worldtransform - m_vehicle->updateWheelTransform(i); + m_vehicle->updateWheelTransform(i,true); } } diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 5dc5bdadd..fc47390d7 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -127,6 +127,52 @@ void btGeneric6DofConstraint::buildJacobian() } } +float getMatrixElem(const btMatrix3x3& mat,int index) +{ + int row = index%3; + int col = index / 3; + return mat[row][col]; +} + +///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html +bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + +/// 0..8 + + if (getMatrixElem(mat,2) < 1.0f) + { + if (getMatrixElem(mat,2) > -1.0f) + { + xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8)); + xyz[1] = btAsin(getMatrixElem(mat,2)); + xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = 0.0f; + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + return false; + } + return false; +} + + void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) { btScalar tau = 0.1f; @@ -201,6 +247,18 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) } } + btVector3 axis; + btScalar angle; + btTransform frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA; + btTransform frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB; + + btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle); + btQuaternion diff(axis,angle); + btMatrix3x3 diffMat (diff); + btVector3 xyz; + ///this is not perfect, we can first check which axis are limited, and choose a more appropriate order + MatrixToEulerXYZ(diffMat,xyz); + // angular for (i=0;i<3;i++) { @@ -221,6 +279,35 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btScalar rel_pos = kSign[i] * axisA.dot(axisB); + btScalar lo = -1e30f; + btScalar hi = 1e30f; + + //handle the twist limit + if (m_lowerLimit[i+3] < m_upperLimit[i+3]) + { + //clamp the values + btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f; + btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f; + + float projAngle = -2.*xyz[i]; + + if (projAngle < loLimit) + { + hi = 0.f; + rel_pos = (loLimit - projAngle); + } else + { + if (projAngle > hiLimit) + { + lo = 0.f; + rel_pos = (hiLimit - projAngle); + } else + { + continue; + } + } + } + //impulse btScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; m_accumulatedImpulse[i + 3] += impulse; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 8c6ee72c0..c97da84e0 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -120,40 +120,7 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() { //debug vehicle wheels - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) - { - for (unsigned int i=0;im_vehicles.size();i++) - { - for (int v=0;vgetNumWheels();v++) - { - btVector3 wheelColor(0,255,255); - if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) - { - wheelColor.setValue(0,0,255); - } else - { - wheelColor.setValue(255,0,255); - } - - //synchronize the wheels with the (interpolated) chassis worldtransform - m_vehicles[i]->updateWheelTransform(v); - - btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); - - btVector3 axle = btVector3( - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); - - - //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS - //debug wheels (cylinders) - m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); - m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); - - } - } - } + { //todo: iterate over awake simulation islands! for (unsigned int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + for (unsigned int i=0;im_vehicles.size();i++) + { + for (int v=0;vgetNumWheels();v++) + { + btVector3 wheelColor(0,255,255); + if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) + { + wheelColor.setValue(0,0,255); + } else + { + wheelColor.setValue(255,0,255); + } + + //synchronize the wheels with the (interpolated) chassis worldtransform + m_vehicles[i]->updateWheelTransform(v,true); + + btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); + + btVector3 axle = btVector3( + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); + + + //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS + //debug wheels (cylinders) + m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); + m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); + + } + } + } } @@ -218,8 +219,15 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo //variable timestep fixedTimeStep = timeStep; m_localTime = timeStep; - numSimulationSubSteps = 1; - maxSubSteps = 1; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } } //process some debugging flags @@ -227,7 +235,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo { gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } - if (!btFuzzyZero(timeStep) && numSimulationSubSteps) + if (numSimulationSubSteps) { saveKinematicState(fixedTimeStep); @@ -238,6 +246,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo for (int i=0;igetMotionState()) + btTransform chassisTrans = getChassisWorldTransform(); + if (interpolatedTransform && (getRigidBody()->getMotionState())) + { getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); - else - chassisTrans = getRigidBody()->getCenterOfMassTransform(); - + } + wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; @@ -156,7 +159,7 @@ void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel ) btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) { - updateWheelTransformsWS( wheel ); + updateWheelTransformsWS( wheel,false); btScalar depth = -1; @@ -240,12 +243,35 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) } +const btTransform& btRaycastVehicle::getChassisWorldTransform() const +{ + /*if (getRigidBody()->getMotionState()) + { + btTransform chassisWorldTrans; + getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); + return chassisWorldTrans; + } + */ + + + return getRigidBody()->getCenterOfMassTransform(); +} + + void btRaycastVehicle::updateVehicle( btScalar step ) { + { + for (int i=0;igetLinearVelocity().length(); - const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform(); + const btTransform& chassisTrans = getChassisWorldTransform(); + btVector3 forwardW ( chassisTrans.getBasis()[0][m_indexForwardAxis], chassisTrans.getBasis()[1][m_indexForwardAxis], @@ -305,10 +331,12 @@ void btRaycastVehicle::updateVehicle( btScalar step ) if (wheel.m_raycastInfo.m_isInContact) { + const btTransform& chassisWorldTransform = getChassisWorldTransform(); + btVector3 fwd ( - getRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis], - getRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis], - getRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]); + chassisWorldTransform.getBasis()[0][m_indexForwardAxis], + chassisWorldTransform.getBasis()[1][m_indexForwardAxis], + chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 0e452565e..9a1a30ff7 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -68,9 +68,8 @@ public: virtual ~btRaycastVehicle() ; - - - + const btTransform& getChassisWorldTransform() const; + btScalar rayCast(btWheelInfo& wheel); virtual void updateVehicle(btScalar step); @@ -86,7 +85,7 @@ public: const btTransform& getWheelTransformWS( int wheelIndex ) const; - void updateWheelTransform( int wheelIndex ); + void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); @@ -103,7 +102,7 @@ public: btWheelInfo& getWheelInfo(int index); - void updateWheelTransformsWS(btWheelInfo& wheel ); + void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); void setBrake(float brake,int wheelIndex); diff --git a/src/LinearMath/btDefaultMotionState.h b/src/LinearMath/btDefaultMotionState.h index d92ea03fa..b2a6d1d02 100644 --- a/src/LinearMath/btDefaultMotionState.h +++ b/src/LinearMath/btDefaultMotionState.h @@ -19,16 +19,25 @@ struct btDefaultMotionState : public btMotionState } ///synchronizes world transform from user to physics - virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) + virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const { centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ; } ///synchronizes world transform from physics to user + ///Bullet only calls the update of worldtransform for active objects virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) { m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; } + + ///Bullet gives a callback for objects that are about to be deactivated (put asleep) + /// You can intercept this callback for your own bookkeeping. + ///Also you can return false to disable deactivation for this object this frame. + virtual bool deactivationCallback(void* userPointer) { + return true; + } + }; #endif //DEFAULT_MOTION_STATE_H diff --git a/src/LinearMath/btMotionState.h b/src/LinearMath/btMotionState.h index de560b99f..4806f6eef 100644 --- a/src/LinearMath/btMotionState.h +++ b/src/LinearMath/btMotionState.h @@ -29,10 +29,15 @@ class btMotionState } - virtual void getWorldTransform(btTransform& worldTrans )=0; + virtual void getWorldTransform(btTransform& worldTrans ) const =0; + //Bullet only calls the update of worldtransform for active objects virtual void setWorldTransform(const btTransform& worldTrans)=0; - + + //future: when Bullet makes attempt to deactivate object, you can intercept this callback (return false to disable deactivation for this object this frame) + virtual bool deactivationCallback(void* userPointer) { + return true; + } }; #endif //BT_MOTIONSTATE_H diff --git a/src/LinearMath/btTransformUtil.h b/src/LinearMath/btTransformUtil.h index 12ca634c2..39fa830f4 100644 --- a/src/LinearMath/btTransformUtil.h +++ b/src/LinearMath/btTransformUtil.h @@ -108,7 +108,16 @@ public: static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) { linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; -#ifdef USE_QUATERNION_DIFF + btVector3 axis; + btScalar angle; + calculateDiffAxisAngle(transform0,transform1,axis,angle); + angVel = axis * angle / timeStep; + } + + static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle) + { + + #ifdef USE_QUATERNION_DIFF btQuaternion orn0 = transform0.getRotation(); btQuaternion orn1a = transform1.getRotation(); btQuaternion orn1 = orn0.farthest(orn1a); @@ -118,9 +127,7 @@ public: btQuaternion dorn; dmat.getRotation(dorn); #endif//USE_QUATERNION_DIFF - - btVector3 axis; - btScalar angle; + angle = dorn.getAngle(); axis = btVector3(dorn.x(),dorn.y(),dorn.z()); axis[3] = 0.f; @@ -130,13 +137,8 @@ public: axis = btVector3(1.f,0.f,0.f); else axis /= btSqrt(len); - - - angVel = axis * angle / timeStep; - } - }; #endif //SIMD_TRANSFORM_UTIL_H