From 5383ed49304c94e6572b67fab8a44a4aa40768de Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Tue, 2 Dec 2008 04:01:56 +0000 Subject: [PATCH] + need to reset rigid body using 'setCenterOfMassTransform' to reset world inertia tensor + fixes in compound algorithm -> recompute contact points + add debug drawing to some demos. + revert btJacobianEntry change + replace dCROSSMAT by btVector3::getSkewSymmetricMatrix --- Demos/ForkLiftDemo/main.cpp | 5 +- Demos/OpenGL/DemoApplication.cpp | 4 +- Demos/RagdollDemo/main.cpp | 14 +++ Demos/SliderConstraintDemo/main.cpp | 7 +- Demos/VehicleDemo/main.cpp | 5 +- .../btCompoundCollisionAlgorithm.cpp | 26 +++++ .../btConeTwistConstraint.cpp | 16 +-- .../ConstraintSolver/btContactConstraint.cpp | 3 +- .../btGeneric6DofConstraint.cpp | 77 ++++++++----- .../ConstraintSolver/btHingeConstraint.cpp | 16 +-- .../ConstraintSolver/btJacobianEntry.h | 16 +-- .../btPoint2PointConstraint.cpp | 107 ++++++++++-------- .../btSequentialImpulseConstraintSolver.cpp | 28 ++--- .../ConstraintSolver/btSliderConstraint.cpp | 2 + .../btSolve2LinearConstraint.cpp | 9 +- src/LinearMath/btVector3.h | 7 ++ 16 files changed, 215 insertions(+), 127 deletions(-) diff --git a/Demos/ForkLiftDemo/main.cpp b/Demos/ForkLiftDemo/main.cpp index 536eeb9e3..e5ee20f17 100644 --- a/Demos/ForkLiftDemo/main.cpp +++ b/Demos/ForkLiftDemo/main.cpp @@ -1,7 +1,9 @@ #include "ForkLiftDemo.h" #include "GlutStuff.h" - +#include "GLDebugDrawer.h" +#include "btBulletDynamicsCommon.h" +GLDebugDrawer gDebugDrawer; int main(int argc,char** argv) { @@ -9,6 +11,7 @@ int main(int argc,char** argv) ForkLiftDemo* pForkLiftDemo = new ForkLiftDemo; pForkLiftDemo->initPhysics(); + pForkLiftDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); return glutmain(argc, argv,640,480,"Bullet ForkLift Demo. http://www.continuousphysics.com/Bullet/phpBB2/", pForkLiftDemo); } diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 7f7fc6d81..9c73b1ade 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -608,7 +608,7 @@ btVector3 DemoApplication::getRayTo(int x,int y) return rayTo; } -btScalar mousePickClamping = 3.f; +btScalar mousePickClamping = 30.f; void DemoApplication::mouseFunc(int button, int state, int x, int y) @@ -1249,7 +1249,7 @@ void DemoApplication::clientResetScene() { btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState(); myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans; - colObj->setWorldTransform( myMotionState->m_graphicsWorldTrans ); + body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans ); colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans ); colObj->activate(); //colObj->setActivationState(WANTS_DEACTIVATION); diff --git a/Demos/RagdollDemo/main.cpp b/Demos/RagdollDemo/main.cpp index 13d29e7c2..e2cf99ebb 100644 --- a/Demos/RagdollDemo/main.cpp +++ b/Demos/RagdollDemo/main.cpp @@ -1,3 +1,17 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 "RagdollDemo.h" #include "GlutStuff.h" diff --git a/Demos/SliderConstraintDemo/main.cpp b/Demos/SliderConstraintDemo/main.cpp index d6aa0e581..f84bee370 100755 --- a/Demos/SliderConstraintDemo/main.cpp +++ b/Demos/SliderConstraintDemo/main.cpp @@ -5,13 +5,18 @@ April 04, 2008 #include "SliderConstraintDemo.h" #include "GlutStuff.h" +#include "GLDebugDrawer.h" +#include "btBulletDynamicsCommon.h" + +GLDebugDrawer gDebugDrawer; + int main(int argc,char** argv) { SliderConstraintDemo* sliderConstraintDemo = new SliderConstraintDemo(); sliderConstraintDemo->initPhysics(); - + sliderConstraintDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); return glutmain(argc, argv,640,480,"Slider Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/", sliderConstraintDemo); diff --git a/Demos/VehicleDemo/main.cpp b/Demos/VehicleDemo/main.cpp index d5814a30c..0bc51c218 100644 --- a/Demos/VehicleDemo/main.cpp +++ b/Demos/VehicleDemo/main.cpp @@ -1,7 +1,9 @@ #include "VehicleDemo.h" #include "GlutStuff.h" - +#include "GLDebugDrawer.h" +#include "btBulletDynamicsCommon.h" +GLDebugDrawer gDebugDrawer; int main(int argc,char** argv) { @@ -9,6 +11,7 @@ int main(int argc,char** argv) VehicleDemo* vehicleDemo = new VehicleDemo; vehicleDemo->initPhysics(); + vehicleDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); return glutmain(argc, argv,640,480,"Bullet Vehicle Demo. http://www.continuousphysics.com/Bullet/phpBB2/", vehicleDemo); } diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 8697fc008..1429605bb 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btAabbUtil2.h" +#include "btManifoldResult.h" btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) :btActivatingCollisionAlgorithm(ci,body0,body1), @@ -174,6 +175,31 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt //use a dynamic aabb tree to cull potential child-overlaps btCompoundLeafCallback callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); + ///we need to refresh all contact manifolds + ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep + ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm + { + int numChildren = m_childCollisionAlgorithms.size(); + int i; + btManifoldArray manifoldArray; + for (i=0;igetAllContactManifolds(manifoldArray); + for (int m=0;mgetNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0);//??necessary? + } + } + manifoldArray.clear(); + } + } + } if (tree) { diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index e760347fb..d17e722c2 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -118,13 +118,15 @@ void btConeTwistConstraint::buildJacobian() for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } } diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 9b81dc798..b5b3fbf85 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -52,7 +52,8 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btVector3 vel = vel1 - vel2; - btJacobianEntry jac( + btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + body2.getCenterOfMassTransform().getBasis().transpose(), rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), body2.getInvInertiaDiagLocal(),body2.getInvMass()); diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 1fc8dce3c..f84576f4f 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -44,17 +44,6 @@ m_useSolveConstraintObsolete(true) } - -#define dCROSSMAT(A,a,skip,plus,minus) \ -{ \ - (A)[1] = minus (a)[2]; \ - (A)[2] = plus (a)[1]; \ - (A)[(skip)+0] = plus (a)[2]; \ - (A)[(skip)+2] = minus (a)[0]; \ - (A)[2*(skip)+0] = minus (a)[1]; \ - (A)[2*(skip)+1] = plus (a)[0]; \ -} - #define GENERIC_D6_DISABLE_WARMSTARTING 1 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); @@ -374,25 +363,25 @@ void btGeneric6DofConstraint::buildLinearJacobian( const btVector3 & pivotAInW,const btVector3 & pivotBInW) { new (&jacLinear) btJacobianEntry( - - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normalWorld, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } void btGeneric6DofConstraint::buildAngularJacobian( btJacobianEntry & jacAngular,const btVector3 & jointAxisW) { - new (&jacAngular) btJacobianEntry(jointAxisW, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + new (&jacAngular) btJacobianEntry(jointAxisW, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); } @@ -517,12 +506,34 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) info->m_J1linearAxis[s+1] = 1; info->m_J1linearAxis[2*s+2] = 1; + /*for (int i=0;i<3;i++) + { + if (m_useLinearReferenceFrameA) + { + btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i]; + *linear_axis = m_calculatedTransformA.getBasis().getColumn(i); + } + else + { + btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i]; + *linear_axis = m_calculatedTransformB.getBasis().getColumn(i); + } + } + */ + + btVector3 a1,a2; a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin(); - //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); - dCROSSMAT (info->m_J1angularAxis,a1,s,-,+); + + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } /*info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[s+1] = -1; @@ -530,15 +541,19 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) */ a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin(); - //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); - dCROSSMAT (info->m_J2angularAxis,a2,s,+,-); + + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } // set right hand side btScalar k = info->fps * info->erp; for (int j=0; j<3; j++) { - info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] - - a1[j] - body0_trans.getOrigin()[j]); + info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); } return 3; diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 5f576f12a..f28be0e7c 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -189,13 +189,15 @@ void btHingeConstraint::buildJacobian() for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } } diff --git a/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index e05fdce76..bfeb24c2d 100644 --- a/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -34,6 +34,8 @@ public: btJacobianEntry() {}; //constraint between two different rigidbodies btJacobianEntry( + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, const btVector3& rel_pos1,const btVector3& rel_pos2, const btVector3& jointAxis, const btVector3& inertiaInvA, @@ -42,8 +44,8 @@ public: const btScalar massInvB) :m_linearJointAxis(jointAxis) { - m_aJ = (rel_pos1.cross(m_linearJointAxis)); - m_bJ = (rel_pos2.cross(-m_linearJointAxis)); + m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); @@ -59,8 +61,8 @@ public: const btVector3& inertiaInvB) :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) { - m_aJ= jointAxis; - m_bJ = -jointAxis; + m_aJ= world2A*jointAxis; + m_bJ = world2B*-jointAxis; m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); @@ -93,8 +95,8 @@ public: const btScalar massInvA) :m_linearJointAxis(jointAxis) { - m_aJ= (rel_pos1.cross(jointAxis)); - m_bJ = (rel_pos2.cross(-jointAxis)); + m_aJ= world2A*(rel_pos1.cross(jointAxis)); + m_bJ = world2A*(rel_pos2.cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); @@ -128,7 +130,7 @@ public: return sum[0]+sum[1]+sum[2]; } - btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) const + btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) { btVector3 linrel = linvelA - linvelB; btVector3 angvela = angvelA * m_aJ; diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 03ca2334b..1da749517 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -43,7 +43,7 @@ m_useSolveConstraintObsolete(false) void btPoint2PointConstraint::buildJacobian() { - if (m_useSolveConstraintObsolete) + ///we need it for both methods { m_appliedImpulse = btScalar(0.); @@ -53,15 +53,16 @@ void btPoint2PointConstraint::buildJacobian() { normal[i] = 1; new (&m_jac[i]) btJacobianEntry( - - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; } } @@ -80,16 +81,6 @@ void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) info->nub = 3; } } -#define dCROSSMAT(A,a,skip,plus,minus) \ -{ \ - (A)[1] = minus (a)[2]; \ - (A)[2] = plus (a)[1]; \ - (A)[(skip)+0] = plus (a)[2]; \ - (A)[(skip)+2] = minus (a)[0]; \ - (A)[2*(skip)+0] = minus (a)[1]; \ - (A)[2*(skip)+1] = plus (a)[0]; \ -} -#include void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) { @@ -102,48 +93,55 @@ void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) body1_trans = m_rbB.getCenterOfMassTransform(); // anchor points in global coordinates with respect to body PORs. - int s = info->rowskip; - + // set jacobian info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[s+1] = 1; - info->m_J1linearAxis[2*s+2] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1,a2; - - a1 = body0_trans.getBasis()*getPivotInA(); - //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); - dCROSSMAT (info->m_J1angularAxis,a1,s,-,+); + btVector3 a1 = body0_trans.getBasis()*getPivotInA(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } /*info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[s+1] = -1; info->m_J2linearAxis[2*s+2] = -1; */ - - a2 = body1_trans.getBasis()*getPivotInB(); - //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); - //dCROSSMAT (info->m_J2angularAxis,a2,s,+,-); - dCROSSMAT (info->m_J2angularAxis,a2,s,+,-); - + + btVector3 a2 = body1_trans.getBasis()*getPivotInB(); + + { + btVector3 a2n = -a2; + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + // set right hand side btScalar k = info->fps * info->erp; int j; for (j=0; j<3; j++) { - info->m_constraintError[j*s] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } - btScalar impulseClamp = m_setting.m_impulseClamp; + btScalar impulseClamp = m_setting.m_impulseClamp;// for (j=0; j<3; j++) { - if (impulseClamp > 0) + if (m_setting.m_impulseClamp > 0) { - info->m_lowerLimit[j*s] = -impulseClamp; - info->m_upperLimit[j*s] = impulseClamp; + info->m_lowerLimit[j*info->rowskip] = -impulseClamp; + info->m_upperLimit[j*info->rowskip] = impulseClamp; } } @@ -190,24 +188,33 @@ void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; btScalar impulseClamp = m_setting.m_impulseClamp; - if (impulseClamp > 0) + + const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse; + if (sum < -impulseClamp) { - if (impulse < -impulseClamp) - impulse = -impulseClamp; - if (impulse > impulseClamp) - impulse = impulseClamp; + deltaImpulse = -impulseClamp-m_appliedImpulse; + m_appliedImpulse = -impulseClamp; + } + else if (sum > impulseClamp) + { + deltaImpulse = impulseClamp-m_appliedImpulse; + m_appliedImpulse = impulseClamp; + } + else + { + m_appliedImpulse = sum; } - m_appliedImpulse+=impulse; - btVector3 impulse_vector = normal * impulse; + + btVector3 impulse_vector = normal * deltaImpulse; btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse); normal[i] = 0; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 84fcce018..cf409b9a6 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -382,6 +382,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol return 0.f; } + if (1) + { + int j; + for (j=0;jbuildJacobian(); + } + } + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); initSolverBody(&fixedBody,0); @@ -406,10 +416,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btTypedConstraint::btConstraintInfo1 info1; info1.m_numConstraintRows = 0; - if (m_tmpSolverNonContactConstraintPool.size()>3) - { - printf("dsadas\n"); - } + ///setup the btSolverConstraints int currentRow = 0; @@ -492,7 +499,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol sum += iMJlB.dot(solverConstraint.m_contactNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - solverConstraint.m_jacDiagABInv = 1./sum; + solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; } @@ -551,6 +558,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol ///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! ///-> if (cp.getDistance() <= btScalar(0.)) + //if (cp.getDistance() <= manifold->getContactBreakingThreshold()) { const btVector3& pos1 = cp.getPositionWorldOnA(); @@ -772,15 +780,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btContactSolverInfo info = infoGlobal; - if (1) - { - int j; - for (j=0;jbuildJacobian(); - } - } + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 990bd00e9..fe22de902 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -119,6 +119,8 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); new (&m_jacLin[i]) btJacobianEntry( + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), m_relPosA, m_relPosB, normalWorld, diff --git a/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp index d2feeb885..0c7dbd668 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -61,10 +61,9 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA( - rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); - btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); @@ -151,9 +150,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); - btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index c03cd2038..5d5c39e85 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -309,6 +309,13 @@ public: m_floats[3] = 0.f; } + void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const + { + v0->setValue(0. ,-z() ,y()); + v1->setValue(z() ,0. ,-x()); + v2->setValue(-y() ,x() ,0.); + } + }; /**@brief Return the sum of two vectors (Point symantics)*/