mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-16 14:40:05 +00:00
Attempts to improve performance. Not much gain yet, but good to experiment what has effect and what hasn't.
Added 'DO_BENCHMARK_PYRAMID' to CcdPhysicsDemo.
This commit is contained in:
ejcoumans
parent
f8fe7e8f2d
commit
c1a54d9edc
@ -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.
|
||||
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 PRINT_CONTACT_STATISTICS 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 USE_CUSTOM_NEAR_CALLBACK 1
|
||||
//#define USE_CUSTOM_NEAR_CALLBACK 1
|
||||
//#define CENTER_OF_MASS_SHIFT 1
|
||||
|
||||
//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 "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"
|
||||
|
||||
#ifdef COMPARE_WITH_QUICKSTEP
|
||||
@ -58,7 +67,11 @@ const int maxProxies = 32766;
|
||||
const int maxOverlap = 65535;
|
||||
|
||||
bool createConstraint = true;//false;
|
||||
#ifdef CENTER_OF_MASS_SHIFT
|
||||
bool useCompound = true;
|
||||
#else
|
||||
bool useCompound = false;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@ -74,9 +87,9 @@ const int maxNumObjects = 32760;
|
||||
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),
|
||||
// btPoint3(50,0,0));
|
||||
static const int numShapes = 4;
|
||||
@ -90,14 +103,18 @@ btCollisionShape* shapePtr[numShapes] =
|
||||
#ifdef USE_GROUND_PLANE
|
||||
new btStaticPlaneShape(btVector3(0,1,0),10),
|
||||
#else
|
||||
new btBoxShape (btVector3(50,10,50)),
|
||||
new btBoxShape (btVector3(200,CUBE_HALF_EXTENTS,200)),
|
||||
#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)),
|
||||
#endif
|
||||
|
||||
//new btSphereShape (CUBE_HALF_EXTENTS),
|
||||
//new btCapsuleShape(0.5*CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-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 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();
|
||||
|
||||
#ifdef DO_BENCHMARK_PYRAMIDS
|
||||
ccdDemo->setCameraDistance(26.f);
|
||||
#endif
|
||||
|
||||
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;
|
||||
m_clock.reset();
|
||||
printf("dt = %f: ",dt);
|
||||
// printf("dt = %f: ",dt);
|
||||
|
||||
if (m_dynamicsWorld)
|
||||
{
|
||||
|
||||
//swap solver
|
||||
#ifdef COMPARE_WITH_QUICKSTEP
|
||||
if (m_debugMode & btIDebugDraw::DBG_DisableBulletLCP)
|
||||
{
|
||||
m_dynamicsWorld->setConstraintSolver( new OdeConstraintSolver());
|
||||
} else
|
||||
{
|
||||
m_dynamicsWorld->setConstraintSolver( new btSequentialImpulseConstraintSolver());
|
||||
}
|
||||
#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
|
||||
int maxSimSubSteps = m_idle ? 1 : 1;
|
||||
if (m_idle)
|
||||
dt = 1.0/420.f;
|
||||
|
||||
int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
|
||||
/*
|
||||
if (!numSimSteps)
|
||||
printf("Interpolated transforms\n");
|
||||
else
|
||||
@ -242,6 +285,9 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
|
||||
printf("Simulated (%i) steps\n",numSimSteps);
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_QUICKPROF
|
||||
@ -336,6 +382,9 @@ float myFrictionModel( btRigidBody& body1, btRigidBody& body2, btManifoldPoint&
|
||||
|
||||
void CcdPhysicsDemo::initPhysics()
|
||||
{
|
||||
#ifdef DO_BENCHMARK_PYRAMIDS
|
||||
m_azi = 90.f;
|
||||
#endif //DO_BENCHMARK_PYRAMIDS
|
||||
|
||||
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
|
||||
|
||||
@ -344,8 +393,8 @@ void CcdPhysicsDemo::initPhysics()
|
||||
dispatcher->setNearCallback(customNearCallback);
|
||||
#endif
|
||||
|
||||
btVector3 worldAabbMin(-10000,-10000,-10000);
|
||||
btVector3 worldAabbMax(10000,10000,10000);
|
||||
btVector3 worldAabbMin(-1000,-1000,-1000);
|
||||
btVector3 worldAabbMax(1000,1000,1000);
|
||||
|
||||
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
|
||||
// btOverlappingPairCache* broadphase = new btSimpleBroadphase;
|
||||
@ -353,7 +402,9 @@ void CcdPhysicsDemo::initPhysics()
|
||||
#ifdef REGISTER_CUSTOM_COLLISION_ALGORITHM
|
||||
dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
|
||||
//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);
|
||||
btSphereTriangleCollisionAlgorithm::CreateFunc* triangleSphereCF = new btSphereTriangleCollisionAlgorithm::CreateFunc;
|
||||
triangleSphereCF->m_swapped = true;
|
||||
@ -370,6 +421,10 @@ void CcdPhysicsDemo::initPhysics()
|
||||
|
||||
#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->setGravity(btVector3(0,-10,0));
|
||||
@ -425,6 +480,8 @@ void CcdPhysicsDemo::initPhysics()
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef DO_WALL
|
||||
|
||||
for (i=0;i<gNumObjects;i++)
|
||||
{
|
||||
btCollisionShape* shape = shapePtr[shapeIndex[i]];
|
||||
@ -456,7 +513,7 @@ void CcdPhysicsDemo::initPhysics()
|
||||
trans.setOrigin(pos);
|
||||
} else
|
||||
{
|
||||
trans.setOrigin(btVector3(0,-30,0));
|
||||
trans.setOrigin(btVector3(0,EXTRA_HEIGHT-CUBE_HALF_EXTENTS,0));
|
||||
}
|
||||
|
||||
float mass = 1.f;
|
||||
@ -486,9 +543,45 @@ void CcdPhysicsDemo::initPhysics()
|
||||
#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();
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -28,6 +28,7 @@ class CcdPhysicsDemo : public DemoApplication
|
||||
|
||||
virtual void displayCallback();
|
||||
|
||||
void createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos );
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -55,6 +55,8 @@ void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btColl
|
||||
/// report a contact. internally this will be kept persistent, and contact reduction is done
|
||||
resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
|
||||
m_manifoldPtr->clearManifold();
|
||||
|
||||
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
|
||||
input.m_maximumDistanceSquared = 1e30f;
|
||||
input.m_transformA = body0->getWorldTransform();
|
||||
|
||||
@ -268,9 +268,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
|
||||
int i,j,invert_normal,code;
|
||||
|
||||
// get vector from centers of box 1 to box 2, relative to box 1
|
||||
p[0] = p2[0] - p1[0];
|
||||
p[1] = p2[1] - p1[1];
|
||||
p[2] = p2[2] - p1[2];
|
||||
p = p2 - p1;
|
||||
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
|
||||
|
||||
// get side lengths / 2
|
||||
@ -646,20 +644,26 @@ void BoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& out
|
||||
dMatrix3 R1;
|
||||
dMatrix3 R2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
for (int j=0;j<3;j++)
|
||||
{
|
||||
R1[i+4*j] = transformA.getBasis()[j][i];
|
||||
R2[i+4*j] = transformB.getBasis()[j][i];
|
||||
}
|
||||
R1[0+4*j] = transformA.getBasis()[j].x();
|
||||
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;
|
||||
btScalar depth;
|
||||
int return_code;
|
||||
int maxc = 10;
|
||||
int maxc = 4;
|
||||
|
||||
|
||||
dBoxBox2 (transformA.getOrigin(),
|
||||
|
||||
@ -155,15 +155,17 @@ int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,OdeSolverBody** bodies
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (orgBody->getCompanionId()>=0)
|
||||
{
|
||||
return orgBody->getCompanionId();
|
||||
}
|
||||
//first try to find
|
||||
int i,j;
|
||||
for (i=0;i<numBodies;i++)
|
||||
{
|
||||
if (bodies[i]->m_originalBody == orgBody)
|
||||
return i;
|
||||
}
|
||||
|
||||
//if not found, create a new body
|
||||
OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
|
||||
orgBody->setCompanionId(numBodies);
|
||||
|
||||
numBodies++;
|
||||
|
||||
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_invI[0+4*0] = orgBody->getInvInertiaDiagLocal()[0];
|
||||
body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal()[1];
|
||||
body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal()[2];
|
||||
body->m_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()[0];
|
||||
body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal()[1];
|
||||
body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal()[2];
|
||||
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()[0];
|
||||
q[2] = orgBody->getOrientation()[1];
|
||||
q[3] = orgBody->getOrientation()[2];
|
||||
q[0] = orgBody->getOrientation()[3];
|
||||
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);
|
||||
|
||||
|
||||
@ -96,9 +96,9 @@ void ContactJoint::GetInfo2(Info2 *info)
|
||||
|
||||
btManifoldPoint& point = m_manifold->getContactPoint(m_index);
|
||||
|
||||
normal[0] = swapFactor*point.m_normalWorldOnB[0];
|
||||
normal[1] = swapFactor*point.m_normalWorldOnB[1];
|
||||
normal[2] = swapFactor*point.m_normalWorldOnB[2];
|
||||
normal[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);
|
||||
@ -107,9 +107,9 @@ void ContactJoint::GetInfo2(Info2 *info)
|
||||
{
|
||||
relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
|
||||
dVector3 c1;
|
||||
c1[0] = relativePositionA[0];
|
||||
c1[1] = relativePositionA[1];
|
||||
c1[2] = relativePositionA[2];
|
||||
c1[0] = relativePositionA.x();
|
||||
c1[1] = relativePositionA.y();
|
||||
c1[2] = relativePositionA.z();
|
||||
|
||||
// set jacobian for normal
|
||||
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] -
|
||||
// j->node[1].body->pos[i];
|
||||
c2[0] = relativePositionB[0];
|
||||
c2[1] = relativePositionB[1];
|
||||
c2[2] = relativePositionB[2];
|
||||
c2[0] = relativePositionB.x();
|
||||
c2[1] = relativePositionB.y();
|
||||
c2[2] = relativePositionB.z();
|
||||
|
||||
info->J2l[0] = -normal[0];
|
||||
info->J2l[1] = -normal[1];
|
||||
@ -176,14 +176,14 @@ void ContactJoint::GetInfo2(Info2 *info)
|
||||
dVector3 t1,t2; // two vectors tangential to normal
|
||||
|
||||
dVector3 c1;
|
||||
c1[0] = relativePositionA[0];
|
||||
c1[1] = relativePositionA[1];
|
||||
c1[2] = relativePositionA[2];
|
||||
c1[0] = relativePositionA.x();
|
||||
c1[1] = relativePositionA.y();
|
||||
c1[2] = relativePositionA.z();
|
||||
|
||||
dVector3 c2;
|
||||
c2[0] = relativePositionB[0];
|
||||
c2[1] = relativePositionB[1];
|
||||
c2[2] = relativePositionB[2];
|
||||
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;
|
||||
|
||||
@ -122,12 +122,9 @@ class btPersistentManifoldSortPredicate
|
||||
{
|
||||
public:
|
||||
|
||||
bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
|
||||
SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
|
||||
{
|
||||
int rIslandId0,lIslandId0;
|
||||
rIslandId0 = getIslandId(rhs);
|
||||
lIslandId0 = getIslandId(lhs);
|
||||
return lIslandId0 < rIslandId0;
|
||||
return getIslandId(lhs) < getIslandId(rhs);
|
||||
}
|
||||
};
|
||||
|
||||
@ -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
|
||||
//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 endIslandIndex=1;
|
||||
int startIslandIndex;
|
||||
|
||||
|
||||
//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;
|
||||
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
|
||||
|
||||
@ -18,6 +18,8 @@ subject to the following restrictions:
|
||||
|
||||
#include "../../LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
#define USE_PATH_COMPRESSION 1
|
||||
|
||||
struct btElement
|
||||
{
|
||||
int m_id;
|
||||
@ -79,6 +81,7 @@ class btUnionFind
|
||||
if (i == j)
|
||||
return;
|
||||
|
||||
#ifndef USE_PATH_COMPRESSION
|
||||
//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)
|
||||
{
|
||||
@ -88,6 +91,9 @@ class btUnionFind
|
||||
{
|
||||
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)
|
||||
@ -98,7 +104,7 @@ class btUnionFind
|
||||
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
|
||||
#define USE_PATH_COMPRESSION 1
|
||||
|
||||
#ifdef USE_PATH_COMPRESSION
|
||||
//
|
||||
m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;
|
||||
|
||||
@ -49,10 +49,9 @@ void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
btScalar ly=btScalar(2.)*(halfExtents.y());
|
||||
btScalar lz=btScalar(2.)*(halfExtents.z());
|
||||
|
||||
inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
|
||||
inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
|
||||
inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
|
||||
|
||||
inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
|
||||
mass/(btScalar(12.0)) * (lx*lx + lz*lz),
|
||||
mass/(btScalar(12.0)) * (lx*lx + ly*ly));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -68,7 +68,7 @@ void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& a
|
||||
void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
{
|
||||
btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin();
|
||||
inertia[0] = inertia[1] = inertia[2] = elem;
|
||||
inertia.setValue(elem,elem,elem);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -52,18 +52,16 @@ struct btOrderIndex
|
||||
static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
|
||||
|
||||
|
||||
static unsigned long btSeed2 = 0;
|
||||
unsigned long btRand2()
|
||||
unsigned long btSequentialImpulseConstraintSolver::btRand2()
|
||||
{
|
||||
btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff;
|
||||
return btSeed2;
|
||||
m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
|
||||
return m_btSeed2;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//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
|
||||
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)
|
||||
{
|
||||
@ -115,7 +105,8 @@ bool MyContactDestroyedCallback(void* userPersistentData)
|
||||
|
||||
|
||||
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;
|
||||
|
||||
@ -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.))
|
||||
//friction
|
||||
@ -248,9 +239,9 @@ SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
|
||||
{
|
||||
|
||||
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);
|
||||
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);
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
|
||||
@ -39,6 +39,8 @@ protected:
|
||||
|
||||
//choose between several modes, different friction model etc.
|
||||
int m_solverMode;
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
public:
|
||||
|
||||
@ -84,6 +86,20 @@ public:
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -254,9 +254,9 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
|
||||
m_inverseMass = btScalar(1.0) / mass;
|
||||
}
|
||||
|
||||
m_invInertiaLocal.setValue(inertia[0] != btScalar(0.0) ? btScalar(1.0) / inertia[0]: btScalar(0.0),
|
||||
inertia[1] != btScalar(0.0) ? btScalar(1.0) / inertia[1]: btScalar(0.0),
|
||||
inertia[2] != btScalar(0.0) ? btScalar(1.0) / inertia[2]: btScalar(0.0));
|
||||
m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
|
||||
inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
|
||||
inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -50,6 +50,8 @@ class btMatrix3x3 {
|
||||
return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE const btVector3& getRow(int i) const
|
||||
{
|
||||
return m_el[i];
|
||||
@ -73,30 +75,19 @@ class btMatrix3x3 {
|
||||
|
||||
void setFromOpenGLSubMatrix(const btScalar *m)
|
||||
{
|
||||
m_el[0][0] = (m[0]);
|
||||
m_el[1][0] = (m[1]);
|
||||
m_el[2][0] = (m[2]);
|
||||
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]);
|
||||
m_el[0].setValue(m[0],m[4],m[8]);
|
||||
m_el[1].setValue(m[1],m[5],m[9]);
|
||||
m_el[2].setValue(m[2],m[6],m[10]);
|
||||
|
||||
}
|
||||
|
||||
void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
|
||||
const btScalar& yx, const btScalar& yy, const btScalar& yz,
|
||||
const btScalar& zx, const btScalar& zy, const btScalar& zz)
|
||||
{
|
||||
m_el[0][0] = btScalar(xx);
|
||||
m_el[0][1] = btScalar(xy);
|
||||
m_el[0][2] = btScalar(xz);
|
||||
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);
|
||||
m_el[0].setValue(xx,xy,xz);
|
||||
m_el[1].setValue(yx,yy,yz);
|
||||
m_el[2].setValue(zx,zy,zz);
|
||||
}
|
||||
|
||||
void setRotation(const btQuaternion& q)
|
||||
@ -104,10 +95,10 @@ class btMatrix3x3 {
|
||||
btScalar d = q.length2();
|
||||
btFullAssert(d != btScalar(0.0));
|
||||
btScalar s = btScalar(2.0) / d;
|
||||
btScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s;
|
||||
btScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs;
|
||||
btScalar xx = q[0] * xs, xy = q[0] * ys, xz = q[0] * zs;
|
||||
btScalar yy = q[1] * ys, yz = q[1] * zs, zz = q[2] * zs;
|
||||
btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
|
||||
btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
|
||||
btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
|
||||
btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
|
||||
setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
|
||||
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
|
||||
xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
|
||||
@ -168,90 +159,92 @@ class btMatrix3x3 {
|
||||
|
||||
void getOpenGLSubMatrix(btScalar *m) const
|
||||
{
|
||||
m[0] = btScalar(m_el[0][0]);
|
||||
m[1] = btScalar(m_el[1][0]);
|
||||
m[2] = btScalar(m_el[2][0]);
|
||||
m[0] = btScalar(m_el[0].x());
|
||||
m[1] = btScalar(m_el[1].x());
|
||||
m[2] = btScalar(m_el[2].x());
|
||||
m[3] = btScalar(0.0);
|
||||
m[4] = btScalar(m_el[0][1]);
|
||||
m[5] = btScalar(m_el[1][1]);
|
||||
m[6] = btScalar(m_el[2][1]);
|
||||
m[4] = btScalar(m_el[0].y());
|
||||
m[5] = btScalar(m_el[1].y());
|
||||
m[6] = btScalar(m_el[2].y());
|
||||
m[7] = btScalar(0.0);
|
||||
m[8] = btScalar(m_el[0][2]);
|
||||
m[9] = btScalar(m_el[1][2]);
|
||||
m[10] = btScalar(m_el[2][2]);
|
||||
m[8] = btScalar(m_el[0].z());
|
||||
m[9] = btScalar(m_el[1].z());
|
||||
m[10] = btScalar(m_el[2].z());
|
||||
m[11] = btScalar(0.0);
|
||||
}
|
||||
|
||||
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))
|
||||
{
|
||||
btScalar s = btSqrt(trace + btScalar(1.0));
|
||||
q[3] = s * btScalar(0.5);
|
||||
temp[3]=(s * btScalar(0.5));
|
||||
s = btScalar(0.5) / s;
|
||||
|
||||
q[0] = (m_el[2][1] - m_el[1][2]) * s;
|
||||
q[1] = (m_el[0][2] - m_el[2][0]) * s;
|
||||
q[2] = (m_el[1][0] - m_el[0][1]) * s;
|
||||
temp[0]=((m_el[2].y() - m_el[1].z()) * s);
|
||||
temp[1]=((m_el[0].z() - m_el[2].x()) * s);
|
||||
temp[2]=((m_el[1].x() - m_el[0].y()) * s);
|
||||
}
|
||||
else
|
||||
{
|
||||
int i = m_el[0][0] < m_el[1][1] ?
|
||||
(m_el[1][1] < m_el[2][2] ? 2 : 1) :
|
||||
(m_el[0][0] < m_el[2][2] ? 2 : 0);
|
||||
int i = m_el[0].x() < m_el[1].y() ?
|
||||
(m_el[1].y() < m_el[2].z() ? 2 : 1) :
|
||||
(m_el[0].x() < m_el[2].z() ? 2 : 0);
|
||||
int j = (i + 1) % 3;
|
||||
int k = (i + 2) % 3;
|
||||
|
||||
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;
|
||||
|
||||
q[3] = (m_el[k][j] - m_el[j][k]) * s;
|
||||
q[j] = (m_el[j][i] + m_el[i][j]) * s;
|
||||
q[k] = (m_el[k][i] + m_el[i][k]) * s;
|
||||
temp[3] = (m_el[k][j] - m_el[j][k]) * s;
|
||||
temp[j] = (m_el[j][i] + m_el[i][j]) * 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
|
||||
{
|
||||
pitch = btScalar(btAsin(-m_el[2][0]));
|
||||
pitch = btScalar(btAsin(-m_el[2].x()));
|
||||
if (pitch < SIMD_2_PI)
|
||||
{
|
||||
if (pitch > SIMD_2_PI)
|
||||
{
|
||||
yaw = btScalar(btAtan2(m_el[1][0], m_el[0][0]));
|
||||
roll = btScalar(btAtan2(m_el[2][1], m_el[2][2]));
|
||||
yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
|
||||
roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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],
|
||||
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][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]);
|
||||
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].y() * m_el[0].y() + m_el[1].y() * m_el[1].y() + m_el[2].y() * m_el[2].y(),
|
||||
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
|
||||
{
|
||||
return btMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2],
|
||||
m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2],
|
||||
m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][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].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
|
||||
m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
|
||||
}
|
||||
|
||||
btScalar determinant() const;
|
||||
@ -263,10 +256,20 @@ class btMatrix3x3 {
|
||||
btMatrix3x3 transposeTimes(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:
|
||||
btScalar cofac(int r1, int c1, int r2, int c2) const
|
||||
@ -280,9 +283,9 @@ class btMatrix3x3 {
|
||||
SIMD_FORCE_INLINE btMatrix3x3&
|
||||
btMatrix3x3::operator*=(const btMatrix3x3& m)
|
||||
{
|
||||
setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
|
||||
m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
|
||||
m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
|
||||
setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
|
||||
m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
|
||||
m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
|
||||
return *this;
|
||||
}
|
||||
|
||||
@ -297,17 +300,17 @@ class btMatrix3x3 {
|
||||
btMatrix3x3::absolute() const
|
||||
{
|
||||
return btMatrix3x3(
|
||||
btFabs(m_el[0][0]), btFabs(m_el[0][1]), btFabs(m_el[0][2]),
|
||||
btFabs(m_el[1][0]), btFabs(m_el[1][1]), btFabs(m_el[1][2]),
|
||||
btFabs(m_el[2][0]), btFabs(m_el[2][1]), btFabs(m_el[2][2]));
|
||||
btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
|
||||
btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
|
||||
btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3
|
||||
btMatrix3x3::transpose() const
|
||||
{
|
||||
return btMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
|
||||
m_el[0][1], m_el[1][1], m_el[2][1],
|
||||
m_el[0][2], m_el[1][2], m_el[2][2]);
|
||||
return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
|
||||
m_el[0].y(), m_el[1].y(), m_el[2].y(),
|
||||
m_el[0].z(), m_el[1].z(), m_el[2].z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3
|
||||
@ -325,24 +328,24 @@ class btMatrix3x3 {
|
||||
btScalar det = (*this)[0].dot(co);
|
||||
btFullAssert(det != btScalar(0.0));
|
||||
btScalar s = btScalar(1.0) / det;
|
||||
return btMatrix3x3(co[0] * 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[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
|
||||
return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
|
||||
co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
|
||||
co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3
|
||||
btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
|
||||
{
|
||||
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][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1],
|
||||
m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2],
|
||||
m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0],
|
||||
m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1],
|
||||
m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2],
|
||||
m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0],
|
||||
m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1],
|
||||
m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]);
|
||||
m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
|
||||
m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
|
||||
m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
|
||||
m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
|
||||
m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
|
||||
m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
|
||||
m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
|
||||
m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
|
||||
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
|
||||
@ -365,19 +368,19 @@ class btMatrix3x3 {
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
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
|
||||
operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
|
||||
{
|
||||
return btMatrix3x3(
|
||||
m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
|
||||
m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
|
||||
m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
|
||||
m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
|
||||
m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
|
||||
m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
|
||||
return btMatrix3x3(
|
||||
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][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -31,8 +31,8 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord)
|
||||
|
||||
public:
|
||||
|
||||
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 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& 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 setW(btScalar w) { m_unusedW = w;};
|
||||
|
||||
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& 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)
|
||||
{
|
||||
@ -78,8 +82,8 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord)
|
||||
m_unusedW=w;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuadWord() :
|
||||
m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.))
|
||||
SIMD_FORCE_INLINE btQuadWord()
|
||||
// :m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.))
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
@ -68,13 +68,13 @@ public:
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -87,16 +87,16 @@ public:
|
||||
|
||||
btQuaternion& operator*=(const btQuaternion& q)
|
||||
{
|
||||
setValue(m_unusedW * q.x() + m_x * q[3] + 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.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
|
||||
m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
|
||||
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.m_unusedW + m_z * q.x() - m_x * q.z(),
|
||||
m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(),
|
||||
m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z());
|
||||
return *this;
|
||||
}
|
||||
|
||||
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
|
||||
@ -165,20 +165,20 @@ public:
|
||||
operator+(const btQuaternion& q2) const
|
||||
{
|
||||
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 btQuaternion& q2) const
|
||||
{
|
||||
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
|
||||
{
|
||||
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
|
||||
@ -202,7 +202,7 @@ public:
|
||||
return btQuaternion((m_x * s0 + q.x() * s1) * d,
|
||||
(m_y * s0 + q.y() * 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
|
||||
{
|
||||
@ -219,7 +219,7 @@ public:
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
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
|
||||
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(),
|
||||
q1[3] * q2.y() + q1.y() * q2[3] + 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[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
|
||||
return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
|
||||
q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
|
||||
q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
|
||||
q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator*(const btQuaternion& q, const btVector3& w)
|
||||
{
|
||||
return btQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
|
||||
q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
|
||||
q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
|
||||
return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
|
||||
q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
|
||||
q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
|
||||
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator*(const btVector3& w, const btQuaternion& q)
|
||||
{
|
||||
return btQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
|
||||
w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
|
||||
w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
|
||||
return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
|
||||
w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
|
||||
w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
|
||||
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
|
||||
}
|
||||
|
||||
|
||||
@ -48,17 +48,19 @@ public:
|
||||
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;
|
||||
m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
|
||||
m_origin = v * t1.m_basis;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
|
||||
{
|
||||
return btVector3(m_basis[0].dot(x) + m_origin[0],
|
||||
m_basis[1].dot(x) + m_origin[1],
|
||||
m_basis[2].dot(x) + m_origin[2]);
|
||||
return btVector3(m_basis[0].dot(x) + m_origin.x(),
|
||||
m_basis[1].dot(x) + m_origin.y(),
|
||||
m_basis[2].dot(x) + m_origin.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
|
||||
@ -88,17 +90,15 @@ public:
|
||||
void setFromOpenGLMatrix(const btScalar *m)
|
||||
{
|
||||
m_basis.setFromOpenGLSubMatrix(m);
|
||||
m_origin[0] = m[12];
|
||||
m_origin[1] = m[13];
|
||||
m_origin[2] = m[14];
|
||||
m_origin.setValue(m[12],m[13],m[14]);
|
||||
}
|
||||
|
||||
void getOpenGLMatrix(btScalar *m) const
|
||||
{
|
||||
m_basis.getOpenGLSubMatrix(m);
|
||||
m[12] = m_origin[0];
|
||||
m[13] = m_origin[1];
|
||||
m[14] = m_origin[2];
|
||||
m[12] = m_origin.x();
|
||||
m[13] = m_origin.y();
|
||||
m[14] = m_origin.z();
|
||||
m[15] = btScalar(1.0);
|
||||
}
|
||||
|
||||
|
||||
@ -35,29 +35,21 @@ inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& sup
|
||||
|
||||
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
|
||||
btScalar a = n[1]*n[1] + n[2]*n[2];
|
||||
btScalar k = btRecipSqrt (a);
|
||||
p[0] = 0;
|
||||
p[1] = -n[2]*k;
|
||||
p[2] = n[1]*k;
|
||||
p.setValue(0,-n[2]*k,n[1]*k);
|
||||
// set q = n x p
|
||||
q[0] = a*k;
|
||||
q[1] = -n[0]*p[2];
|
||||
q[2] = n[0]*p[1];
|
||||
q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
|
||||
}
|
||||
else {
|
||||
// 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);
|
||||
p[0] = -n[1]*k;
|
||||
p[1] = n[0]*k;
|
||||
p[2] = 0;
|
||||
p.setValue(-n.y()*k,n.x()*k,0);
|
||||
// set q = n x p
|
||||
q[0] = -n[2]*p[1];
|
||||
q[1] = n[2]*p[0];
|
||||
q[2] = a*k;
|
||||
q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -149,9 +149,9 @@ public:
|
||||
SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
|
||||
{
|
||||
btScalar s = btScalar(1.0) - rt;
|
||||
m_x = s * v0[0] + rt * v1.x();
|
||||
m_y = s * v0[1] + rt * v1.y();
|
||||
m_z = s * v0[2] + rt * v1.z();
|
||||
m_x = s * v0.x() + rt * v1.x();
|
||||
m_y = s * v0.y() + rt * v1.y();
|
||||
m_z = s * v0.z() + rt * v1.z();
|
||||
//don't do the unused w component
|
||||
// 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)
|
||||
{
|
||||
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user