diff --git a/Extras/quickstep/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp index f1b6acbb5..63589e0e6 100644 --- a/Extras/quickstep/OdeConstraintSolver.cpp +++ b/Extras/quickstep/OdeConstraintSolver.cpp @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +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. @@ -24,6 +24,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "OdeJoint.h" #include "OdeContactJoint.h" +#include "OdeTypedJoint.h" #include "OdeSolverBody.h" #include #include "LinearMath/btQuickprof.h" @@ -61,11 +62,12 @@ class BU_Joint; #define ODE_MAX_SOLVER_JOINTS 65535 static OdeSolverBody gSolverBodyArray[ODE_MAX_SOLVER_BODIES]; static ContactJoint gJointArray[ODE_MAX_SOLVER_JOINTS]; +static OdeTypedJoint gTypedJointArray[ODE_MAX_SOLVER_JOINTS]; OdeConstraintSolver::OdeConstraintSolver(): -m_cfm(0.f),//1e-5f), -m_erp(0.4f) + m_cfm(0.f),//1e-5f), + m_erp(0.4f) { } @@ -74,45 +76,56 @@ m_erp(0.4f) //iterative lcp and penalty method btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher) { - BEGIN_PROFILE("prepareConstraints"); + BEGIN_PROFILE("prepareConstraints"); - m_CurBody = 0; - m_CurJoint = 0; + m_CurBody = 0; + m_CurJoint = 0; + m_CurTypedJoint = 0; - int numBodies = 0; - OdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES]; - int numJoints = 0; - BU_Joint* joints [ODE_MAX_SOLVER_JOINTS]; - - for (int j=0;jgetNumContacts() > 0) + { + body0 = ConvertBody((btRigidBody*)manifold->getBody0(),odeBodies,numBodies); + body1 = ConvertBody((btRigidBody*)manifold->getBody1(),odeBodies,numBodies); + ConvertConstraint(manifold,joints,numJoints,odeBodies,body0,body1,debugDrawer); + } + } - btPersistentManifold* manifold = manifoldPtr[j]; - if (manifold->getNumContacts() > 0) - { - body0 = ConvertBody((btRigidBody*)manifold->getBody0(),odeBodies,numBodies); - body1 = ConvertBody((btRigidBody*)manifold->getBody1(),odeBodies,numBodies); - ConvertConstraint(manifold,joints,numJoints,odeBodies,body0,body1,debugDrawer); - } - } + //capture constraints + for (j=0;jgetRigidBodyA(),odeBodies,numBodies); + body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),odeBodies,numBodies); + ConvertTypedConstraint(typedconstraint,joints,numJoints,odeBodies,body0,body1,debugDrawer); + } - END_PROFILE("prepareConstraints"); - BEGIN_PROFILE("solveConstraints"); - SolveInternal1(m_cfm,m_erp,odeBodies,numBodies,joints,numJoints,infoGlobal); - //write back resulting velocities - for (int i=0;im_invMass) - { - odeBodies[i]->m_originalBody->setLinearVelocity(odeBodies[i]->m_linearVelocity); - odeBodies[i]->m_originalBody->setAngularVelocity(odeBodies[i]->m_angularVelocity); - } - } - END_PROFILE("solveConstraints"); - return 0.f; + END_PROFILE("prepareConstraints"); + BEGIN_PROFILE("solveConstraints"); + SolveInternal1(m_cfm,m_erp,odeBodies,numBodies,joints,numJoints,infoGlobal); + + //write back resulting velocities + for (int i=0;im_invMass) + { + odeBodies[i]->m_originalBody->setLinearVelocity(odeBodies[i]->m_linearVelocity); + odeBodies[i]->m_originalBody->setAngularVelocity(odeBodies[i]->m_angularVelocity); + } + } + END_PROFILE("solveConstraints"); + return 0.f; } @@ -124,24 +137,24 @@ typedef btScalar dQuaternion[4]; void dRfromQ1 (dMatrix3 R, const dQuaternion q) { - // q = (s,vx,vy,vz) - btScalar qq1 = 2.f*q[1]*q[1]; - btScalar qq2 = 2.f*q[2]*q[2]; - btScalar qq3 = 2.f*q[3]*q[3]; - _R(0,0) = 1.f - qq2 - qq3; - _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); - _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); - _R(0,3) = 0.f; - - _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); - _R(1,1) = 1.f - qq1 - qq3; - _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); - _R(1,3) = 0.f; + // q = (s,vx,vy,vz) + btScalar qq1 = 2.f*q[1]*q[1]; + btScalar qq2 = 2.f*q[2]*q[2]; + btScalar qq3 = 2.f*q[3]*q[3]; + _R(0,0) = 1.f - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(0,3) = 0.f; - _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); - _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); - _R(2,2) = 1.f - qq1 - qq2; - _R(2,3) = 0.f; + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1.f - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(1,3) = 0.f; + + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1.f - qq1 - qq2; + _R(2,3) = 0.f; } @@ -149,149 +162,207 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q) int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,OdeSolverBody** bodies,int& numBodies) { - assert(orgBody); - if (!orgBody || (orgBody->getInvMass() == 0.f) ) - { - return -1; - } + assert(orgBody); + if (!orgBody || (orgBody->getInvMass() == 0.f) ) + { + return -1; + } - if (orgBody->getCompanionId()>=0) - { - return orgBody->getCompanionId(); - } - //first try to find - int i,j; - - //if not found, create a new body - OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies]; - orgBody->setCompanionId(numBodies); + if (orgBody->getCompanionId()>=0) + { + return orgBody->getCompanionId(); + } + //first try to find + int i,j; - numBodies++; + //if not found, create a new body + OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies]; + orgBody->setCompanionId(numBodies); - body->m_originalBody = orgBody; + numBodies++; - body->m_facc.setValue(0,0,0,0); - body->m_tacc.setValue(0,0,0,0); + body->m_originalBody = orgBody; - body->m_linearVelocity = orgBody->getLinearVelocity(); - body->m_angularVelocity = orgBody->getAngularVelocity(); - body->m_invMass = orgBody->getInvMass(); - body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition(); - body->m_friction = orgBody->getFriction(); - - //are the indices the same ? - for (i=0;i<4;i++) - { - for ( j=0;j<3;j++) - { - body->m_invI[i+4*j] = 0.f; - body->m_I[i+4*j] = 0.f; - } - } - body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal().x(); - body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y(); - body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z(); + body->m_facc.setValue(0,0,0,0); + body->m_tacc.setValue(0,0,0,0); - body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x(); - body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y(); - body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z(); - + body->m_linearVelocity = orgBody->getLinearVelocity(); + body->m_angularVelocity = orgBody->getAngularVelocity(); + body->m_invMass = orgBody->getInvMass(); + body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition(); + body->m_friction = orgBody->getFriction(); - - - dQuaternion q; + //are the indices the same ? + for (i=0;i<4;i++) + { + for ( j=0;j<3;j++) + { + body->m_invI[i+4*j] = 0.f; + body->m_I[i+4*j] = 0.f; + } + } + body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal().x(); + body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y(); + body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z(); - q[1] = orgBody->getOrientation().x(); - q[2] = orgBody->getOrientation().y(); - q[3] = orgBody->getOrientation().z(); - q[0] = orgBody->getOrientation().w(); - - dRfromQ1(body->m_R,q); - - return numBodies-1; + body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x(); + body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y(); + body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z(); + + + + + dQuaternion q; + + q[1] = orgBody->getOrientation().x(); + q[2] = orgBody->getOrientation().y(); + q[3] = orgBody->getOrientation().z(); + q[0] = orgBody->getOrientation().w(); + + dRfromQ1(body->m_R,q); + + return numBodies-1; } - + void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints, - OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer) + OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer) { - manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), - ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform()); + manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), + ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform()); - int bodyId0 = _bodyId0,bodyId1 = _bodyId1; + int bodyId0 = _bodyId0,bodyId1 = _bodyId1; - int i,numContacts = manifold->getNumContacts(); - - bool swapBodies = (bodyId0 < 0); + int i,numContacts = manifold->getNumContacts(); - - OdeSolverBody* body0,*body1; + bool swapBodies = (bodyId0 < 0); - if (swapBodies) - { - bodyId0 = _bodyId1; - bodyId1 = _bodyId0; - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); + OdeSolverBody* body0,*body1; - } else - { - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); - } + if (swapBodies) + { + bodyId0 = _bodyId1; + bodyId1 = _bodyId0; - assert(bodyId0 >= 0); + body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); - btVector3 color(0,1,0); - for (i=0;i=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); + } - if (debugDrawer) - { - const btManifoldPoint& cp = manifold->getContactPoint(i); + assert(bodyId0 >= 0); - debugDrawer->drawContactPoint( - cp.m_positionWorldOnB, - cp.m_normalWorldOnB, - cp.getDistance(), - cp.getLifeTime(), - color); + btVector3 color(0,1,0); + for (i=0;igetContactPoint(i); + + debugDrawer->drawContactPoint( + cp.m_positionWorldOnB, + cp.m_normalWorldOnB, + cp.getDistance(), + cp.getLifeTime(), + color); + + } + assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS); // if (manifold->getContactPoint(i).getDistance() < 0.0f) - { - ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); + { + ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); - cont->node[0].joint = cont; - cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; - - cont->node[1].joint = cont; - cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; - - joints[numJoints++] = cont; - for (int i=0;i<6;i++) - cont->lambda[i] = 0.f; + cont->node[0].joint = cont; + cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; - cont->flags = 0; - } + cont->node[1].joint = cont; + cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; + + joints[numJoints++] = cont; + for (int i=0;i<6;i++) + cont->lambda[i] = 0.f; + + cont->flags = 0; + } + } + + //create a new contact constraint +} + +void OdeConstraintSolver::ConvertTypedConstraint( + btTypedConstraint * constraint,BU_Joint** joints,int& numJoints, + OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer) +{ + + int bodyId0 = _bodyId0,bodyId1 = _bodyId1; + bool swapBodies = (bodyId0 < 0); + + + OdeSolverBody* body0,*body1; + + if (swapBodies) + { + bodyId0 = _bodyId1; + bodyId1 = _bodyId0; + + body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); + + } + else + { + body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); + } + + assert(bodyId0 >= 0); + + + assert (m_CurTypedJoint < ODE_MAX_SOLVER_JOINTS); + + + OdeTypedJoint * cont = NULL; + + // Determine constraint type + int joint_type = constraint->getConstraintType(); + switch(joint_type) + { + case POINT2POINT_CONSTRAINT_TYPE: + case D6_CONSTRAINT_TYPE: + cont = new (&gTypedJointArray[m_CurTypedJoint ++]) OdeTypedJoint(constraint,0, swapBodies,body0,body1); + break; + + }; + + if(cont) + { + cont->node[0].joint = cont; + cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; + + cont->node[1].joint = cont; + cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; + + joints[numJoints++] = cont; + for (int i=0;i<6;i++) + cont->lambda[i] = 0.f; + + cont->flags = 0; } - //create a new contact constraint -}; - - -void OdeConstraintSolver::reset() -{ } diff --git a/Extras/quickstep/OdeConstraintSolver.h b/Extras/quickstep/OdeConstraintSolver.h index 4b2e5e60b..3b3401a94 100644 --- a/Extras/quickstep/OdeConstraintSolver.h +++ b/Extras/quickstep/OdeConstraintSolver.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +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. @@ -20,7 +20,6 @@ subject to the following restrictions: class btRigidBody; struct OdeSolverBody; -class btDispatcher; class BU_Joint; /// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework @@ -31,26 +30,33 @@ private: int m_CurBody; int m_CurJoint; + int m_CurTypedJoint; float m_cfm; float m_erp; - + int ConvertBody(btRigidBody* body,OdeSolverBody** bodies,int& numBodies); void ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints, OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); + + void ConvertTypedConstraint( + btTypedConstraint * constraint,BU_Joint** joints,int& numJoints, + OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); + + public: OdeConstraintSolver(); virtual ~OdeConstraintSolver() {} - + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher); ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' - void setConstraintForceMixing(float cfm) { + void setConstraintForceMixing(float cfm) { m_cfm = cfm; } @@ -61,7 +67,9 @@ public: m_erp = erp; } - virtual void reset(); + void reset() + { + } }; diff --git a/Extras/quickstep/OdeMacros.h b/Extras/quickstep/OdeMacros.h new file mode 100644 index 000000000..658a07465 --- /dev/null +++ b/Extras/quickstep/OdeMacros.h @@ -0,0 +1,188 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser bteral Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * bteral Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#define ODE_MACROS +#ifdef ODE_MACROS + +#include "LinearMath/btScalar.h" + +typedef btScalar dVector4[4]; +typedef btScalar dMatrix3[4*3]; +#define dInfinity FLT_MAX + + + +#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ + + + +#define dMULTIPLY0_331NEW(A,op,B,C) \ +{\ + btScalar tmp[3];\ + tmp[0] = C.getX();\ + tmp[1] = C.getY();\ + tmp[2] = C.getZ();\ + dMULTIPLYOP0_331(A,op,B,tmp);\ +} + +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) +#define dMULTIPLYOP0_331(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B+4),(C)); \ + (A)[2] op dDOT1((B+8),(C)); + +#define dAASSERT btAssert +#define dIASSERT btAssert + +#define REAL float +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +inline btScalar dDOT1 (const btScalar *a, const btScalar *b) +{ return dDOTpq(a,b,1,1); } +#define dDOT14(a,b) dDOTpq(a,b,1,4) + +#define dCROSS(a,op,b,c) \ + (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ + (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ + (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); + +/* + * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b. + * A is stored by rows, and has `skip' elements per row. the matrix is + * assumed to be already zero, so this does not write zero elements! + * if (plus,minus) is (+,-) then a positive version will be written. + * if (plus,minus) is (-,+) then a negative version will be written. + */ + +#define dCROSSMAT(A,a,skip,plus,minus) \ +do { \ + (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]; \ +} while(0) + + +#define dMULTIPLYOP2_333(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B),(C+4)); \ + (A)[2] op dDOT1((B),(C+8)); \ + (A)[4] op dDOT1((B+4),(C)); \ + (A)[5] op dDOT1((B+4),(C+4)); \ + (A)[6] op dDOT1((B+4),(C+8)); \ + (A)[8] op dDOT1((B+8),(C)); \ + (A)[9] op dDOT1((B+8),(C+4)); \ + (A)[10] op dDOT1((B+8),(C+8)); +#define dMULTIPLYOP0_333(A,op,B,C) \ + (A)[0] op dDOT14((B),(C)); \ + (A)[1] op dDOT14((B),(C+1)); \ + (A)[2] op dDOT14((B),(C+2)); \ + (A)[4] op dDOT14((B+4),(C)); \ + (A)[5] op dDOT14((B+4),(C+1)); \ + (A)[6] op dDOT14((B+4),(C+2)); \ + (A)[8] op dDOT14((B+8),(C)); \ + (A)[9] op dDOT14((B+8),(C+1)); \ + (A)[10] op dDOT14((B+8),(C+2)); + +#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) +#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) +#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) + + +//////////////////////////////////////////////////////////////////// +#define EFFICIENT_ALIGNMENT 16 +#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ + +#define dALLOCA16(n) \ + ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) + + + +///////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////// + +#ifdef DEBUG +#define ANSI_FTOL 1 + +extern "C" { + __declspec(naked) void _ftol2() { + __asm { +#if ANSI_FTOL + fnstcw WORD PTR [esp-2] + mov ax, WORD PTR [esp-2] + + OR AX, 0C00h + + mov WORD PTR [esp-4], ax + fldcw WORD PTR [esp-4] + fistp QWORD PTR [esp-12] + fldcw WORD PTR [esp-2] + mov eax, DWORD PTR [esp-12] + mov edx, DWORD PTR [esp-8] +#else + fistp DWORD PTR [esp-12] + mov eax, DWORD PTR [esp-12] + mov ecx, DWORD PTR [esp-8] +#endif + ret + } + } +} +#endif //DEBUG + + + + + + +#define ALLOCA dALLOCA16 + +typedef const btScalar *dRealPtr; +typedef btScalar *dRealMutablePtr; +#define dRealArray(name,n) btScalar name[n]; +#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar)); + +inline void dSetZero1 (btScalar *a, int n) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = 0; + n--; + } +} + +inline void dSetValue1 (btScalar *a, int n, btScalar value) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = value; + n--; + } +} + + +#endif //USE_SOR_SOLVER + diff --git a/Extras/quickstep/OdeTypedJoint.cpp b/Extras/quickstep/OdeTypedJoint.cpp new file mode 100644 index 000000000..ca45c91a4 --- /dev/null +++ b/Extras/quickstep/OdeTypedJoint.cpp @@ -0,0 +1,451 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "OdeTypedJoint.h" +#include "OdeSolverBody.h" +#include "OdeMacros.h" +#include + +void OdeTypedJoint::GetInfo1(Info1 *info) +{ + int joint_type = m_constraint->getConstraintType(); + switch (joint_type) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + p2pjoint.GetInfo1(info); + } + break; + case D6_CONSTRAINT_TYPE: + { + OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + d6joint.GetInfo1(info); + } + break; + }; +} + +void OdeTypedJoint::GetInfo2(Info2 *info) +{ + int joint_type = m_constraint->getConstraintType(); + switch (joint_type) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + p2pjoint.GetInfo2(info); + } + break; + case D6_CONSTRAINT_TYPE: + { + OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + d6joint.GetInfo2(info); + } + break; + }; +} + + +OdeP2PJoint::OdeP2PJoint( + btTypedConstraint * constraint, + int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1): + OdeTypedJoint(constraint,index,swap,body0,body1) +{ +} + + +void OdeP2PJoint::GetInfo1(Info1 *info) +{ + info->m = 3; + info->nub = 3; +} + + +void OdeP2PJoint::GetInfo2(Info2 *info) +{ + + btPoint2PointConstraint * p2pconstraint = this->getP2PConstraint(); + + //retrieve matrices + btTransform body0_trans; + if (m_body0) + { + body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); + } +// btScalar body0_mat[12]; +// body0_mat[0] = body0_trans.getBasis()[0][0]; +// body0_mat[1] = body0_trans.getBasis()[0][1]; +// body0_mat[2] = body0_trans.getBasis()[0][2]; +// body0_mat[4] = body0_trans.getBasis()[1][0]; +// body0_mat[5] = body0_trans.getBasis()[1][1]; +// body0_mat[6] = body0_trans.getBasis()[1][2]; +// body0_mat[8] = body0_trans.getBasis()[2][0]; +// body0_mat[9] = body0_trans.getBasis()[2][1]; +// body0_mat[10] = body0_trans.getBasis()[2][2]; + + btTransform body1_trans; + + if (m_body1) + { + body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); + } +// btScalar body1_mat[12]; +// body1_mat[0] = body1_trans.getBasis()[0][0]; +// body1_mat[1] = body1_trans.getBasis()[0][1]; +// body1_mat[2] = body1_trans.getBasis()[0][2]; +// body1_mat[4] = body1_trans.getBasis()[1][0]; +// body1_mat[5] = body1_trans.getBasis()[1][1]; +// body1_mat[6] = body1_trans.getBasis()[1][2]; +// body1_mat[8] = body1_trans.getBasis()[2][0]; +// body1_mat[9] = body1_trans.getBasis()[2][1]; +// body1_mat[10] = body1_trans.getBasis()[2][2]; + + + + + // anchor points in global coordinates with respect to body PORs. + + + int s = info->rowskip; + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + + btVector3 a1,a2; + + a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA(); + //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); + dCROSSMAT (info->J1a,a1,s,-,+); + if (m_body1) + { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB(); + //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); + dCROSSMAT (info->J2a,a2,s,+,-); + } + + + // set right hand side + btScalar k = info->fps * info->erp; + if (m_body1) + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - + a1[j] - body0_trans.getOrigin()[j]); + } + } + else + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - + body0_trans.getOrigin()[j]); + } + } +} + + +///////////////////limit motor support + +/*! \pre testLimitValue must be called on limot*/ +int bt_get_limit_motor_info2( + btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + BU_Joint::Info2 *info, int row, btVector3 ax1, int rotational) +{ + + + int srow = row * info->rowskip; + + // if the joint is powered, or has joint limits, add in the extra row + int powered = limot->m_enableMotor; + int limit = limot->m_currentLimit; + + if (powered || limit) + { + btScalar *J1 = rotational ? info->J1a : info->J1l; + btScalar *J2 = rotational ? info->J2a : info->J2l; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if (body1) + { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), we have to be careful + // that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + btVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (!rotational && body1) + { + btVector3 c; + c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0] + -body0->getCenterOfMassPosition()[0]); + c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1] + -body0->getCenterOfMassPosition()[1]); + c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2] + -body0->getCenterOfMassPosition()[2]); + + ltd = c.cross(ax1); + + info->J1a[srow+0] = ltd[0]; + info->J1a[srow+1] = ltd[1]; + info->J1a[srow+2] = ltd[2]; + info->J2a[srow+0] = ltd[0]; + info->J2a[srow+1] = ltd[1]; + info->J2a[srow+2] = ltd[2]; + } + + // if we're limited low and high simultaneously, the joint motor is + // ineffective + + if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; + + if (powered) + { + info->cfm[row] = 0.0f;//limot->m_normalCFM; + if (! limit) + { + info->c[row] = limot->m_targetVelocity; + info->lo[row] = -limot->m_maxMotorForce; + info->hi[row] = limot->m_maxMotorForce; + } + } + + if (limit) + { + btScalar k = info->fps * limot->m_ERP; + info->c[row] = -k * limot->m_currentLimitError; + info->cfm[row] = 0.0f;//limot->m_stopCFM; + + if (limot->m_loLimit == limot->m_hiLimit) + { + // limited low and high simultaneously + info->lo[row] = -dInfinity; + info->hi[row] = dInfinity; + } + else + { + if (limit == 1) + { + // low limit + info->lo[row] = 0; + info->hi[row] = SIMD_INFINITY; + } + else + { + // high limit + info->lo[row] = -SIMD_INFINITY; + info->hi[row] = 0; + } + + // deal with bounce + if (limot->m_bounce > 0) + { + // calculate joint velocity + btScalar vel; + if (rotational) + { + vel = body0->getAngularVelocity().dot(ax1); + if (body1) + vel -= body1->getAngularVelocity().dot(ax1); + } + else + { + vel = body0->getLinearVelocity().dot(ax1); + if (body1) + vel -= body1->getLinearVelocity().dot(ax1); + } + + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) + { + // low limit + if (vel < 0) + { + btScalar newc = -limot->m_bounce* vel; + if (newc > info->c[row]) info->c[row] = newc; + } + } + else + { + // high limit - all those computations are reversed + if (vel > 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc < info->c[row]) info->c[row] = newc; + } + } + } + } + } + return 1; + } + else return 0; +} + + +///////////////////OdeD6Joint + + + + + +OdeD6Joint::OdeD6Joint( + btTypedConstraint * constraint, + int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1): + OdeTypedJoint(constraint,index,swap,body0,body1) +{ +} + + +void OdeD6Joint::GetInfo1(Info1 *info) +{ + btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); + //prepare constraint + d6constraint->calculateTransforms(); + info->m = 3; + info->nub = 3; + + //test angular limits + for (int i=0;i<3 ;i++ ) + { + //if(i==2) continue; + if(d6constraint->testAngularLimitMotor(i)) + { + info->m++; + } + } + + +} + + +int OdeD6Joint::setLinearLimits(Info2 *info) +{ + + btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); + + //retrieve matrices + btTransform body0_trans; + if (m_body0) + { + body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); + } + + btTransform body1_trans; + + if (m_body1) + { + body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); + } + + // anchor points in global coordinates with respect to body PORs. + + int s = info->rowskip; + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + + btVector3 a1,a2; + + a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin(); + //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); + dCROSSMAT (info->J1a,a1,s,-,+); + if (m_body1) + { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin(); + + //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); + dCROSSMAT (info->J2a,a2,s,+,-); + } + + + // set right hand side + btScalar k = info->fps * info->erp; + if (m_body1) + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - + a1[j] - body0_trans.getOrigin()[j]); + } + } + else + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] - + body0_trans.getOrigin()[j]); + } + } + + return 3; + +} + +int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset) +{ + btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); + int row = row_offset; + //solve angular limits + for (int i=0;i<3 ;i++ ) + { + //if(i==2) continue; + if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + { + btVector3 axis = d6constraint->getAxis(i); + row += bt_get_limit_motor_info2( + d6constraint->getRotationalLimitMotor(i), + m_body0->m_originalBody,m_body1->m_originalBody,info,row,axis,1); + } + } + + return row; +} + +void OdeD6Joint::GetInfo2(Info2 *info) +{ + int row = setLinearLimits(info); + setAngularLimits(info, row); +} + diff --git a/Extras/quickstep/OdeTypedJoint.h b/Extras/quickstep/OdeTypedJoint.h new file mode 100644 index 000000000..ab74df0e6 --- /dev/null +++ b/Extras/quickstep/OdeTypedJoint.h @@ -0,0 +1,111 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +/* +2007-09-09 +Added support for typed joints by Francisco León +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + +#ifndef TYPED_JOINT_H +#define TYPED_JOINT_H + +#include "OdeJoint.h" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" + +struct OdeSolverBody; + +class OdeTypedJoint : public BU_Joint +{ +public: + btTypedConstraint * m_constraint; + int m_index; + bool m_swapBodies; + OdeSolverBody* m_body0; + OdeSolverBody* m_body1; + + OdeTypedJoint(){} + OdeTypedJoint( + btTypedConstraint * constraint, + int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1): + m_constraint(constraint), + m_index(index), + m_swapBodies(swap), + m_body0(body0), + m_body1(body1) + { + } + + virtual void GetInfo1(Info1 *info); + virtual void GetInfo2(Info2 *info); +}; + + + +class OdeP2PJoint : public OdeTypedJoint +{ +protected: + inline btPoint2PointConstraint * getP2PConstraint() + { + return static_cast(m_constraint); + } +public: + + OdeP2PJoint() {}; + + OdeP2PJoint(btTypedConstraint* constraint,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1); + + //BU_Joint interface for solver + + virtual void GetInfo1(Info1 *info); + + virtual void GetInfo2(Info2 *info); +}; + + +class OdeD6Joint : public OdeTypedJoint +{ +protected: + inline btGeneric6DofConstraint * getD6Constraint() + { + return static_cast(m_constraint); + } + + int setLinearLimits(Info2 *info); + int setAngularLimits(Info2 *info, int row_offset); + +public: + + OdeD6Joint() {}; + + OdeD6Joint(btTypedConstraint* constraint,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1); + + //BU_Joint interface for solver + + virtual void GetInfo1(Info1 *info); + + virtual void GetInfo2(Info2 *info); +}; + +//! retrieves the constraint info from a btRotationalLimitMotor object +/*! \pre testLimitValue must be called on limot*/ +int bt_get_limit_motor_info2( + btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + BU_Joint::Info2 *info, int row, btVector3 ax1, int rotational); + +#endif //CONTACT_JOINT_H + diff --git a/Extras/quickstep/SorLcp.cpp b/Extras/quickstep/SorLcp.cpp index e1a05ca14..85dba98c1 100644 --- a/Extras/quickstep/SorLcp.cpp +++ b/Extras/quickstep/SorLcp.cpp @@ -53,145 +53,7 @@ //////////////////////////////////////////////////////////////////// //math stuff -typedef btScalar dVector4[4]; -typedef btScalar dMatrix3[4*3]; -#define dInfinity FLT_MAX - - - -#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ - - - -#define dMULTIPLY0_331NEW(A,op,B,C) \ -{\ - btScalar tmp[3];\ - tmp[0] = C.getX();\ - tmp[1] = C.getY();\ - tmp[2] = C.getZ();\ - dMULTIPLYOP0_331(A,op,B,tmp);\ -} - -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) -#define dMULTIPLYOP0_331(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B+4),(C)); \ - (A)[2] op dDOT1((B+8),(C)); - -#define dAASSERT btAssert -#define dIASSERT btAssert - -#define REAL float -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) -btScalar dDOT1 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } -#define dDOT14(a,b) dDOTpq(a,b,1,4) - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); - - -#define dMULTIPLYOP2_333(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B),(C+4)); \ - (A)[2] op dDOT1((B),(C+8)); \ - (A)[4] op dDOT1((B+4),(C)); \ - (A)[5] op dDOT1((B+4),(C+4)); \ - (A)[6] op dDOT1((B+4),(C+8)); \ - (A)[8] op dDOT1((B+8),(C)); \ - (A)[9] op dDOT1((B+8),(C+4)); \ - (A)[10] op dDOT1((B+8),(C+8)); -#define dMULTIPLYOP0_333(A,op,B,C) \ - (A)[0] op dDOT14((B),(C)); \ - (A)[1] op dDOT14((B),(C+1)); \ - (A)[2] op dDOT14((B),(C+2)); \ - (A)[4] op dDOT14((B+4),(C)); \ - (A)[5] op dDOT14((B+4),(C+1)); \ - (A)[6] op dDOT14((B+4),(C+2)); \ - (A)[8] op dDOT14((B+8),(C)); \ - (A)[9] op dDOT14((B+8),(C+1)); \ - (A)[10] op dDOT14((B+8),(C+2)); - -#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) -#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) -#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) - - -//////////////////////////////////////////////////////////////////// -#define EFFICIENT_ALIGNMENT 16 -#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) -/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste - * up to 15 bytes per allocation, depending on what alloca() returns. - */ - -#define dALLOCA16(n) \ - ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) - - - -///////////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////////// - -#ifdef DEBUG -#define ANSI_FTOL 1 - -extern "C" { - __declspec(naked) void _ftol2() { - __asm { -#if ANSI_FTOL - fnstcw WORD PTR [esp-2] - mov ax, WORD PTR [esp-2] - - OR AX, 0C00h - - mov WORD PTR [esp-4], ax - fldcw WORD PTR [esp-4] - fistp QWORD PTR [esp-12] - fldcw WORD PTR [esp-2] - mov eax, DWORD PTR [esp-12] - mov edx, DWORD PTR [esp-8] -#else - fistp DWORD PTR [esp-12] - mov eax, DWORD PTR [esp-12] - mov ecx, DWORD PTR [esp-8] -#endif - ret - } - } -} -#endif //DEBUG - - - - - - -#define ALLOCA dALLOCA16 - -typedef const btScalar *dRealPtr; -typedef btScalar *dRealMutablePtr; -#define dRealArray(name,n) btScalar name[n]; -#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar)); - -void dSetZero1 (btScalar *a, int n) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = 0; - n--; - } -} - -void dSetValue1 (btScalar *a, int n, btScalar value) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = value; - n--; - } -} - +#include "OdeMacros.h" //*************************************************************************** // configuration @@ -224,7 +86,7 @@ static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int dRealMutablePtr iMJ_ptr = iMJ; dRealMutablePtr J_ptr = J; for (i=0; im_invMass; for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; @@ -245,48 +107,48 @@ static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb { int i,j; - + dRealMutablePtr out_ptr1 = out + onlyBody1*6; - - for (j=0; j<6; j++) + + for (j=0; j<6; j++) out_ptr1[j] = 0; if (onlyBody2 >= 0) { out_ptr1 = out + onlyBody2*6; - for (j=0; j<6; j++) + for (j=0; j<6; j++) out_ptr1[j] = 0; } dRealPtr iMJ_ptr = iMJ; for (i=0; i= 0) + if (b2 >= 0) { out_ptr = out + b2*6; - for (j=0; j<6; j++) + for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; } } - + iMJ_ptr += 6; - + } } #endif @@ -302,10 +164,10 @@ static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, dSetZero1 (out,6*nb); dRealPtr iMJ_ptr = iMJ; for (i=0; i= 0) { @@ -326,15 +188,15 @@ static void multiply_J (int m, dRealMutablePtr J, int *jb, int i,j; dRealPtr J_ptr = J; for (i=0; i= 0) { in_ptr = in + b2*6; - for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; + for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; } J_ptr += 6; out[i] = sum; @@ -440,7 +302,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, OdeSolverBody * } // scale Ad by CFM - for (i=0; i= 0) + for (i=0; i= 0) order[j++].index = i; dIASSERT (j==m); #endif @@ -512,11 +374,11 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, OdeSolverBody * // linearizing access to those arrays. hmmm, this does not seem // like a win, but we should think carefully about our memory // access pattern. - + int index = order[i].index; J_ptr = J + index*12; iMJ_ptr = iMJ + index*12; - + // set the limits for this constraint. note that 'hicopy' is used. // this is the place where the QuickStep method differs from the // direct LCP solving method, since that method only performs this @@ -533,7 +395,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, OdeSolverBody * int b2 = jb[index*2+1]; float delta = rhs[index] - lambda[index]*Ad[index]; dRealMutablePtr fc_ptr = invMforce + 6*b1; - + // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + @@ -566,7 +428,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, OdeSolverBody * //@@@ a trick that may or may not help //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); //delta *= ramp; - + // update invMforce. // @@@ potential optimization: SIMD for this and the b2 >= 0 case fc_ptr = invMforce + 6*b1; @@ -596,8 +458,8 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, OdeSolverBody * void SolveInternal1 (float global_cfm, float global_erp, OdeSolverBody* const *body, int nb, - BU_Joint **joint, - int nj, + BU_Joint **joint, + int nj, const btContactSolverInfo& solverInfo) { @@ -605,20 +467,20 @@ void SolveInternal1 (float global_cfm, float sOr = solverInfo.m_sor; int i,j; - + btScalar stepsize1 = dRecip(solverInfo.m_timeStep); // number all bodies in the body list - set their tag values - for (i=0; im_odeTag = i; - + // make a local copy of the joint array, because we might want to modify it. // (the "BU_Joint *const*" declaration says we're allowed to modify the joints // but not the joint array, because the caller might need it unchanged). //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints //BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*)); //memcpy (joint,_joint,nj * sizeof(BU_Joint*)); - + // for all bodies, compute the inertia tensor and its inverse in the global // frame, and compute the rotational force and add it to the torque // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. @@ -692,7 +554,7 @@ void SolveInternal1 (float global_cfm, dSetValue1 (lo,m,-dInfinity); dSetValue1 (hi,m, dInfinity); for (i=0; i= 0) + if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i]; } } @@ -754,10 +616,10 @@ void SolveInternal1 (float global_cfm, // put v/h + invM*fe into tmp1 for (i=0; im_invMass; - for (j=0; j<3; j++) + for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1; dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); - for (j=0; j<3; j++) + for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1; } @@ -769,7 +631,7 @@ void SolveInternal1 (float global_cfm, for (i=0; im_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j]; - for (j=0; j<3; j++) + for (j=0; j<3; j++) body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; - + } } @@ -819,13 +681,13 @@ void SolveInternal1 (float global_cfm, btVector3 linvel = body[i]->m_linearVelocity; btVector3 angvel = body[i]->m_angularVelocity; - for (j=0; j<3; j++) + for (j=0; j<3; j++) { linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; } - for (j=0; j<3; j++) + for (j=0; j<3; j++) { - body[i]->m_tacc[j] *= solverInfo.m_timeStep; + body[i]->m_tacc[j] *= solverInfo.m_timeStep; } dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); body[i]->m_angularVelocity = angvel;