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

merged most of the changes from the branch into trunk, except for COLLADA, libxml and glut glitches.

Browse Source 
Still need to verify to make sure no unwanted renaming is introduced.
This commit is contained in:
ejcoumans
2006-09-27 20:43:51 +00:00
parent d1e9a885f3
commit eb23bb5c0c
263 changed files with 7528 additions and 6714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -20,12 +20,12 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btContactConstraint.h"
#include "Solve2LinearConstraint.h"
#include "btSolve2LinearConstraint.h"
#include "btContactSolverInfo.h"
#include "Dynamics/BU_Joint.h"
#include "Dynamics/ContactJoint.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#define USE_SOR_SOLVER
@@ -69,13 +69,13 @@ m_erp(0.4f)
//iterative lcp and penalty method
float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
float OdeConstraintSolver::SolveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
m_CurBody = 0;
m_CurJoint = 0;
RigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
btRigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
int numBodies = 0;
BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4];
@@ -86,11 +86,11 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
int body0=-1,body1=-1;
PersistentManifold* manifold = manifoldPtr[j];
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->GetNumContacts() > 0)
{
body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies);
body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies);
body0 = ConvertBody((btRigidBody*)manifold->GetBody0(),bodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->GetBody1(),bodies,numBodies);
ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
}
}
@@ -104,15 +104,15 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
/////////////////////////////////////////////////////////////////////////////////
typedef SimdScalar dQuaternion[4];
typedef btScalar dQuaternion[4];
#define _R(i,j) R[(i)*4+(j)]
void dRfromQ1 (dMatrix3 R, const dQuaternion q)
{
// q = (s,vx,vy,vz)
SimdScalar qq1 = 2.f*q[1]*q[1];
SimdScalar qq2 = 2.f*q[2]*q[2];
SimdScalar qq3 = 2.f*q[3]*q[3];
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]);
@@ -132,7 +132,7 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q)
int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
int OdeConstraintSolver::ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies)
{
if (!body || (body->getInvMass() == 0.f) )
{
@@ -194,13 +194,13 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num
static ContactJoint gJointArray[MAX_JOINTS_1];
void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer)
void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
manifold->RefreshContactPoints(((btRigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
@@ -209,31 +209,31 @@ void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Join
bool swapBodies = (bodyId0 < 0);
RigidBody* body0,*body1;
btRigidBody* body0,*body1;
if (swapBodies)
{
bodyId0 = _bodyId1;
bodyId1 = _bodyId0;
body0 = (RigidBody*)manifold->GetBody1();
body1 = (RigidBody*)manifold->GetBody0();
body0 = (btRigidBody*)manifold->GetBody1();
body1 = (btRigidBody*)manifold->GetBody0();
} else
{
body0 = (RigidBody*)manifold->GetBody0();
body1 = (RigidBody*)manifold->GetBody1();
body0 = (btRigidBody*)manifold->GetBody0();
body1 = (btRigidBody*)manifold->GetBody1();
}
assert(bodyId0 >= 0);
SimdVector3 color(0,1,0);
btVector3 color(0,1,0);
for (i=0;i<numContacts;i++)
{
if (debugDrawer)
{
const ManifoldPoint& cp = manifold->GetContactPoint(i);
const btManifoldPoint& cp = manifold->GetContactPoint(i);
debugDrawer->DrawContactPoint(
cp.m_positionWorldOnB,

14
Extras/quickstep/OdeConstraintSolver.h
View File

@@ -16,14 +16,14 @@ subject to the following restrictions:
#ifndef ODE_CONSTRAINT_SOLVER_H
#define ODE_CONSTRAINT_SOLVER_H
#include "ConstraintSolver.h"
#include "btConstraintSolver.h"
class RigidBody;
class btRigidBody;
class BU_Joint;
/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework
/// It uses the the unmodified version of quickstep solver from the open dynamics project
class OdeConstraintSolver : public ConstraintSolver
class OdeConstraintSolver : public btConstraintSolver
{
private:
@@ -34,9 +34,9 @@ private:
float m_erp;
int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies);
void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer);
int ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies);
void ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
public:
@@ -44,7 +44,7 @@ public:
virtual ~OdeConstraintSolver() {}
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0);
virtual float SolveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0);
///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'

58
Extras/quickstep/SorLcp.cpp
View File

@@ -5,10 +5,10 @@
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* (1) The GNU Lesser bteral Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* bteral Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
@@ -28,7 +28,7 @@
// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver
#include "LinearMath/SimdScalar.h"
#include "LinearMath/btScalar.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include <math.h>
@@ -55,8 +55,8 @@
////////////////////////////////////////////////////////////////////
//math stuff
typedef SimdScalar dVector4[4];
typedef SimdScalar dMatrix3[4*3];
typedef btScalar dVector4[4];
typedef btScalar dMatrix3[4*3];
#define dInfinity FLT_MAX
@@ -85,7 +85,7 @@ typedef SimdScalar dMatrix3[4*3];
#define REAL float
#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
SimdScalar dDOT1 (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); }
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) \
@@ -171,12 +171,12 @@ extern "C" {
#define ALLOCA dALLOCA16
typedef const SimdScalar *dRealPtr;
typedef SimdScalar *dRealMutablePtr;
#define dRealArray(name,n) SimdScalar name[n];
#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar));
typedef const btScalar *dRealPtr;
typedef btScalar *dRealMutablePtr;
#define dRealArray(name,n) btScalar name[n];
#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));
void dSetZero1 (SimdScalar *a, int n)
void dSetZero1 (btScalar *a, int n)
{
dAASSERT (a && n >= 0);
while (n > 0) {
@@ -185,7 +185,7 @@ void dSetZero1 (SimdScalar *a, int n)
}
}
void dSetValue1 (SimdScalar *a, int n, SimdScalar value)
void dSetValue1 (btScalar *a, int n, btScalar value)
{
dAASSERT (a && n >= 0);
while (n > 0) {
@@ -220,7 +220,7 @@ void dSetValue1 (SimdScalar *a, int n, SimdScalar value)
// compute iMJ = inv(M)*J'
static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
RigidBody * const *body, dRealPtr invI)
btRigidBody * const *body, dRealPtr invI)
{
int i,j;
dRealMutablePtr iMJ_ptr = iMJ;
@@ -228,7 +228,7 @@ 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];
SimdScalar k = body[b1]->getInvMass();
btScalar k = body[b1]->getInvMass();
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) {
@@ -330,7 +330,7 @@ static void multiply_J (int m, dRealMutablePtr J, int *jb,
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
SimdScalar sum = 0;
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;
@@ -358,7 +358,7 @@ static void multiply_J (int m, dRealMutablePtr J, int *jb,
struct IndexError {
SimdScalar error; // error to sort on
btScalar error; // error to sort on
int findex;
int index; // row index
};
@@ -378,7 +378,7 @@ int dRandInt2 (int n)
}
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body,
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, btRigidBody * const *body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
int numiter,float overRelax)
@@ -527,7 +527,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons
// the constraints are ordered so that all lambda[] values needed have
// already been computed.
if (findex[index] >= 0) {
hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]);
hi[index] = btFabs (hicopy[index] * lambda[findex[index]]);
lo[index] = -hi[index];
}
@@ -597,10 +597,10 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons
void SolveInternal1 (float global_cfm,
float global_erp,
RigidBody * const *body, int nb,
btRigidBody * const *body, int nb,
BU_Joint * const *_joint,
int nj,
const ContactSolverInfo& solverInfo)
const btContactSolverInfo& solverInfo)
{
int numIter = solverInfo.m_numIterations;
@@ -608,7 +608,7 @@ void SolveInternal1 (float global_cfm,
int i,j;
SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep);
btScalar stepsize1 = dRecip(solverInfo.m_timeStep);
// number all bodies in the body list - set their tag values
for (i=0; i<nb; i++)
@@ -755,7 +755,7 @@ void SolveInternal1 (float global_cfm,
dRealAllocaArray (tmp1,nb*6);
// put v/h + invM*fe into tmp1
for (i=0; i<nb; i++) {
SimdScalar body_invMass = body[i]->getInvMass();
btScalar body_invMass = body[i]->getInvMass();
for (j=0; j<3; j++)
tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1;
dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc);
@@ -779,7 +779,7 @@ void SolveInternal1 (float global_cfm,
#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(SimdScalar));
memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar));
}
#endif
@@ -793,7 +793,7 @@ void SolveInternal1 (float global_cfm,
//@@@ 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(SimdScalar));
memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar));
}
#endif
@@ -803,8 +803,8 @@ void SolveInternal1 (float global_cfm,
// add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) {
SimdVector3 linvel = body[i]->getLinearVelocity();
SimdVector3 angvel = body[i]->getAngularVelocity();
btVector3 linvel = body[i]->getLinearVelocity();
btVector3 angvel = body[i]->getAngularVelocity();
for (j=0; j<3; j++)
linvel[j] += solverInfo.m_timeStep* cforce[i*6+j];
@@ -824,9 +824,9 @@ void SolveInternal1 (float global_cfm,
// add stepsize * invM * fe to the body velocity
for (i=0; i<nb; i++) {
SimdScalar body_invMass = body[i]->getInvMass();
SimdVector3 linvel = body[i]->getLinearVelocity();
SimdVector3 angvel = body[i]->getAngularVelocity();
btScalar body_invMass = body[i]->getInvMass();
btVector3 linvel = body[i]->getLinearVelocity();
btVector3 angvel = body[i]->getAngularVelocity();
for (j=0; j<3; j++)
{

14
Extras/quickstep/SorLcp.h
View File

@@ -5,10 +5,10 @@
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* (1) The GNU Lesser bteral Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* bteral Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
@@ -25,16 +25,16 @@
#ifndef SOR_LCP_H
#define SOR_LCP_H
class RigidBody;
class btRigidBody;
class BU_Joint;
#include "LinearMath/SimdScalar.h"
#include "LinearMath/btScalar.h"
struct ContactSolverInfo;
struct btContactSolverInfo;
void SolveInternal1 (float global_cfm,
float global_erp,
RigidBody * const *body, int nb,
BU_Joint * const *_joint, int nj, const ContactSolverInfo& info);
btRigidBody * const *body, int nb,
BU_Joint * const *_joint, int nj, const btContactSolverInfo& info);
int dRandInt2 (int n);