Changeset 7983 for code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
- Timestamp:
- Feb 27, 2011, 7:43:24 AM (14 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
r5781 r7983 35 35 #include <string.h> //for memset 36 36 37 int gNumSplitImpulseRecoveries = 0; 38 37 39 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() 38 40 :m_btSeed2(0) … … 56 58 57 59 // Project Gauss Seidel or the equivalent Sequential Impulse 58 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(bt SolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)60 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 59 61 { 60 62 #ifdef USE_SIMD … … 63 65 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 64 66 __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))); 65 __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1. m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));66 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2. m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));67 __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); 68 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); 67 69 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 68 70 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); … … 77 79 deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); 78 80 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); 79 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, _mm_set1_ps(body1.m_invMass));80 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, _mm_set1_ps(body2.m_invMass));81 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); 82 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); 81 83 __m128 impulseMagnitude = deltaImpulse; 82 body1. m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));83 body1. m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));84 body2. m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));85 body2. m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));84 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); 85 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); 86 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); 87 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); 86 88 #else 87 89 resolveSingleConstraintRowGeneric(body1,body2,c); … … 90 92 91 93 // Project Gauss Seidel or the equivalent Sequential Impulse 92 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(bt SolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)94 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 93 95 { 94 96 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; 95 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1. m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);96 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2. m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);97 98 const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;97 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); 98 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); 99 100 // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; 99 101 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 100 102 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; … … 115 117 c.m_appliedImpulse = sum; 116 118 } 117 if (body1.m_invMass) 118 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); 119 if (body2.m_invMass) 120 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); 121 } 122 123 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) 119 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 120 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 121 } 122 123 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 124 124 { 125 125 #ifdef USE_SIMD … … 128 128 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 129 129 __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))); 130 __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1. m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));131 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2. m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));130 __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); 131 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); 132 132 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 133 133 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); … … 139 139 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 140 140 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); 141 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, _mm_set1_ps(body1.m_invMass));142 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, _mm_set1_ps(body2.m_invMass));141 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); 142 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); 143 143 __m128 impulseMagnitude = deltaImpulse; 144 body1. m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));145 body1. m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));146 body2. m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));147 body2. m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));144 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); 145 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); 146 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); 147 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); 148 148 #else 149 149 resolveSingleConstraintRowLowerLimit(body1,body2,c); … … 152 152 153 153 // Project Gauss Seidel or the equivalent Sequential Impulse 154 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(bt SolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)154 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 155 155 { 156 156 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; 157 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1. m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);158 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2. m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);157 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); 158 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); 159 159 160 160 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; … … 170 170 c.m_appliedImpulse = sum; 171 171 } 172 if (body1.m_invMass) 173 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); 174 if (body2.m_invMass) 175 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); 172 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 173 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 174 } 175 176 177 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( 178 btRigidBody& body1, 179 btRigidBody& body2, 180 const btSolverConstraint& c) 181 { 182 if (c.m_rhsPenetration) 183 { 184 gNumSplitImpulseRecoveries++; 185 btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; 186 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); 187 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); 188 189 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 190 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; 191 const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; 192 if (sum < c.m_lowerLimit) 193 { 194 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; 195 c.m_appliedPushImpulse = c.m_lowerLimit; 196 } 197 else 198 { 199 c.m_appliedPushImpulse = sum; 200 } 201 body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 202 body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 203 } 204 } 205 206 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 207 { 208 #ifdef USE_SIMD 209 if (!c.m_rhsPenetration) 210 return; 211 212 gNumSplitImpulseRecoveries++; 213 214 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); 215 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); 216 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 217 __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); 218 __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); 219 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); 220 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 221 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 222 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); 223 btSimdScalar resultLowerLess,resultUpperLess; 224 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); 225 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); 226 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); 227 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 228 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); 229 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); 230 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); 231 __m128 impulseMagnitude = deltaImpulse; 232 body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); 233 body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); 234 body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); 235 body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); 236 #else 237 resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); 238 #endif 176 239 } 177 240 … … 215 278 216 279 217 280 #if 0 218 281 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) 219 282 { 220 283 btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; 221 284 222 solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); 223 solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); 285 solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 286 solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 287 solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); 288 solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); 224 289 225 290 if (rb) 226 291 { 227 solverBody-> m_invMass = rb->getInvMass();292 solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); 228 293 solverBody->m_originalBody = rb; 229 294 solverBody->m_angularFactor = rb->getAngularFactor(); 230 295 } else 231 296 { 232 solverBody-> m_invMass = 0.f;297 solverBody->internalGetInvMass().setValue(0,0,0); 233 298 solverBody->m_originalBody = 0; 234 solverBody->m_angularFactor = 1.f; 235 } 236 } 237 238 239 int gNumSplitImpulseRecoveries = 0; 299 solverBody->m_angularFactor.setValue(1,1,1); 300 } 301 } 302 #endif 303 304 305 306 240 307 241 308 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) … … 263 330 264 331 265 266 btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) 332 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) 267 333 { 268 334 … … 271 337 btRigidBody* body1=btRigidBody::upcast(colObj1); 272 338 273 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();274 memset(&solverConstraint,0xff,sizeof(btSolverConstraint));275 339 solverConstraint.m_contactNormal = normalAxis; 276 340 277 solverConstraint.m_solverBodyIdA = solverBodyIdA; 278 solverConstraint.m_solverBodyIdB = solverBodyIdB; 279 solverConstraint.m_frictionIndex = frictionIndex; 341 solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); 342 solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); 280 343 281 344 solverConstraint.m_friction = cp.m_combinedFriction; … … 283 346 284 347 solverConstraint.m_appliedImpulse = 0.f; 285 //solverConstraint.m_appliedPushImpulse = 0.f;348 solverConstraint.m_appliedPushImpulse = 0.f; 286 349 287 350 { … … 338 401 rel_vel = vel1Dotn+vel2Dotn; 339 402 340 btScalar positionalError = 0.f;341 342 btSimdScalar velocityError = - rel_vel;403 // btScalar positionalError = 0.f; 404 405 btSimdScalar velocityError = desiredVelocity - rel_vel; 343 406 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); 344 407 solverConstraint.m_rhs = velocityImpulse; 345 solverConstraint.m_cfm = 0.f;408 solverConstraint.m_cfm = cfmSlip; 346 409 solverConstraint.m_lowerLimit = 0; 347 410 solverConstraint.m_upperLimit = 1e10f; 348 411 } 349 412 } 413 414 415 416 btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) 417 { 418 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); 419 solverConstraint.m_frictionIndex = frictionIndex; 420 setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, 421 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); 350 422 return solverConstraint; 351 423 } … … 353 425 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) 354 426 { 427 #if 0 355 428 int solverBodyIdA = -1; 356 429 … … 374 447 } 375 448 return solverBodyIdA; 449 #endif 450 return 0; 376 451 } 377 452 #include <stdio.h> 378 453 379 454 380 381 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) 382 { 383 btCollisionObject* colObj0=0,*colObj1=0; 384 385 colObj0 = (btCollisionObject*)manifold->getBody0(); 386 colObj1 = (btCollisionObject*)manifold->getBody1(); 387 388 int solverBodyIdA=-1; 389 int solverBodyIdB=-1; 390 391 if (manifold->getNumContacts()) 392 { 393 solverBodyIdA = getOrInitSolverBody(*colObj0); 394 solverBodyIdB = getOrInitSolverBody(*colObj1); 395 } 396 397 ///avoid collision response between two static objects 398 if (!solverBodyIdA && !solverBodyIdB) 399 return; 400 401 btVector3 rel_pos1; 402 btVector3 rel_pos2; 403 btScalar relaxation; 404 405 for (int j=0;j<manifold->getNumContacts();j++) 406 { 407 408 btManifoldPoint& cp = manifold->getContactPoint(j); 409 410 if (cp.getDistance() <= manifold->getContactProcessingThreshold()) 411 { 455 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 456 btCollisionObject* colObj0, btCollisionObject* colObj1, 457 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, 458 btVector3& vel, btScalar& rel_vel, btScalar& relaxation, 459 btVector3& rel_pos1, btVector3& rel_pos2) 460 { 461 btRigidBody* rb0 = btRigidBody::upcast(colObj0); 462 btRigidBody* rb1 = btRigidBody::upcast(colObj1); 412 463 413 464 const btVector3& pos1 = cp.getPositionWorldOnA(); 414 465 const btVector3& pos2 = cp.getPositionWorldOnB(); 415 466 467 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 468 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); 416 469 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 417 470 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); 418 471 419 420 472 relaxation = 1.f; 421 btScalar rel_vel; 422 btVector3 vel; 423 424 int frictionIndex = m_tmpSolverContactConstraintPool.size(); 425 426 { 427 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand(); 428 btRigidBody* rb0 = btRigidBody::upcast(colObj0); 429 btRigidBody* rb1 = btRigidBody::upcast(colObj1); 430 431 solverConstraint.m_solverBodyIdA = solverBodyIdA; 432 solverConstraint.m_solverBodyIdB = solverBodyIdB; 433 434 solverConstraint.m_originalContactPoint = &cp; 435 436 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); 437 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); 438 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); 439 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); 473 474 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); 475 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); 476 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); 477 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); 478 440 479 { 441 480 #ifdef COMPUTE_IMPULSE_DENOM … … 467 506 468 507 469 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); 470 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); 471 472 vel = vel1 - vel2;473 474 508 509 510 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); 511 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); 512 vel = vel1 - vel2; 513 rel_vel = cp.m_normalWorldOnB.dot(vel); 475 514 476 515 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; … … 499 538 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; 500 539 if (rb0) 501 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);540 rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); 502 541 if (rb1) 503 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);542 rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); 504 543 } else 505 544 { … … 507 546 } 508 547 509 //solverConstraint.m_appliedPushImpulse = 0.f;548 solverConstraint.m_appliedPushImpulse = 0.f; 510 549 511 550 { … … 523 562 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 524 563 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; 525 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; 564 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) 565 { 566 //combine position and velocity into rhs 567 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; 568 solverConstraint.m_rhsPenetration = 0.f; 569 } else 570 { 571 //split position and velocity into rhs and m_rhsPenetration 572 solverConstraint.m_rhs = velocityImpulse; 573 solverConstraint.m_rhsPenetration = penetrationImpulse; 574 } 526 575 solverConstraint.m_cfm = 0.f; 527 576 solverConstraint.m_lowerLimit = 0; … … 530 579 531 580 532 /////setup the friction constraints 533 534 535 536 if (1) 537 { 538 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); 539 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) 540 { 541 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; 542 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); 543 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) 544 { 545 cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); 546 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); 547 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); 548 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 549 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 550 { 551 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); 552 cp.m_lateralFrictionDir2.normalize();//?? 553 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); 554 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); 555 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 556 } 557 cp.m_lateralFrictionInitialized = true; 558 } else 559 { 560 //re-calculate friction direction every frame, todo: check if this is really needed 561 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); 562 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); 563 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); 564 565 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 566 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 567 { 568 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); 569 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); 570 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 571 } 572 cp.m_lateralFrictionInitialized = true; 573 } 574 575 } else 576 { 577 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 578 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 579 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 580 } 581 581 582 583 } 584 585 586 587 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 588 btRigidBody* rb0, btRigidBody* rb1, 589 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) 590 { 582 591 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) 583 592 { … … 588 597 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; 589 598 if (rb0) 590 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);599 rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); 591 600 if (rb1) 592 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);601 rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); 593 602 } else 594 603 { … … 604 613 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; 605 614 if (rb0) 606 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);615 rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); 607 616 if (rb1) 608 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);617 rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); 609 618 } else 610 619 { … … 622 631 } 623 632 } 633 } 634 635 636 637 638 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) 639 { 640 btCollisionObject* colObj0=0,*colObj1=0; 641 642 colObj0 = (btCollisionObject*)manifold->getBody0(); 643 colObj1 = (btCollisionObject*)manifold->getBody1(); 644 645 646 btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); 647 btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); 648 649 ///avoid collision response between two static objects 650 if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass())) 651 return; 652 653 for (int j=0;j<manifold->getNumContacts();j++) 654 { 655 656 btManifoldPoint& cp = manifold->getContactPoint(j); 657 658 if (cp.getDistance() <= manifold->getContactProcessingThreshold()) 659 { 660 btVector3 rel_pos1; 661 btVector3 rel_pos2; 662 btScalar relaxation; 663 btScalar rel_vel; 664 btVector3 vel; 665 666 int frictionIndex = m_tmpSolverContactConstraintPool.size(); 667 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); 668 btRigidBody* rb0 = btRigidBody::upcast(colObj0); 669 btRigidBody* rb1 = btRigidBody::upcast(colObj1); 670 solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); 671 solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); 672 solverConstraint.m_originalContactPoint = &cp; 673 674 setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); 675 676 // const btVector3& pos1 = cp.getPositionWorldOnA(); 677 // const btVector3& pos2 = cp.getPositionWorldOnB(); 678 679 /////setup the friction constraints 680 681 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); 682 683 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) 684 { 685 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; 686 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); 687 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) 688 { 689 cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); 690 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 691 { 692 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); 693 cp.m_lateralFrictionDir2.normalize();//?? 694 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); 695 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); 696 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 697 } 698 699 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); 700 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); 701 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 702 cp.m_lateralFrictionInitialized = true; 703 } else 704 { 705 //re-calculate friction direction every frame, todo: check if this is really needed 706 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); 707 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 708 { 709 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); 710 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); 711 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 712 } 713 714 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); 715 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); 716 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 717 718 cp.m_lateralFrictionInitialized = true; 624 719 } 625 } 626 627 628 } 629 } 630 } 631 632 633 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 720 721 } else 722 { 723 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); 724 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 725 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); 726 } 727 728 setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal); 729 730 } 731 } 732 } 733 734 735 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 634 736 { 635 737 BT_PROFILE("solveGroupCacheFriendlySetup"); … … 644 746 } 645 747 748 if (infoGlobal.m_splitImpulse) 749 { 750 for (int i = 0; i < numBodies; i++) 751 { 752 btRigidBody* body = btRigidBody::upcast(bodies[i]); 753 if (body) 754 { 755 body->internalGetDeltaLinearVelocity().setZero(); 756 body->internalGetDeltaAngularVelocity().setZero(); 757 body->internalGetPushVelocity().setZero(); 758 body->internalGetTurnVelocity().setZero(); 759 } 760 } 761 } 762 else 763 { 764 for (int i = 0; i < numBodies; i++) 765 { 766 btRigidBody* body = btRigidBody::upcast(bodies[i]); 767 if (body) 768 { 769 body->internalGetDeltaLinearVelocity().setZero(); 770 body->internalGetDeltaAngularVelocity().setZero(); 771 } 772 } 773 } 774 646 775 if (1) 647 776 { … … 653 782 } 654 783 } 655 656 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();657 initSolverBody(&fixedBody,0);658 659 784 //btRigidBody* rb0=0,*rb1=0; 660 785 … … 665 790 int totalNumRows = 0; 666 791 int i; 792 793 m_tmpConstraintSizesPool.resize(numConstraints); 667 794 //calculate the total number of contraint rows 668 795 for (i=0;i<numConstraints;i++) 669 796 { 670 671 btTypedConstraint::btConstraintInfo1 info1; 797 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 672 798 constraints[i]->getInfo1(&info1); 673 799 totalNumRows += info1.m_numConstraintRows; … … 675 801 m_tmpSolverNonContactConstraintPool.resize(totalNumRows); 676 802 677 btTypedConstraint::btConstraintInfo1 info1; 678 info1.m_numConstraintRows = 0; 679 680 803 681 804 ///setup the btSolverConstraints 682 805 int currentRow = 0; 683 806 684 for (i=0;i<numConstraints;i++ ,currentRow+=info1.m_numConstraintRows)807 for (i=0;i<numConstraints;i++) 685 808 { 686 constraints[i]->getInfo1(&info1); 809 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 810 687 811 if (info1.m_numConstraintRows) 688 812 { … … 697 821 btRigidBody& rbB = constraint->getRigidBodyB(); 698 822 699 int solverBodyIdA = getOrInitSolverBody(rbA); 700 int solverBodyIdB = getOrInitSolverBody(rbB); 701 702 btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; 703 btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; 704 823 705 824 int j; 706 825 for ( j=0;j<info1.m_numConstraintRows;j++) … … 711 830 currentConstraintRow[j].m_appliedImpulse = 0.f; 712 831 currentConstraintRow[j].m_appliedPushImpulse = 0.f; 713 currentConstraintRow[j].m_solverBody IdA = solverBodyIdA;714 currentConstraintRow[j].m_solverBody IdB = solverBodyIdB;832 currentConstraintRow[j].m_solverBodyA = &rbA; 833 currentConstraintRow[j].m_solverBodyB = &rbB; 715 834 } 716 835 717 bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);718 bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);719 bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);720 bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);836 rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 837 rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 838 rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 839 rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 721 840 722 841 … … 733 852 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); 734 853 info2.m_constraintError = ¤tConstraintRow->m_rhs; 854 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; 855 info2.m_damping = infoGlobal.m_damping; 735 856 info2.cfm = ¤tConstraintRow->m_cfm; 736 857 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; 737 858 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; 859 info2.m_numIterations = infoGlobal.m_numIterations; 738 860 constraints[i]->getInfo2(&info2); 739 861 … … 742 864 { 743 865 btSolverConstraint& solverConstraint = currentConstraintRow[j]; 866 solverConstraint.m_originalContactPoint = constraint; 744 867 745 868 { … … 778 901 btScalar restitution = 0.f; 779 902 btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 780 btScalar velocityError = restitution - rel_vel ;// *damping;903 btScalar velocityError = restitution - rel_vel * info2.m_damping; 781 904 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 782 905 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; … … 787 910 } 788 911 } 912 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; 789 913 } 790 914 } … … 793 917 int i; 794 918 btPersistentManifold* manifold = 0; 795 btCollisionObject* colObj0=0,*colObj1=0;919 // btCollisionObject* colObj0=0,*colObj1=0; 796 920 797 921 … … 830 954 } 831 955 832 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) 833 { 834 BT_PROFILE("solveGroupCacheFriendlyIterations"); 956 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) 957 { 835 958 836 959 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 837 960 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 838 961 962 int j; 963 964 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) 965 { 966 if ((iteration & 7) == 0) { 967 for (j=0; j<numConstraintPool; ++j) { 968 int tmp = m_orderTmpConstraintPool[j]; 969 int swapi = btRandInt2(j+1); 970 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; 971 m_orderTmpConstraintPool[swapi] = tmp; 972 } 973 974 for (j=0; j<numFrictionPool; ++j) { 975 int tmp = m_orderFrictionConstraintPool[j]; 976 int swapi = btRandInt2(j+1); 977 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; 978 m_orderFrictionConstraintPool[swapi] = tmp; 979 } 980 } 981 } 982 983 if (infoGlobal.m_solverMode & SOLVER_SIMD) 984 { 985 ///solve all joint constraints, using SIMD, if available 986 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 987 { 988 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; 989 resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); 990 } 991 992 for (j=0;j<numConstraints;j++) 993 { 994 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 995 } 996 997 ///solve all contact constraints using SIMD, if available 998 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 999 for (j=0;j<numPoolConstraints;j++) 1000 { 1001 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1002 resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 1003 1004 } 1005 ///solve all friction constraints, using SIMD, if available 1006 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 1007 for (j=0;j<numFrictionPoolConstraints;j++) 1008 { 1009 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 1010 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 1011 1012 if (totalImpulse>btScalar(0)) 1013 { 1014 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 1015 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 1016 1017 resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); 1018 } 1019 } 1020 } else 1021 { 1022 1023 ///solve all joint constraints 1024 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 1025 { 1026 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; 1027 resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); 1028 } 1029 1030 for (j=0;j<numConstraints;j++) 1031 { 1032 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 1033 } 1034 ///solve all contact constraints 1035 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1036 for (j=0;j<numPoolConstraints;j++) 1037 { 1038 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1039 resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 1040 } 1041 ///solve all friction constraints 1042 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 1043 for (j=0;j<numFrictionPoolConstraints;j++) 1044 { 1045 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 1046 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 1047 1048 if (totalImpulse>btScalar(0)) 1049 { 1050 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 1051 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 1052 1053 resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 1054 } 1055 } 1056 } 1057 return 0.f; 1058 } 1059 1060 1061 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 1062 { 1063 int iteration; 1064 if (infoGlobal.m_splitImpulse) 1065 { 1066 if (infoGlobal.m_solverMode & SOLVER_SIMD) 1067 { 1068 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 1069 { 1070 { 1071 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1072 int j; 1073 for (j=0;j<numPoolConstraints;j++) 1074 { 1075 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1076 1077 resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 1078 } 1079 } 1080 } 1081 } 1082 else 1083 { 1084 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 1085 { 1086 { 1087 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1088 int j; 1089 for (j=0;j<numPoolConstraints;j++) 1090 { 1091 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1092 1093 resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 1094 } 1095 } 1096 } 1097 } 1098 } 1099 } 1100 1101 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 1102 { 1103 BT_PROFILE("solveGroupCacheFriendlyIterations"); 1104 1105 839 1106 //should traverse the contacts random order... 840 1107 int iteration; … … 842 1109 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 843 1110 { 844 845 int j; 846 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) 847 { 848 if ((iteration & 7) == 0) { 849 for (j=0; j<numConstraintPool; ++j) { 850 int tmp = m_orderTmpConstraintPool[j]; 851 int swapi = btRandInt2(j+1); 852 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; 853 m_orderTmpConstraintPool[swapi] = tmp; 854 } 855 856 for (j=0; j<numFrictionPool; ++j) { 857 int tmp = m_orderFrictionConstraintPool[j]; 858 int swapi = btRandInt2(j+1); 859 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; 860 m_orderFrictionConstraintPool[swapi] = tmp; 861 } 862 } 863 } 864 865 if (infoGlobal.m_solverMode & SOLVER_SIMD) 866 { 867 ///solve all joint constraints, using SIMD, if available 868 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 869 { 870 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; 871 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); 872 } 873 874 for (j=0;j<numConstraints;j++) 875 { 876 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); 877 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); 878 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; 879 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; 880 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); 881 } 882 883 ///solve all contact constraints using SIMD, if available 884 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 885 for (j=0;j<numPoolConstraints;j++) 886 { 887 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 888 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 889 890 } 891 ///solve all friction constraints, using SIMD, if available 892 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 893 for (j=0;j<numFrictionPoolConstraints;j++) 894 { 895 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 896 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 897 898 if (totalImpulse>btScalar(0)) 899 { 900 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 901 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 902 903 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 904 } 905 } 906 } else 907 { 908 909 ///solve all joint constraints 910 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 911 { 912 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; 913 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); 914 } 915 916 for (j=0;j<numConstraints;j++) 917 { 918 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); 919 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); 920 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; 921 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; 922 923 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); 924 } 925 926 ///solve all contact constraints 927 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 928 for (j=0;j<numPoolConstraints;j++) 929 { 930 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 931 resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 932 } 933 ///solve all friction constraints 934 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 935 for (j=0;j<numFrictionPoolConstraints;j++) 936 { 937 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 938 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 939 940 if (totalImpulse>btScalar(0)) 941 { 942 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 943 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 944 945 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 946 } 947 } 948 } 949 950 951 952 } 1111 solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); 1112 } 1113 1114 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); 953 1115 } 954 1116 return 0.f; 955 1117 } 956 1118 957 958 959 /// btSequentialImpulseConstraintSolver Sequentially applies impulses 960 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) 961 { 962 963 964 965 BT_PROFILE("solveGroup"); 966 //we only implement SOLVER_CACHE_FRIENDLY now 967 //you need to provide at least some bodies 968 btAssert(bodies); 969 btAssert(numBodies); 970 971 int i; 972 973 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 974 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 975 1119 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) 1120 { 976 1121 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 977 int j;1122 int i,j; 978 1123 979 1124 for (j=0;j<numPoolConstraints;j++) … … 993 1138 } 994 1139 1140 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); 1141 for (j=0;j<numPoolConstraints;j++) 1142 { 1143 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; 1144 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; 1145 btScalar sum = constr->internalGetAppliedImpulse(); 1146 sum += solverConstr.m_appliedImpulse; 1147 constr->internalSetAppliedImpulse(sum); 1148 } 1149 1150 995 1151 if (infoGlobal.m_splitImpulse) 996 1152 { 997 for ( i=0;i<m_tmpSolverBodyPool.size();i++) 998 { 999 m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep); 1153 for ( i=0;i<numBodies;i++) 1154 { 1155 btRigidBody* body = btRigidBody::upcast(bodies[i]); 1156 if (body) 1157 body->internalWritebackVelocity(infoGlobal.m_timeStep); 1000 1158 } 1001 1159 } else 1002 1160 { 1003 for ( i=0;i<m_tmpSolverBodyPool.size();i++) 1004 { 1005 m_tmpSolverBodyPool[i].writebackVelocity(); 1006 } 1007 } 1008 1009 1010 m_tmpSolverBodyPool.resize(0); 1161 for ( i=0;i<numBodies;i++) 1162 { 1163 btRigidBody* body = btRigidBody::upcast(bodies[i]); 1164 if (body) 1165 body->internalWritebackVelocity(); 1166 } 1167 } 1168 1169 1011 1170 m_tmpSolverContactConstraintPool.resize(0); 1012 1171 m_tmpSolverNonContactConstraintPool.resize(0); … … 1018 1177 1019 1178 1020 1021 1022 1023 1024 1179 /// btSequentialImpulseConstraintSolver Sequentially applies impulses 1180 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) 1181 { 1182 1183 BT_PROFILE("solveGroup"); 1184 //you need to provide at least some bodies 1185 btAssert(bodies); 1186 btAssert(numBodies); 1187 1188 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 1189 1190 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 1191 1192 solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 1193 1194 return 0.f; 1195 } 1025 1196 1026 1197 void btSequentialImpulseConstraintSolver::reset() … … 1029 1200 } 1030 1201 1031 1202 btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody() 1203 { 1204 static btRigidBody s_fixed(0, 0,0); 1205 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); 1206 return s_fixed; 1207 } 1208
Note: See TracChangeset
for help on using the changeset viewer.
