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

Attempts to improve performance. Not much gain yet, but good to experiment what has effect and what hasn't.

Browse Source 
Added 'DO_BENCHMARK_PYRAMID' to CcdPhysicsDemo.
This commit is contained in:
ejcoumans
2007-03-20 20:12:23 +00:00
parent f8fe7e8f2d
commit c1a54d9edc
19 changed files with 356 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
View File

@@ -12,11 +12,18 @@ subject to the following restrictions:
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
//enable just one, DO_BENCHMARK_PYRAMIDS or DO_WALL
//#define DO_BENCHMARK_PYRAMIDS 1
#define DO_WALL 1
//Note: some of those settings need 'DO_WALL' demo
//#define USE_KINEMATIC_GROUND 1 //#define USE_KINEMATIC_GROUND 1
//#define PRINT_CONTACT_STATISTICS 1 //#define PRINT_CONTACT_STATISTICS 1
//#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1 //#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
//#define REGISTER_BOX_BOX 1 //needs to be used in combination with REGISTER_CUSTOM_COLLISION_ALGORITHM
//#define USER_DEFINED_FRICTION_MODEL 1 //#define USER_DEFINED_FRICTION_MODEL 1
#define USE_CUSTOM_NEAR_CALLBACK 1 //#define USE_CUSTOM_NEAR_CALLBACK 1
//#define CENTER_OF_MASS_SHIFT 1 //#define CENTER_OF_MASS_SHIFT 1
//following define allows to compare/replace Bullet's constraint solver with ODE quickstep //following define allows to compare/replace Bullet's constraint solver with ODE quickstep
@@ -26,7 +33,9 @@ subject to the following restrictions:
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
//#include "../Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.h" #ifdef REGISTER_BOX_BOX
#include "../Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.h"
#endif //REGISTER_BOX_BOX
#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
#ifdef COMPARE_WITH_QUICKSTEP #ifdef COMPARE_WITH_QUICKSTEP
@@ -58,7 +67,11 @@ const int maxProxies = 32766;
const int maxOverlap = 65535; const int maxOverlap = 65535;
bool createConstraint = true;//false; bool createConstraint = true;//false;
#ifdef CENTER_OF_MASS_SHIFT
bool useCompound = true;
#else
bool useCompound = false; bool useCompound = false;
#endif
@@ -74,9 +87,9 @@ const int maxNumObjects = 32760;
int shapeIndex[maxNumObjects]; int shapeIndex[maxNumObjects];
#define CUBE_HALF_EXTENTS 1 #define CUBE_HALF_EXTENTS 0.5
#define EXTRA_HEIGHT -20.f #define EXTRA_HEIGHT -10.f
//GL_LineSegmentShape shapeE(btPoint3(-50,0,0), //GL_LineSegmentShape shapeE(btPoint3(-50,0,0),
// btPoint3(50,0,0)); // btPoint3(50,0,0));
static const int numShapes = 4; static const int numShapes = 4;
@@ -90,14 +103,18 @@ btCollisionShape* shapePtr[numShapes] =
#ifdef USE_GROUND_PLANE #ifdef USE_GROUND_PLANE
new btStaticPlaneShape(btVector3(0,1,0),10), new btStaticPlaneShape(btVector3(0,1,0),10),
#else #else
new btBoxShape (btVector3(50,10,50)), new btBoxShape (btVector3(200,CUBE_HALF_EXTENTS,200)),
#endif #endif
#ifdef DO_BENCHMARK_PYRAMIDS
new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
#else
new btCylinderShape (btVector3(CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin)), new btCylinderShape (btVector3(CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin)),
#endif
//new btSphereShape (CUBE_HALF_EXTENTS), //new btSphereShape (CUBE_HALF_EXTENTS),
//new btCapsuleShape(0.5*CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin), //new btCapsuleShape(0.5*CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin),
//new btCylinderShape (btVector3(1-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,1-gCollisionMargin)), //new btCylinderShape (btVector3(1-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,1-gCollisionMargin)),
//new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
//new btConeShape(CUBE_HALF_EXTENTS-gCollisionMargin,2.f*CUBE_HALF_EXTENTS-gCollisionMargin), //new btConeShape(CUBE_HALF_EXTENTS-gCollisionMargin,2.f*CUBE_HALF_EXTENTS-gCollisionMargin),
new btSphereShape (CUBE_HALF_EXTENTS), new btSphereShape (CUBE_HALF_EXTENTS),
@@ -111,6 +128,31 @@ btCollisionShape* shapePtr[numShapes] =
}; };
void CcdPhysicsDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos )
{
btTransform trans;
trans.setIdentity();
for(int i=0; i<size; i++)
{
// This constructs a row, from left to right
int rowSize = size - i;
for(int j=0; j< rowSize; j++)
{
btVector4 pos;
pos.setValue(
-rowSize * halfCubeSize + halfCubeSize + j * 2.0f * halfCubeSize,
halfCubeSize + i * halfCubeSize * 2.0f,
zPos);
trans.setOrigin(pos);
btScalar mass = 1.f;
btRigidBody* body = localCreateRigidBody(mass,trans,boxShape);
}
}
}
//////////////////////////////////// ////////////////////////////////////
@@ -168,7 +210,9 @@ int main(int argc,char** argv)
ccdDemo->initPhysics(); ccdDemo->initPhysics();
#ifdef DO_BENCHMARK_PYRAMIDS
ccdDemo->setCameraDistance(26.f); ccdDemo->setCameraDistance(26.f);
#endif
return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bullet.sf.net",ccdDemo); return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bullet.sf.net",ccdDemo);
} }
@@ -206,29 +250,28 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
float dt = m_clock.getTimeMicroseconds() * 0.000001f; float dt = m_clock.getTimeMicroseconds() * 0.000001f;
m_clock.reset(); m_clock.reset();
printf("dt = %f: ",dt); // printf("dt = %f: ",dt);
if (m_dynamicsWorld) if (m_dynamicsWorld)
{ {
//swap solver //swap solver
#ifdef COMPARE_WITH_QUICKSTEP #ifdef COMPARE_WITH_QUICKSTEP
if (m_debugMode & btIDebugDraw::DBG_DisableBulletLCP)
{
m_dynamicsWorld->setConstraintSolver( new OdeConstraintSolver()); m_dynamicsWorld->setConstraintSolver( new OdeConstraintSolver());
} else
{
m_dynamicsWorld->setConstraintSolver( new btSequentialImpulseConstraintSolver());
}
#endif //COMPARE_WITH_QUICKSTEP #endif //COMPARE_WITH_QUICKSTEP
#define FIXED_STEP 1
#ifdef FIXED_STEP
m_dynamicsWorld->stepSimulation(1.0f/60.f,0);
#else
//during idle mode, just run 1 simulation step maximum //during idle mode, just run 1 simulation step maximum
int maxSimSubSteps = m_idle ? 1 : 1; int maxSimSubSteps = m_idle ? 1 : 1;
if (m_idle) if (m_idle)
dt = 1.0/420.f; dt = 1.0/420.f;
int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps); int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
/*
if (!numSimSteps) if (!numSimSteps)
printf("Interpolated transforms\n"); printf("Interpolated transforms\n");
else else
@@ -242,6 +285,9 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
printf("Simulated (%i) steps\n",numSimSteps); printf("Simulated (%i) steps\n",numSimSteps);
} }
} }
*/
#endif
} }
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
@@ -336,6 +382,9 @@ float myFrictionModel( btRigidBody& body1, btRigidBody& body2, btManifoldPoint&
void CcdPhysicsDemo::initPhysics() void CcdPhysicsDemo::initPhysics()
{ {
#ifdef DO_BENCHMARK_PYRAMIDS
m_azi = 90.f;
#endif //DO_BENCHMARK_PYRAMIDS
btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
@@ -344,8 +393,8 @@ void CcdPhysicsDemo::initPhysics()
dispatcher->setNearCallback(customNearCallback); dispatcher->setNearCallback(customNearCallback);
#endif #endif
btVector3 worldAabbMin(-10000,-10000,-10000); btVector3 worldAabbMin(-1000,-1000,-1000);
btVector3 worldAabbMax(10000,10000,10000); btVector3 worldAabbMax(1000,1000,1000);
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies); btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
// btOverlappingPairCache* broadphase = new btSimpleBroadphase; // btOverlappingPairCache* broadphase = new btSimpleBroadphase;
@@ -353,7 +402,9 @@ void CcdPhysicsDemo::initPhysics()
#ifdef REGISTER_CUSTOM_COLLISION_ALGORITHM #ifdef REGISTER_CUSTOM_COLLISION_ALGORITHM
dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc); dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
//box-box is in Extras/AlternativeCollisionAlgorithms:it requires inclusion of those files //box-box is in Extras/AlternativeCollisionAlgorithms:it requires inclusion of those files
//dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,new BoxBoxCollisionAlgorithm::CreateFunc); #ifdef REGISTER_BOX_BOX
dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,new BoxBoxCollisionAlgorithm::CreateFunc);
#endif //REGISTER_BOX_BOX
dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,TRIANGLE_SHAPE_PROXYTYPE,new btSphereTriangleCollisionAlgorithm::CreateFunc); dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,TRIANGLE_SHAPE_PROXYTYPE,new btSphereTriangleCollisionAlgorithm::CreateFunc);
btSphereTriangleCollisionAlgorithm::CreateFunc* triangleSphereCF = new btSphereTriangleCollisionAlgorithm::CreateFunc; btSphereTriangleCollisionAlgorithm::CreateFunc* triangleSphereCF = new btSphereTriangleCollisionAlgorithm::CreateFunc;
triangleSphereCF->m_swapped = true; triangleSphereCF->m_swapped = true;
@@ -370,6 +421,10 @@ void CcdPhysicsDemo::initPhysics()
#endif #endif
#ifdef USER_DEFINED_FRICTION_MODEL
//user defined friction model is not supported in 'cache friendly' solver yet, so switch to old solver
solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
#endif //USER_DEFINED_FRICTION_MODEL
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver); m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
m_dynamicsWorld->setGravity(btVector3(0,-10,0)); m_dynamicsWorld->setGravity(btVector3(0,-10,0));
@@ -425,6 +480,8 @@ void CcdPhysicsDemo::initPhysics()
#endif #endif
} }
#ifdef DO_WALL
for (i=0;i<gNumObjects;i++) for (i=0;i<gNumObjects;i++)
{ {
btCollisionShape* shape = shapePtr[shapeIndex[i]]; btCollisionShape* shape = shapePtr[shapeIndex[i]];
@@ -456,7 +513,7 @@ void CcdPhysicsDemo::initPhysics()
trans.setOrigin(pos); trans.setOrigin(pos);
} else } else
{ {
trans.setOrigin(btVector3(0,-30,0)); trans.setOrigin(btVector3(0,EXTRA_HEIGHT-CUBE_HALF_EXTENTS,0));
} }
float mass = 1.f; float mass = 1.f;
@@ -486,9 +543,45 @@ void CcdPhysicsDemo::initPhysics()
#endif //USER_DEFINED_FRICTION_MODEL #endif //USER_DEFINED_FRICTION_MODEL
} }
#endif
clientResetScene(); #ifdef DO_BENCHMARK_PYRAMIDS
btTransform trans;
trans.setIdentity();
btScalar halfExtents = CUBE_HALF_EXTENTS;
trans.setOrigin(btVector3(0,-halfExtents,0));
localCreateRigidBody(0.f,trans,shapePtr[shapeIndex[0]]);
int numWalls = 10;
int wallHeight = 10;
float wallDistance = 3;
for (int i=0;i<numWalls;i++)
{
float zPos = (i-numWalls/2) * wallDistance;
createStack(shapePtr[shapeIndex[1]],halfExtents,wallHeight,zPos);
}
// createStack(shapePtr[shapeIndex[1]],halfExtends,20,10);
// createStack(shapePtr[shapeIndex[1]],halfExtends,20,20);
#define DESTROYER_BALL 1
#ifdef DESTROYER_BALL
btTransform sphereTrans;
sphereTrans.setIdentity();
sphereTrans.setOrigin(btVector3(0,2,40));
btSphereShape* ball = new btSphereShape(2.f);
btRigidBody* ballBody = localCreateRigidBody(10000.f,sphereTrans,ball);
ballBody->setLinearVelocity(btVector3(0,0,-10));
#endif
#endif //DO_BENCHMARK_PYRAMIDS
// clientResetScene();
} }

1
Demos/CcdPhysicsDemo/CcdPhysicsDemo.h
View File

@@ -28,6 +28,7 @@ class CcdPhysicsDemo : public DemoApplication
virtual void displayCallback(); virtual void displayCallback();
void createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos );
}; };

2
Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp
View File

@@ -55,6 +55,8 @@ void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btColl
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr); resultOut->setPersistentManifold(m_manifoldPtr);
m_manifoldPtr->clearManifold();
btDiscreteCollisionDetectorInterface::ClosestPointInput input; btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = 1e30f; input.m_maximumDistanceSquared = 1e30f;
input.m_transformA = body0->getWorldTransform(); input.m_transformA = body0->getWorldTransform();

22
Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp
View File

@@ -268,9 +268,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
int i,j,invert_normal,code; int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1 // get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0]; p = p2 - p1;
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2 // get side lengths / 2
@@ -646,20 +644,26 @@ void BoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& out
dMatrix3 R1; dMatrix3 R1;
dMatrix3 R2; dMatrix3 R2;
for (int i=0;i<3;i++)
{
for (int j=0;j<3;j++) for (int j=0;j<3;j++)
{ {
R1[i+4*j] = transformA.getBasis()[j][i]; R1[0+4*j] = transformA.getBasis()[j].x();
R2[i+4*j] = transformB.getBasis()[j][i]; R2[0+4*j] = transformB.getBasis()[j].x();
}
R1[1+4*j] = transformA.getBasis()[j].y();
R2[1+4*j] = transformB.getBasis()[j].y();
R1[2+4*j] = transformA.getBasis()[j].z();
R2[2+4*j] = transformB.getBasis()[j].z();
} }
btVector3 normal; btVector3 normal;
btScalar depth; btScalar depth;
int return_code; int return_code;
int maxc = 10; int maxc = 4;
dBoxBox2 (transformA.getOrigin(), dBoxBox2 (transformA.getOrigin(),

32
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -155,15 +155,17 @@ int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,OdeSolverBody** bodies
return -1; return -1;
} }
if (orgBody->getCompanionId()>=0)
{
return orgBody->getCompanionId();
}
//first try to find //first try to find
int i,j; int i,j;
for (i=0;i<numBodies;i++)
{
if (bodies[i]->m_originalBody == orgBody)
return i;
}
//if not found, create a new body //if not found, create a new body
OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies]; OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
orgBody->setCompanionId(numBodies);
numBodies++; numBodies++;
body->m_originalBody = orgBody; body->m_originalBody = orgBody;
@@ -186,23 +188,23 @@ int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,OdeSolverBody** bodies
body->m_I[i+4*j] = 0.f; body->m_I[i+4*j] = 0.f;
} }
} }
body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal()[0]; body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal().x();
body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal()[1]; body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y();
body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal()[2]; body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z();
body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal()[0]; body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x();
body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal()[1]; body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y();
body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal()[2]; body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z();
dQuaternion q; dQuaternion q;
q[1] = orgBody->getOrientation()[0]; q[1] = orgBody->getOrientation().x();
q[2] = orgBody->getOrientation()[1]; q[2] = orgBody->getOrientation().y();
q[3] = orgBody->getOrientation()[2]; q[3] = orgBody->getOrientation().z();
q[0] = orgBody->getOrientation()[3]; q[0] = orgBody->getOrientation().w();
dRfromQ1(body->m_R,q); dRfromQ1(body->m_R,q);

30
Extras/quickstep/OdeContactJoint.cpp
View File

@@ -96,9 +96,9 @@ void ContactJoint::GetInfo2(Info2 *info)
btManifoldPoint& point = m_manifold->getContactPoint(m_index); btManifoldPoint& point = m_manifold->getContactPoint(m_index);
normal[0] = swapFactor*point.m_normalWorldOnB[0]; normal[0] = swapFactor*point.m_normalWorldOnB.x();
normal[1] = swapFactor*point.m_normalWorldOnB[1]; normal[1] = swapFactor*point.m_normalWorldOnB.y();
normal[2] = swapFactor*point.m_normalWorldOnB[2]; normal[2] = swapFactor*point.m_normalWorldOnB.z();
normal[3] = 0; // @@@ hmmm normal[3] = 0; // @@@ hmmm
assert(m_body0); assert(m_body0);
@@ -107,9 +107,9 @@ void ContactJoint::GetInfo2(Info2 *info)
{ {
relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition; relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
dVector3 c1; dVector3 c1;
c1[0] = relativePositionA[0]; c1[0] = relativePositionA.x();
c1[1] = relativePositionA[1]; c1[1] = relativePositionA.y();
c1[2] = relativePositionA[2]; c1[2] = relativePositionA.z();
// set jacobian for normal // set jacobian for normal
info->J1l[0] = normal[0]; info->J1l[0] = normal[0];
@@ -131,9 +131,9 @@ void ContactJoint::GetInfo2(Info2 *info)
// for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - // for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
// j->node[1].body->pos[i]; // j->node[1].body->pos[i];
c2[0] = relativePositionB[0]; c2[0] = relativePositionB.x();
c2[1] = relativePositionB[1]; c2[1] = relativePositionB.y();
c2[2] = relativePositionB[2]; c2[2] = relativePositionB.z();
info->J2l[0] = -normal[0]; info->J2l[0] = -normal[0];
info->J2l[1] = -normal[1]; info->J2l[1] = -normal[1];
@@ -176,14 +176,14 @@ void ContactJoint::GetInfo2(Info2 *info)
dVector3 t1,t2; // two vectors tangential to normal dVector3 t1,t2; // two vectors tangential to normal
dVector3 c1; dVector3 c1;
c1[0] = relativePositionA[0]; c1[0] = relativePositionA.x();
c1[1] = relativePositionA[1]; c1[1] = relativePositionA.y();
c1[2] = relativePositionA[2]; c1[2] = relativePositionA.z();
dVector3 c2; dVector3 c2;
c2[0] = relativePositionB[0]; c2[0] = relativePositionB.x();
c2[1] = relativePositionB[1]; c2[1] = relativePositionB.y();
c2[2] = relativePositionB[2]; c2[2] = relativePositionB.z();
//combined friction is available in the contact point //combined friction is available in the contact point
float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction; float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction;

22
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -122,12 +122,9 @@ class btPersistentManifoldSortPredicate
{ {
public: public:
bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
{ {
int rIslandId0,lIslandId0; return getIslandId(lhs) < getIslandId(rhs);
rIslandId0 = getIslandId(rhs);
lIslandId0 = getIslandId(lhs);
return lIslandId0 < rIslandId0;
} }
}; };
@@ -142,8 +139,17 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
{ {
BEGIN_PROFILE("islandUnionFindAndHeapSort");
if (0)
{
int maxNumManifolds = dispatcher->getNumManifolds();
btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher;
btPersistentManifold** manifold = colDis->getInternalManifoldPointer();
callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0);
return;
}
BEGIN_PROFILE("islandUnionFindAndHeapSort");
//we are going to sort the unionfind array, and store the element id in the size //we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore //afterwards, we clean unionfind, to make sure no-one uses it anymore
@@ -152,9 +158,11 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
int numElem = getUnionFind().getNumElements(); int numElem = getUnionFind().getNumElements();
int endIslandIndex=1; int endIslandIndex=1;
int startIslandIndex;
//update the sleeping state for bodies, if all are sleeping //update the sleeping state for bodies, if all are sleeping
for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{ {
int islandId = getUnionFind().getElement(startIslandIndex).m_id; int islandId = getUnionFind().getElement(startIslandIndex).m_id;
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)

8
src/BulletCollision/CollisionDispatch/btUnionFind.h
View File

@@ -18,6 +18,8 @@ subject to the following restrictions:
#include "../../LinearMath/btAlignedObjectArray.h" #include "../../LinearMath/btAlignedObjectArray.h"
#define USE_PATH_COMPRESSION 1
struct btElement struct btElement
{ {
int m_id; int m_id;
@@ -79,6 +81,7 @@ class btUnionFind
if (i == j) if (i == j)
return; return;
#ifndef USE_PATH_COMPRESSION
//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
if (m_elements[i].m_sz < m_elements[j].m_sz) if (m_elements[i].m_sz < m_elements[j].m_sz)
{ {
@@ -88,6 +91,9 @@ class btUnionFind
{ {
m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz;
} }
#else
m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz;
#endif //USE_PATH_COMPRESSION
} }
int find(int x) int find(int x)
@@ -98,7 +104,7 @@ class btUnionFind
while (x != m_elements[x].m_id) while (x != m_elements[x].m_id)
{ {
//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
#define USE_PATH_COMPRESSION 1
#ifdef USE_PATH_COMPRESSION #ifdef USE_PATH_COMPRESSION
// //
m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;

7
src/BulletCollision/CollisionShapes/btBoxShape.cpp
View File

@@ -49,10 +49,9 @@ void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
btScalar ly=btScalar(2.)*(halfExtents.y()); btScalar ly=btScalar(2.)*(halfExtents.y());
btScalar lz=btScalar(2.)*(halfExtents.z()); btScalar lz=btScalar(2.)*(halfExtents.z());
inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); mass/(btScalar(12.0)) * (lx*lx + lz*lz),
inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); mass/(btScalar(12.0)) * (lx*lx + ly*ly));
} }

2
src/BulletCollision/CollisionShapes/btSphereShape.cpp
View File

@@ -68,7 +68,7 @@ void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& a
void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
{ {
btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin(); btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin();
inertia[0] = inertia[1] = inertia[2] = elem; inertia.setValue(elem,elem,elem);
} }

31
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -52,18 +52,16 @@ struct btOrderIndex
static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
static unsigned long btSeed2 = 0; unsigned long btSequentialImpulseConstraintSolver::btRand2()
unsigned long btRand2()
{ {
btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff; m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
return btSeed2; return m_btSeed2;
} }
//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
int btRandInt2 (int n) int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
{ {
// seems good; xor-fold and modulus // seems good; xor-fold and modulus
const unsigned long un = n; const unsigned long un = n;
@@ -92,15 +90,7 @@ int btRandInt2 (int n)
int btRandIntWrong (int n)
{
btScalar a = btScalar(n) / btScalar(4294967296.0);
// printf("n = %d\n",n);
// printf("a = %f\n",a);
int res = (int) (btScalar(btRand2()) * a);
// printf("res=%d\n",res);
return res;
}
bool MyContactDestroyedCallback(void* userPersistentData) bool MyContactDestroyedCallback(void* userPersistentData)
{ {
@@ -115,7 +105,8 @@ bool MyContactDestroyedCallback(void* userPersistentData)
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY) //not using SOLVER_USE_WARMSTARTING :m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY), //not using SOLVER_USE_WARMSTARTING,
m_btSeed2(0)
{ {
gContactDestroyedCallback = &MyContactDestroyedCallback; gContactDestroyedCallback = &MyContactDestroyedCallback;
@@ -236,9 +227,9 @@ SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
{ {
btScalar combinedFriction = contactConstraint.m_friction; const btScalar combinedFriction = contactConstraint.m_friction;
btScalar limit = appliedNormalImpulse * combinedFriction; const btScalar limit = appliedNormalImpulse * combinedFriction;
if (appliedNormalImpulse>btScalar(0.)) if (appliedNormalImpulse>btScalar(0.))
//friction //friction
@@ -248,9 +239,9 @@ SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
{ {
btScalar rel_vel; btScalar rel_vel;
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
rel_vel = vel1Dotn-vel2Dotn; rel_vel = vel1Dotn-vel2Dotn;

16
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -39,6 +39,8 @@ protected:
//choose between several modes, different friction model etc. //choose between several modes, different friction model etc.
int m_solverMode; int m_solverMode;
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
public: public:
@@ -84,6 +86,20 @@ public:
{ {
return m_solverMode; return m_solverMode;
} }
unsigned long btRand2();
int btRandInt2 (int n);
void setRandSeed(unsigned long seed)
{
m_btSeed2 = seed;
}
unsigned long getRandSeed() const
{
return m_btSeed2;
}
}; };

6
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -254,9 +254,9 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
m_inverseMass = btScalar(1.0) / mass; m_inverseMass = btScalar(1.0) / mass;
} }
m_invInertiaLocal.setValue(inertia[0] != btScalar(0.0) ? btScalar(1.0) / inertia[0]: btScalar(0.0), m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
inertia[1] != btScalar(0.0) ? btScalar(1.0) / inertia[1]: btScalar(0.0), inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
inertia[2] != btScalar(0.0) ? btScalar(1.0) / inertia[2]: btScalar(0.0)); inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
} }

168
src/LinearMath/btMatrix3x3.h
View File

@@ -50,6 +50,8 @@ class btMatrix3x3 {
return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
} }
SIMD_FORCE_INLINE const btVector3& getRow(int i) const SIMD_FORCE_INLINE const btVector3& getRow(int i) const
{ {
return m_el[i]; return m_el[i];
@@ -73,30 +75,19 @@ class btMatrix3x3 {
void setFromOpenGLSubMatrix(const btScalar *m) void setFromOpenGLSubMatrix(const btScalar *m)
{ {
m_el[0][0] = (m[0]); m_el[0].setValue(m[0],m[4],m[8]);
m_el[1][0] = (m[1]); m_el[1].setValue(m[1],m[5],m[9]);
m_el[2][0] = (m[2]); m_el[2].setValue(m[2],m[6],m[10]);
m_el[0][1] = (m[4]);
m_el[1][1] = (m[5]);
m_el[2][1] = (m[6]);
m_el[0][2] = (m[8]);
m_el[1][2] = (m[9]);
m_el[2][2] = (m[10]);
} }
void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& yx, const btScalar& yy, const btScalar& yz,
const btScalar& zx, const btScalar& zy, const btScalar& zz) const btScalar& zx, const btScalar& zy, const btScalar& zz)
{ {
m_el[0][0] = btScalar(xx); m_el[0].setValue(xx,xy,xz);
m_el[0][1] = btScalar(xy); m_el[1].setValue(yx,yy,yz);
m_el[0][2] = btScalar(xz); m_el[2].setValue(zx,zy,zz);
m_el[1][0] = btScalar(yx);
m_el[1][1] = btScalar(yy);
m_el[1][2] = btScalar(yz);
m_el[2][0] = btScalar(zx);
m_el[2][1] = btScalar(zy);
m_el[2][2] = btScalar(zz);
} }
void setRotation(const btQuaternion& q) void setRotation(const btQuaternion& q)
@@ -104,10 +95,10 @@ class btMatrix3x3 {
btScalar d = q.length2(); btScalar d = q.length2();
btFullAssert(d != btScalar(0.0)); btFullAssert(d != btScalar(0.0));
btScalar s = btScalar(2.0) / d; btScalar s = btScalar(2.0) / d;
btScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s; btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
btScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs; btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
btScalar xx = q[0] * xs, xy = q[0] * ys, xz = q[0] * zs; btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
btScalar yy = q[1] * ys, yz = q[1] * zs, zz = q[2] * zs; btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, btScalar(1.0) - (xx + zz), yz - wx, xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
@@ -168,90 +159,92 @@ class btMatrix3x3 {
void getOpenGLSubMatrix(btScalar *m) const void getOpenGLSubMatrix(btScalar *m) const
{ {
m[0] = btScalar(m_el[0][0]); m[0] = btScalar(m_el[0].x());
m[1] = btScalar(m_el[1][0]); m[1] = btScalar(m_el[1].x());
m[2] = btScalar(m_el[2][0]); m[2] = btScalar(m_el[2].x());
m[3] = btScalar(0.0); m[3] = btScalar(0.0);
m[4] = btScalar(m_el[0][1]); m[4] = btScalar(m_el[0].y());
m[5] = btScalar(m_el[1][1]); m[5] = btScalar(m_el[1].y());
m[6] = btScalar(m_el[2][1]); m[6] = btScalar(m_el[2].y());
m[7] = btScalar(0.0); m[7] = btScalar(0.0);
m[8] = btScalar(m_el[0][2]); m[8] = btScalar(m_el[0].z());
m[9] = btScalar(m_el[1][2]); m[9] = btScalar(m_el[1].z());
m[10] = btScalar(m_el[2][2]); m[10] = btScalar(m_el[2].z());
m[11] = btScalar(0.0); m[11] = btScalar(0.0);
} }
void getRotation(btQuaternion& q) const void getRotation(btQuaternion& q) const
{ {
btScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2]; btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
float temp[4];
if (trace > btScalar(0.0)) if (trace > btScalar(0.0))
{ {
btScalar s = btSqrt(trace + btScalar(1.0)); btScalar s = btSqrt(trace + btScalar(1.0));
q[3] = s * btScalar(0.5); temp[3]=(s * btScalar(0.5));
s = btScalar(0.5) / s; s = btScalar(0.5) / s;
q[0] = (m_el[2][1] - m_el[1][2]) * s; temp[0]=((m_el[2].y() - m_el[1].z()) * s);
q[1] = (m_el[0][2] - m_el[2][0]) * s; temp[1]=((m_el[0].z() - m_el[2].x()) * s);
q[2] = (m_el[1][0] - m_el[0][1]) * s; temp[2]=((m_el[1].x() - m_el[0].y()) * s);
} }
else else
{ {
int i = m_el[0][0] < m_el[1][1] ? int i = m_el[0].x() < m_el[1].y() ?
(m_el[1][1] < m_el[2][2] ? 2 : 1) : (m_el[1].y() < m_el[2].z() ? 2 : 1) :
(m_el[0][0] < m_el[2][2] ? 2 : 0); (m_el[0].x() < m_el[2].z() ? 2 : 0);
int j = (i + 1) % 3; int j = (i + 1) % 3;
int k = (i + 2) % 3; int k = (i + 2) % 3;
btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
q[i] = s * btScalar(0.5); temp[i] = s * btScalar(0.5);
s = btScalar(0.5) / s; s = btScalar(0.5) / s;
q[3] = (m_el[k][j] - m_el[j][k]) * s; temp[3] = (m_el[k][j] - m_el[j][k]) * s;
q[j] = (m_el[j][i] + m_el[i][j]) * s; temp[j] = (m_el[j][i] + m_el[i][j]) * s;
q[k] = (m_el[k][i] + m_el[i][k]) * s; temp[k] = (m_el[k][i] + m_el[i][k]) * s;
} }
q.setValue(temp[0],temp[1],temp[2],temp[3]);
} }
void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const
{ {
pitch = btScalar(btAsin(-m_el[2][0])); pitch = btScalar(btAsin(-m_el[2].x()));
if (pitch < SIMD_2_PI) if (pitch < SIMD_2_PI)
{ {
if (pitch > SIMD_2_PI) if (pitch > SIMD_2_PI)
{ {
yaw = btScalar(btAtan2(m_el[1][0], m_el[0][0])); yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
roll = btScalar(btAtan2(m_el[2][1], m_el[2][2])); roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
} }
else else
{ {
yaw = btScalar(-btAtan2(-m_el[0][1], m_el[0][2])); yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z()));
roll = btScalar(0.0); roll = btScalar(0.0);
} }
} }
else else
{ {
yaw = btScalar(btAtan2(-m_el[0][1], m_el[0][2])); yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z()));
roll = btScalar(0.0); roll = btScalar(0.0);
} }
} }
btVector3 getScaling() const btVector3 getScaling() const
{ {
return btVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0], return btVector3(m_el[0][0] * m_el[0].x() + m_el[1].x() * m_el[1].x() + m_el[2].x() * m_el[2].x(),
m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1], m_el[0].y() * m_el[0].y() + m_el[1].y() * m_el[1].y() + m_el[2].y() * m_el[2].y(),
m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]); m_el[0].z() * m_el[0].z() + m_el[1].z() * m_el[1].z() + m_el[2].z() * m_el[2].z());
} }
btMatrix3x3 scaled(const btVector3& s) const btMatrix3x3 scaled(const btVector3& s) const
{ {
return btMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2], return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2], m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]); m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
} }
btScalar determinant() const; btScalar determinant() const;
@@ -263,10 +256,20 @@ class btMatrix3x3 {
btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
btScalar tdot(int c, const btVector3& v) const SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
{ {
return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2]; return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
} }
SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
{
return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
}
SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
{
return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
}
protected: protected:
btScalar cofac(int r1, int c1, int r2, int c2) const btScalar cofac(int r1, int c1, int r2, int c2) const
@@ -280,9 +283,9 @@ class btMatrix3x3 {
SIMD_FORCE_INLINE btMatrix3x3& SIMD_FORCE_INLINE btMatrix3x3&
btMatrix3x3::operator*=(const btMatrix3x3& m) btMatrix3x3::operator*=(const btMatrix3x3& m)
{ {
setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]), setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]), m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2])); m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
return *this; return *this;
} }
@@ -297,17 +300,17 @@ class btMatrix3x3 {
btMatrix3x3::absolute() const btMatrix3x3::absolute() const
{ {
return btMatrix3x3( return btMatrix3x3(
btFabs(m_el[0][0]), btFabs(m_el[0][1]), btFabs(m_el[0][2]), btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
btFabs(m_el[1][0]), btFabs(m_el[1][1]), btFabs(m_el[1][2]), btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
btFabs(m_el[2][0]), btFabs(m_el[2][1]), btFabs(m_el[2][2])); btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
} }
SIMD_FORCE_INLINE btMatrix3x3 SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::transpose() const btMatrix3x3::transpose() const
{ {
return btMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0], return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
m_el[0][1], m_el[1][1], m_el[2][1], m_el[0].y(), m_el[1].y(), m_el[2].y(),
m_el[0][2], m_el[1][2], m_el[2][2]); m_el[0].z(), m_el[1].z(), m_el[2].z());
} }
SIMD_FORCE_INLINE btMatrix3x3 SIMD_FORCE_INLINE btMatrix3x3
@@ -325,24 +328,24 @@ class btMatrix3x3 {
btScalar det = (*this)[0].dot(co); btScalar det = (*this)[0].dot(co);
btFullAssert(det != btScalar(0.0)); btFullAssert(det != btScalar(0.0));
btScalar s = btScalar(1.0) / det; btScalar s = btScalar(1.0) / det;
return btMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
} }
SIMD_FORCE_INLINE btMatrix3x3 SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::transposeTimes(const btMatrix3x3& m) const btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
{ {
return btMatrix3x3( return btMatrix3x3(
m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0], m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1], m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2], m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0], m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1], m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2], m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0], m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1], m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]); m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].x());
} }
SIMD_FORCE_INLINE btMatrix3x3 SIMD_FORCE_INLINE btMatrix3x3
@@ -365,19 +368,19 @@ class btMatrix3x3 {
SIMD_FORCE_INLINE btVector3 SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v, const btMatrix3x3& m) operator*(const btVector3& v, const btMatrix3x3& m)
{ {
return btVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v)); return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
} }
SIMD_FORCE_INLINE btMatrix3x3 SIMD_FORCE_INLINE btMatrix3x3
operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
{ {
return btMatrix3x3( return btMatrix3x3(
m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2])); m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
} }
/*
SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
return btMatrix3x3( return btMatrix3x3(
m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
@@ -390,6 +393,7 @@ class btMatrix3x3 {
m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
} }
*/
#endif #endif

16
src/LinearMath/btQuadWord.h
View File

@@ -31,8 +31,8 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord)
public: public:
SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } // SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; }
SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } // SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; }
SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; } SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; }
@@ -46,15 +46,19 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord)
SIMD_FORCE_INLINE void setZ(btScalar z) { m_z = z;}; SIMD_FORCE_INLINE void setZ(btScalar z) { m_z = z;};
SIMD_FORCE_INLINE void setW(btScalar w) { m_unusedW = w;};
SIMD_FORCE_INLINE const btScalar& x() const { return m_x; } SIMD_FORCE_INLINE const btScalar& x() const { return m_x; }
SIMD_FORCE_INLINE const btScalar& y() const { return m_y; } SIMD_FORCE_INLINE const btScalar& y() const { return m_y; }
SIMD_FORCE_INLINE const btScalar& z() const { return m_z; } SIMD_FORCE_INLINE const btScalar& z() const { return m_z; }
SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; }
operator btScalar *() { return &m_x; }
operator const btScalar *() const { return &m_x; } SIMD_FORCE_INLINE operator btScalar *() { return &m_x; }
SIMD_FORCE_INLINE operator const btScalar *() const { return &m_x; }
SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
{ {
@@ -78,8 +82,8 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord)
m_unusedW=w; m_unusedW=w;
} }
SIMD_FORCE_INLINE btQuadWord() : SIMD_FORCE_INLINE btQuadWord()
m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.)) // :m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.))
{ {
} }

44
src/LinearMath/btQuaternion.h
View File

@@ -68,13 +68,13 @@ public:
btQuaternion& operator+=(const btQuaternion& q) btQuaternion& operator+=(const btQuaternion& q)
{ {
m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3]; m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW;
return *this; return *this;
} }
btQuaternion& operator-=(const btQuaternion& q) btQuaternion& operator-=(const btQuaternion& q)
{ {
m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3]; m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;
return *this; return *this;
} }
@@ -87,16 +87,16 @@ public:
btQuaternion& operator*=(const btQuaternion& q) btQuaternion& operator*=(const btQuaternion& q)
{ {
setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(), setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(),
m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(), m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(),
m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(), m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(),
m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z()); m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z());
return *this; return *this;
} }
btScalar dot(const btQuaternion& q) const btScalar dot(const btQuaternion& q) const
{ {
return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3]; return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW;
} }
btScalar length2() const btScalar length2() const
@@ -165,20 +165,20 @@ public:
operator+(const btQuaternion& q2) const operator+(const btQuaternion& q2) const
{ {
const btQuaternion& q1 = *this; const btQuaternion& q1 = *this;
return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]); return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW);
} }
SIMD_FORCE_INLINE btQuaternion SIMD_FORCE_INLINE btQuaternion
operator-(const btQuaternion& q2) const operator-(const btQuaternion& q2) const
{ {
const btQuaternion& q1 = *this; const btQuaternion& q1 = *this;
return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]); return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW);
} }
SIMD_FORCE_INLINE btQuaternion operator-() const SIMD_FORCE_INLINE btQuaternion operator-() const
{ {
const btQuaternion& q2 = *this; const btQuaternion& q2 = *this;
return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2[3]); return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_unusedW);
} }
SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
@@ -202,7 +202,7 @@ public:
return btQuaternion((m_x * s0 + q.x() * s1) * d, return btQuaternion((m_x * s0 + q.x() * s1) * d,
(m_y * s0 + q.y() * s1) * d, (m_y * s0 + q.y() * s1) * d,
(m_z * s0 + q.z() * s1) * d, (m_z * s0 + q.z() * s1) * d,
(m_unusedW * s0 + q[3] * s1) * d); (m_unusedW * s0 + q.m_unusedW * s1) * d);
} }
else else
{ {
@@ -219,7 +219,7 @@ public:
SIMD_FORCE_INLINE btQuaternion SIMD_FORCE_INLINE btQuaternion
operator-(const btQuaternion& q) operator-(const btQuaternion& q)
{ {
return btQuaternion(-q.x(), -q.y(), -q.z(), -q[3]); return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w());
} }
@@ -227,27 +227,27 @@ operator-(const btQuaternion& q)
SIMD_FORCE_INLINE btQuaternion SIMD_FORCE_INLINE btQuaternion
operator*(const btQuaternion& q1, const btQuaternion& q2) { operator*(const btQuaternion& q1, const btQuaternion& q2) {
return btQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(), return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(), q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(), q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
} }
SIMD_FORCE_INLINE btQuaternion SIMD_FORCE_INLINE btQuaternion
operator*(const btQuaternion& q, const btVector3& w) operator*(const btQuaternion& q, const btVector3& w)
{ {
return btQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(), return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
q[3] * w.y() + q.z() * w.x() - q.x() * w.z(), q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
q[3] * w.z() + q.x() * w.y() - q.y() * w.x(), q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
} }
SIMD_FORCE_INLINE btQuaternion SIMD_FORCE_INLINE btQuaternion
operator*(const btVector3& w, const btQuaternion& q) operator*(const btVector3& w, const btQuaternion& q)
{ {
return btQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(), return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
w.y() * q[3] + w.z() * q.x() - w.x() * q.z(), w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
w.z() * q[3] + w.x() * q.y() - w.y() * q.x(), w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
} }

20
src/LinearMath/btTransform.h
View File

@@ -48,17 +48,19 @@ public:
m_origin = t1(t2.m_origin); m_origin = t1(t2.m_origin);
} }
void multInverseLeft(const btTransform& t1, const btTransform& t2) { /* void multInverseLeft(const btTransform& t1, const btTransform& t2) {
btVector3 v = t2.m_origin - t1.m_origin; btVector3 v = t2.m_origin - t1.m_origin;
m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis); m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v * t1.m_basis; m_origin = v * t1.m_basis;
} }
*/
SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
{ {
return btVector3(m_basis[0].dot(x) + m_origin[0], return btVector3(m_basis[0].dot(x) + m_origin.x(),
m_basis[1].dot(x) + m_origin[1], m_basis[1].dot(x) + m_origin.y(),
m_basis[2].dot(x) + m_origin[2]); m_basis[2].dot(x) + m_origin.z());
} }
SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
@@ -88,17 +90,15 @@ public:
void setFromOpenGLMatrix(const btScalar *m) void setFromOpenGLMatrix(const btScalar *m)
{ {
m_basis.setFromOpenGLSubMatrix(m); m_basis.setFromOpenGLSubMatrix(m);
m_origin[0] = m[12]; m_origin.setValue(m[12],m[13],m[14]);
m_origin[1] = m[13];
m_origin[2] = m[14];
} }
void getOpenGLMatrix(btScalar *m) const void getOpenGLMatrix(btScalar *m) const
{ {
m_basis.getOpenGLSubMatrix(m); m_basis.getOpenGLSubMatrix(m);
m[12] = m_origin[0]; m[12] = m_origin.x();
m[13] = m_origin[1]; m[13] = m_origin.y();
m[14] = m_origin[2]; m[14] = m_origin.z();
m[15] = btScalar(1.0); m[15] = btScalar(1.0);
} }

20
src/LinearMath/btTransformUtil.h
View File

@@ -35,29 +35,21 @@ inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& sup
inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
{ {
if (btFabs(n[2]) > SIMDSQRT12) { if (btFabs(n.z()) > SIMDSQRT12) {
// choose p in y-z plane // choose p in y-z plane
btScalar a = n[1]*n[1] + n[2]*n[2]; btScalar a = n[1]*n[1] + n[2]*n[2];
btScalar k = btRecipSqrt (a); btScalar k = btRecipSqrt (a);
p[0] = 0; p.setValue(0,-n[2]*k,n[1]*k);
p[1] = -n[2]*k;
p[2] = n[1]*k;
// set q = n x p // set q = n x p
q[0] = a*k; q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
q[1] = -n[0]*p[2];
q[2] = n[0]*p[1];
} }
else { else {
// choose p in x-y plane // choose p in x-y plane
btScalar a = n[0]*n[0] + n[1]*n[1]; btScalar a = n.x()*n.x() + n.y()*n.y();
btScalar k = btRecipSqrt (a); btScalar k = btRecipSqrt (a);
p[0] = -n[1]*k; p.setValue(-n.y()*k,n.x()*k,0);
p[1] = n[0]*k;
p[2] = 0;
// set q = n x p // set q = n x p
q[0] = -n[2]*p[1]; q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
q[1] = n[2]*p[0];
q[2] = a*k;
} }
} }

8
src/LinearMath/btVector3.h
View File

@@ -149,9 +149,9 @@ public:
SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
{ {
btScalar s = btScalar(1.0) - rt; btScalar s = btScalar(1.0) - rt;
m_x = s * v0[0] + rt * v1.x(); m_x = s * v0.x() + rt * v1.x();
m_y = s * v0[1] + rt * v1.y(); m_y = s * v0.y() + rt * v1.y();
m_z = s * v0[2] + rt * v1.z(); m_z = s * v0.z() + rt * v1.z();
//don't do the unused w component //don't do the unused w component
// m_co[3] = s * v0[3] + rt * v1[3]; // m_co[3] = s * v0[3] + rt * v1[3];
} }
@@ -271,7 +271,7 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2)
{ {
return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2]; return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
} }
SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const