diff options
author | Sergej Reich <sergej.reich@googlemail.com> | 2013-12-26 15:41:52 +0400 |
---|---|---|
committer | Sergej Reich <sergej.reich@googlemail.com> | 2013-12-26 15:45:57 +0400 |
commit | 05eebf49d3c3af47bf157207b82cf559ac1fe274 (patch) | |
tree | d1f82266237e929e6cc19aac3dd001a32ee7d052 /extern/bullet2 | |
parent | 709041ed0b7e1848068c9d53543ed114229a9f5b (diff) |
Bullet: Update to svn r2719
Fixes part of T37905, fixed constraint didn't work correctly.
Diffstat (limited to 'extern/bullet2')
18 files changed, 455 insertions, 300 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 093c6f9b200..d739a2a08d7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -770,7 +770,7 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, hitPointLocal, hitFraction); - bool normalInWorldSpace = false; + bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 991841ee20b..286e6c31f2f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -229,6 +229,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap removeChildAlgorithms(); preallocateChildAlgorithms(body0Wrap,body1Wrap); + m_compoundShapeRevision = compoundShape->getUpdateRevision(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 53675145637..7d792c18d34 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -36,6 +36,7 @@ extern btShapePairCallback gCompoundChildShapePairCallback; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { +protected: btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index a52dd34fe91..95780fb2d27 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -27,10 +27,8 @@ subject to the following restrictions: btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_sharedManifold(ci.m_manifold) +:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) { - m_ownsManifold = false; void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); @@ -292,12 +290,21 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); + const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); + const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); + if (!tree0 || !tree1) + { + return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); + } ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) { ///clear all removeChildAlgorithms(); + m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); + m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); + } @@ -329,8 +336,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb } - const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); - const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); + btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index 7e2d7ad7085..06a762f209d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -17,6 +17,8 @@ subject to the following restrictions: #ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H #define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#include "btCompoundCollisionAlgorithm.h" + #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" @@ -35,15 +37,12 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol extern btShapePairCallback gCompoundCompoundChildShapePairCallback; /// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes -class btCompoundCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm +class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm { class btHashedSimplePairCache* m_childCollisionAlgorithmCache; btSimplePairArray m_removePairs; - class btPersistentManifold* m_sharedManifold; - bool m_ownsManifold; - int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated int m_compoundShapeRevision1; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 1ba1cd1e839..890afe6da42 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -33,7 +33,8 @@ class btDispatcher; enum btConstraintSolverType { BT_SEQUENTIAL_IMPULSE_SOLVER=1, - BT_MLCP_SOLVER=2 + BT_MLCP_SOLVER=2, + BT_NNCG_SOLVER=4 }; class btConstraintSolver diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp index f93a3280f35..3428e0b069d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -23,9 +23,8 @@ subject to the following restrictions: btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) :btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) { - m_pivotInA = frameInA.getOrigin(); - m_pivotInB = frameInB.getOrigin(); - m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); + m_frameInA = frameInA; + m_frameInB = frameInB; } @@ -37,14 +36,16 @@ btFixedConstraint::~btFixedConstraint () void btFixedConstraint::getInfo1 (btConstraintInfo1* info) { info->m_numConstraintRows = 6; - info->nub = 6; + info->nub = 0; } void btFixedConstraint::getInfo2 (btConstraintInfo2* info) { //fix the 3 linear degrees of freedom - + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); @@ -55,15 +56,15 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info) info->m_J1linearAxis[info->rowskip+1] = 1; info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1 = worldOrnA*m_pivotInA; - { + btVector3 a1 = worldOrnA * m_frameInA.getOrigin(); + { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); btVector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } - + if (info->m_J2linearAxis) { info->m_J2linearAxis[0] = -1; @@ -71,10 +72,8 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info) info->m_J2linearAxis[2*info->rowskip+2] = -1; } - btVector3 a2 = worldOrnB*m_pivotInB; - - { - // btVector3 a2n = -a2; + btVector3 a2 = worldOrnB*m_frameInB.getOrigin(); + { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); @@ -88,42 +87,100 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info) int j; for (j=0; j<3; j++) { - - - info->m_constraintError[j*info->rowskip] = linearError[j]; //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } - //fix the 3 angular degrees of freedom - - int start_row = 3; - int s = info->rowskip; - int start_index = start_row * s; - - // 3 rows to make body rotations equal - info->m_J1angularAxis[start_index] = 1; - info->m_J1angularAxis[start_index + s + 1] = 1; - info->m_J1angularAxis[start_index + s*2+2] = 1; - if ( info->m_J2angularAxis) - { - info->m_J2angularAxis[start_index] = -1; - info->m_J2angularAxis[start_index + s+1] = -1; - info->m_J2angularAxis[start_index + s*2+2] = -1; - } - - // set right hand side for the angular dofs - - btVector3 diff; - btScalar angle; - btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse(); - btQuaternion qrelCur; - mrelCur.getRotation(qrelCur); - btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle); - diff*=-angle; - for (j=0; j<3; j++) - { - info->m_constraintError[(3+j)*info->rowskip] = k * diff[j]; - } - + btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0); + btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1); + btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2); + btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0); + btVector3 target; + btScalar x = ivB.dot(ivA); + btScalar y = ivB.dot(jvA); + btScalar z = ivB.dot(kvA); + btVector3 swingAxis(0,0,0); + { + if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) + { + swingAxis = -ivB.cross(ivA); + } + } + btVector3 vTwist(1,0,0); + + // compute rotation of A wrt B (in constraint space) + btQuaternion qA = transA.getRotation() * m_frameInA.getRotation(); + btQuaternion qB = transB.getRotation() * m_frameInB.getRotation(); + btQuaternion qAB = qB.inverse() * qA; + // split rotation into cone and twist + // (all this is done from B's perspective. Maybe I should be averaging axes...) + btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); + + int row = 3; + int srow = row * info->rowskip; + btVector3 ax1; + // angular limits + { + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + btTransform trA = transA*m_frameInA; + btVector3 twistAxis = trA.getBasis().getColumn(0); + + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + int srow1 = srow + info->rowskip; + J1[srow+0] = p[0]; + J1[srow+1] = p[1]; + J1[srow+2] = p[2]; + J1[srow1+0] = q[0]; + J1[srow1+1] = q[1]; + J1[srow1+2] = q[2]; + J2[srow+0] = -p[0]; + J2[srow+1] = -p[1]; + J2[srow+2] = -p[2]; + J2[srow1+0] = -q[0]; + J2[srow1+1] = -q[1]; + J2[srow1+2] = -q[2]; + btScalar fact = info->fps; + info->m_constraintError[srow] = fact * swingAxis.dot(p); + info->m_constraintError[srow1] = fact * swingAxis.dot(q); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->m_lowerLimit[srow1] = -SIMD_INFINITY; + info->m_upperLimit[srow1] = SIMD_INFINITY; + srow = srow1 + info->rowskip; + + { + btQuaternion qMinTwist = qABTwist; + btScalar twistAngle = qABTwist.getAngle(); + + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. + { + qMinTwist = -(qABTwist); + twistAngle = qMinTwist.getAngle(); + } + + if (twistAngle > SIMD_EPSILON) + { + twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); + twistAxis.normalize(); + twistAxis = quatRotate(qB, -twistAxis); + } + ax1 = twistAxis; + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + btScalar k = info->fps; + info->m_constraintError[srow] = k * twistAngle; + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + } }
\ No newline at end of file diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h index 697e319e2ce..e1c9430fd3c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -20,10 +20,9 @@ subject to the following restrictions: ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint { - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btQuaternion m_relTargetAB; + btTransform m_frameInA; + btTransform m_frameInB; public: btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index be93e35434c..f855581c712 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -58,13 +58,13 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) #endif//USE_SIMD // Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); @@ -86,13 +86,15 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + + return deltaImpulse; #else - resolveSingleConstraintRowGeneric(body1,body2,c); + return resolveSingleConstraintRowGeneric(body1,body2,c); #endif } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); @@ -120,15 +122,17 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + + return deltaImpulse; } - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); @@ -147,13 +151,14 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + return deltaImpulse; #else - resolveSingleConstraintRowLowerLimit(body1,body2,c); + return resolveSingleConstraintRowLowerLimit(body1,body2,c); #endif } -// Projected Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + +btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); @@ -173,6 +178,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( } body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + + return deltaImpulse; } @@ -430,6 +437,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btSimdScalar velocityError = desiredVelocity - rel_vel; btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; @@ -812,7 +820,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///avoid collision response between two static objects - if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) + if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero()))) return; int rollingFriction=1; @@ -1452,8 +1460,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration for (j=0;j<numPoolConstraints;j++) { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } @@ -1472,8 +1479,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 180d2a385d3..47dbbe3393d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -88,13 +88,10 @@ protected: int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep); - void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); protected: diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 8888d3961f5..0635b958bed 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -19,9 +19,11 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "btSolveProjectedGaussSeidel.h" + btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) :m_solver(solver), -m_fallback(0) +m_fallback(0), +m_cfm(0.000001)//0.0000001 { } @@ -160,13 +162,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) BT_PROFILE("init b (rhs)"); m_b.resize(numConstraintRows); m_bSplit.resize(numConstraintRows); - //m_b.setZero(); + m_b.setZero(); + m_bSplit.setZero(); for (int i=0;i<numConstraintRows ;i++) { - if (m_allConstraintArray[i].m_jacDiagABInv) + btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv; + if (!btFuzzyZero(jacDiag)) { - m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; - m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; + btScalar rhs = m_allConstraintArray[i].m_rhs; + btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration; + m_b[i]=rhs/jacDiag; + m_bSplit[i] = rhsPenetration/jacDiag; } } @@ -425,14 +431,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } } - ///todo: use proper cfm values from the constraints (getInfo2) if (1) { // add cfm to the diagonal of m_A for ( int i=0; i<m_A.rows(); ++i) { - float cfm = 0.00001f; - m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep); + m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep); } } @@ -473,6 +477,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) if (infoGlobal.m_splitImpulse) m_bSplit.resize(numConstraintRows); + m_bSplit.setZero(); + m_b.setZero(); + for (int i=0;i<numConstraintRows ;i++) { if (m_allConstraintArray[i].m_jacDiagABInv) @@ -554,12 +561,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) if (1) { - ///todo: use proper cfm values from the constraints (getInfo2) // add cfm to the diagonal of m_A for ( int i=0; i<m_A.rows(); ++i) { - float cfm = 0.0001f; - m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep); + m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep); } } @@ -618,6 +623,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod } else { + // printf("m_fallback = %d\n",m_fallback); m_fallback++; btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); } diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h index a5dae2d28f6..31e8eb88ba5 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h @@ -42,6 +42,7 @@ protected: btConstraintArray m_allConstraintArray; btMLCPSolverInterface* m_solver; int m_fallback; + btScalar m_cfm; virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); @@ -70,6 +71,15 @@ public: m_fallback = num; } + btScalar getCfm() const + { + return m_cfm; + } + void setCfm(btScalar cfm) + { + m_cfm = cfm; + } + virtual btConstraintSolverType getSolverType() const { return BT_MLCP_SOLVER; diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index 44fbfeadddc..d2ad54d21a8 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -20,11 +20,17 @@ subject to the following restrictions: #include "btMLCPSolverInterface.h" +///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix) class btSolveProjectedGaussSeidel : public btMLCPSolverInterface { public: virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) { + if (!A.rows()) + return true; + //the A matrix is sparse, so compute the non-zero elements + A.rowComputeNonZeroElements(); + //A is a m-n matrix, m rows, n columns btAssert(A.rows() == b.rows()); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index 9429d56e05a..2bfd62f2a2f 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -3027,7 +3027,8 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { const RContact& c = psb->m_rcontacts[i]; const sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) { + if (cti.m_colObj->hasContactResponse()) + { btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); const btVector3 vb = c.m_node->m_x-c.m_node->m_q; diff --git a/extern/bullet2/src/LinearMath/btMatrixX.h b/extern/bullet2/src/LinearMath/btMatrixX.h index 1c29632c536..865d77967a9 100644 --- a/extern/bullet2/src/LinearMath/btMatrixX.h +++ b/extern/bullet2/src/LinearMath/btMatrixX.h @@ -20,6 +20,12 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btAlignedObjectArray.h" +//#define BT_DEBUG_OSTREAM +#ifdef BT_DEBUG_OSTREAM +#include <iostream> +#include <iomanip> // std::setw +#endif //BT_DEBUG_OSTREAM + class btIntSortPredicate { public: @@ -30,6 +36,121 @@ class btIntSortPredicate }; +template <typename T> +struct btVectorX +{ + btAlignedObjectArray<T> m_storage; + + btVectorX() + { + } + btVectorX(int numRows) + { + m_storage.resize(numRows); + } + + void resize(int rows) + { + m_storage.resize(rows); + } + int cols() const + { + return 1; + } + int rows() const + { + return m_storage.size(); + } + int size() const + { + return rows(); + } + + T nrm2() const + { + T norm = T(0); + + int nn = rows(); + + { + if (nn == 1) + { + norm = btFabs((*this)[0]); + } + else + { + T scale = 0.0; + T ssq = 1.0; + + /* The following loop is equivalent to this call to the LAPACK + auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */ + + for (int ix=0;ix<nn;ix++) + { + if ((*this)[ix] != 0.0) + { + T absxi = btFabs((*this)[ix]); + if (scale < absxi) + { + T temp; + temp = scale / absxi; + ssq = ssq * (temp * temp) + 1.0; + scale = absxi; + } + else + { + T temp; + temp = absxi / scale; + ssq += temp * temp; + } + } + } + norm = scale * sqrt(ssq); + } + } + return norm; + + } + void setZero() + { + if (m_storage.size()) + { + // for (int i=0;i<m_storage.size();i++) + // m_storage[i]=0; + //memset(&m_storage[0],0,sizeof(T)*m_storage.size()); + btSetZero(&m_storage[0],m_storage.size()); + } + } + const T& operator[] (int index) const + { + return m_storage[index]; + } + + T& operator[] (int index) + { + return m_storage[index]; + } + + T* getBufferPointerWritable() + { + return m_storage.size() ? &m_storage[0] : 0; + } + + const T* getBufferPointer() const + { + return m_storage.size() ? &m_storage[0] : 0; + } + +}; +/* + template <typename T> + void setElem(btMatrixX<T>& mat, int row, int col, T val) + { + mat.setElem(row,col,val); + } + */ + + template <typename T> struct btMatrixX { @@ -40,8 +161,7 @@ struct btMatrixX int m_setElemOperations; btAlignedObjectArray<T> m_storage; - btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1; - btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements; + mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1; T* getBufferPointerWritable() { @@ -78,7 +198,6 @@ struct btMatrixX BT_PROFILE("m_storage.resize"); m_storage.resize(rows*cols); } - clearSparseInfo(); } int cols() const { @@ -109,49 +228,44 @@ struct btMatrixX } } + + void setElem(int row,int col, T val) + { + m_setElemOperations++; + m_storage[row*m_cols+col] = val; + } + + void mulElem(int row,int col, T val) + { + m_setElemOperations++; + //mul doesn't change sparsity info + + m_storage[row*m_cols+col] *= val; + } + + + + void copyLowerToUpperTriangle() { int count=0; - for (int row=0;row<m_rowNonZeroElements1.size();row++) + for (int row=0;row<rows();row++) { - for (int j=0;j<m_rowNonZeroElements1[row].size();j++) + for (int col=0;col<row;col++) { - int col = m_rowNonZeroElements1[row][j]; setElem(col,row, (*this)(row,col)); count++; - + } } //printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows()); } - void setElem(int row,int col, T val) - { - m_setElemOperations++; - if (val) - { - if (m_storage[col+row*m_cols]==0.f) - { - m_rowNonZeroElements1[row].push_back(col); - m_colNonZeroElements[col].push_back(row); - } - m_storage[row*m_cols+col] = val; - } - } + const T& operator() (int row,int col) const { return m_storage[col+row*m_cols]; } - void clearSparseInfo() - { - BT_PROFILE("clearSparseInfo=0"); - m_rowNonZeroElements1.resize(m_rows); - m_colNonZeroElements.resize(m_cols); - for (int i=0;i<m_rows;i++) - m_rowNonZeroElements1[i].resize(0); - for (int j=0;j<m_cols;j++) - m_colNonZeroElements[j].resize(0); - } void setZero() { @@ -162,12 +276,21 @@ struct btMatrixX //for (int i=0;i<m_storage.size();i++) // m_storage[i]=0; } + } + + void setIdentity() + { + btAssert(rows() == cols()); + + setZero(); + for (int row=0;row<rows();row++) { - BT_PROFILE("clearSparseInfo=0"); - clearSparseInfo(); + setElem(row,row,1); } } + + void printMatrix(const char* msg) { printf("%s ---------------------\n",msg); @@ -182,28 +305,9 @@ struct btMatrixX printf("\n---------------------\n"); } - void printNumZeros(const char* msg) - { - printf("%s: ",msg); - int numZeros = 0; - for (int i=0;i<m_storage.size();i++) - if (m_storage[i]==0) - numZeros++; - int total = m_cols*m_rows; - int computedNonZero = total-numZeros; - int nonZero = 0; - for (int i=0;i<m_colNonZeroElements.size();i++) - nonZero += m_colNonZeroElements[i].size(); - btAssert(computedNonZero==nonZero); - if(computedNonZero!=nonZero) - { - printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero); - } - //printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows)); - printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total)); - } - /* - void rowComputeNonZeroElements() + + + void rowComputeNonZeroElements() const { m_rowNonZeroElements1.resize(rows()); for (int i=0;i<rows();i++) @@ -218,13 +322,11 @@ struct btMatrixX } } } - */ btMatrixX transpose() const { //transpose is optimized for sparse matrices btMatrixX tr(m_cols,m_rows); tr.setZero(); -#if 0 for (int i=0;i<m_cols;i++) for (int j=0;j<m_rows;j++) { @@ -234,37 +336,13 @@ struct btMatrixX tr.setElem(i,j,v); } } -#else - for (int i=0;i<m_colNonZeroElements.size();i++) - for (int h=0;h<m_colNonZeroElements[i].size();h++) - { - int j = m_colNonZeroElements[i][h]; - T v = (*this)(j,i); - tr.setElem(i,j,v); - } -#endif return tr; } - void sortRowIndexArrays() - { - for (int i=0;i<m_rowNonZeroElements1[i].size();i++) - { - m_rowNonZeroElements1[i].quickSort(btIntSortPredicate()); - } - } - - void sortColIndexArrays() - { - for (int i=0;i<m_colNonZeroElements[i].size();i++) - { - m_colNonZeroElements[i].quickSort(btIntSortPredicate()); - } - } btMatrixX operator*(const btMatrixX& other) { - //btMatrixX*btMatrixX implementation, optimized for sparse matrices + //btMatrixX*btMatrixX implementation, brute force btAssert(cols() == other.rows()); btMatrixX res(rows(),other.cols()); @@ -272,76 +350,18 @@ struct btMatrixX // BT_PROFILE("btMatrixX mul"); for (int j=0; j < res.cols(); ++j) { - //int numZero=other.m_colNonZeroElements[j].size(); - //if (numZero) { for (int i=0; i < res.rows(); ++i) - //for (int g = 0;g<m_colNonZeroElements[j].size();g++) { T dotProd=0; T dotProd2=0; int waste=0,waste2=0; - bool doubleWalk = false; - if (doubleWalk) - { - int numRows = m_rowNonZeroElements1[i].size(); - int numOtherCols = other.m_colNonZeroElements[j].size(); - for (int ii=0;ii<numRows;ii++) - { - int vThis=m_rowNonZeroElements1[i][ii]; - } - - for (int ii=0;ii<numOtherCols;ii++) - { - int vOther = other.m_colNonZeroElements[j][ii]; - } - - - int indexRow = 0; - int indexOtherCol = 0; - while (indexRow < numRows && indexOtherCol < numOtherCols) - { - int vThis=m_rowNonZeroElements1[i][indexRow]; - int vOther = other.m_colNonZeroElements[j][indexOtherCol]; - if (vOther==vThis) - { - dotProd += (*this)(i,vThis) * other(vThis,j); - } - if (vThis<vOther) - { - indexRow++; - } else - { - indexOtherCol++; - } - } - - } else { bool useOtherCol = true; - if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size()) - { - useOtherCol=true; - } - if (!useOtherCol ) { - for (int q=0;q<other.m_colNonZeroElements[j].size();q++) + for (int v=0;v<rows();v++) { - int v = other.m_colNonZeroElements[j][q]; - T w = (*this)(i,v); - if (w!=0.f) - { - dotProd+=w*other(v,j); - } - - } - } - else - { - for (int q=0;q<m_rowNonZeroElements1[i].size();q++) - { - int v=m_rowNonZeroElements1[i][q]; T w = (*this)(i,v); if (other(v,j)!=0.f) { @@ -404,73 +424,61 @@ struct btMatrixX bb += 8; } } - -}; - -template <typename T> -struct btVectorX -{ - btAlignedObjectArray<T> m_storage; - - btVectorX() - { - } - btVectorX(int numRows) - { - m_storage.resize(numRows); - } - - void resize(int rows) - { - m_storage.resize(rows); - } - int cols() const - { - return 1; - } - int rows() const - { - return m_storage.size(); - } - int size() const - { - return rows(); - } - void setZero() - { - // for (int i=0;i<m_storage.size();i++) - // m_storage[i]=0; - //memset(&m_storage[0],0,sizeof(T)*m_storage.size()); - btSetZero(&m_storage[0],m_storage.size()); - } - const T& operator[] (int index) const + + void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value) { - return m_storage[index]; + int numRows = rowend+1-rowstart; + int numCols = colend+1-colstart; + + for (int row=0;row<numRows;row++) + { + for (int col=0;col<numCols;col++) + { + setElem(rowstart+row,colstart+col,value); + } + } } - - T& operator[] (int index) + + void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block) { - return m_storage[index]; + btAssert(rowend+1-rowstart == block.rows()); + btAssert(colend+1-colstart == block.cols()); + for (int row=0;row<block.rows();row++) + { + for (int col=0;col<block.cols();col++) + { + setElem(rowstart+row,colstart+col,block(row,col)); + } + } } - - T* getBufferPointerWritable() + void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block) { - return m_storage.size() ? &m_storage[0] : 0; + btAssert(rowend+1-rowstart == block.rows()); + btAssert(colend+1-colstart == block.cols()); + for (int row=0;row<block.rows();row++) + { + for (int col=0;col<block.cols();col++) + { + setElem(rowstart+row,colstart+col,block[row]); + } + } } - - const T* getBufferPointer() const + + + btMatrixX negative() { - return m_storage.size() ? &m_storage[0] : 0; + btMatrixX neg(rows(),cols()); + for (int i=0;i<rows();i++) + for (int j=0;j<cols();j++) + { + T v = (*this)(i,j); + neg.setElem(i,j,-v); + } + return neg; } }; -/* -template <typename T> -void setElem(btMatrixX<T>& mat, int row, int col, T val) -{ - mat.setElem(row,col,val); -} -*/ + typedef btMatrixX<float> btMatrixXf; @@ -480,6 +488,47 @@ typedef btMatrixX<double> btMatrixXd; typedef btVectorX<double> btVectorXd; +#ifdef BT_DEBUG_OSTREAM +template <typename T> +std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat) + { + + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i=0;i<mat.rows();i++) + { + for (int j=0;j<mat.cols();j++) + { + os << std::setw(12) << mat(i,j); + } + if (i!=mat.rows()-1) + os << std::endl << " "; + } + os << " ]"; + //printf("\n---------------------\n"); + + return os; + } +template <typename T> +std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat) + { + + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i=0;i<mat.rows();i++) + { + os << std::setw(12) << mat[i]; + if (i!=mat.rows()-1) + os << std::endl << " "; + } + os << " ]"; + //printf("\n---------------------\n"); + + return os; + } + +#endif //BT_DEBUG_OSTREAM + inline void setElem(btMatrixXd& mat, int row, int col, double val) { diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 37c6dec191d..401e11eaaa8 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -284,6 +284,10 @@ static int btNanMask = 0x7F800001; #ifndef BT_INFINITY static int btInfinityMask = 0x7F800000; #define BT_INFINITY (*(float*)&btInfinityMask) +inline int btGetInfinityMask()//suppress stupid compiler warning +{ + return btInfinityMask; +} #endif //use this, in case there are clashes (such as xnamath.h) @@ -336,6 +340,10 @@ inline __m128 operator * (const __m128 A, const __m128 B) #ifndef BT_INFINITY static int btInfinityMask = 0x7F800000; #define BT_INFINITY (*(float*)&btInfinityMask) + inline int btGetInfinityMask()//suppress stupid compiler warning + { + return btInfinityMask; + } #endif #endif//BT_USE_NEON @@ -432,7 +440,7 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } #endif #define SIMD_PI btScalar(3.1415926535897932384626433832795029) -#define SIMD_2_PI btScalar(2.0) * SIMD_PI +#define SIMD_2_PI (btScalar(2.0) * SIMD_PI) #define SIMD_HALF_PI (SIMD_PI * btScalar(0.5)) #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) #define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) @@ -728,4 +736,5 @@ template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment) return converter.ptr; } + #endif //BT_SCALAR_H diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp index a84f7baee2e..2ba7029c9be 100644 --- a/extern/bullet2/src/LinearMath/btVector3.cpp +++ b/extern/bullet2/src/LinearMath/btVector3.cpp @@ -821,6 +821,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa #elif defined BT_USE_NEON + #define ARM_NEON_GCC_COMPATIBILITY 1 #include <arm_neon.h> #include <sys/types.h> @@ -884,7 +885,12 @@ static long _mindot_large_sel( const float *vv, const float *vec, unsigned long -#define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; }) +#if defined __arm__ +# define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; }) +#else +//support 64bit arm +# define vld1q_f32_aligned_postincrement( _ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; }) +#endif long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ) diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 89685929288..112b70dd66b 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -297,7 +297,7 @@ public: SIMD_FORCE_INLINE btVector3& normalize() { - btAssert(length() != btScalar(0)); + btAssert(!fuzzyZero()); #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) // dot product first @@ -685,9 +685,10 @@ public: return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0); } + SIMD_FORCE_INLINE bool fuzzyZero() const { - return length2() < SIMD_EPSILON; + return length2() < SIMD_EPSILON*SIMD_EPSILON; } SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const; @@ -950,9 +951,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const SIMD_FORCE_INLINE btVector3 btVector3::normalized() const { - btVector3 norm = *this; + btVector3 nrm = *this; - return norm.normalize(); + return nrm.normalize(); } SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const @@ -1010,21 +1011,21 @@ SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long arra if( array_count < scalar_cutoff ) #endif { - btScalar maxDot = -SIMD_INFINITY; + btScalar maxDot1 = -SIMD_INFINITY; int i = 0; int ptIndex = -1; for( i = 0; i < array_count; i++ ) { btScalar dot = array[i].dot(*this); - if( dot > maxDot ) + if( dot > maxDot1 ) { - maxDot = dot; + maxDot1 = dot; ptIndex = i; } } - dotOut = maxDot; + dotOut = maxDot1; return ptIndex; } #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON) |