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

Moved ODE quickstep solver as optional solver into Bullet core, redistributed under the ZLib licensed with permission from Russell L. Smith

Browse Source
This commit is contained in:
erwin.coumans
2008-03-30 21:03:35 +00:00
parent b12c52efa8
commit 8d38ef49ef
15 changed files with 2593 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/BulletDynamics/CMakeLists.txt
View File

@@ -20,6 +20,18 @@ ADD_LIBRARY(LibBulletDynamics
ConstraintSolver/btSolve2LinearConstraint.h
ConstraintSolver/btTypedConstraint.cpp
ConstraintSolver/btTypedConstraint.h
ConstraintSolver/btOdeContactJoint.cpp
ConstraintSolver/btOdeContactJoint.h
ConstraintSolver/btOdeJoint.cpp
ConstraintSolver/btOdeJoint.h
ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
ConstraintSolver/btOdeQuickstepConstraintSolver.h
ConstraintSolver/btOdeTypedJoint.cpp
ConstraintSolver/btOdeTypedJoint.h
ConstraintSolver/btSorLcp.cpp
ConstraintSolver/btSorLcp.h
ConstraintSolver/btOdeQuickstepConstraintSolver.h
ConstraintSolver/btOdeMacros.h
Dynamics/Bullet-C-API.cpp
Dynamics/btDiscreteDynamicsWorld.cpp
Dynamics/btDiscreteDynamicsWorld.h

277
src/BulletDynamics/ConstraintSolver/btOdeContactJoint.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 "btOdeContactJoint.h"
#include "btOdeSolverBody.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
//this constant needs to be set up so different solvers give 'similar' results
#define FRICTION_CONSTANT 120.f
btOdeContactJoint::btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1)
:m_manifold(manifold),
m_index(index),
m_swapBodies(swap),
m_body0(body0),
m_body1(body1)
{
}
int m_numRows = 3;
void btOdeContactJoint::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 btOdeContactJoint::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.x();
normal[1] = swapFactor*point.m_normalWorldOnB.y();
normal[2] = swapFactor*point.m_normalWorldOnB.z();
normal[3] = 0; // @@@ hmmm
assert(m_body0);
// if (GetBody0())
btVector3 relativePositionA;
{
relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
dVector3 c1;
c1[0] = relativePositionA.x();
c1[1] = relativePositionA.y();
c1[2] = relativePositionA.z();
// 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.x();
c2[1] = relativePositionB.y();
c2[2] = relativePositionB.z();
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.x();
c1[1] = relativePositionA.y();
c1[2] = relativePositionA.z();
dVector3 c2;
c2[0] = relativePositionB.x();
c2[1] = relativePositionB.y();
c2[2] = relativePositionB.z();
//combined friction is available in the contact point
float friction = 0.25;//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
}

50
src/BulletDynamics/ConstraintSolver/btOdeContactJoint.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 "btOdeJoint.h"
struct btOdeSolverBody;
class btPersistentManifold;
class btOdeContactJoint : public btOdeJoint
{
btPersistentManifold* m_manifold;
int m_index;
bool m_swapBodies;
btOdeSolverBody* m_body0;
btOdeSolverBody* m_body1;
public:
btOdeContactJoint() {};
btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
//btOdeJoint interface for solver
virtual void GetInfo1(Info1 *info);
virtual void GetInfo2(Info2 *info);
};
#endif //CONTACT_JOINT_H

25
src/BulletDynamics/ConstraintSolver/btOdeJoint.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 "btOdeJoint.h"
btOdeJoint::btOdeJoint()
{
}
btOdeJoint::~btOdeJoint()
{
}

94
src/BulletDynamics/ConstraintSolver/btOdeJoint.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 btOdeJoint_H
#define btOdeJoint_H
struct btOdeSolverBody;
class btOdeJoint;
#include "LinearMath/btScalar.h"
struct BU_ContactJointNode {
btOdeJoint *joint; // pointer to enclosing btOdeJoint object
btOdeSolverBody* body; // *other* body this joint is connected to
};
typedef btScalar dVector3[4];
class btOdeJoint {
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).
btOdeJoint();
virtual ~btOdeJoint();
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 //btOdeJoint_H

212
src/BulletDynamics/ConstraintSolver/btOdeMacros.h Normal file
View File

@@ -0,0 +1,212 @@
/*
* Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
* All rights reserved. Email: russ@q12.org Web: www.q12.org
Bullet Continuous Collision Detection and Physics Library
Bullet is 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.
*/
#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))))))
//#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));
///////////////////////////////////////////////////////////////////////////////
//Remotion: 10.10.2007
#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) );
//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar));
#define dRealAllocaArray(name,size) btScalar *name = NULL; \
int memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \
if (memNeeded_##name < stackAlloc->getAvailableMemory()) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \
else{ btAssert(memNeeded_##name < stackAlloc->getAvailableMemory()); name = (btScalar*) alloca(memNeeded_##name); }
///////////////////////////////////////////////////////////////////////////////
#if 0
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--;
}
}
#else
/// This macros are for MSVC and XCode compilers. Remotion.
#include <string.h> //for memset
//Remotion: 10.10.2007
//------------------------------------------------------------------------------
#define IS_ALIGNED_16(x) ((size_t(x)&15)==0)
//------------------------------------------------------------------------------
inline void dSetZero1 (btScalar *dest, int size)
{
dAASSERT (dest && size >= 0);
memset(dest, 0, size * sizeof(btScalar));
}
//------------------------------------------------------------------------------
inline void dSetValue1 (btScalar *dest, int size, btScalar val)
{
dAASSERT (dest && size >= 0);
int n_mod4 = size & 3;
int n4 = size - n_mod4;
/*#ifdef __USE_SSE__
//it is not supported on double precision, todo...
if(IS_ALIGNED_16(dest)){
__m128 xmm0 = _mm_set_ps1(val);
for (int i=0; i<n4; i+=4)
{
_mm_store_ps(&dest[i],xmm0);
}
}else
#endif
*/
{
for (int i=0; i<n4; i+=4) // Unrolled Loop
{
dest[i ] = val;
dest[i+1] = val;
dest[i+2] = val;
dest[i+3] = val;
}
}
for (int i=n4; i<size; i++){
dest[i] = val;
}
}
#endif
/////////////////////////////////////////////////////////////////////
#endif //USE_SOR_SOLVER

405
src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp Normal file
View File

@@ -0,0 +1,405 @@
/*
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 "btOdeQuickstepConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "btOdeJoint.h"
#include "btOdeContactJoint.h"
#include "btOdeTypedJoint.h"
#include "btOdeSolverBody.h"
#include <new.h>
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.h"
#define USE_SOR_SOLVER
#include "btSorLcp.h"
#include <math.h>
#include <float.h>//FLT_MAX
#ifdef WIN32
#include <memory.h>
#endif
#include <string.h>
#include <stdio.h>
#if defined (WIN32)
#include <malloc.h>
#else
#if defined (__FreeBSD__)
#include <stdlib.h>
#else
#include <alloca.h>
#endif
#endif
class btOdeJoint;
//see below
//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island
btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver():
m_cfm(0.f),//1e-5f),
m_erp(0.4f)
{
}
//iterative lcp and penalty method
btScalar btOdeQuickstepConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
{
m_CurBody = 0;
m_CurJoint = 0;
m_CurTypedJoint = 0;
int j;
int max_contacts = 0; /// should be 4 //Remotion
for ( j=0;j<numManifolds;j++){
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->getNumContacts() > max_contacts) max_contacts = manifold->getNumContacts();
}
//if(max_contacts > 4)
// printf(" max_contacts > 4");
int numBodies = 0;
m_odeBodies.clear();
m_odeBodies.reserve(numBulletBodies + 1); //???
// btOdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES];
int numJoints = 0;
m_joints.clear();
m_joints.reserve(numManifolds * max_contacts + 4 + numConstraints + 1); //???
// btOdeJoint* joints [ODE_MAX_SOLVER_JOINTS*2];
m_SolverBodyArray.resize(numBulletBodies + 1);
m_JointArray.resize(numManifolds * max_contacts + 4);
m_TypedJointArray.resize(numConstraints + 1);
//capture contacts
int body0=-1,body1=-1;
for (j=0;j<numManifolds;j++)
{
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->getNumContacts() > 0)
{
body0 = ConvertBody((btRigidBody*)manifold->getBody0(),m_odeBodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->getBody1(),m_odeBodies,numBodies);
ConvertConstraint(manifold,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer);
}
}
//capture constraints
for (j=0;j<numConstraints;j++)
{
btTypedConstraint * typedconstraint = constraints[j];
body0 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyA(),m_odeBodies,numBodies);
body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),m_odeBodies,numBodies);
ConvertTypedConstraint(typedconstraint,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer);
}
//if(numBodies > numBulletBodies)
// printf(" numBodies > numBulletBodies");
//if(numJoints > numManifolds * 4 + numConstraints)
// printf(" numJoints > numManifolds * 4 + numConstraints");
m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do
//write back resulting velocities
for (int i=0;i<numBodies;i++)
{
if (m_odeBodies[i]->m_invMass)
{
m_odeBodies[i]->m_originalBody->setLinearVelocity(m_odeBodies[i]->m_linearVelocity);
m_odeBodies[i]->m_originalBody->setAngularVelocity(m_odeBodies[i]->m_angularVelocity);
}
}
/// Remotion, just free all this here
m_odeBodies.clear();
m_joints.clear();
m_SolverBodyArray.clear();
m_JointArray.clear();
m_TypedJointArray.clear();
return 0.f;
}
/////////////////////////////////////////////////////////////////////////////////
typedef btScalar dQuaternion[4];
#define _R(i,j) R[(i)*4+(j)]
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;
_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;
}
//int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btOdeSolverBody** bodies,int& numBodies)
int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies)
{
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
// btOdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
btOdeSolverBody* body = &m_SolverBodyArray[numBodies];
bodies.push_back(body); // Remotion 10.10.07:
orgBody->setCompanionId(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++)
{
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_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 btOdeQuickstepConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,
btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
const btAlignedObjectArray< btOdeSolverBody*> &bodies,
int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
int i,numContacts = manifold->getNumContacts();
bool swapBodies = (bodyId0 < 0);
btOdeSolverBody* 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);
btVector3 color(0,1,0);
for (i=0;i<numContacts;i++)
{
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)
{
btOdeContactJoint* cont = new (&m_JointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1);
//btOdeContactJoint* cont = new (&gJointArray[m_CurJoint++]) btOdeContactJoint( 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;
joints.push_back(cont); // Remotion 10.10.07:
numJoints++;
for (int i=0;i<6;i++)
cont->lambda[i] = 0.f;
cont->flags = 0;
}
}
//create a new contact constraint
}
void btOdeQuickstepConstraintSolver::ConvertTypedConstraint(
btTypedConstraint * constraint,
btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
bool swapBodies = (bodyId0 < 0);
btOdeSolverBody* 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);
btOdeTypedJoint * cont = NULL;
// Determine constraint type
int joint_type = constraint->getConstraintType();
switch(joint_type)
{
case POINT2POINT_CONSTRAINT_TYPE:
case D6_CONSTRAINT_TYPE:
cont = new (&m_TypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1);
//cont = new (&gTypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(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;
joints.push_back(cont); // Remotion 10.10.07:
numJoints++;
for (int i=0;i<6;i++)
cont->lambda[i] = 0.f;
cont->flags = 0;
}
}

109
src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h Normal file
View File

@@ -0,0 +1,109 @@
/*
* Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
* All rights reserved. Email: russ@q12.org Web: www.q12.org
Bullet Continuous Collision Detection and Physics Library
Bullet is 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_CONSTRAINT_SOLVER_H
#define ODE_CONSTRAINT_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btOdeContactJoint.h"
#include "btOdeTypedJoint.h"
#include "btOdeSolverBody.h"
#include "btSorLcp.h"
class btRigidBody;
struct btOdeSolverBody;
class btOdeJoint;
/// btOdeQuickstepConstraintSolver is one of the available solvers for Bullet dynamics framework
/// It uses an adapted version quickstep solver from the Open Dynamics Engine project
class btOdeQuickstepConstraintSolver : public btConstraintSolver
{
private:
int m_CurBody;
int m_CurJoint;
int m_CurTypedJoint;
float m_cfm;
float m_erp;
btSorLcpSolver m_SorLcpSolver;
btAlignedObjectArray<btOdeSolverBody*> m_odeBodies;
btAlignedObjectArray<btOdeJoint*> m_joints;
btAlignedObjectArray<btOdeSolverBody> m_SolverBodyArray;
btAlignedObjectArray<btOdeContactJoint> m_JointArray;
btAlignedObjectArray<btOdeTypedJoint> m_TypedJointArray;
private:
int ConvertBody(btRigidBody* body,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies);
void ConvertConstraint(btPersistentManifold* manifold,
btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
const btAlignedObjectArray< btOdeSolverBody*> &bodies,
int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
void ConvertTypedConstraint(
btTypedConstraint * constraint,
btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
public:
btOdeQuickstepConstraintSolver();
virtual ~btOdeQuickstepConstraintSolver() {}
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) {
m_cfm = cfm;
}
///setErrorReductionParamter sets the maximum amount of error reduction
///which limits energy addition during penetration depth recovery
void setErrorReductionParamter(float erp)
{
m_erp = erp;
}
///clear internal cached data and reset random seed
void reset()
{
m_SorLcpSolver.dRand2_seed = 0;
}
void setRandSeed(unsigned long seed)
{
m_SorLcpSolver.dRand2_seed = seed;
}
unsigned long getRandSeed() const
{
return m_SorLcpSolver.dRand2_seed;
}
};
#endif //ODE_CONSTRAINT_SOLVER_H

0
src/BulletDynamics/ConstraintSolver/btOdeSolverBody.cpp Normal file
View File

48
src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h Normal file
View File

@@ -0,0 +1,48 @@
/*
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 btOdeSolverBody
{
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

451
src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.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 "btOdeTypedJoint.h"
#include "btOdeSolverBody.h"
#include "btOdeMacros.h"
#include <stdio.h>
void btOdeTypedJoint::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 btOdeTypedJoint::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,btOdeSolverBody* body0,btOdeSolverBody* body1):
btOdeTypedJoint(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,
btOdeJoint::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,btOdeSolverBody* body0,btOdeSolverBody* body1):
btOdeTypedJoint(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
src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.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<4C>n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef TYPED_JOINT_H
#define TYPED_JOINT_H
#include "btOdeJoint.h"
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
struct btOdeSolverBody;
class btOdeTypedJoint : public btOdeJoint
{
public:
btTypedConstraint * m_constraint;
int m_index;
bool m_swapBodies;
btOdeSolverBody* m_body0;
btOdeSolverBody* m_body1;
btOdeTypedJoint(){}
btOdeTypedJoint(
btTypedConstraint * constraint,
int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* 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 btOdeTypedJoint
{
protected:
inline btPoint2PointConstraint * getP2PConstraint()
{
return static_cast<btPoint2PointConstraint * >(m_constraint);
}
public:
OdeP2PJoint() {};
OdeP2PJoint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
//btOdeJoint interface for solver
virtual void GetInfo1(Info1 *info);
virtual void GetInfo2(Info2 *info);
};
class OdeD6Joint : public btOdeTypedJoint
{
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,btOdeSolverBody* body0,btOdeSolverBody* body1);
//btOdeJoint 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,
btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational);
#endif //CONTACT_JOINT_H

683
src/BulletDynamics/ConstraintSolver/btSorLcp.cpp Normal file
View File

@@ -0,0 +1,683 @@
/*
* Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
* All rights reserved. Email: russ@q12.org Web: www.q12.org
Bullet Continuous Collision Detection and Physics Library
Bullet is 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 "btSorLcp.h"
#include "btOdeSolverBody.h"
#ifdef USE_SOR_SOLVER
// SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver.
#include "LinearMath/btScalar.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include <math.h>
#include <float.h>//FLT_MAX
#ifdef WIN32
#include <memory.h>
#endif
#include <string.h>
#include <stdio.h>
#if defined (WIN32)
#include <malloc.h>
#else
#if defined (__FreeBSD__)
#include <stdlib.h>
#else
#include <alloca.h>
#endif
#endif
#include "btOdeJoint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
////////////////////////////////////////////////////////////////////
//math stuff
#include "btOdeMacros.h"
//***************************************************************************
// configuration
// for the SOR and CG methods:
// uncomment the following line to use warm starting. this definitely
// help for motor-driven joints. unfortunately it appears to hurt
// with high-friction contacts using the SOR method. use with care
//#define WARM_STARTING 1
// for the SOR method:
// uncomment the following line to randomly reorder constraint rows
// during the solution. depending on the situation, this can help a lot
// or hardly at all, but it doesn't seem to hurt.
#define RANDOMLY_REORDER_CONSTRAINTS 1
//***************************************************************************
// various common computations involving the matrix J
// compute iMJ = inv(M)*J'
inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
//OdeSolverBody* const *body,
const btAlignedObjectArray<btOdeSolverBody*> &body,
dRealPtr invI)
{
int i,j;
dRealMutablePtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
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];
dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
if (b2 >= 0) {
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);
}
J_ptr += 12;
iMJ_ptr += 12;
}
}
#if 0
static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb,
dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2)
{
int i,j;
dRealMutablePtr out_ptr1 = out + onlyBody1*6;
for (j=0; j<6; j++)
out_ptr1[j] = 0;
if (onlyBody2 >= 0)
{
out_ptr1 = out + onlyBody2*6;
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];
dRealMutablePtr out_ptr = out + b1*6;
if ((b1 == onlyBody1) || (b1 == onlyBody2))
{
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)
{
out_ptr = out + b2*6;
for (j=0; j<6; j++)
out_ptr[j] += iMJ_ptr[j] * in[i];
}
}
iMJ_ptr += 6;
}
}
#endif
// compute out = inv(M)*J'*in.
#if 0
static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
dRealMutablePtr in, dRealMutablePtr out)
{
int i,j;
dSetZero1 (out,6*nb);
dRealPtr iMJ_ptr = iMJ;
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
dRealMutablePtr out_ptr = out + b1*6;
for (j=0; j<6; j++)
out_ptr[j] += iMJ_ptr[j] * in[i];
iMJ_ptr += 6;
if (b2 >= 0) {
out_ptr = out + b2*6;
for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
}
iMJ_ptr += 6;
}
}
#endif
// compute out = J*in.
inline void multiply_J (int m, dRealMutablePtr J, int *jb,
dRealMutablePtr in, dRealMutablePtr out)
{
int i,j;
dRealPtr J_ptr = J;
for (i=0; i<m; i++) {
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];
J_ptr += 6;
if (b2 >= 0) {
in_ptr = in + b2*6;
for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
}
J_ptr += 6;
out[i] = sum;
}
}
//***************************************************************************
// SOR-LCP method
// nb is the number of bodies in the body array.
// J is an m*12 matrix of constraint rows
// jb is an array of first and second body numbers for each constraint row
// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
//
// this returns lambda and fc (the constraint force).
// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
//
// b, lo and hi are modified on exit
//------------------------------------------------------------------------------
ATTRIBUTE_ALIGNED16(struct) IndexError {
btScalar error; // error to sort on
int findex;
int index; // row index
};
//------------------------------------------------------------------------------
void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
const btAlignedObjectArray<btOdeSolverBody*> &body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
int numiter,float overRelax,
btStackAlloc* stackAlloc
)
{
//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
AutoBlockSa asaBlock(stackAlloc);
const int num_iterations = numiter;
const float sor_w = overRelax; // SOR over-relaxation parameter
int i,j;
#ifdef WARM_STARTING
// for warm starting, this seems to be necessary to prevent
// jerkiness in motor-driven joints. i have no idea why this works.
for (i=0; i<m; i++) lambda[i] *= 0.9;
#else
dSetZero1 (lambda,m);
#endif
// the lambda computed at the previous iteration.
// this is used to measure error for when we are reordering the indexes.
dRealAllocaArray (last_lambda,m);
// a copy of the 'hi' vector in case findex[] is being used
dRealAllocaArray (hicopy,m);
memcpy (hicopy,hi,m*sizeof(float));
// precompute iMJ = inv(M)*J'
dRealAllocaArray (iMJ,m*12);
compute_invM_JT (m,J,iMJ,jb,body,invI);
// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
// as we change lambda.
#ifdef WARM_STARTING
multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
#else
dSetZero1 (invMforce,nb*6);
#endif
// precompute 1 / diagonals of A
dRealAllocaArray (Ad,m);
dRealPtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
float sum = 0;
for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
if (jb[i*2+1] >= 0) {
for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
}
iMJ_ptr += 12;
J_ptr += 12;
Ad[i] = sor_w / sum;//(sum + cfm[i]);
}
// scale J and b by Ad
J_ptr = J;
for (i=0; i<m; i++) {
for (j=0; j<12; j++) {
J_ptr[0] *= Ad[i];
J_ptr++;
}
rhs[i] *= Ad[i];
}
// scale Ad by CFM
for (i=0; i<m; i++)
Ad[i] *= cfm[i];
// order to solve constraint rows in
//IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));
#ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first.
j=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)
order[j++].index = i;
dIASSERT (j==m);
#endif
for (int iteration=0; iteration < num_iterations; iteration++) {
#ifdef REORDER_CONSTRAINTS
// constraints with findex < 0 always come first.
if (iteration < 2) {
// for the first two iterations, solve the constraints in
// the given order
for (i=0; i<m; i++) {
order[i].error = i;
order[i].findex = findex[i];
order[i].index = i;
}
}
else {
// sort the constraints so that the ones converging slowest
// get solved last. use the absolute (not relative) error.
for (i=0; i<m; i++) {
float v1 = dFabs (lambda[i]);
float v2 = dFabs (last_lambda[i]);
float max = (v1 > v2) ? v1 : v2;
if (max > 0) {
//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
order[i].error = dFabs(lambda[i]-last_lambda[i]);
}
else {
order[i].error = dInfinity;
}
order[i].findex = findex[i];
order[i].index = i;
}
}
qsort (order,m,sizeof(IndexError),&compare_index_error);
#endif
#ifdef RANDOMLY_REORDER_CONSTRAINTS
if ((iteration & 7) == 0) {
for (i=1; i<m; ++i) {
IndexError tmp = order[i];
int swapi = dRandInt2(i+1);
order[i] = order[swapi];
order[swapi] = tmp;
}
}
#endif
//@@@ potential optimization: swap lambda and last_lambda pointers rather
// than copying the data. we must make sure lambda is properly
// returned to the caller
memcpy (last_lambda,lambda,m*sizeof(float));
for (int i=0; i<m; i++) {
// @@@ potential optimization: we could pre-sort J and iMJ, thereby
// 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
// limit adjustment once per time step, whereas this method performs
// once per iteration per constraint row.
// the constraints are ordered so that all lambda[] values needed have
// already been computed.
if (findex[index] >= 0) {
hi[index] = btFabs (hicopy[index] * lambda[findex[index]]);
lo[index] = -hi[index];
}
int b1 = jb[index*2];
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] +
fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = invMforce + 6*b2;
delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
}
// compute lambda and clamp it to [lo,hi].
// @@@ potential optimization: does SSE have clamping instructions
// to save test+jump penalties here?
float new_lambda = lambda[index] + delta;
if (new_lambda < lo[index]) {
delta = lo[index]-lambda[index];
lambda[index] = lo[index];
}
else if (new_lambda > hi[index]) {
delta = hi[index]-lambda[index];
lambda[index] = hi[index];
}
else {
lambda[index] = new_lambda;
}
//@@@ 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;
fc_ptr[0] += delta * iMJ_ptr[0];
fc_ptr[1] += delta * iMJ_ptr[1];
fc_ptr[2] += delta * iMJ_ptr[2];
fc_ptr[3] += delta * iMJ_ptr[3];
fc_ptr[4] += delta * iMJ_ptr[4];
fc_ptr[5] += delta * iMJ_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = invMforce + 6*b2;
fc_ptr[0] += delta * iMJ_ptr[6];
fc_ptr[1] += delta * iMJ_ptr[7];
fc_ptr[2] += delta * iMJ_ptr[8];
fc_ptr[3] += delta * iMJ_ptr[9];
fc_ptr[4] += delta * iMJ_ptr[10];
fc_ptr[5] += delta * iMJ_ptr[11];
}
}
}
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
}
//------------------------------------------------------------------------------
void btSorLcpSolver::SolveInternal1 (
float global_cfm,
float global_erp,
const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
btAlignedObjectArray<btOdeJoint*> &joint,
int nj, const btContactSolverInfo& solverInfo,
btStackAlloc* stackAlloc)
{
//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
AutoBlockSa asaBlock(stackAlloc);
int numIter = solverInfo.m_numIterations;
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++)
body[i]->m_odeTag = i;
// make a local copy of the joint array, because we might want to modify it.
// (the "btOdeJoint *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
//btOdeJoint **joint = (btOdeJoint**) alloca (nj * sizeof(btOdeJoint*));
//memcpy (joint,_joint,nj * sizeof(btOdeJoint*));
// 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.
dRealAllocaArray (I,3*4*nb);
dRealAllocaArray (invI,3*4*nb);
/* for (i=0; i<nb; i++) {
dMatrix3 tmp;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
// compute rotational force
dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
}
*/
for (i=0; i<nb; i++) {
dMatrix3 tmp;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp);
// compute inverse inertia tensor in global frame
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]->m_angularVelocity);
// dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp);
}
// get joint information (m = total constraint dimension, nub = number of unbounded variables).
// joints with m=0 are inactive and are removed from the joints array
// entirely, so that the code that follows does not consider them.
//@@@ do we really need to save all the info1's
btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1));
for (i=0, j=0; j<nj; j++) { // i=dest, j=src
joint[j]->GetInfo1 (info+i);
dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
if (info[i].m > 0) {
joint[i] = joint[j];
i++;
}
}
nj = i;
// create the row offset array
int m = 0;
int *ofs = (int*) ALLOCA (nj*sizeof(int));
for (i=0; i<nj; i++) {
ofs[i] = m;
m += info[i].m;
}
// if there are constraints, compute the constraint force
dRealAllocaArray (J,m*12);
int *jb = (int*) ALLOCA (m*2*sizeof(int));
if (m > 0) {
// create a constraint equation right hand side vector `c', a constraint
// force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector.
dRealAllocaArray (c,m);
dRealAllocaArray (cfm,m);
dRealAllocaArray (lo,m);
dRealAllocaArray (hi,m);
int *findex = (int*) ALLOCA (m*sizeof(int));
dSetZero1 (c,m);
dSetValue1 (cfm,m,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:
//
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ .
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows)
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
// etc...
//
// (lll) = linear jacobian data
// (aaa) = angular jacobian data
//
dSetZero1 (J,m*12);
btOdeJoint::Info2 Jinfo;
Jinfo.rowskip = 12;
Jinfo.fps = stepsize1;
Jinfo.erp = global_erp;
for (i=0; i<nj; i++) {
Jinfo.J1l = J + ofs[i]*12;
Jinfo.J1a = Jinfo.J1l + 3;
Jinfo.J2l = Jinfo.J1l + 6;
Jinfo.J2a = Jinfo.J1l + 9;
Jinfo.c = c + ofs[i];
Jinfo.cfm = cfm + ofs[i];
Jinfo.lo = lo + ofs[i];
Jinfo.hi = hi + ofs[i];
Jinfo.findex = findex + ofs[i];
joint[i]->GetInfo2 (&Jinfo);
if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
Jinfo.c[0] = solverInfo.m_maxErrorReduction;
// adjust returned findex values for global index numbering
for (j=0; j<info[i].m; j++) {
if (findex[ofs[i] + j] >= 0)
findex[ofs[i] + j] += ofs[i];
}
}
// create an array of body numbers for each joint row
int *jb_ptr = jb;
for (i=0; i<nj; i++) {
int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1;
int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1;
for (j=0; j<info[i].m; j++) {
jb_ptr[0] = b1;
jb_ptr[1] = b2;
jb_ptr += 2;
}
}
dIASSERT (jb_ptr == jb+2*m);
// compute the right hand side `rhs'
dRealAllocaArray (tmp1,nb*6);
// 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++)
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]->m_angularVelocity[j] * stepsize1;
}
// put J*tmp1 into rhs
dRealAllocaArray (rhs,m);
multiply_J (m,J,jb,tmp1,rhs);
// complete rhs
for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
// scale CFM
for (i=0; i<m; i++)
cfm[i] *= stepsize1;
// load lambda from the value saved on the previous iteration
dRealAllocaArray (lambda,m);
#ifdef WARM_STARTING
dSetZero1 (lambda,m); //@@@ shouldn't be necessary
for (i=0; i<nj; i++) {
memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar));
}
#endif
// solve the LCP problem and get lambda and invM*constraint_force
dRealAllocaArray (cforce,nb*6);
/// SOR_LCP
SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc);
#ifdef WARM_STARTING
// save lambda for the next iteration
//@@@ note that this doesn't work for contact joints yet, as they are
// recreated every iteration
for (i=0; i<nj; i++) {
memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar));
}
#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++)
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];
}
}
// compute the velocity update:
// add stepsize * invM * fe to the body velocity
for (i=0; i<nb; i++) {
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++)
{
linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j];
}
for (j=0; j<3; j++)
{
body[i]->m_tacc[j] *= solverInfo.m_timeStep;
}
dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
body[i]->m_angularVelocity = angvel;
}
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
}
#endif //USE_SOR_SOLVER

112
src/BulletDynamics/ConstraintSolver/btSorLcp.h Normal file
View File

@@ -0,0 +1,112 @@
/*
* Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
* All rights reserved. Email: russ@q12.org Web: www.q12.org
Bullet Continuous Collision Detection and Physics Library
Bullet is 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.
*/
#define USE_SOR_SOLVER
#ifdef USE_SOR_SOLVER
#ifndef SOR_LCP_H
#define SOR_LCP_H
struct btOdeSolverBody;
class btOdeJoint;
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btStackAlloc.h"
struct btContactSolverInfo;
//=============================================================================
class btSorLcpSolver //Remotion: 11.10.2007
{
public:
btSorLcpSolver()
{
dRand2_seed = 0;
}
void SolveInternal1 (float global_cfm,
float global_erp,
const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
btAlignedObjectArray<btOdeJoint*> &joint,
int nj, const btContactSolverInfo& solverInfo,
btStackAlloc* stackAlloc
);
public: //data
unsigned long dRand2_seed;
protected: //typedef
typedef const btScalar *dRealPtr;
typedef btScalar *dRealMutablePtr;
protected: //members
//------------------------------------------------------------------------------
SIMD_FORCE_INLINE unsigned long dRand2()
{
dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff;
return dRand2_seed;
}
//------------------------------------------------------------------------------
SIMD_FORCE_INLINE int dRandInt2 (int n)
{
float a = float(n) / 4294967296.0f;
return (int) (float(dRand2()) * a);
}
//------------------------------------------------------------------------------
void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
const btAlignedObjectArray<btOdeSolverBody*> &body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
int numiter,float overRelax,
btStackAlloc* stackAlloc
);
};
//=============================================================================
class AutoBlockSa //Remotion: 10.10.2007
{
btStackAlloc* stackAlloc;
btBlock* saBlock;
public:
AutoBlockSa(btStackAlloc* stackAlloc_)
{
stackAlloc = stackAlloc_;
saBlock = stackAlloc->beginBlock();
}
~AutoBlockSa()
{
stackAlloc->endBlock(saBlock);
}
//operator btBlock* () { return saBlock; }
};
// //Usage
//void function(btStackAlloc* stackAlloc)
//{
// AutoBlockSa(stackAlloc);
// ...
// if(...) return;
// return;
//}
//------------------------------------------------------------------------------
#endif //SOR_LCP_H
#endif //USE_SOR_SOLVER

5
src/btBulletDynamicsCommon.h
View File

@@ -30,8 +30,11 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
///Optional ODE quickstep constraint solver, redistributed under ZLib license
#include "BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btOdeTypedJoint.h"
///Vehicle simulation, with wheel contact simulated by raycasts
#include "BulletDynamics/Vehicle/btRaycastVehicle.h"