From 811c105c2473a42f37a94d07710c4358e6a595f0 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Tue, 25 Sep 2007 06:58:53 +0000 Subject: [PATCH] align btQuadWord on 16byte boundary slightly improved friction model --- .../SpuCollisionTaskProcess.h | 2 +- .../SpuLocalSupport.h | 2 +- Extras/quickstep/OdeTypedJoint.cpp | 2 +- Extras/quickstep/OdeTypedJoint.h | 2 +- .../ConstraintSolver/btContactSolverInfo.h | 2 +- .../btGeneric6DofConstraint.cpp | 2 +- .../btGeneric6DofConstraint.h | 2 +- .../btSequentialImpulseConstraintSolver.cpp | 208 ++++++++++-------- .../btSequentialImpulseConstraintSolver.h | 3 + .../ConstraintSolver/btSolverBody.h | 149 +++++++------ src/LinearMath/btQuadWord.h | 2 +- src/LinearMath/btQuaternion.h | 2 +- src/LinearMath/btScalar.h | 21 +- 13 files changed, 224 insertions(+), 175 deletions(-) diff --git a/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.h b/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.h index ce07a8a42..7d6ae7b6d 100644 --- a/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.h +++ b/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.h @@ -23,7 +23,7 @@ subject to the following restrictions: #include "PlatformDefinitions.h" #include "LinearMath/btAlignedObjectArray.h" -#define DEBUG_SpuCollisionTaskProcess 1 +//#define DEBUG_SpuCollisionTaskProcess 1 #define CMD_GATHER_AND_PROCESS_PAIRLIST 1 diff --git a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h index a46e9eae7..02017e87a 100644 --- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h +++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h @@ -36,7 +36,7 @@ struct SpuConvexPolyhedronVertexData }; -inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3 localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) +inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) { switch (shapeType) { diff --git a/Extras/quickstep/OdeTypedJoint.cpp b/Extras/quickstep/OdeTypedJoint.cpp index ca45c91a4..88c704214 100644 --- a/Extras/quickstep/OdeTypedJoint.cpp +++ b/Extras/quickstep/OdeTypedJoint.cpp @@ -169,7 +169,7 @@ void OdeP2PJoint::GetInfo2(Info2 *info) int bt_get_limit_motor_info2( btRotationalLimitMotor * limot, btRigidBody * body0, btRigidBody * body1, - BU_Joint::Info2 *info, int row, btVector3 ax1, int rotational) + BU_Joint::Info2 *info, int row, btVector3& ax1, int rotational) { diff --git a/Extras/quickstep/OdeTypedJoint.h b/Extras/quickstep/OdeTypedJoint.h index ab74df0e6..0888a85ca 100644 --- a/Extras/quickstep/OdeTypedJoint.h +++ b/Extras/quickstep/OdeTypedJoint.h @@ -105,7 +105,7 @@ public: int bt_get_limit_motor_info2( btRotationalLimitMotor * limot, btRigidBody * body0, btRigidBody * body1, - BU_Joint::Info2 *info, int row, btVector3 ax1, int rotational); + BU_Joint::Info2 *info, int row, btVector3& ax1, int rotational); #endif //CONTACT_JOINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index ad2c40e21..e3479000f 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -40,7 +40,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_friction = btScalar(0.3); m_restitution = btScalar(0.); m_maxErrorReduction = btScalar(20.); - m_numIterations = 10; + m_numIterations = 4; m_erp = btScalar(0.4); m_sor = btScalar(1.3); } diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index d4ac85d58..96d48f9f7 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -112,7 +112,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3 axis,btScalar jacDiagABInv, + btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, btRigidBody * body0, btRigidBody * body1) { if (needApplyTorques()==false) return 0.0f; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 7922e47e8..e4683b91b 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -110,7 +110,7 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3 axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); }; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index c5f8fa654..a468bdf72 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +//#define COMPUTE_IMPULSE_DENOM 1 #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -336,9 +337,57 @@ btScalar resolveSingleFrictionCacheFriendly( btAlignedObjectArray tmpSolverBodyPool; btAlignedObjectArray tmpSolverConstraintPool; btAlignedObjectArray tmpSolverFrictionConstraintPool; +btAlignedObjectArray gOrderTmpConstraintPool; +btAlignedObjectArray gOrderFrictionConstraintPool; -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) + +void addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btRigidBody* rb0,btRigidBody* rb1, btScalar relaxation) +{ + + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = normalAxis; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + solverConstraint.m_penetration = 0.f; + { + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + } + +#ifdef COMPUTE_IMPULSE_DENOM + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); +#else + btVector3 vec; + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + btScalar denom0 = rb0->getInvMass() + normalAxis.dot(vec); + vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + btScalar denom1 = rb1->getInvMass() + normalAxis.dot(vec); + + +#endif //COMPUTE_IMPULSE_DENOM + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + +} + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { (void)stackAlloc; (void)debugDrawer; @@ -445,6 +494,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } } + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + for (int j=0;jgetNumContacts();j++) { @@ -458,11 +511,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); - btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); + rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); + rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); - btScalar relaxation = 1.f; + relaxation = 1.f; + btScalar rel_vel; + btVector3 vel; { btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand(); @@ -471,12 +526,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; - - + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; { - //can be optimized, the cross products are already calculated +#ifdef COMPUTE_IMPULSE_DENOM btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); +#else + btVector3 vec; + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + btScalar denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); + vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + btScalar denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); +#endif //COMPUTE_IMPULSE_DENOM + btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; } @@ -489,8 +554,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); @@ -516,83 +581,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio solverConstraint.m_appliedVelocityImpulse = 0.f; - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; - } - - //create 2 '1d axis' constraints for 2 tangential friction directions - - //re-calculate friction direction every frame, todo: check if this is really needed - btVector3 frictionTangential0a, frictionTangential1b; - - btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); - - { - btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); - solverConstraint.m_contactNormal = frictionTangential0a; - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; - solverConstraint.m_frictionIndex = frictionIndex; - - solverConstraint.m_friction = cp.m_combinedFriction; - - solverConstraint.m_appliedImpulse = btScalar(0.); - solverConstraint.m_appliedVelocityImpulse = 0.f; - - btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - - { - btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; - } - - } - - - { - - btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); - solverConstraint.m_contactNormal = frictionTangential1b; - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; - solverConstraint.m_frictionIndex = frictionIndex; - - solverConstraint.m_friction = cp.m_combinedFriction; - - solverConstraint.m_appliedImpulse = btScalar(0.); - solverConstraint.m_appliedVelocityImpulse = 0.f; - btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; + } + + + { + btVector3 lat_vel = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = lat_vel.length2(); + if (lat_rel_vel > SIMD_EPSILON)//0.0f) { - btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; - } - { - btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; - } + lat_vel /= btSqrt(lat_rel_vel); + + addFrictionConstraint(lat_vel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation); + btVector3 frictionDir2 = lat_vel.cross(cp.m_normalWorldOnB); + frictionDir2.normalize();//?? + addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation); + + } + } } } } @@ -613,8 +621,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } } - btAlignedObjectArray gOrderTmpConstraintPool; - btAlignedObjectArray gOrderFrictionConstraintPool; + int numConstraintPool = tmpSolverConstraintPool.size(); int numFrictionPool = tmpSolverFrictionConstraintPool.size(); @@ -638,14 +645,20 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio END_PROFILE("prepareConstraints"); + return 0.f; +} - BEGIN_PROFILE("solveConstraints"); +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + BEGIN_PROFILE("solveConstraintsIterations"); + int numConstraintPool = tmpSolverConstraintPool.size(); + int numFrictionPool = tmpSolverFrictionConstraintPool.size(); //should traverse the contacts random order... int iteration; { - for ( iteration = 0;iterationgetRigidBodyB().getCompanionId()].writebackVelocity(); } - constraint->solveConstraint(info.m_timeStep); + constraint->solveConstraint(infoGlobal.m_timeStep); if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) { @@ -701,7 +714,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio { btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info); + tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); } } @@ -713,7 +726,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse); + tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,appliedNormalImpulse); } } @@ -721,13 +734,28 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } } + + END_PROFILE("solveConstraintsIterations"); + + return 0.f; +} + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + int i; + + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + + BEGIN_PROFILE("solveWriteBackVelocity"); for ( i=0;isetLinearVelocity(m_linearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity); - } - } - - void readVelocity() - { - if (m_invMass) - { - m_linearVelocity = m_originalBody->getLinearVelocity(); - m_angularVelocity = m_originalBody->getAngularVelocity(); - } - } - - - - -}; - -#endif //BT_SOLVER_BODY_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOLVER_BODY_H +#define BT_SOLVER_BODY_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btAlignedAllocator.h" + + +///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. +ATTRIBUTE_ALIGNED16 (struct) btSolverBody +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btVector3 m_centerOfMassPosition; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btRigidBody* m_originalBody; + float m_invMass; + float m_friction; + float m_angularFactor; + + inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + m_linearVelocity += linearComponent*impulseMagnitude; + m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + + void writebackVelocity() + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity); + } + } + + void readVelocity() + { + if (m_invMass) + { + m_linearVelocity = m_originalBody->getLinearVelocity(); + m_angularVelocity = m_originalBody->getAngularVelocity(); + } + } + + + + +}; + +#endif //BT_SOLVER_BODY_H \ No newline at end of file diff --git a/src/LinearMath/btQuadWord.h b/src/LinearMath/btQuadWord.h index f7f5f27ba..2411591f1 100644 --- a/src/LinearMath/btQuadWord.h +++ b/src/LinearMath/btQuadWord.h @@ -19,7 +19,7 @@ subject to the following restrictions: #include "btScalar.h" #include "btSimdMinMax.h" -class btQuadWordStorage +ATTRIBUTE_ALIGNED16(class) btQuadWordStorage { protected: btScalar m_x; diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 785adb43f..27ab1fcd2 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -308,7 +308,7 @@ shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Ge } SIMD_FORCE_INLINE btQuaternion -shortestArcQuatNormalize(btVector3 v0,btVector3 v1) +shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) { v0.normalize(); v1.normalize(); diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index b61d2cbbd..0e8a517ae 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -154,7 +154,26 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } #else -SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrtf(x); } +SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) +{ +#ifdef USE_APPROXIMATION + double x, z, tempf; + unsigned long *tfptr = ((unsigned long *)&tempf) + 1; + + tempf = y; + *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ + x = tempf; + z = y*btScalar(0.5); /* hoist out the “/2” */ + x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + return x*y; +#else + return sqrtf(y); +#endif +} SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }