From 05eebf49d3c3af47bf157207b82cf559ac1fe274 Mon Sep 17 00:00:00 2001 From: Sergej Reich Date: Thu, 26 Dec 2013 12:41:52 +0100 Subject: Bullet: Update to svn r2719 Fixes part of T37905, fixed constraint didn't work correctly. --- .../CollisionDispatch/btCollisionWorld.cpp | 2 +- .../btCompoundCollisionAlgorithm.cpp | 1 + .../btCompoundCollisionAlgorithm.h | 1 + .../btCompoundCompoundCollisionAlgorithm.cpp | 16 +- .../btCompoundCompoundCollisionAlgorithm.h | 7 +- .../ConstraintSolver/btConstraintSolver.h | 3 +- .../ConstraintSolver/btFixedConstraint.cpp | 149 ++++--- .../ConstraintSolver/btFixedConstraint.h | 5 +- .../btSequentialImpulseConstraintSolver.cpp | 34 +- .../btSequentialImpulseConstraintSolver.h | 11 +- .../BulletDynamics/MLCPSolvers/btMLCPSolver.cpp | 28 +- .../src/BulletDynamics/MLCPSolvers/btMLCPSolver.h | 10 + .../MLCPSolvers/btSolveProjectedGaussSeidel.h | 6 + extern/bullet2/src/BulletSoftBody/btSoftBody.cpp | 3 +- extern/bullet2/src/LinearMath/btMatrixX.h | 443 ++++++++++++--------- extern/bullet2/src/LinearMath/btScalar.h | 11 +- extern/bullet2/src/LinearMath/btVector3.cpp | 8 +- extern/bullet2/src/LinearMath/btVector3.h | 17 +- 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 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(col0ObjWrap->getCollisionShape()); const btCompoundShape* compoundShape1 = static_cast(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& 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 +#include // std::setw +#endif //BT_DEBUG_OSTREAM + class btIntSortPredicate { public: @@ -30,6 +36,121 @@ class btIntSortPredicate }; +template +struct btVectorX +{ + btAlignedObjectArray 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 + void setElem(btMatrixX& mat, int row, int col, T val) + { + mat.setElem(row,col,val); + } + */ + + template struct btMatrixX { @@ -40,8 +161,7 @@ struct btMatrixX int m_setElemOperations; btAlignedObjectArray m_storage; - btAlignedObjectArray< btAlignedObjectArray > m_rowNonZeroElements1; - btAlignedObjectArray< btAlignedObjectArray > m_colNonZeroElements; + mutable btAlignedObjectArray< btAlignedObjectArray > 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 -struct btVectorX -{ - btAlignedObjectArray 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& 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 -void setElem(btMatrixX& mat, int row, int col, T val) -{ - mat.setElem(row,col,val); -} -*/ + typedef btMatrixX btMatrixXf; @@ -480,6 +488,47 @@ typedef btMatrixX btMatrixXd; typedef btVectorX btVectorXd; +#ifdef BT_DEBUG_OSTREAM +template +std::ostream& operator<< (std::ostream& os, const btMatrixX& mat) + { + + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i=0;i +std::ostream& operator<< (std::ostream& os, const btVectorX& mat) + { + + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i=0;iT* 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 #include @@ -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) -- cgit v1.2.3