AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-10 17:30:12 +00:00
Code Issues Releases Wiki Activity

added quickstep improvements, to allow for constraints (point to point etc).

Browse Source 
Thanks Francisco Leon/projectileman
This commit is contained in:
ejcoumans 2007-09-13 07:44:05 +00:00
parent 7f5823ee16
commit 0bf8124668
6 changed files with 1043 additions and 352 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

393
Extras/quickstep/OdeConstraintSolver.cpp
View File

@ -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 <new.h>
#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;j<numManifolds;j++)
{
int numBodies = 0;
OdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES];
int numJoints = 0;
BU_Joint* joints [ODE_MAX_SOLVER_JOINTS*2];
int body0=-1,body1=-1;
//capture contacts
int j;
int body0=-1,body1=-1;
for (j=0;j<numManifolds;j++)
{
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);
}
}
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;j<numConstraints;j++)
{
btTypedConstraint * typedconstraint = constraints[j];
body0 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyA(),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;i<numBodies;i++)
{
if (odeBodies[i]->m_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;i<numBodies;i++)
{
if (odeBodies[i]->m_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<numContacts;i++)
{
}
else
{
body0 = bodyId0>=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;i<numContacts;i++)
{
}
assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS);
if (debugDrawer)
{
const btManifoldPoint& cp = manifold->getContactPoint(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()
{
}

22
Extras/quickstep/OdeConstraintSolver.h
View File

@ -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()
{
}
};

188
Extras/quickstep/OdeMacros.h Normal file
View File

@ -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

451
Extras/quickstep/OdeTypedJoint.cpp Normal file
View File

@ -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 <stdio.h>
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);
}

111
Extras/quickstep/OdeTypedJoint.h Normal file
View File

@ -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<btPoint2PointConstraint * >(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<btGeneric6DofConstraint * >(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

230
Extras/quickstep/SorLcp.cpp
View File

@ -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; i<m; i++) {
int b1 = jb[i*2];
int b1 = jb[i*2];
int b2 = jb[i*2+1];
btScalar k = body[b1]->m_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<m; i++) {
int b1 = jb[i*2];
int b1 = jb[i*2];
dRealMutablePtr out_ptr = out + b1*6;
if ((b1 == onlyBody1) || (b1 == onlyBody2))
{
for (j=0; j<6; j++)
for (j=0; j<6; j++)
out_ptr[j] += iMJ_ptr[j] * in[i] ;
}
iMJ_ptr += 6;
int b2 = jb[i*2+1];
if ((b2 == onlyBody1) || (b2 == onlyBody2))
{
if (b2 >= 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<m; i++) {
int b1 = jb[i*2];
int b1 = jb[i*2];
int b2 = jb[i*2+1];
dRealMutablePtr out_ptr = out + b1*6;
for (j=0; j<6; j++)
for (j=0; j<6; j++)
out_ptr[j] += iMJ_ptr[j] * in[i];
iMJ_ptr += 6;
if (b2 >= 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<m; i++) {
int b1 = jb[i*2];
int b1 = jb[i*2];
int b2 = jb[i*2+1];
btScalar sum = 0;
dRealMutablePtr in_ptr = in + b1*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;
if (b2 >= 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<m; i++)
for (i=0; i<m; i++)
Ad[i] *= cfm[i];
// order to solve constraint rows in
@ -449,11 +311,11 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, OdeSolverBody *
#ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first.
j=0;
for (i=0; i<m; i++)
if (findex[i] < 0)
for (i=0; i<m; i++)
if (findex[i] < 0)
order[j++].index = i;
for (i=0; i<m; i++)
if (findex[i] >= 0)
for (i=0; i<m; i++)
if (findex[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; i<nb; i++)
for (i=0; i<nb; i++)
body[i]->m_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<m; i++) findex[i] = -1;
// get jacobian data from constraints. an m*12 matrix will be created
// to store the two jacobian blocks from each constraint. it has this
// format:
@ -731,7 +593,7 @@ void SolveInternal1 (float global_cfm,
// adjust returned findex values for global index numbering
for (j=0; j<info[i].m; j++) {
if (findex[ofs[i] + j] >= 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; i<nb; i++) {
btScalar body_invMass = body[i]->m_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; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
// scale CFM
for (i=0; i<m; i++)
for (i=0; i<m; i++)
cfm[i] *= stepsize1;
// load lambda from the value saved on the previous iteration
@ -795,17 +657,17 @@ void SolveInternal1 (float global_cfm,
}
#endif
// note that the SOR method overwrites rhs and J at this point, so
// they should not be used again.
// add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) {
for (j=0; j<3; j++)
for (j=0; j<3; j++)
body[i]->m_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;