From c1a54d9edcd9201edb88131c0be34b7e521ed3a1 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Tue, 20 Mar 2007 20:12:23 +0000 Subject: [PATCH] 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. --- Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp | 133 +++++++++++--- Demos/CcdPhysicsDemo/CcdPhysicsDemo.h | 1 + .../BoxBoxCollisionAlgorithm.cpp | 2 + .../BoxBoxDetector.cpp | 24 +-- Extras/quickstep/OdeConstraintSolver.cpp | 32 ++-- Extras/quickstep/OdeContactJoint.cpp | 30 ++-- .../btSimulationIslandManager.cpp | 22 ++- .../CollisionDispatch/btUnionFind.h | 8 +- .../CollisionShapes/btBoxShape.cpp | 7 +- .../CollisionShapes/btSphereShape.cpp | 2 +- .../btSequentialImpulseConstraintSolver.cpp | 31 ++-- .../btSequentialImpulseConstraintSolver.h | 16 ++ src/BulletDynamics/Dynamics/btRigidBody.cpp | 6 +- src/LinearMath/btMatrix3x3.h | 168 +++++++++--------- src/LinearMath/btQuadWord.h | 16 +- src/LinearMath/btQuaternion.h | 44 ++--- src/LinearMath/btTransform.h | 20 +-- src/LinearMath/btTransformUtil.h | 20 +-- src/LinearMath/btVector3.h | 8 +- 19 files changed, 356 insertions(+), 234 deletions(-) diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index 9d1317e42..7aa56c33e 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -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; iinitPhysics(); +#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()); - } + m_dynamicsWorld->setConstraintSolver( new OdeConstraintSolver()); #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;isetLinearVelocity(btVector3(0,0,-10)); +#endif +#endif //DO_BENCHMARK_PYRAMIDS +// clientResetScene(); } diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.h b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.h index 82022af5e..24b373e0d 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.h +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.h @@ -28,6 +28,7 @@ class CcdPhysicsDemo : public DemoApplication virtual void displayCallback(); + void createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos ); }; diff --git a/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp b/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp index 90725eb15..10a63bf75 100644 --- a/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp +++ b/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp @@ -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(); diff --git a/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp b/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp index ae851693b..b519df395 100644 --- a/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp +++ b/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp @@ -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++) { - 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(), diff --git a/Extras/quickstep/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp index e7c2b4bbe..b91ddba08 100644 --- a/Extras/quickstep/OdeConstraintSolver.cpp +++ b/Extras/quickstep/OdeConstraintSolver.cpp @@ -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;im_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); diff --git a/Extras/quickstep/OdeContactJoint.cpp b/Extras/quickstep/OdeContactJoint.cpp index da25fe36d..89977cdaa 100644 --- a/Extras/quickstep/OdeContactJoint.cpp +++ b/Extras/quickstep/OdeContactJoint.cpp @@ -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; diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 5ded3c2c2..cfb3c445e 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -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;startIslandIndexbtScalar(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; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 2ac3b5804..13e70c41b 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -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; + } + }; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 8a3c7f58a..e7470f703 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -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)); } diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 44899640f..d59191207 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -49,6 +49,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 { @@ -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 diff --git a/src/LinearMath/btQuadWord.h b/src/LinearMath/btQuadWord.h index c8c0a713c..703d1c794 100644 --- a/src/LinearMath/btQuadWord.h +++ b/src/LinearMath/btQuadWord.h @@ -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.)) { } diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 607687c55..d5a7913b7 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -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()); } diff --git a/src/LinearMath/btTransform.h b/src/LinearMath/btTransform.h index 82d684e32..b1f3dfca4 100644 --- a/src/LinearMath/btTransform.h +++ b/src/LinearMath/btTransform.h @@ -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); } diff --git a/src/LinearMath/btTransformUtil.h b/src/LinearMath/btTransformUtil.h index d3b219c3a..bc42fd166 100644 --- a/src/LinearMath/btTransformUtil.h +++ b/src/LinearMath/btTransformUtil.h @@ -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); } } diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 4222e713f..d2bd707c6 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -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