Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

updated the Extras/quickstep files, for comparison

Browse Source
This commit is contained in:
ejcoumans
2006-10-13 22:33:28 +00:00
parent f3b9dcd714
commit 6fa35ba9a4
10 changed files with 577 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -19,11 +19,12 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btContactConstraint.h"
#include "btSolve2LinearConstraint.h"
#include "btContactSolverInfo.h"
#include "Dynamics/BU_Joint.h"
#include "Dynamics/ContactJoint.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "OdeJoint.h"
#include "OdeContactJoint.h"
#include "OdeSolverBody.h"
#include "LinearMath/btIDebugDraw.h"
@@ -74,13 +75,11 @@ float OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int nu
m_CurBody = 0;
m_CurJoint = 0;
btRigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
int numBodies = 0;
BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4];
OdeSolverBody* odeBodies [MAX_QUICKSTEP_RIGIDBODIES];
int numJoints = 0;
BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4];
for (int j=0;j<numManifolds;j++)
{
@@ -89,14 +88,23 @@ float OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int nu
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->getNumContacts() > 0)
{
body0 = ConvertBody((btRigidBody*)manifold->getBody0(),bodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->getBody1(),bodies,numBodies);
ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
body0 = ConvertBody((btRigidBody*)manifold->getBody0(),odeBodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->getBody1(),odeBodies,numBodies);
ConvertConstraint(manifold,joints,numJoints,odeBodies,body0,body1,debugDrawer);
}
}
SolveInternal1(m_cfm,m_erp,bodies,numBodies,joints,numJoints,infoGlobal);
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);
}
}
return 0.f;
}
@@ -130,28 +138,39 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q)
}
#define ODE_MAX_SOLVER_BODIES 16384
static OdeSolverBody gSolverBodyArray[ODE_MAX_SOLVER_BODIES];
int OdeConstraintSolver::ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies)
int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,OdeSolverBody** bodies,int& numBodies)
{
if (!body || (body->getInvMass() == 0.f) )
assert(orgBody);
if (!orgBody || (orgBody->getInvMass() == 0.f) )
{
return -1;
}
//first try to find
int i,j;
for (i=0;i<numBodies;i++)
{
if (bodies[i] == body)
if (bodies[i]->m_originalBody == orgBody)
return i;
}
//if not found, create a new body
bodies[numBodies++] = body;
//convert data
OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
numBodies++;
body->m_originalBody = orgBody;
body->m_facc.setValue(0,0,0,0);
body->m_tacc.setValue(0,0,0,0);
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++)
@@ -162,23 +181,23 @@ int OdeConstraintSolver::ConvertBody(btRigidBody* body,btRigidBody** bodies,int&
body->m_I[i+4*j] = 0.f;
}
}
body->m_invI[0+4*0] = body->getInvInertiaDiagLocal()[0];
body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1];
body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2];
body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal()[0];
body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal()[1];
body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal()[2];
body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0];
body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1];
body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2];
body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal()[0];
body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal()[1];
body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal()[2];
dQuaternion q;
q[1] = body->getOrientation()[0];
q[2] = body->getOrientation()[1];
q[3] = body->getOrientation()[2];
q[0] = body->getOrientation()[3];
q[1] = orgBody->getOrientation()[0];
q[2] = orgBody->getOrientation()[1];
q[3] = orgBody->getOrientation()[2];
q[0] = orgBody->getOrientation()[3];
dRfromQ1(body->m_R,q);
@@ -189,13 +208,12 @@ int OdeConstraintSolver::ConvertBody(btRigidBody* body,btRigidBody** bodies,int&
#define MAX_JOINTS_1 65535
static ContactJoint gJointArray[MAX_JOINTS_1];
#define ODE_MAX_SOLVER_JOINTS 65535
static ContactJoint gJointArray[ODE_MAX_SOLVER_JOINTS];
void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
@@ -209,20 +227,20 @@ void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Jo
bool swapBodies = (bodyId0 < 0);
btRigidBody* body0,*body1;
OdeSolverBody* body0,*body1;
if (swapBodies)
{
bodyId0 = _bodyId1;
bodyId1 = _bodyId0;
body0 = (btRigidBody*)manifold->getBody1();
body1 = (btRigidBody*)manifold->getBody0();
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
} else
{
body0 = (btRigidBody*)manifold->getBody0();
body1 = (btRigidBody*)manifold->getBody1();
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
}
assert(bodyId0 >= 0);
@@ -243,7 +261,7 @@ void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Jo
color);
}
assert (m_CurJoint < MAX_JOINTS_1);
assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS);
// if (manifold->getContactPoint(i).getDistance() < 0.0f)
{

7
Extras/quickstep/OdeConstraintSolver.h
View File

@@ -16,9 +16,10 @@ subject to the following restrictions:
#ifndef ODE_CONSTRAINT_SOLVER_H
#define ODE_CONSTRAINT_SOLVER_H
#include "btConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
class btRigidBody;
struct OdeSolverBody;
class BU_Joint;
/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework
@@ -34,9 +35,9 @@ private:
float m_erp;
int ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies);
int ConvertBody(btRigidBody* body,OdeSolverBody** bodies,int& numBodies);
void ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
public:

277
Extras/quickstep/OdeContactJoint.cpp Normal file
View File

@@ -0,0 +1,277 @@
/*
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 "OdeContactJoint.h"
#include "OdeSolverBody.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
//this constant needs to be set up so different solvers give 'similar' results
#define FRICTION_CONSTANT 120.f
ContactJoint::ContactJoint(btPersistentManifold* manifold,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1)
:m_manifold(manifold),
m_index(index),
m_swapBodies(swap),
m_body0(body0),
m_body1(body1)
{
}
int m_numRows = 3;
void ContactJoint::GetInfo1(Info1 *info)
{
info->m = m_numRows;
//friction adds another 2...
info->nub = 0;
}
#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 M_SQRT12 btScalar(0.7071067811865475244008443621048490)
#define dRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */
void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q)
{
if (btFabs(n[2]) > M_SQRT12) {
// choose p in y-z plane
btScalar a = n[1]*n[1] + n[2]*n[2];
btScalar k = dRecipSqrt (a);
p[0] = 0;
p[1] = -n[2]*k;
p[2] = n[1]*k;
// set q = n x p
q[0] = a*k;
q[1] = -n[0]*p[2];
q[2] = n[0]*p[1];
}
else {
// choose p in x-y plane
btScalar a = n[0]*n[0] + n[1]*n[1];
btScalar k = dRecipSqrt (a);
p[0] = -n[1]*k;
p[1] = n[0]*k;
p[2] = 0;
// set q = n x p
q[0] = -n[2]*p[1];
q[1] = n[2]*p[0];
q[2] = a*k;
}
}
void ContactJoint::GetInfo2(Info2 *info)
{
int s = info->rowskip;
int s2 = 2*s;
float swapFactor = m_swapBodies ? -1.f : 1.f;
// get normal, with sign adjusted for body1/body2 polarity
dVector3 normal;
btManifoldPoint& point = m_manifold->getContactPoint(m_index);
normal[0] = swapFactor*point.m_normalWorldOnB[0];
normal[1] = swapFactor*point.m_normalWorldOnB[1];
normal[2] = swapFactor*point.m_normalWorldOnB[2];
normal[3] = 0; // @@@ hmmm
assert(m_body0);
// if (GetBody0())
btVector3 relativePositionA;
{
relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
dVector3 c1;
c1[0] = relativePositionA[0];
c1[1] = relativePositionA[1];
c1[2] = relativePositionA[2];
// set jacobian for normal
info->J1l[0] = normal[0];
info->J1l[1] = normal[1];
info->J1l[2] = normal[2];
dCROSS (info->J1a,=,c1,normal);
}
btVector3 relativePositionB(0,0,0);
if (m_body1)
{
// if (GetBody1())
{
dVector3 c2;
btVector3 posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0);
relativePositionB = point.getPositionWorldOnB() - posBody1;
// for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
// j->node[1].body->pos[i];
c2[0] = relativePositionB[0];
c2[1] = relativePositionB[1];
c2[2] = relativePositionB[2];
info->J2l[0] = -normal[0];
info->J2l[1] = -normal[1];
info->J2l[2] = -normal[2];
dCROSS (info->J2a,= -,c2,normal);
}
}
btScalar k = info->fps * info->erp;
float depth = -point.getDistance();
// if (depth < 0.f)
// depth = 0.f;
info->c[0] = k * depth;
//float maxvel = .2f;
// if (info->c[0] > maxvel)
// info->c[0] = maxvel;
//can override it, not necessary
// info->cfm[0] = 0.f;
// info->cfm[1] = 0.f;
// info->cfm[2] = 0.f;
// set LCP limits for normal
info->lo[0] = 0;
info->hi[0] = 1e30f;//dInfinity;
info->lo[1] = 0;
info->hi[1] = 0.f;
info->lo[2] = 0.f;
info->hi[2] = 0.f;
#define DO_THE_FRICTION_2
#ifdef DO_THE_FRICTION_2
// now do jacobian for tangential forces
dVector3 t1,t2; // two vectors tangential to normal
dVector3 c1;
c1[0] = relativePositionA[0];
c1[1] = relativePositionA[1];
c1[2] = relativePositionA[2];
dVector3 c2;
c2[0] = relativePositionB[0];
c2[1] = relativePositionB[1];
c2[2] = relativePositionB[2];
//combined friction is available in the contact point
float friction = FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction;
// first friction direction
if (m_numRows >= 2)
{
dPlaneSpace1 (normal,t1,t2);
info->J1l[s+0] = t1[0];
info->J1l[s+1] = t1[1];
info->J1l[s+2] = t1[2];
dCROSS (info->J1a+s,=,c1,t1);
if (1) { //j->node[1].body) {
info->J2l[s+0] = -t1[0];
info->J2l[s+1] = -t1[1];
info->J2l[s+2] = -t1[2];
dCROSS (info->J2a+s,= -,c2,t1);
}
// set right hand side
if (0) {//j->contact.surface.mode & dContactMotion1) {
//info->c[1] = j->contact.surface.motion1;
}
// set LCP bounds and friction index. this depends on the approximation
// mode
//1e30f
info->lo[1] = -friction;//-j->contact.surface.mu;
info->hi[1] = friction;//j->contact.surface.mu;
if (1)//j->contact.surface.mode & dContactApprox1_1)
info->findex[1] = 0;
// set slip (constraint force mixing)
if (0)//j->contact.surface.mode & dContactSlip1)
{
// info->cfm[1] = j->contact.surface.slip1;
} else
{
//info->cfm[1] = 0.f;
}
}
// second friction direction
if (m_numRows >= 3) {
info->J1l[s2+0] = t2[0];
info->J1l[s2+1] = t2[1];
info->J1l[s2+2] = t2[2];
dCROSS (info->J1a+s2,=,c1,t2);
if (1) { //j->node[1].body) {
info->J2l[s2+0] = -t2[0];
info->J2l[s2+1] = -t2[1];
info->J2l[s2+2] = -t2[2];
dCROSS (info->J2a+s2,= -,c2,t2);
}
// set right hand side
if (0){//j->contact.surface.mode & dContactMotion2) {
//info->c[2] = j->contact.surface.motion2;
}
// set LCP bounds and friction index. this depends on the approximation
// mode
if (0){//j->contact.surface.mode & dContactMu2) {
//info->lo[2] = -j->contact.surface.mu2;
//info->hi[2] = j->contact.surface.mu2;
}
else {
info->lo[2] = -friction;
info->hi[2] = friction;
}
if (0)//j->contact.surface.mode & dContactApprox1_2)
{
info->findex[2] = 0;
}
// set slip (constraint force mixing)
if (0) //j->contact.surface.mode & dContactSlip2)
{
//info->cfm[2] = j->contact.surface.slip2;
}
}
#endif //DO_THE_FRICTION_2
}

25
Extras/quickstep/OdeJoint.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*
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 "OdeJoint.h"
BU_Joint::BU_Joint()
{
}
BU_Joint::~BU_Joint()
{
}

94
Extras/quickstep/OdeJoint.h Normal file
View File

@@ -0,0 +1,94 @@
/*
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.
*/
#ifndef BU_Joint_H
#define BU_Joint_H
struct OdeSolverBody;
class BU_Joint;
#include "LinearMath/btScalar.h"
struct BU_ContactJointNode {
BU_Joint *joint; // pointer to enclosing BU_Joint object
OdeSolverBody* body; // *other* body this joint is connected to
};
typedef btScalar dVector3[4];
class BU_Joint {
public:
// naming convention: the "first" body this is connected to is node[0].body,
// and the "second" body is node[1].body. if this joint is only connected
// to one body then the second body is 0.
// info returned by getInfo1 function. the constraint dimension is m (<=6).
// i.e. that is the total number of rows in the jacobian. `nub' is the
// number of unbounded variables (which have lo,hi = -/+ infinity).
BU_Joint();
virtual ~BU_Joint();
struct Info1 {
int m,nub;
};
// info returned by getInfo2 function
struct Info2 {
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
btScalar fps,erp;
// for the first and second body, pointers to two (linear and angular)
// n*3 jacobian sub matrices, stored by rows. these matrices will have
// been initialized to 0 on entry. if the second body is zero then the
// J2xx pointers may be 0.
btScalar *J1l,*J1a,*J2l,*J2a;
// elements to jump from one row to the next in J's
int rowskip;
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
// "constraint force mixing" vector. c is set to zero on entry, cfm is
// set to a constant value (typically very small or zero) value on entry.
btScalar *c,*cfm;
// lo and hi limits for variables (set to -/+ infinity on entry).
btScalar *lo,*hi;
// findex vector for variables. see the LCP solver interface for a
// description of what this does. this is set to -1 on entry.
// note that the returned indexes are relative to the first index of
// the constraint.
int *findex;
};
// virtual function table: size of the joint structure, function pointers.
// we do it this way instead of using C++ virtual functions because
// sometimes we need to allocate joints ourself within a memory pool.
virtual void GetInfo1 (Info1 *info)=0;
virtual void GetInfo2 (Info2 *info)=0;
int flags; // dJOINT_xxx flags
BU_ContactJointNode node[2]; // connections to bodies. node[1].body can be 0
btScalar lambda[6]; // lambda generated by last step
};
#endif //BU_Joint_H

0
Extras/quickstep/OdeSolverBody.cpp Normal file
View File

47
Extras/quickstep/OdeSolverBody.h Normal file
View File

@@ -0,0 +1,47 @@
/*
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.
*/
#ifndef ODE_SOLVER_BODY_H
#define ODE_SOLVER_BODY_H
class btRigidBody;
#include "LinearMath/btVector3.h"
typedef btScalar dMatrix3[4*3];
///ODE's quickstep needs just a subset of the rigidbody data in its own layout, so make a temp copy
struct OdeSolverBody
{
btRigidBody* m_originalBody;
btVector3 m_centerOfMassPosition;
/// for ode solver-binding
dMatrix3 m_R;//temp
dMatrix3 m_I;
dMatrix3 m_invI;
int m_odeTag;
float m_invMass;
float m_friction;
btVector3 m_tacc;//temp
btVector3 m_facc;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
};
#endif //#ifndef ODE_SOLVER_BODY_H

51
Extras/quickstep/SorLcp.cpp
View File

@@ -21,6 +21,7 @@
*************************************************************************/
#include "SorLcp.h"
#include "OdeSolverBody.h"
#ifdef USE_SOR_SOLVER
@@ -49,8 +50,8 @@
#endif
#endif
#include "Dynamics/BU_Joint.h"
#include "btContactSolverInfo.h"
#include "OdeJoint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
////////////////////////////////////////////////////////////////////
//math stuff
@@ -220,7 +221,7 @@ void dSetValue1 (btScalar *a, int n, btScalar value)
// compute iMJ = inv(M)*J'
static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
btRigidBody * const *body, dRealPtr invI)
OdeSolverBody* const *body, dRealPtr invI)
{
int i,j;
dRealMutablePtr iMJ_ptr = iMJ;
@@ -228,11 +229,11 @@ static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
btScalar k = body[b1]->getInvMass();
btScalar k = body[b1]->m_invMass;
for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
if (b2 >= 0) {
k = body[b2]->getInvMass();
k = body[b2]->m_invMass;
for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
}
@@ -378,7 +379,7 @@ int dRandInt2 (int n)
}
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, btRigidBody * const *body,
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, OdeSolverBody * const *body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
int numiter,float overRelax)
@@ -597,7 +598,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, btRigidBody * co
void SolveInternal1 (float global_cfm,
float global_erp,
btRigidBody * const *body, int nb,
OdeSolverBody* const *body, int nb,
BU_Joint * const *_joint,
int nj,
const btContactSolverInfo& solverInfo)
@@ -647,8 +648,8 @@ void SolveInternal1 (float global_cfm,
dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
// compute rotational force
dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity());
dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity);
dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp);
}
@@ -755,12 +756,12 @@ void SolveInternal1 (float global_cfm,
dRealAllocaArray (tmp1,nb*6);
// put v/h + invM*fe into tmp1
for (i=0; i<nb; i++) {
btScalar body_invMass = body[i]->getInvMass();
btScalar body_invMass = body[i]->m_invMass;
for (j=0; j<3; j++)
tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1;
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++)
tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1;
tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1;
}
// put J*tmp1 into rhs
@@ -803,19 +804,12 @@ void SolveInternal1 (float global_cfm,
// add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) {
btVector3 linvel = body[i]->getLinearVelocity();
btVector3 angvel = body[i]->getAngularVelocity();
for (j=0; j<3; j++)
body[i]->m_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j];
for (j=0; j<3; j++)
body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
for (j=0; j<3; j++)
linvel[j] += solverInfo.m_timeStep* cforce[i*6+j];
for (j=0; j<3; j++)
angvel[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
body[i]->setLinearVelocity(linvel);
body[i]->setAngularVelocity(angvel);
}
}
@@ -824,9 +818,9 @@ void SolveInternal1 (float global_cfm,
// add stepsize * invM * fe to the body velocity
for (i=0; i<nb; i++) {
btScalar body_invMass = body[i]->getInvMass();
btVector3 linvel = body[i]->getLinearVelocity();
btVector3 angvel = body[i]->getAngularVelocity();
btScalar body_invMass = body[i]->m_invMass;
btVector3 linvel = body[i]->m_linearVelocity;
btVector3 angvel = body[i]->m_angularVelocity;
for (j=0; j<3; j++)
{
@@ -837,12 +831,9 @@ void SolveInternal1 (float global_cfm,
body[i]->m_tacc[j] *= solverInfo.m_timeStep;
}
dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
body[i]->setAngularVelocity(angvel);
body[i]->m_angularVelocity = angvel;
}
}

4
Extras/quickstep/SorLcp.h
View File

@@ -25,7 +25,7 @@
#ifndef SOR_LCP_H
#define SOR_LCP_H
class btRigidBody;
struct OdeSolverBody;
class BU_Joint;
#include "LinearMath/btScalar.h"
@@ -33,7 +33,7 @@ struct btContactSolverInfo;
void SolveInternal1 (float global_cfm,
float global_erp,
btRigidBody * const *body, int nb,
OdeSolverBody * const *body, int nb,
BU_Joint * const *_joint, int nj, const btContactSolverInfo& info);
int dRandInt2 (int n);

50
Extras/quickstep/odecontactjoint.h Normal file
View File

@@ -0,0 +1,50 @@
/*
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.
*/
#ifndef CONTACT_JOINT_H
#define CONTACT_JOINT_H
#include "OdeJoint.h"
struct OdeSolverBody;
class btPersistentManifold;
class ContactJoint : public BU_Joint
{
btPersistentManifold* m_manifold;
int m_index;
bool m_swapBodies;
OdeSolverBody* m_body0;
OdeSolverBody* m_body1;
public:
ContactJoint() {};
ContactJoint(btPersistentManifold* manifold,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1);
//BU_Joint interface for solver
virtual void GetInfo1(Info1 *info);
virtual void GetInfo2(Info2 *info);
};
#endif //CONTACT_JOINT_H