diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics')
20 files changed, 2001 insertions, 2399 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h index 7a12257b33..049c9116fd 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h @@ -18,7 +18,7 @@ subject to the following restrictions: #include "Bullet3Common/b3Scalar.h" -enum b3SolverMode +enum b3SolverMode { B3_SOLVER_RANDMIZE_ORDER = 1, B3_SOLVER_FRICTION_SEPARATE = 2, @@ -34,45 +34,38 @@ enum b3SolverMode struct b3ContactSolverInfoData { - - - b3Scalar m_tau; - b3Scalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. - b3Scalar m_friction; - b3Scalar m_timeStep; - b3Scalar m_restitution; - int m_numIterations; - b3Scalar m_maxErrorReduction; - b3Scalar m_sor; - b3Scalar m_erp;//used as Baumgarte factor - b3Scalar m_erp2;//used in Split Impulse - b3Scalar m_globalCfm;//constraint force mixing - int m_splitImpulse; - b3Scalar m_splitImpulsePenetrationThreshold; - b3Scalar m_splitImpulseTurnErp; - b3Scalar m_linearSlop; - b3Scalar m_warmstartingFactor; - - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - b3Scalar m_maxGyroscopicForce; - b3Scalar m_singleAxisRollingFrictionThreshold; - - + b3Scalar m_tau; + b3Scalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + b3Scalar m_friction; + b3Scalar m_timeStep; + b3Scalar m_restitution; + int m_numIterations; + b3Scalar m_maxErrorReduction; + b3Scalar m_sor; + b3Scalar m_erp; //used as Baumgarte factor + b3Scalar m_erp2; //used in Split Impulse + b3Scalar m_globalCfm; //constraint force mixing + int m_splitImpulse; + b3Scalar m_splitImpulsePenetrationThreshold; + b3Scalar m_splitImpulseTurnErp; + b3Scalar m_linearSlop; + b3Scalar m_warmstartingFactor; + + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + b3Scalar m_maxGyroscopicForce; + b3Scalar m_singleAxisRollingFrictionThreshold; }; struct b3ContactSolverInfo : public b3ContactSolverInfoData { - - - inline b3ContactSolverInfo() { m_tau = b3Scalar(0.6); m_damping = b3Scalar(1.0); m_friction = b3Scalar(0.3); - m_timeStep = b3Scalar(1.f/60.f); + m_timeStep = b3Scalar(1.f / 60.f); m_restitution = b3Scalar(0.); m_maxErrorReduction = b3Scalar(20.); m_numIterations = 10; @@ -84,76 +77,73 @@ struct b3ContactSolverInfo : public b3ContactSolverInfoData m_splitImpulsePenetrationThreshold = -.04f; m_splitImpulseTurnErp = 0.1f; m_linearSlop = b3Scalar(0.0); - m_warmstartingFactor=b3Scalar(0.85); + m_warmstartingFactor = b3Scalar(0.85); //m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER; - m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_SOLVER_RANDMIZE_ORDER; - m_restingContactRestitutionThreshold = 2;//unused as of 2.81 - m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit - m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag) - m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. + m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD; // | B3_SOLVER_RANDMIZE_ORDER; + m_restingContactRestitutionThreshold = 2; //unused as of 2.81 + m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit + m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag) + m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. } }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct b3ContactSolverInfoDoubleData { - double m_tau; - double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. - double m_friction; - double m_timeStep; - double m_restitution; - double m_maxErrorReduction; - double m_sor; - double m_erp;//used as Baumgarte factor - double m_erp2;//used in Split Impulse - double m_globalCfm;//constraint force mixing - double m_splitImpulsePenetrationThreshold; - double m_splitImpulseTurnErp; - double m_linearSlop; - double m_warmstartingFactor; - double m_maxGyroscopicForce; - double m_singleAxisRollingFrictionThreshold; - - int m_numIterations; - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - int m_splitImpulse; - char m_padding[4]; - + double m_tau; + double m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + double m_friction; + double m_timeStep; + double m_restitution; + double m_maxErrorReduction; + double m_sor; + double m_erp; //used as Baumgarte factor + double m_erp2; //used in Split Impulse + double m_globalCfm; //constraint force mixing + double m_splitImpulsePenetrationThreshold; + double m_splitImpulseTurnErp; + double m_linearSlop; + double m_warmstartingFactor; + double m_maxGyroscopicForce; + double m_singleAxisRollingFrictionThreshold; + + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + int m_splitImpulse; + char m_padding[4]; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct b3ContactSolverInfoFloatData { - float m_tau; - float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. - float m_friction; - float m_timeStep; - - float m_restitution; - float m_maxErrorReduction; - float m_sor; - float m_erp;//used as Baumgarte factor - - float m_erp2;//used in Split Impulse - float m_globalCfm;//constraint force mixing - float m_splitImpulsePenetrationThreshold; - float m_splitImpulseTurnErp; - - float m_linearSlop; - float m_warmstartingFactor; - float m_maxGyroscopicForce; - float m_singleAxisRollingFrictionThreshold; - - int m_numIterations; - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - - int m_splitImpulse; - char m_padding[4]; + float m_tau; + float m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + float m_friction; + float m_timeStep; + + float m_restitution; + float m_maxErrorReduction; + float m_sor; + float m_erp; //used as Baumgarte factor + + float m_erp2; //used in Split Impulse + float m_globalCfm; //constraint force mixing + float m_splitImpulsePenetrationThreshold; + float m_splitImpulseTurnErp; + + float m_linearSlop; + float m_warmstartingFactor; + float m_maxGyroscopicForce; + float m_singleAxisRollingFrictionThreshold; + + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + + int m_splitImpulse; + char m_padding[4]; }; - - -#endif //B3_CONTACT_SOLVER_INFO +#endif //B3_CONTACT_SOLVER_INFO diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp index 5e11e74935..ace4b18388 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp @@ -4,105 +4,100 @@ #include "Bullet3Common/b3TransformUtil.h" #include <new> - -b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB) -:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB) +b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB) + : b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB) { m_pivotInA = frameInA.getOrigin(); m_pivotInB = frameInB.getOrigin(); - m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); - + m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse(); } -b3FixedConstraint::~b3FixedConstraint () +b3FixedConstraint::~b3FixedConstraint() { } - -void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) { info->m_numConstraintRows = 6; info->nub = 6; } -void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies) +void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies) { //fix the 3 linear degrees of freedom const b3Vector3& worldPosA = bodies[m_rbA].m_pos; const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat; - const b3Vector3& worldPosB= bodies[m_rbB].m_pos; + const b3Vector3& worldPosB = bodies[m_rbB].m_pos; const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat; info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; + info->m_J1linearAxis[info->rowskip + 1] = 1; + info->m_J1linearAxis[2 * info->rowskip + 2] = 1; - b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA); + b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA); { b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip); b3Vector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } - + if (info->m_J2linearAxis) { info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; + info->m_J2linearAxis[info->rowskip + 1] = -1; + info->m_J2linearAxis[2 * info->rowskip + 2] = -1; } - - b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB); - + + b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB); + { - // b3Vector3 a2n = -a2; + // b3Vector3 a2n = -a2; b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } - // set right hand side for the linear dofs + // set right hand side for the linear dofs b3Scalar k = info->fps * info->erp; - b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA); - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = linearError[j]; + b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA); + 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 + //fix the 3 angular degrees of freedom int start_row = 3; int s = info->rowskip; - int start_index = start_row * s; + int start_index = start_row * s; - // 3 rows to make body rotations equal + // 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; - } - + 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 + // set right hand side for the angular dofs b3Vector3 diff; b3Scalar angle; - b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse(); - - b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle); - diff*=-angle; - for (j=0; j<3; j++) - { - info->m_constraintError[(3+j)*info->rowskip] = k * diff[j]; - } + b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse(); + b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle); + diff *= -angle; + for (j = 0; j < 3; j++) + { + info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j]; + } }
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h index e884a82912..64809666e4 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h @@ -4,32 +4,31 @@ #include "b3TypedConstraint.h" -B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint +B3_ATTRIBUTE_ALIGNED16(class) +b3FixedConstraint : public b3TypedConstraint { b3Vector3 m_pivotInA; b3Vector3 m_pivotInB; b3Quaternion m_relTargetAB; public: - b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB); - + b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB); + virtual ~b3FixedConstraint(); - - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies); + virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies); - virtual void setParam(int num, b3Scalar value, int axis = -1) + virtual void setParam(int num, b3Scalar value, int axis = -1) { b3Assert(0); } - virtual b3Scalar getParam(int num, int axis = -1) const + virtual b3Scalar getParam(int num, int axis = -1) const { b3Assert(0); return 0.f; } - }; -#endif //B3_FIXED_CONSTRAINT_H +#endif //B3_FIXED_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp index 3ae2922e58..0d5bb2014b 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp @@ -26,69 +26,48 @@ http://gimpact.sf.net #include "Bullet3Common/b3TransformUtil.h" #include <new> - - #define D6_USE_OBSOLETE_METHOD false #define D6_USE_FRAME_OFFSET true - - - - - -b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies) -: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB) -, m_frameInA(frameInA) -, m_frameInB(frameInB), -m_useLinearReferenceFrameA(useLinearReferenceFrameA), -m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), -m_flags(0) +b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies) + : b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0) { calculateTransforms(bodies); } - - - - - #define GENERIC_D6_DISABLE_WARMSTARTING 1 - - b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index); b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index) { - int i = index%3; - int j = index/3; + int i = index % 3; + int j = index / 3; return mat[i][j]; } - - ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz); -bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz) +bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz); +bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz) { // // rot = cy*cz -cy*sz sy // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy // - b3Scalar fi = btGetMatrixElem(mat,2); + b3Scalar fi = btGetMatrixElem(mat, 2); if (fi < b3Scalar(1.0f)) { if (fi > b3Scalar(-1.0f)) { - xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); - xyz[1] = b3Asin(btGetMatrixElem(mat,2)); - xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8)); + xyz[1] = b3Asin(btGetMatrixElem(mat, 2)); + xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0)); return true; } else { // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = -B3_HALF_PI; xyz[2] = b3Scalar(0.0); return false; @@ -97,7 +76,7 @@ bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz) else { // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4)); xyz[1] = B3_HALF_PI; xyz[2] = 0.0; } @@ -108,85 +87,75 @@ bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz) int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value) { - if(m_loLimit>m_hiLimit) + if (m_loLimit > m_hiLimit) { - m_currentLimit = 0;//Free from violation + m_currentLimit = 0; //Free from violation return 0; } if (test_value < m_loLimit) { - m_currentLimit = 1;//low limit violation - m_currentLimitError = test_value - m_loLimit; - if(m_currentLimitError>B3_PI) - m_currentLimitError-=B3_2_PI; - else if(m_currentLimitError<-B3_PI) - m_currentLimitError+=B3_2_PI; + m_currentLimit = 1; //low limit violation + m_currentLimitError = test_value - m_loLimit; + if (m_currentLimitError > B3_PI) + m_currentLimitError -= B3_2_PI; + else if (m_currentLimitError < -B3_PI) + m_currentLimitError += B3_2_PI; return 1; } - else if (test_value> m_hiLimit) + else if (test_value > m_hiLimit) { - m_currentLimit = 2;//High limit violation + m_currentLimit = 2; //High limit violation m_currentLimitError = test_value - m_hiLimit; - if(m_currentLimitError>B3_PI) - m_currentLimitError-=B3_2_PI; - else if(m_currentLimitError<-B3_PI) - m_currentLimitError+=B3_2_PI; + if (m_currentLimitError > B3_PI) + m_currentLimitError -= B3_2_PI; + else if (m_currentLimitError < -B3_PI) + m_currentLimitError += B3_2_PI; return 2; }; - m_currentLimit = 0;//Free from violation + m_currentLimit = 0; //Free from violation return 0; - } - - - //////////////////////////// End b3RotationalLimitMotor //////////////////////////////////// - - - //////////////////////////// b3TranslationalLimitMotor //////////////////////////////////// - int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value) { b3Scalar loLimit = m_lowerLimit[limitIndex]; b3Scalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) + if (loLimit > hiLimit) { - m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimit[limitIndex] = 0; //Free from violation m_currentLimitError[limitIndex] = b3Scalar(0.f); return 0; } if (test_value < loLimit) { - m_currentLimit[limitIndex] = 2;//low limit violation - m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimit[limitIndex] = 2; //low limit violation + m_currentLimitError[limitIndex] = test_value - loLimit; return 2; } - else if (test_value> hiLimit) + else if (test_value > hiLimit) { - m_currentLimit[limitIndex] = 1;//High limit violation + m_currentLimit[limitIndex] = 1; //High limit violation m_currentLimitError[limitIndex] = test_value - hiLimit; return 1; }; - m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimit[limitIndex] = 0; //Free from violation m_currentLimitError[limitIndex] = b3Scalar(0.f); return 0; } - - //////////////////////////// b3TranslationalLimitMotor //////////////////////////////////// void b3Generic6DofConstraint::calculateAngleInfo() { - b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); + b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis(); + matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff); // in euler angle mode we do not actually constrain the angular velocity // along the axes axis[0] and axis[2] (although we do use axis[1]) : // @@ -211,12 +180,11 @@ void b3Generic6DofConstraint::calculateAngleInfo() m_calculatedAxis[0].normalize(); m_calculatedAxis[1].normalize(); m_calculatedAxis[2].normalize(); - } static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body) { - b3Transform tr(body.m_quat,body.m_pos); + b3Transform tr(body.m_quat, body.m_pos); return tr; } @@ -226,26 +194,26 @@ void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies) b3Transform transB; transA = getCenterOfMassTransform(bodies[m_rbA]); transB = getCenterOfMassTransform(bodies[m_rbB]); - calculateTransforms(transA,transB,bodies); + calculateTransforms(transA, transB, bodies); } -void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies) +void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies) { m_calculatedTransformA = transA * m_frameInA; m_calculatedTransformB = transB * m_frameInB; calculateLinearInfo(); calculateAngleInfo(); - if(m_useOffsetForConstraintFrame) - { // get weight factors depending on masses + if (m_useOffsetForConstraintFrame) + { // get weight factors depending on masses b3Scalar miA = bodies[m_rbA].m_invMass; b3Scalar miB = bodies[m_rbB].m_invMass; m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON); b3Scalar miS = miA + miB; - if(miS > b3Scalar(0.f)) + if (miS > b3Scalar(0.f)) { m_factA = miB / miS; } - else + else { m_factA = b3Scalar(0.5f); } @@ -253,12 +221,6 @@ void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,cons } } - - - - - - bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index) { b3Scalar angle = m_calculatedAxisAngleDiff[axis_index]; @@ -269,48 +231,43 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index) return m_angularLimits[axis_index].needApplyTorques(); } - - - -void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) { //prepare constraint - calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies); + calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies); info->m_numConstraintRows = 0; info->nub = 6; int i; //test linear limits - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { - if(m_linearLimits.needApplyForce(i)) + if (m_linearLimits.needApplyForce(i)) { info->m_numConstraintRows++; info->nub--; } } //test angular limits - for (i=0;i<3 ;i++ ) + for (i = 0; i < 3; i++) { - if(testAngularLimitMotor(i)) + if (testAngularLimitMotor(i)) { info->m_numConstraintRows++; info->nub--; } } -// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows); + // printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows); } -void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) { //pre-allocate all 6 info->m_numConstraintRows = 6; info->nub = 0; } - -void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies) +void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies) { - b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]); b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]); const b3Vector3& linVelA = bodies[m_rbA].m_linVel; @@ -318,136 +275,124 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod const b3Vector3& angVelA = bodies[m_rbA].m_angVel; const b3Vector3& angVelB = bodies[m_rbB].m_angVel; - if(m_useOffsetForConstraintFrame) - { // for stability better to solve angular limits first - int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); - setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + if (m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } else - { // leave old version for compatibility - int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); - setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } - } - -void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies) +void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies) { - //prepare constraint - calculateTransforms(transA,transB,bodies); + calculateTransforms(transA, transB, bodies); int i; - for (i=0;i<3 ;i++ ) + for (i = 0; i < 3; i++) { testAngularLimitMotor(i); } - if(m_useOffsetForConstraintFrame) - { // for stability better to solve angular limits first - int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); - setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + if (m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } else - { // leave old version for compatibility - int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); - setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB); + setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB); } } - - -int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB) +int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB) { -// int row = 0; + // int row = 0; //solve linear limits b3RotationalLimitMotor limot; - for (int i=0;i<3 ;i++ ) + for (int i = 0; i < 3; i++) { - if(m_linearLimits.needApplyForce(i)) - { // re-use rotational motor code + if (m_linearLimits.needApplyForce(i)) + { // re-use rotational motor code limot.m_bounce = b3Scalar(0.f); limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; - limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; - limot.m_damping = m_linearLimits.m_damping; - limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; - limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; - limot.m_limitSoftness = m_linearLimits.m_limitSoftness; - limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; - limot.m_maxLimitForce = b3Scalar(0.f); - limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; - limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; + limot.m_damping = m_linearLimits.m_damping; + limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; + limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_limitSoftness = m_linearLimits.m_limitSoftness; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxLimitForce = b3Scalar(0.f); + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i); int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT); - limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; - limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; - limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; - if(m_useOffsetForConstraintFrame) + limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; + limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; + if (m_useOffsetForConstraintFrame) { int indx1 = (i + 1) % 3; int indx2 = (i + 2) % 3; - int rotAllowed = 1; // rotations around orthos to current axis - if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) + int rotAllowed = 1; // rotations around orthos to current axis + if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) { rotAllowed = 0; } - row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed); } else { - row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0); + row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0); } } } return row; } - - -int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB) +int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB) { - b3Generic6DofConstraint * d6constraint = this; + b3Generic6DofConstraint* d6constraint = this; int row = row_offset; //solve angular limits - for (int i=0;i<3 ;i++ ) + for (int i = 0; i < 3; i++) { - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) { b3Vector3 axis = d6constraint->getAxis(i); int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT); - if(!(flags & B3_6DOF_FLAGS_CFM_NORM)) + if (!(flags & B3_6DOF_FLAGS_CFM_NORM)) { m_angularLimits[i].m_normalCFM = info->cfm[0]; } - if(!(flags & B3_6DOF_FLAGS_CFM_STOP)) + if (!(flags & B3_6DOF_FLAGS_CFM_STOP)) { m_angularLimits[i].m_stopCFM = info->cfm[0]; } - if(!(flags & B3_6DOF_FLAGS_ERP_STOP)) + if (!(flags & B3_6DOF_FLAGS_ERP_STOP)) { m_angularLimits[i].m_stopERP = info->erp; } row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i), - transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); + transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1); } } return row; } - - - -void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep) +void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep) { (void)timeStep; - } - -void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies) +void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies) { m_frameInA = frameA; m_frameInB = frameB; @@ -455,33 +400,27 @@ void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Trans calculateTransforms(bodies); } - - b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } - -b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const +b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const { return m_calculatedLinearDiff[axisIndex]; } - b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const { return m_calculatedAxisAngleDiff[axisIndex]; } - - void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies) { b3Scalar imA = bodies[m_rbA].m_invMass; b3Scalar imB = bodies[m_rbB].m_invMass; b3Scalar weight; - if(imB == b3Scalar(0.0)) + if (imB == b3Scalar(0.0)) { weight = b3Scalar(1.0); } @@ -495,47 +434,43 @@ void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies) return; } - - void b3Generic6DofConstraint::calculateLinearInfo() { m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); } } - - int b3Generic6DofConstraint::get_limit_motor_info2( - b3RotationalLimitMotor * limot, - const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB, - b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed) + b3RotationalLimitMotor* limot, + const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, + b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed) { - int srow = row * info->rowskip; - bool powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - if (powered || limit) - { // if the joint is powered, or has joint limits, add in the extra row - b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; + int srow = row * info->rowskip; + bool powered = limot->m_enableMotor; + int limit = limot->m_currentLimit; + if (powered || limit) + { // if the joint is powered, or has joint limits, add in the extra row + b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; if (J1) { - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; + J1[srow + 0] = ax1[0]; + J1[srow + 1] = ax1[1]; + J1[srow + 2] = ax1[2]; } if (J2) { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; + J2[srow + 0] = -ax1[0]; + J2[srow + 1] = -ax1[1]; + J2[srow + 2] = -ax1[2]; } - if((!rotational)) - { + if ((!rotational)) + { if (m_useOffsetForConstraintFrame) { b3Vector3 tmpA, tmpB, relA, relB; @@ -558,55 +493,56 @@ int b3Generic6DofConstraint::get_limit_motor_info2( relB = orthoB - totalDist * m_factB; tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); - if(m_hasStaticBody && (!rotAllowed)) + if (m_hasStaticBody && (!rotAllowed)) { tmpA *= m_factA; tmpB *= m_factB; } int i; - for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; - } else + for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i]; + for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i]; + } + else { - b3Vector3 ltd; // Linear Torque Decoupling vector + b3Vector3 ltd; // Linear Torque Decoupling vector b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = ltd[0]; - info->m_J1angularAxis[srow+1] = ltd[1]; - info->m_J1angularAxis[srow+2] = ltd[2]; + info->m_J1angularAxis[srow + 0] = ltd[0]; + info->m_J1angularAxis[srow + 1] = ltd[1]; + info->m_J1angularAxis[srow + 2] = ltd[2]; c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); ltd = -c.cross(ax1); - info->m_J2angularAxis[srow+0] = ltd[0]; - info->m_J2angularAxis[srow+1] = ltd[1]; - info->m_J2angularAxis[srow+2] = ltd[2]; + info->m_J2angularAxis[srow + 0] = ltd[0]; + info->m_J2angularAxis[srow + 1] = ltd[1]; + info->m_J2angularAxis[srow + 2] = ltd[2]; } - } - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false; - info->m_constraintError[srow] = b3Scalar(0.f); - if (powered) - { + } + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false; + info->m_constraintError[srow] = b3Scalar(0.f); + if (powered) + { info->cfm[srow] = limot->m_normalCFM; - if(!limit) - { + if (!limit) + { b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; - b3Scalar mot_fact = getMotorFactor( limot->m_currentPosition, - limot->m_loLimit, - limot->m_hiLimit, - tag_vel, - info->fps * limot->m_stopERP); + b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition, + limot->m_loLimit, + limot->m_hiLimit, + tag_vel, + info->fps * limot->m_stopERP); info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; - info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; - } - } - if(limit) - { - b3Scalar k = info->fps * limot->m_stopERP; - if(!rotational) + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; + } + } + if (limit) + { + b3Scalar k = info->fps * limot->m_stopERP; + if (!rotational) { info->m_constraintError[srow] += k * limot->m_currentLimitError; } @@ -615,116 +551,112 @@ int b3Generic6DofConstraint::get_limit_motor_info2( info->m_constraintError[srow] += -k * limot->m_currentLimitError; } info->cfm[srow] = limot->m_stopCFM; - if (limot->m_loLimit == limot->m_hiLimit) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -B3_INFINITY; - info->m_upperLimit[srow] = B3_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = B3_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -B3_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - b3Scalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - b3Scalar newc = -limot->m_bounce* vel; - if (newc > info->m_constraintError[srow]) + if (limot->m_loLimit == limot->m_hiLimit) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -B3_INFINITY; + info->m_upperLimit[srow] = B3_INFINITY; + } + else + { + if (limit == 1) + { + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = B3_INFINITY; + } + else + { + info->m_lowerLimit[srow] = -B3_INFINITY; + info->m_upperLimit[srow] = 0; + } + // deal with bounce + if (limot->m_bounce > 0) + { + // calculate joint velocity + b3Scalar vel; + if (rotational) + { + vel = angVelA.dot(ax1); + //make sure that if no body -> angVelB == zero vec + // if (body1) + vel -= angVelB.dot(ax1); + } + else + { + vel = linVelA.dot(ax1); + //make sure that if no body -> angVelB == zero vec + // if (body1) + vel -= linVelB.dot(ax1); + } + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) + { + if (vel < 0) + { + b3Scalar newc = -limot->m_bounce * vel; + if (newc > info->m_constraintError[srow]) info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - b3Scalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) + } + } + else + { + if (vel > 0) + { + b3Scalar newc = -limot->m_bounce * vel; + if (newc < info->m_constraintError[srow]) info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else return 0; + } + } + } + } + } + return 1; + } + else + return 0; } - - - - - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis) { - if((axis >= 0) && (axis < 3)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case B3_CONSTRAINT_STOP_ERP : + case B3_CONSTRAINT_STOP_ERP: m_linearLimits.m_stopERP[axis] = value; m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); break; - case B3_CONSTRAINT_STOP_CFM : + case B3_CONSTRAINT_STOP_CFM: m_linearLimits.m_stopCFM[axis] = value; m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); break; - case B3_CONSTRAINT_CFM : + case B3_CONSTRAINT_CFM: m_linearLimits.m_normalCFM[axis] = value; m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); break; - default : + default: b3AssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case B3_CONSTRAINT_STOP_ERP : + case B3_CONSTRAINT_STOP_ERP: m_angularLimits[axis - 3].m_stopERP = value; m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); break; - case B3_CONSTRAINT_STOP_CFM : + case B3_CONSTRAINT_STOP_CFM: m_angularLimits[axis - 3].m_stopCFM = value; m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); break; - case B3_CONSTRAINT_CFM : + case B3_CONSTRAINT_CFM: m_angularLimits[axis - 3].m_normalCFM = value; m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); break; - default : + default: b3AssertConstrParams(0); } } @@ -734,47 +666,47 @@ void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis) } } - ///return the local value of parameter -b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const +///return the local value of parameter +b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const { b3Scalar retVal = 0; - if((axis >= 0) && (axis < 3)) + if ((axis >= 0) && (axis < 3)) { - switch(num) + switch (num) { - case B3_CONSTRAINT_STOP_ERP : + case B3_CONSTRAINT_STOP_ERP: b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_stopERP[axis]; break; - case B3_CONSTRAINT_STOP_CFM : + case B3_CONSTRAINT_STOP_CFM: b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_stopCFM[axis]; break; - case B3_CONSTRAINT_CFM : + case B3_CONSTRAINT_CFM: b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_linearLimits.m_normalCFM[axis]; break; - default : + default: b3AssertConstrParams(0); } } - else if((axis >=3) && (axis < 6)) + else if ((axis >= 3) && (axis < 6)) { - switch(num) + switch (num) { - case B3_CONSTRAINT_STOP_ERP : + case B3_CONSTRAINT_STOP_ERP: b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_stopERP; break; - case B3_CONSTRAINT_STOP_CFM : + case B3_CONSTRAINT_STOP_CFM: b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_stopCFM; break; - case B3_CONSTRAINT_CFM : + case B3_CONSTRAINT_CFM: b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); retVal = m_angularLimits[axis - 3].m_normalCFM; break; - default : + default: b3AssertConstrParams(0); } } @@ -785,23 +717,21 @@ b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const return retVal; } - - -void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies) +void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies) { b3Vector3 zAxis = axis1.normalized(); b3Vector3 yAxis = axis2.normalized(); - b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system - + b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + b3Transform frameInW; frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - + frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + // now get constraint frame in local coordinate systems m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW; m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW; - + calculateTransforms(bodies); } diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h index 169b1b94ad..1597809db3 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -23,7 +23,6 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - #ifndef B3_GENERIC_6DOF_CONSTRAINT_H #define B3_GENERIC_6DOF_CONSTRAINT_H @@ -33,88 +32,83 @@ http://gimpact.sf.net struct b3RigidBodyData; - - - //! Rotation Limit structure for generic joints class b3RotationalLimitMotor { public: - //! limit_parameters - //!@{ - b3Scalar m_loLimit;//!< joint limit - b3Scalar m_hiLimit;//!< joint limit - b3Scalar m_targetVelocity;//!< target motor velocity - b3Scalar m_maxMotorForce;//!< max force on motor - b3Scalar m_maxLimitForce;//!< max force on limit - b3Scalar m_damping;//!< Damping. - b3Scalar m_limitSoftness;//! Relaxation factor - b3Scalar m_normalCFM;//!< Constraint force mixing factor - b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit - b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit - b3Scalar m_bounce;//!< restitution factor - bool m_enableMotor; - - //!@} - - //! temp_variables - //!@{ - b3Scalar m_currentLimitError;//! How much is violated this limit - b3Scalar m_currentPosition; //! current value of angle - int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit - b3Scalar m_accumulatedImpulse; - //!@} - - b3RotationalLimitMotor() - { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 6.0f; - m_maxLimitForce = 300.0f; - m_loLimit = 1.0f; - m_hiLimit = -1.0f; + //! limit_parameters + //!@{ + b3Scalar m_loLimit; //!< joint limit + b3Scalar m_hiLimit; //!< joint limit + b3Scalar m_targetVelocity; //!< target motor velocity + b3Scalar m_maxMotorForce; //!< max force on motor + b3Scalar m_maxLimitForce; //!< max force on limit + b3Scalar m_damping; //!< Damping. + b3Scalar m_limitSoftness; //! Relaxation factor + b3Scalar m_normalCFM; //!< Constraint force mixing factor + b3Scalar m_stopERP; //!< Error tolerance factor when joint is at limit + b3Scalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit + b3Scalar m_bounce; //!< restitution factor + bool m_enableMotor; + + //!@} + + //! temp_variables + //!@{ + b3Scalar m_currentLimitError; //! How much is violated this limit + b3Scalar m_currentPosition; //! current value of angle + int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit + b3Scalar m_accumulatedImpulse; + //!@} + + b3RotationalLimitMotor() + { + m_accumulatedImpulse = 0.f; + m_targetVelocity = 0; + m_maxMotorForce = 6.0f; + m_maxLimitForce = 300.0f; + m_loLimit = 1.0f; + m_hiLimit = -1.0f; m_normalCFM = 0.f; m_stopERP = 0.2f; m_stopCFM = 0.f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - } - - b3RotationalLimitMotor(const b3RotationalLimitMotor & limot) - { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; + m_bounce = 0.0f; + m_damping = 1.0f; + m_limitSoftness = 0.5f; + m_currentLimit = 0; + m_currentLimitError = 0; + m_enableMotor = false; + } + + b3RotationalLimitMotor(const b3RotationalLimitMotor& limot) + { + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_limitSoftness = limot.m_limitSoftness; + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; m_normalCFM = limot.m_normalCFM; m_stopERP = limot.m_stopERP; - m_stopCFM = limot.m_stopCFM; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - - + m_stopCFM = limot.m_stopCFM; + m_bounce = limot.m_bounce; + m_currentLimit = limot.m_currentLimit; + m_currentLimitError = limot.m_currentLimitError; + m_enableMotor = limot.m_enableMotor; + } //! Is limited - bool isLimited() - { - if(m_loLimit > m_hiLimit) return false; - return true; - } + bool isLimited() + { + if (m_loLimit > m_hiLimit) return false; + return true; + } //! Need apply correction - bool needApplyTorques() - { - if(m_currentLimit == 0 && m_enableMotor == false) return false; - return true; - } + bool needApplyTorques() + { + if (m_currentLimit == 0 && m_enableMotor == false) return false; + return true; + } //! calculates error /*! @@ -123,104 +117,98 @@ public: int testLimitValue(b3Scalar test_value); //! apply the correction impulses for two bodies - b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1); - + b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1); }; - - class b3TranslationalLimitMotor { public: - b3Vector3 m_lowerLimit;//!< the constraint lower limits - b3Vector3 m_upperLimit;//!< the constraint upper limits - b3Vector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - b3Vector3 m_normalCFM;//!< Constraint force mixing factor - b3Vector3 m_stopERP;//!< Error tolerance factor when joint is at limit - b3Vector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit - b3Vector3 m_targetVelocity;//!< target motor velocity - b3Vector3 m_maxMotorForce;//!< max force on motor - b3Vector3 m_currentLimitError;//! How much is violated this limit - b3Vector3 m_currentLinearDiff;//! Current relative offset of constraint frames - b3Scalar m_limitSoftness;//!< Softness for linear limit - b3Scalar m_damping;//!< Damping for linear limit - b3Scalar m_restitution;//! Bounce parameter for linear limit + b3Vector3 m_lowerLimit; //!< the constraint lower limits + b3Vector3 m_upperLimit; //!< the constraint upper limits + b3Vector3 m_accumulatedImpulse; + //! Linear_Limit_parameters + //!@{ + b3Vector3 m_normalCFM; //!< Constraint force mixing factor + b3Vector3 m_stopERP; //!< Error tolerance factor when joint is at limit + b3Vector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit + b3Vector3 m_targetVelocity; //!< target motor velocity + b3Vector3 m_maxMotorForce; //!< max force on motor + b3Vector3 m_currentLimitError; //! How much is violated this limit + b3Vector3 m_currentLinearDiff; //! Current relative offset of constraint frames + b3Scalar m_limitSoftness; //!< Softness for linear limit + b3Scalar m_damping; //!< Damping for linear limit + b3Scalar m_restitution; //! Bounce parameter for linear limit //!@} - bool m_enableMotor[3]; - int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit - - b3TranslationalLimitMotor() - { - m_lowerLimit.setValue(0.f,0.f,0.f); - m_upperLimit.setValue(0.f,0.f,0.f); - m_accumulatedImpulse.setValue(0.f,0.f,0.f); + bool m_enableMotor[3]; + int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit + + b3TranslationalLimitMotor() + { + m_lowerLimit.setValue(0.f, 0.f, 0.f); + m_upperLimit.setValue(0.f, 0.f, 0.f); + m_accumulatedImpulse.setValue(0.f, 0.f, 0.f); m_normalCFM.setValue(0.f, 0.f, 0.f); m_stopERP.setValue(0.2f, 0.2f, 0.2f); m_stopCFM.setValue(0.f, 0.f, 0.f); - m_limitSoftness = 0.7f; - m_damping = b3Scalar(1.0f); - m_restitution = b3Scalar(0.5f); - for(int i=0; i < 3; i++) + m_limitSoftness = 0.7f; + m_damping = b3Scalar(1.0f); + m_restitution = b3Scalar(0.5f); + for (int i = 0; i < 3; i++) { m_enableMotor[i] = false; m_targetVelocity[i] = b3Scalar(0.f); m_maxMotorForce[i] = b3Scalar(0.f); } - } + } - b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other ) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; + b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other) + { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_accumulatedImpulse = other.m_accumulatedImpulse; - m_limitSoftness = other.m_limitSoftness ; - m_damping = other.m_damping; - m_restitution = other.m_restitution; + m_limitSoftness = other.m_limitSoftness; + m_damping = other.m_damping; + m_restitution = other.m_restitution; m_normalCFM = other.m_normalCFM; m_stopERP = other.m_stopERP; m_stopCFM = other.m_stopCFM; - for(int i=0; i < 3; i++) + for (int i = 0; i < 3; i++) { m_enableMotor[i] = other.m_enableMotor[i]; m_targetVelocity[i] = other.m_targetVelocity[i]; m_maxMotorForce[i] = other.m_maxMotorForce[i]; } - } + } - //! Test limit + //! Test limit /*! - free means upper < lower, - locked means upper == lower - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - inline bool isLimited(int limitIndex) - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - inline bool needApplyForce(int limitIndex) - { - if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; - return true; - } + inline bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + inline bool needApplyForce(int limitIndex) + { + if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; + return true; + } int testLimitValue(int limitIndex, b3Scalar test_value); - - b3Scalar solveLinearAxis( - b3Scalar timeStep, - b3Scalar jacDiagABInv, - b3RigidBodyData& body1,const b3Vector3 &pointInA, - b3RigidBodyData& body2,const b3Vector3 &pointInB, - int limit_index, - const b3Vector3 & axis_normal_on_a, - const b3Vector3 & anchorPos); - - + b3Scalar solveLinearAxis( + b3Scalar timeStep, + b3Scalar jacDiagABInv, + b3RigidBodyData& body1, const b3Vector3& pointInA, + b3RigidBodyData& body2, const b3Vector3& pointInB, + int limit_index, + const b3Vector3& axis_normal_on_a, + const b3Vector3& anchorPos); }; enum b36DofFlags @@ -229,8 +217,7 @@ enum b36DofFlags B3_6DOF_FLAGS_CFM_STOP = 2, B3_6DOF_FLAGS_ERP_STOP = 4 }; -#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis - +#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis /// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /*! @@ -268,240 +255,229 @@ This brings support for limit parameters and motors. </li> </ul> */ -B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint +B3_ATTRIBUTE_ALIGNED16(class) +b3Generic6DofConstraint : public b3TypedConstraint { protected: - //! relative_frames - //!@{ - b3Transform m_frameInA;//!< the constraint space w.r.t body A - b3Transform m_frameInB;//!< the constraint space w.r.t body B - //!@} + //!@{ + b3Transform m_frameInA; //!< the constraint space w.r.t body A + b3Transform m_frameInB; //!< the constraint space w.r.t body B + //!@} - //! Jacobians - //!@{ -// b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints -// b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} + //! Jacobians + //!@{ + // b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints + // b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints + //!@} //! Linear_Limit_parameters - //!@{ - b3TranslationalLimitMotor m_linearLimits; - //!@} - - - //! hinge_parameters - //!@{ - b3RotationalLimitMotor m_angularLimits[3]; + //!@{ + b3TranslationalLimitMotor m_linearLimits; //!@} + //! hinge_parameters + //!@{ + b3RotationalLimitMotor m_angularLimits[3]; + //!@} protected: - //! temporal variables - //!@{ - b3Transform m_calculatedTransformA; - b3Transform m_calculatedTransformB; - b3Vector3 m_calculatedAxisAngleDiff; - b3Vector3 m_calculatedAxis[3]; - b3Vector3 m_calculatedLinearDiff; - b3Scalar m_timeStep; - b3Scalar m_factA; - b3Scalar m_factB; - bool m_hasStaticBody; - - b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes + //! temporal variables + //!@{ + b3Transform m_calculatedTransformA; + b3Transform m_calculatedTransformB; + b3Vector3 m_calculatedAxisAngleDiff; + b3Vector3 m_calculatedAxis[3]; + b3Vector3 m_calculatedLinearDiff; + b3Scalar m_timeStep; + b3Scalar m_factA; + b3Scalar m_factB; + bool m_hasStaticBody; - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; + b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - //!@} + bool m_useLinearReferenceFrameA; + bool m_useOffsetForConstraintFrame; - b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) - { - b3Assert(0); - (void) other; - return *this; - } + int m_flags; + //!@} - int setAngularLimits(b3ConstraintInfo2 *info, int row_offset,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB); + b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) + { + b3Assert(0); + (void)other; + return *this; + } - int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB); + int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB); + int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB); // tests linear limits void calculateLinearInfo(); //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - - + void calculateAngleInfo(); public: - B3_DECLARE_ALIGNED_ALLOCATOR(); - - b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies); - + + b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies); + //! Calcs global transform of the offsets /*! Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. \sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo */ - void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies); + void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies); void calculateTransforms(const b3RigidBodyData* bodies); //! Gets the global transform of the offset for body A - /*! + /*! \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. */ - const b3Transform & getCalculatedTransformA() const - { - return m_calculatedTransformA; - } + const b3Transform& getCalculatedTransformA() const + { + return m_calculatedTransformA; + } - //! Gets the global transform of the offset for body B - /*! + //! Gets the global transform of the offset for body B + /*! \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. */ - const b3Transform & getCalculatedTransformB() const - { - return m_calculatedTransformB; - } - - const b3Transform & getFrameOffsetA() const - { - return m_frameInA; - } - - const b3Transform & getFrameOffsetB() const - { - return m_frameInB; - } - - - b3Transform & getFrameOffsetA() - { - return m_frameInA; - } + const b3Transform& getCalculatedTransformB() const + { + return m_calculatedTransformB; + } - b3Transform & getFrameOffsetB() - { - return m_frameInB; - } + const b3Transform& getFrameOffsetA() const + { + return m_frameInA; + } + const b3Transform& getFrameOffsetB() const + { + return m_frameInB; + } + b3Transform& getFrameOffsetA() + { + return m_frameInA; + } - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + b3Transform& getFrameOffsetB() + { + return m_frameInB; + } - void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies); + void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies); + virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies); + void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies); - void updateRHS(b3Scalar timeStep); + void updateRHS(b3Scalar timeStep); //! Get the rotation axis in global coordinates - b3Vector3 getAxis(int axis_index) const; + b3Vector3 getAxis(int axis_index) const; - //! Get the relative Euler angle - /*! + //! Get the relative Euler angle + /*! \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. */ - b3Scalar getAngle(int axis_index) const; + b3Scalar getAngle(int axis_index) const; //! Get the relative position of the constraint pivot - /*! + /*! \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. */ b3Scalar getRelativePivotPosition(int axis_index) const; - void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyData* bodies); + void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies); //! Test angular limit. /*! Calculates angular correction and returns true if limit needs to be corrected. \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. */ - bool testAngularLimitMotor(int axis_index); + bool testAngularLimitMotor(int axis_index); - void setLinearLowerLimit(const b3Vector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } + void setLinearLowerLimit(const b3Vector3& linearLower) + { + m_linearLimits.m_lowerLimit = linearLower; + } - void getLinearLowerLimit(b3Vector3& linearLower) + void getLinearLowerLimit(b3Vector3 & linearLower) { linearLower = m_linearLimits.m_lowerLimit; } - void setLinearUpperLimit(const b3Vector3& linearUpper) + void setLinearUpperLimit(const b3Vector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } - void getLinearUpperLimit(b3Vector3& linearUpper) + void getLinearUpperLimit(b3Vector3 & linearUpper) { linearUpper = m_linearLimits.m_upperLimit; } - void setAngularLowerLimit(const b3Vector3& angularLower) - { - for(int i = 0; i < 3; i++) + void setAngularLowerLimit(const b3Vector3& angularLower) + { + for (int i = 0; i < 3; i++) m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]); - } + } - void getAngularLowerLimit(b3Vector3& angularLower) + void getAngularLowerLimit(b3Vector3 & angularLower) { - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) angularLower[i] = m_angularLimits[i].m_loLimit; } - void setAngularUpperLimit(const b3Vector3& angularUpper) - { - for(int i = 0; i < 3; i++) + void setAngularUpperLimit(const b3Vector3& angularUpper) + { + for (int i = 0; i < 3; i++) m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]); - } + } - void getAngularUpperLimit(b3Vector3& angularUpper) + void getAngularUpperLimit(b3Vector3 & angularUpper) { - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) angularUpper[i] = m_angularLimits[i].m_hiLimit; } //! Retrieves the angular limit informacion - b3RotationalLimitMotor * getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - b3TranslationalLimitMotor * getTranslationalLimitMotor() - { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, b3Scalar lo, b3Scalar hi) - { - if(axis<3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { + b3RotationalLimitMotor* getRotationalLimitMotor(int index) + { + return &m_angularLimits[index]; + } + + //! Retrieves the limit informacion + b3TranslationalLimitMotor* getTranslationalLimitMotor() + { + return &m_linearLimits; + } + + //first 3 are linear, next 3 are angular + void setLimit(int axis, b3Scalar lo, b3Scalar hi) + { + if (axis < 3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { lo = b3NormalizeAngle(lo); hi = b3NormalizeAngle(hi); - m_angularLimits[axis-3].m_loLimit = lo; - m_angularLimits[axis-3].m_hiLimit = hi; - } - } + m_angularLimits[axis - 3].m_loLimit = lo; + m_angularLimits[axis - 3].m_hiLimit = hi; + } + } //! Test limit /*! @@ -510,41 +486,32 @@ public: - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - bool isLimited(int limitIndex) - { - if(limitIndex<3) - { + bool isLimited(int limitIndex) + { + if (limitIndex < 3) + { return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex - 3].isLimited(); + } - } - return m_angularLimits[limitIndex-3].isLimited(); - } - - virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable + virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable - int get_limit_motor_info2( b3RotationalLimitMotor * limot, - const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB, - b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false); + int get_limit_motor_info2(b3RotationalLimitMotor * limot, + const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, + b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false); // access for UseFrameOffset bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, b3Scalar value, int axis = -1); + virtual void setParam(int num, b3Scalar value, int axis = -1); ///return the local value of parameter - virtual b3Scalar getParam(int num, int axis = -1) const; - - void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyData* bodies); + virtual b3Scalar getParam(int num, int axis = -1) const; - - - + void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies); }; - - - - -#endif //B3_GENERIC_6DOF_CONSTRAINT_H +#endif //B3_GENERIC_6DOF_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h index a55168eb38..13269debf6 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h @@ -18,7 +18,6 @@ subject to the following restrictions: #include "Bullet3Common/b3Matrix3x3.h" - //notes: // Another memory optimization would be to store m_1MinvJt in the remaining 3 w components // which makes the b3JacobianEntry memory layout 16 bytes @@ -27,25 +26,26 @@ subject to the following restrictions: /// Jacobian entry is an abstraction that allows to describe constraints /// it can be used in combination with a constraint solver /// Can be used to relate the effect of an impulse to the constraint error -B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry +B3_ATTRIBUTE_ALIGNED16(class) +b3JacobianEntry { public: - b3JacobianEntry() {}; + b3JacobianEntry(){}; //constraint between two different rigidbodies b3JacobianEntry( const b3Matrix3x3& world2A, const b3Matrix3x3& world2B, - const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, const b3Vector3& jointAxis, - const b3Vector3& inertiaInvA, + const b3Vector3& inertiaInvA, const b3Scalar massInvA, const b3Vector3& inertiaInvB, const b3Scalar massInvB) - :m_linearJointAxis(jointAxis) + : m_linearJointAxis(jointAxis) { - m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); - m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; + m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); @@ -54,33 +54,31 @@ public: //angular constraint between two different rigidbodies b3JacobianEntry(const b3Vector3& jointAxis, - const b3Matrix3x3& world2A, - const b3Matrix3x3& world2B, - const b3Vector3& inertiaInvA, - const b3Vector3& inertiaInvB) - :m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.))) + const b3Matrix3x3& world2A, + const b3Matrix3x3& world2B, + const b3Vector3& inertiaInvA, + const b3Vector3& inertiaInvB) + : m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))) { - m_aJ= world2A*jointAxis; - m_bJ = world2B*-jointAxis; - m_0MinvJt = inertiaInvA * m_aJ; + m_aJ = world2A * jointAxis; + m_bJ = world2B * -jointAxis; + m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); b3Assert(m_Adiag > b3Scalar(0.0)); } //angular constraint between two different rigidbodies b3JacobianEntry(const b3Vector3& axisInA, - const b3Vector3& axisInB, - const b3Vector3& inertiaInvA, - const b3Vector3& inertiaInvB) - : m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.))) - , m_aJ(axisInA) - , m_bJ(-axisInB) + const b3Vector3& axisInB, + const b3Vector3& inertiaInvA, + const b3Vector3& inertiaInvB) + : m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB) { - m_0MinvJt = inertiaInvA * m_aJ; + m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); b3Assert(m_Adiag > b3Scalar(0.0)); } @@ -88,25 +86,25 @@ public: //constraint on one rigidbody b3JacobianEntry( const b3Matrix3x3& world2A, - const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, const b3Vector3& jointAxis, - const b3Vector3& inertiaInvA, + const b3Vector3& inertiaInvA, const b3Scalar massInvA) - :m_linearJointAxis(jointAxis) + : m_linearJointAxis(jointAxis) { - m_aJ= world2A*(rel_pos1.cross(jointAxis)); - m_bJ = world2A*(rel_pos2.cross(-jointAxis)); - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + m_aJ = world2A * (rel_pos1.cross(jointAxis)); + m_bJ = world2A * (rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); b3Assert(m_Adiag > b3Scalar(0.0)); } - b3Scalar getDiagonal() const { return m_Adiag; } + b3Scalar getDiagonal() const { return m_Adiag; } // for two constraints on the same rigidbody (for example vehicle friction) - b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const + b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const { const b3JacobianEntry& jacA = *this; b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); @@ -114,42 +112,39 @@ public: return lin + ang; } - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - b3Scalar getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const + b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const { const b3JacobianEntry& jacA = *this; b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; - b3Vector3 lin0 = massInvA * lin ; + b3Vector3 lin0 = massInvA * lin; b3Vector3 lin1 = massInvB * lin; - b3Vector3 sum = ang0+ang1+lin0+lin1; - return sum[0]+sum[1]+sum[2]; + b3Vector3 sum = ang0 + ang1 + lin0 + lin1; + return sum[0] + sum[1] + sum[2]; } - b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB) + b3Scalar getRelativeVelocity(const b3Vector3& linvelA, const b3Vector3& angvelA, const b3Vector3& linvelB, const b3Vector3& angvelB) { b3Vector3 linrel = linvelA - linvelB; - b3Vector3 angvela = angvelA * m_aJ; - b3Vector3 angvelb = angvelB * m_bJ; + b3Vector3 angvela = angvelA * m_aJ; + b3Vector3 angvelb = angvelB * m_bJ; linrel *= m_linearJointAxis; angvela += angvelb; angvela += linrel; - b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + b3Scalar rel_vel2 = angvela[0] + angvela[1] + angvela[2]; return rel_vel2 + B3_EPSILON; } -//private: + //private: - b3Vector3 m_linearJointAxis; - b3Vector3 m_aJ; - b3Vector3 m_bJ; - b3Vector3 m_0MinvJt; - b3Vector3 m_1MinvJt; + b3Vector3 m_linearJointAxis; + b3Vector3 m_aJ; + b3Vector3 m_bJ; + b3Vector3 m_0MinvJt; + b3Vector3 m_1MinvJt; //Optimization: can be stored in the w/last component of one of the vectors - b3Scalar m_Adiag; - + b3Scalar m_Adiag; }; -#endif //B3_JACOBIAN_ENTRY_H +#endif //B3_JACOBIAN_ENTRY_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp index de729d4556..b7050b1070 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -29,14 +29,13 @@ subject to the following restrictions: //#include "b3SolverBody.h" //#include "b3SolverConstraint.h" #include "Bullet3Common/b3AlignedObjectArray.h" -#include <string.h> //for memset +#include <string.h> //for memset //#include "../../dynamics/basic_demo/Stubs/AdlContact4.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" - #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" -static b3Transform getWorldTransform(b3RigidBodyData* rb) +static b3Transform getWorldTransform(b3RigidBodyData* rb) { b3Transform newTrans; newTrans.setOrigin(rb->m_pos); @@ -44,19 +43,17 @@ static b3Transform getWorldTransform(b3RigidBodyData* rb) return newTrans; } -static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) { return inertia->m_invInertiaWorld; } - - -static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) { return rb->m_linVel; } -static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) { return rb->m_angVel; } @@ -65,47 +62,46 @@ static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& r { //we also calculate lin/ang velocity for kinematic objects return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); - } -struct b3ContactPoint +struct b3ContactPoint { - b3Vector3 m_positionWorldOnA; - b3Vector3 m_positionWorldOnB; - b3Vector3 m_normalWorldOnB; - b3Scalar m_appliedImpulse; - b3Scalar m_distance; - b3Scalar m_combinedRestitution; + b3Vector3 m_positionWorldOnA; + b3Vector3 m_positionWorldOnB; + b3Vector3 m_normalWorldOnB; + b3Scalar m_appliedImpulse; + b3Scalar m_distance; + b3Scalar m_combinedRestitution; ///information related to friction - b3Scalar m_combinedFriction; - b3Vector3 m_lateralFrictionDir1; - b3Vector3 m_lateralFrictionDir2; - b3Scalar m_appliedImpulseLateral1; - b3Scalar m_appliedImpulseLateral2; - b3Scalar m_combinedRollingFriction; - b3Scalar m_contactMotion1; - b3Scalar m_contactMotion2; - b3Scalar m_contactCFM1; - b3Scalar m_contactCFM2; - - bool m_lateralFrictionInitialized; - - b3Vector3 getPositionWorldOnA() + b3Scalar m_combinedFriction; + b3Vector3 m_lateralFrictionDir1; + b3Vector3 m_lateralFrictionDir2; + b3Scalar m_appliedImpulseLateral1; + b3Scalar m_appliedImpulseLateral2; + b3Scalar m_combinedRollingFriction; + b3Scalar m_contactMotion1; + b3Scalar m_contactMotion2; + b3Scalar m_contactCFM1; + b3Scalar m_contactCFM2; + + bool m_lateralFrictionInitialized; + + b3Vector3 getPositionWorldOnA() { return m_positionWorldOnA; } - b3Vector3 getPositionWorldOnB() + b3Vector3 getPositionWorldOnB() { return m_positionWorldOnB; } - b3Scalar getDistance() + b3Scalar getDistance() { return m_distance; } }; -void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut) +void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut) { pointOut.m_appliedImpulse = 0.f; pointOut.m_appliedImpulseLateral1 = 0.f; @@ -117,160 +113,145 @@ void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& poin pointOut.m_contactCFM2 = 0.f; pointOut.m_contactMotion1 = 0.f; pointOut.m_contactMotion2 = 0.f; - pointOut.m_distance = contact->getPenetration(contactIndex);//??0.01f + pointOut.m_distance = contact->getPenetration(contactIndex); //??0.01f b3Vector3 normalOnB = contact->m_worldNormalOnB; - normalOnB.normalize();//is this needed? + normalOnB.normalize(); //is this needed? - b3Vector3 l1,l2; - b3PlaneSpace1(normalOnB,l1,l2); + b3Vector3 l1, l2; + b3PlaneSpace1(normalOnB, l1, l2); pointOut.m_normalWorldOnB = normalOnB; //printf("normalOnB = %f,%f,%f\n",normalOnB.getX(),normalOnB.getY(),normalOnB.getZ()); pointOut.m_lateralFrictionDir1 = l1; pointOut.m_lateralFrictionDir2 = l2; pointOut.m_lateralFrictionInitialized = true; - - + b3Vector3 worldPosB = contact->m_worldPosB[contactIndex]; pointOut.m_positionWorldOnB = worldPosB; - pointOut.m_positionWorldOnA = worldPosB+normalOnB*pointOut.m_distance; + pointOut.m_positionWorldOnA = worldPosB + normalOnB * pointOut.m_distance; } -int getNumContacts(b3Contact4* contact) +int getNumContacts(b3Contact4* contact) { return contact->getNPoints(); } b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs) -:m_usePgs(usePgs), -m_numSplitImpulseRecoveries(0), -m_btSeed2(0) + : m_usePgs(usePgs), + m_numSplitImpulseRecoveries(0), + m_btSeed2(0) { - } b3PgsJacobiSolver::~b3PgsJacobiSolver() { } -void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints) +void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints) { b3ContactSolverInfo infoGlobal; infoGlobal.m_splitImpulse = false; - infoGlobal.m_timeStep = 1.f/60.f; - infoGlobal.m_numIterations = 4;//4; -// infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; + infoGlobal.m_timeStep = 1.f / 60.f; + infoGlobal.m_numIterations = 4; //4; + // infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; //infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; - infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS; + infoGlobal.m_solverMode |= B3_SOLVER_USE_2_FRICTION_DIRECTIONS; //if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) //if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - - solveGroup(bodies,inertias,numBodies,contacts,numContacts,constraints,numConstraints,infoGlobal); + solveGroup(bodies, inertias, numBodies, contacts, numContacts, constraints, numConstraints, infoGlobal); if (!numContacts) return; } - - - /// b3PgsJacobiSolver Sequentially applies impulses b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies, - b3InertiaData* inertias, - int numBodies, - b3Contact4* manifoldPtr, - int numManifolds, - b3TypedConstraint** constraints, - int numConstraints, - const b3ContactSolverInfo& infoGlobal) + b3InertiaData* inertias, + int numBodies, + b3Contact4* manifoldPtr, + int numManifolds, + b3TypedConstraint** constraints, + int numConstraints, + const b3ContactSolverInfo& infoGlobal) { - B3_PROFILE("solveGroup"); //you need to provide at least some bodies - - solveGroupCacheFriendlySetup( bodies, inertias,numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal); - - solveGroupCacheFriendlyIterations(constraints, numConstraints,infoGlobal); - - solveGroupCacheFriendlyFinish(bodies, inertias,numBodies, infoGlobal); - - return 0.f; -} - - - - + solveGroupCacheFriendlySetup(bodies, inertias, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal); + solveGroupCacheFriendlyIterations(constraints, numConstraints, infoGlobal); + solveGroupCacheFriendlyFinish(bodies, inertias, numBodies, infoGlobal); + return 0.f; +} #ifdef USE_SIMD #include <emmintrin.h> -#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) -static inline __m128 b3SimdDot3( __m128 vec0, __m128 vec1 ) +#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e)) +static inline __m128 b3SimdDot3(__m128 vec0, __m128 vec1) { - __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( b3VecSplat( result, 0 ), _mm_add_ps( b3VecSplat( result, 1 ), b3VecSplat( result, 2 ) ) ); + __m128 result = _mm_mul_ps(vec0, vec1); + return _mm_add_ps(b3VecSplat(result, 0), _mm_add_ps(b3VecSplat(result, 1), b3VecSplat(result, 2))); } -#endif//USE_SIMD +#endif //USE_SIMD // Project Gauss Seidel or the equivalent Sequential Impulse -void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& 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))); - __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - b3SimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __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))); + __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetDeltaLinearVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); + b3SimdScalar resultLowerLess, resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum, upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); + deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); + c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); + __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp); + deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied)); + c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1)); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_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)); + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_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)); #else - resolveSingleConstraintRowGeneric(body1,body2,c); + resolveSingleConstraintRowGeneric(body1, body2, c); #endif } // Project Gauss Seidel or the equivalent Sequential Impulse - void b3PgsJacobiSolver::resolveSingleConstraintRowGeneric(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +void b3PgsJacobiSolver::resolveSingleConstraintRowGeneric(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c) { - b3Scalar deltaImpulse = c.m_rhs-b3Scalar(c.m_appliedImpulse)*c.m_cfm; - const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + b3Scalar deltaImpulse = c.m_rhs - b3Scalar(c.m_appliedImpulse) * c.m_cfm; + const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); -// const b3Scalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + // const b3Scalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; } - else if (sum > c.m_upperLimit) + else if (sum > c.m_upperLimit) { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_upperLimit; } else @@ -278,94 +259,93 @@ void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + body1.internalApplyImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } - void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& 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))); - __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - b3SimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __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))); + __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetDeltaLinearVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); + b3SimdScalar resultLowerLess, resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum, upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); + deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); + c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_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)); + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_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)); #else - resolveSingleConstraintRowLowerLimit(body1,body2,c); + resolveSingleConstraintRowLowerLimit(body1, body2, c); #endif } // Project Gauss Seidel or the equivalent Sequential Impulse - void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimit(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimit(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c) { - b3Scalar deltaImpulse = c.m_rhs-b3Scalar(c.m_appliedImpulse)*c.m_cfm; - const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + b3Scalar deltaImpulse = c.m_rhs - b3Scalar(c.m_appliedImpulse) * c.m_cfm; + const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; } else { c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + body1.internalApplyImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } - -void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly( - b3SolverBody& body1, - b3SolverBody& body2, - const b3SolverConstraint& c) +void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly( + b3SolverBody& body1, + b3SolverBody& body2, + const b3SolverConstraint& c) { - if (c.m_rhsPenetration) - { - m_numSplitImpulseRecoveries++; - b3Scalar deltaImpulse = c.m_rhsPenetration-b3Scalar(c.m_appliedPushImpulse)*c.m_cfm; - const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); - const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const b3Scalar sum = b3Scalar(c.m_appliedPushImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; - c.m_appliedPushImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedPushImpulse = sum; - } - body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } + if (c.m_rhsPenetration) + { + m_numSplitImpulseRecoveries++; + b3Scalar deltaImpulse = c.m_rhsPenetration - b3Scalar(c.m_appliedPushImpulse) * c.m_cfm; + const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); + const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); + + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; + const b3Scalar sum = b3Scalar(c.m_appliedPushImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse; + c.m_appliedPushImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedPushImpulse = sum; + } + body1.internalApplyPushImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + body2.internalApplyPushImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + } } - void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -374,44 +354,40 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly( m_numSplitImpulseRecoveries++; __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); - __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_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - b3SimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __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_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse), _mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetTurnVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetPushVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); + b3SimdScalar resultLowerLess, resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum, upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); + deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); + c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); #else - resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); + resolveSplitPenetrationImpulseCacheFriendly(body1, body2, c); #endif } - - unsigned long b3PgsJacobiSolver::b3Rand2() { - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff; return m_btSeed2; } - - //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int b3PgsJacobiSolver::b3RandInt2 (int n) +int b3PgsJacobiSolver::b3RandInt2(int n) { // seems good; xor-fold and modulus const unsigned long un = static_cast<unsigned long>(n); @@ -419,15 +395,20 @@ int b3PgsJacobiSolver::b3RandInt2 (int n) // note: probably more aggressive than it needs to be -- might be // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { + if (un <= 0x00010000UL) + { r ^= (r >> 16); - if (un <= 0x00000100UL) { + if (un <= 0x00000100UL) + { r ^= (r >> 8); - if (un <= 0x00000010UL) { + if (un <= 0x00000010UL) + { r ^= (r >> 4); - if (un <= 0x00000004UL) { + if (un <= 0x00000004UL) + { r ^= (r >> 2); - if (un <= 0x00000002UL) { + if (un <= 0x00000002UL) + { r ^= (r >> 1); } } @@ -435,62 +416,46 @@ int b3PgsJacobiSolver::b3RandInt2 (int n) } } - return (int) (r % un); + return (int)(r % un); } - - -void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb) +void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb) { - - solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); - solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); - solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + solverBody->m_deltaLinearVelocity.setValue(0.f, 0.f, 0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f, 0.f, 0.f); + solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); if (rb) { solverBody->m_worldTransform = getWorldTransform(rb); - solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass,rb->m_invMass,rb->m_invMass)); + solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass, rb->m_invMass, rb->m_invMass)); solverBody->m_originalBodyIndex = bodyIndex; - solverBody->m_angularFactor = b3MakeVector3(1,1,1); - solverBody->m_linearFactor = b3MakeVector3(1,1,1); + solverBody->m_angularFactor = b3MakeVector3(1, 1, 1); + solverBody->m_linearFactor = b3MakeVector3(1, 1, 1); solverBody->m_linearVelocity = getLinearVelocity(rb); solverBody->m_angularVelocity = getAngularVelocity(rb); - } else + } + else { solverBody->m_worldTransform.setIdentity(); - solverBody->internalSetInvMass(b3MakeVector3(0,0,0)); + solverBody->internalSetInvMass(b3MakeVector3(0, 0, 0)); solverBody->m_originalBodyIndex = bodyIndex; - solverBody->m_angularFactor.setValue(1,1,1); - solverBody->m_linearFactor.setValue(1,1,1); - solverBody->m_linearVelocity.setValue(0,0,0); - solverBody->m_angularVelocity.setValue(0,0,0); + solverBody->m_angularFactor.setValue(1, 1, 1); + solverBody->m_linearFactor.setValue(1, 1, 1); + solverBody->m_linearVelocity.setValue(0, 0, 0); + solverBody->m_angularVelocity.setValue(0, 0, 0); } - - } - - - - - b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitution) { b3Scalar rest = restitution * -rel_vel; return rest; } - - - - - -void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) { - - solverConstraint.m_contactNormal = normalAxis; b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; @@ -498,7 +463,6 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex]; b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex]; - solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -511,12 +475,12 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti { b3Vector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0); } { b3Vector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0); } b3Scalar scaledDenom; @@ -527,72 +491,66 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti b3Scalar denom1 = 0.f; if (body0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = body0->m_invMass + normalAxis.dot(vec); } if (body1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = body1->m_invMass + normalAxis.dot(vec); } b3Scalar denom; if (m_usePgs) { - scaledDenom = denom = relaxation/(denom0+denom1); - } else + scaledDenom = denom = relaxation / (denom0 + denom1); + } + else { - denom = relaxation/(denom0+denom1); - b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]): 1.f; - b3Scalar countB = body1->m_invMass ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]): 1.f; + denom = relaxation / (denom0 + denom1); + b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]) : 1.f; + b3Scalar countB = body1->m_invMass ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]) : 1.f; - scaledDenom = relaxation/(denom0*countA+denom1*countB); + scaledDenom = relaxation / (denom0 * countA + denom1 * countB); } solverConstraint.m_jacDiagABInv = denom; } { - - b3Scalar rel_vel; - b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0)); - b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0)); + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0)); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0)); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; -// b3Scalar positionalError = 0.f; + // b3Scalar positionalError = 0.f; - b3SimdScalar velocityError = desiredVelocity - rel_vel; - b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(scaledDenom);//solverConstraint.m_jacDiagABInv); + b3SimdScalar velocityError = desiredVelocity - rel_vel; + b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(scaledDenom); //solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; - } } -b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) { b3SolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(bodies,inertias,solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + setupFrictionConstraint(bodies, inertias, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } - -void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, - b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, - b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, - b3Scalar desiredVelocity, b3Scalar cfmSlip) +void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity, b3Scalar cfmSlip) { - b3Vector3 normalAxis=b3MakeVector3(0,0,0); - + b3Vector3 normalAxis = b3MakeVector3(0, 0, 0); solverConstraint.m_contactNormal = normalAxis; b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; @@ -613,283 +571,256 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b { b3Vector3 ftorqueAxis1 = -normalAxis1; solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0); } { b3Vector3 ftorqueAxis1 = normalAxis1; solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0); } - { - b3Vector3 iMJaA = body0?getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*solverConstraint.m_relpos1CrossNormal:b3MakeVector3(0,0,0); - b3Vector3 iMJaB = body1?getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*solverConstraint.m_relpos2CrossNormal:b3MakeVector3(0,0,0); + b3Vector3 iMJaA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * solverConstraint.m_relpos1CrossNormal : b3MakeVector3(0, 0, 0); + b3Vector3 iMJaB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * solverConstraint.m_relpos2CrossNormal : b3MakeVector3(0, 0, 0); b3Scalar sum = 0; sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - solverConstraint.m_jacDiagABInv = b3Scalar(1.)/sum; + solverConstraint.m_jacDiagABInv = b3Scalar(1.) / sum; } { - - b3Scalar rel_vel; - b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0)); - b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0)); + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0)); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0)); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; -// b3Scalar positionalError = 0.f; + // b3Scalar positionalError = 0.f; - b3SimdScalar velocityError = desiredVelocity - rel_vel; - b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv); + b3SimdScalar velocityError = desiredVelocity - rel_vel; + b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; - } } - - - - - - - -b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) { b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupRollingFrictionConstraint(bodies,inertias,solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + setupRollingFrictionConstraint(bodies, inertias, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } - -int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias) +int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias) { //b3Assert(bodyIndex< m_tmpSolverBodyPool.size()); b3RigidBodyData& body = bodies[bodyIndex]; int curIndex = -1; - if (m_usePgs || body.m_invMass==0.f) + if (m_usePgs || body.m_invMass == 0.f) { - if (m_bodyCount[bodyIndex]<0) + if (m_bodyCount[bodyIndex] < 0) { curIndex = m_tmpSolverBodyPool.size(); b3SolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(bodyIndex,&solverBody,&body); + initSolverBody(bodyIndex, &solverBody, &body); solverBody.m_originalBodyIndex = bodyIndex; m_bodyCount[bodyIndex] = curIndex; - } else + } + else { curIndex = m_bodyCount[bodyIndex]; } - } else + } + else { - b3Assert(m_bodyCount[bodyIndex]>0); + b3Assert(m_bodyCount[bodyIndex] > 0); m_bodyCountCheck[bodyIndex]++; curIndex = m_tmpSolverBodyPool.size(); b3SolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(bodyIndex,&solverBody,&body); + initSolverBody(bodyIndex, &solverBody, &body); solverBody.m_originalBodyIndex = bodyIndex; } - b3Assert(curIndex>=0); + b3Assert(curIndex >= 0); return curIndex; - } #include <stdio.h> - -void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal, - b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, - b3Vector3& rel_pos1, b3Vector3& rel_pos2) +void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal, + b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, + b3Vector3& rel_pos1, b3Vector3& rel_pos2) { - - const b3Vector3& pos1 = cp.getPositionWorldOnA(); - const b3Vector3& pos2 = cp.getPositionWorldOnB(); + const b3Vector3& pos1 = cp.getPositionWorldOnA(); + const b3Vector3& pos2 = cp.getPositionWorldOnB(); - b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex]; - b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex]; + b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex]; + b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex]; -// b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); -// b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + // b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + // b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - relaxation = 1.f; + relaxation = 1.f; - b3Vector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? getInvInertiaTensorWorld(&inertias[bodyA->m_originalBodyIndex])*torqueAxis0 : b3MakeVector3(0,0,0); - b3Vector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? getInvInertiaTensorWorld(&inertias[bodyB->m_originalBodyIndex])*-torqueAxis1 : b3MakeVector3(0,0,0); + b3Vector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? getInvInertiaTensorWorld(&inertias[bodyA->m_originalBodyIndex]) * torqueAxis0 : b3MakeVector3(0, 0, 0); + b3Vector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? getInvInertiaTensorWorld(&inertias[bodyB->m_originalBodyIndex]) * -torqueAxis1 : b3MakeVector3(0, 0, 0); - b3Scalar scaledDenom; - { + b3Scalar scaledDenom; + { #ifdef COMPUTE_IMPULSE_DENOM - b3Scalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - b3Scalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else - b3Vector3 vec; - b3Scalar denom0 = 0.f; - b3Scalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - - b3Scalar denom; - if (m_usePgs) - { - scaledDenom = denom = relaxation/(denom0+denom1); - } else - { - denom = relaxation/(denom0+denom1); - - b3Scalar countA = rb0->m_invMass? b3Scalar(m_bodyCount[bodyA->m_originalBodyIndex]) : 1.f; - b3Scalar countB = rb1->m_invMass? b3Scalar(m_bodyCount[bodyB->m_originalBodyIndex]) : 1.f; - scaledDenom = relaxation/(denom0*countA+denom1*countB); - } - solverConstraint.m_jacDiagABInv = denom; - } - - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - - b3Scalar restitution = 0.f; - b3Scalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; - - { - b3Vector3 vel1,vel2; + b3Scalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB); + b3Scalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB); +#else + b3Vector3 vec; + b3Scalar denom0 = 0.f; + b3Scalar denom1 = 0.f; + if (rb0) + { + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec); + } + if (rb1) + { + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec); + } +#endif //COMPUTE_IMPULSE_DENOM - vel1 = rb0? getVelocityInLocalPoint(rb0,rel_pos1) : b3MakeVector3(0,0,0); - vel2 = rb1? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0,0,0); + b3Scalar denom; + if (m_usePgs) + { + scaledDenom = denom = relaxation / (denom0 + denom1); + } + else + { + denom = relaxation / (denom0 + denom1); - // b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); + b3Scalar countA = rb0->m_invMass ? b3Scalar(m_bodyCount[bodyA->m_originalBodyIndex]) : 1.f; + b3Scalar countB = rb1->m_invMass ? b3Scalar(m_bodyCount[bodyB->m_originalBodyIndex]) : 1.f; + scaledDenom = relaxation / (denom0 * countA + denom1 * countB); + } + solverConstraint.m_jacDiagABInv = denom; + } - + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_friction = cp.m_combinedFriction; + b3Scalar restitution = 0.f; + b3Scalar penetration = cp.getDistance() + infoGlobal.m_linearSlop; - - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= b3Scalar(0.)) - { - restitution = 0.f; - }; - } + { + b3Vector3 vel1, vel2; + vel1 = rb0 ? getVelocityInLocalPoint(rb0, rel_pos1) : b3MakeVector3(0, 0, 0); + vel2 = rb1 ? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0, 0, 0); - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - if (rb1) - bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass(),-solverConstraint.m_angularComponentB,-(b3Scalar)solverConstraint.m_appliedImpulse); - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } + // b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0); + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); - solverConstraint.m_appliedPushImpulse = 0.f; + solverConstraint.m_friction = cp.m_combinedFriction; - { - b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:b3MakeVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:b3MakeVector3(0,0,0)); - b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:b3MakeVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:b3MakeVector3(0,0,0)); - b3Scalar rel_vel = vel1Dotn+vel2Dotn; - - b3Scalar positionalError = 0.f; - b3Scalar velocityError = restitution - rel_vel;// * damping; - - - b3Scalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= b3Scalar(0.)) + { + restitution = 0.f; + }; + } - if (penetration>0) - { - positionalError = 0; + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(solverConstraint.m_contactNormal * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(b3Scalar)solverConstraint.m_appliedImpulse); + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + } - velocityError -= penetration / infoGlobal.m_timeStep; - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } + solverConstraint.m_appliedPushImpulse = 0.f; - b3Scalar penetrationImpulse = positionalError*scaledDenom;//solverConstraint.m_jacDiagABInv; - b3Scalar velocityImpulse = velocityError *scaledDenom;//solverConstraint.m_jacDiagABInv; + { + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0 ? bodyA->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? bodyA->m_angularVelocity : b3MakeVector3(0, 0, 0)); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1 ? bodyB->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? bodyB->m_angularVelocity : b3MakeVector3(0, 0, 0)); + b3Scalar rel_vel = vel1Dotn + vel2Dotn; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; + b3Scalar positionalError = 0.f; + b3Scalar velocityError = restitution - rel_vel; // * damping; - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } + b3Scalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + if (penetration > 0) + { + positionalError = 0; + velocityError -= penetration / infoGlobal.m_timeStep; + } + else + { + positionalError = -penetration * erp / infoGlobal.m_timeStep; + } + b3Scalar penetrationImpulse = positionalError * scaledDenom; //solverConstraint.m_jacDiagABInv; + b3Scalar velocityImpulse = velocityError * scaledDenom; //solverConstraint.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + } + else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } } - - -void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal) +void b3PgsJacobiSolver::setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal) { - b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - { b3SolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (bodies[bodyA->m_originalBodyIndex].m_invMass) - bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*bodies[bodyA->m_originalBodyIndex].m_invMass,frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); if (bodies[bodyB->m_originalBodyIndex].m_invMass) - bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*bodies[bodyB->m_originalBodyIndex].m_invMass,-frictionConstraint1.m_angularComponentB,-(b3Scalar)frictionConstraint1.m_appliedImpulse); - } else + bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint1.m_angularComponentB, -(b3Scalar)frictionConstraint1.m_appliedImpulse); + } + else { frictionConstraint1.m_appliedImpulse = 0.f; } @@ -897,51 +828,45 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) { - b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (bodies[bodyA->m_originalBodyIndex].m_invMass) - bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*bodies[bodyA->m_originalBodyIndex].m_invMass,frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse); if (bodies[bodyB->m_originalBodyIndex].m_invMass) - bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*bodies[bodyB->m_originalBodyIndex].m_invMass,-frictionConstraint2.m_angularComponentB,-(b3Scalar)frictionConstraint2.m_appliedImpulse); - } else + bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint2.m_angularComponentB, -(b3Scalar)frictionConstraint2.m_appliedImpulse); + } + else { frictionConstraint2.m_appliedImpulse = 0.f; } } } - - - -void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal) +void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal) { - b3RigidBodyData* colObj0=0,*colObj1=0; + b3RigidBodyData *colObj0 = 0, *colObj1 = 0; - - int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias); - int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(),bodies,inertias); + int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(), bodies, inertias); + int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(), bodies, inertias); -// b3RigidBody* bodyA = b3RigidBody::upcast(colObj0); -// b3RigidBody* bodyB = b3RigidBody::upcast(colObj1); + // b3RigidBody* bodyA = b3RigidBody::upcast(colObj0); + // b3RigidBody* bodyB = b3RigidBody::upcast(colObj1); b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - ///avoid collision response between two static objects if (solverBodyA->m_invMass.isZero() && solverBodyB->m_invMass.isZero()) return; - int rollingFriction=1; + int rollingFriction = 1; int numContacts = getNumContacts(manifold); - for (int j=0;j<numContacts;j++) + for (int j = 0; j < numContacts; j++) { - b3ContactPoint cp; - getContactPoint(manifold,j,cp); + getContactPoint(manifold, j, cp); if (cp.getDistance() <= getContactProcessingThreshold(manifold)) { @@ -953,61 +878,60 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i int frictionIndex = m_tmpSolverContactConstraintPool.size(); b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); -// b3RigidBody* rb0 = b3RigidBody::upcast(colObj0); -// b3RigidBody* rb1 = b3RigidBody::upcast(colObj1); + // b3RigidBody* rb0 = b3RigidBody::upcast(colObj0); + // b3RigidBody* rb1 = b3RigidBody::upcast(colObj1); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_originalContactPoint = &cp; - setupContactConstraint(bodies,inertias,solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); + setupContactConstraint(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); -// const b3Vector3& pos1 = cp.getPositionWorldOnA(); -// const b3Vector3& pos2 = cp.getPositionWorldOnB(); + // const b3Vector3& pos1 = cp.getPositionWorldOnA(); + // const b3Vector3& pos2 = cp.getPositionWorldOnB(); /////setup the friction constraints solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - b3Vector3 angVelA,angVelB; + b3Vector3 angVelA, angVelB; solverBodyA->getAngularVelocity(angVelA); - solverBodyB->getAngularVelocity(angVelB); - b3Vector3 relAngVel = angVelB-angVelA; + solverBodyB->getAngularVelocity(angVelB); + b3Vector3 relAngVel = angVelB - angVelA; - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0)) { //only a single rollingFriction per manifold rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + if (relAngVel.length() > infoGlobal.m_singleAxisRollingFrictionThreshold) { relAngVel.normalize(); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(bodies,inertias,relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else + if (relAngVel.length() > 0.001) + addRollingFrictionConstraint(bodies, inertias, relAngVel, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } + else { - addRollingFrictionConstraint(bodies,inertias,cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - b3Vector3 axis0,axis1; - b3PlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - if (axis0.length()>0.001) - addRollingFrictionConstraint(bodies,inertias,axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(bodies,inertias,axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - + addRollingFrictionConstraint(bodies, inertias, cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + b3Vector3 axis0, axis1; + b3PlaneSpace1(cp.m_normalWorldOnB, axis0, axis1); + if (axis0.length() > 0.001) + addRollingFrictionConstraint(bodies, inertias, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + if (axis1.length() > 0.001) + addRollingFrictionConstraint(bodies, inertias, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); } } ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///By default, each contact has only a single friction direction that is recomputed automatically very frame ///based on the relative linear velocity. ///If the relative velocity it zero, it will automatically compute a friction direction. - + ///You can also enable two friction directions, using the B3_SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. /// ///If you choose B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// - ///The user can manually override the friction directions for certain contacts using a contact callback, + ///The user can manually override the friction directions for certain contacts using a contact callback, ///and set the cp.m_lateralFrictionInitialized to true ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect @@ -1018,99 +942,91 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > B3_EPSILON) { - cp.m_lateralFrictionDir1 *= 1.f/b3Sqrt(lat_rel_vel); - if((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + cp.m_lateralFrictionDir1 *= 1.f / b3Sqrt(lat_rel_vel); + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - + cp.m_lateralFrictionDir2.normalize(); //?? + addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); } - addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else + addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } + else { - b3PlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + b3PlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2); if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) { - addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); } - addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { cp.m_lateralFrictionInitialized = true; } } - - } else + } + else { - addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1); if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); - setFrictionConstraintImpulse( bodies,inertias,solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); + setFrictionConstraintImpulse(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } - - - - } } } -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("solveGroupCacheFriendlySetup"); - m_maxOverrideNumSolverIterations = 0; - - m_tmpSolverBodyPool.resize(0); - - + m_bodyCount.resize(0); - m_bodyCount.resize(numBodies,0); + m_bodyCount.resize(numBodies, 0); m_bodyCountCheck.resize(0); - m_bodyCountCheck.resize(numBodies,0); - + m_bodyCountCheck.resize(numBodies, 0); + m_deltaLinearVelocities.resize(0); - m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); m_deltaAngularVelocities.resize(0); - m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0)); - + m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); + //int totalBodies = 0; - for (int i=0;i<numConstraints;i++) + for (int i = 0; i < numConstraints; i++) { int bodyIndexA = constraints[i]->getRigidBodyA(); int bodyIndexB = constraints[i]->getRigidBodyB(); if (m_usePgs) { - m_bodyCount[bodyIndexA]=-1; - m_bodyCount[bodyIndexB]=-1; - } else + m_bodyCount[bodyIndexA] = -1; + m_bodyCount[bodyIndexB] = -1; + } + else { //didn't implement joints with Jacobi version yet b3Assert(0); } - } - for (int i=0;i<numManifolds;i++) + for (int i = 0; i < numManifolds; i++) { int bodyIndexA = manifoldPtr[i].getBodyA(); int bodyIndexB = manifoldPtr[i].getBodyB(); if (m_usePgs) { - m_bodyCount[bodyIndexA]=-1; - m_bodyCount[bodyIndexB]=-1; - } else + m_bodyCount[bodyIndexA] = -1; + m_bodyCount[bodyIndexB] = -1; + } + else { if (bodies[bodyIndexA].m_invMass) { @@ -1118,26 +1034,23 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies m_bodyCount[bodyIndexA]++; } else - m_bodyCount[bodyIndexA]=-1; + m_bodyCount[bodyIndexA] = -1; if (bodies[bodyIndexB].m_invMass) - // m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints(); + // m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints(); m_bodyCount[bodyIndexB]++; else - m_bodyCount[bodyIndexB]=-1; + m_bodyCount[bodyIndexB] = -1; } - } - - if (1) { int j; - for (j=0;j<numConstraints;j++) + for (j = 0; j < numConstraints; j++) { b3TypedConstraint* constraint = constraints[j]; - + constraint->internalSetAppliedImpulse(0.0f); } } @@ -1146,13 +1059,12 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies //if (1) { { - int totalNumRows = 0; int i; - + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows - for (i=0;i<numConstraints;i++) + for (i = 0; i < numConstraints; i++) { b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; b3JointFeedback* fb = constraints[i]->getJointFeedback(); @@ -1169,8 +1081,9 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies } if (constraints[i]->isEnabled()) { - constraints[i]->getInfo1(&info1,bodies); - } else + constraints[i]->getInfo1(&info1, bodies); + } + else { info1.m_numConstraintRows = 0; info1.nub = 0; @@ -1179,45 +1092,40 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies } m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); - #ifndef DISABLE_JOINTS ///setup the b3SolverConstraints int currentRow = 0; - for (i=0;i<numConstraints;i++) + for (i = 0; i < numConstraints; i++) { const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - + if (info1.m_numConstraintRows) { - b3Assert(currentRow<totalNumRows); + b3Assert(currentRow < totalNumRows); b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; b3TypedConstraint* constraint = constraints[i]; - b3RigidBodyData& rbA = bodies[ constraint->getRigidBodyA()]; + b3RigidBodyData& rbA = bodies[constraint->getRigidBodyA()]; //b3RigidBody& rbA = constraint->getRigidBodyA(); - // b3RigidBody& rbB = constraint->getRigidBodyB(); - b3RigidBodyData& rbB = bodies[ constraint->getRigidBodyB()]; - - int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias); - int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(),bodies,inertias); - - b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; - + // b3RigidBody& rbB = constraint->getRigidBodyB(); + b3RigidBodyData& rbB = bodies[constraint->getRigidBodyB()]; + int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(), bodies, inertias); + int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(), bodies, inertias); + b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - int j; - for ( j=0;j<info1.m_numConstraintRows;j++) + for (j = 0; j < info1.m_numConstraintRows; j++) { - memset(¤tConstraintRow[j],0,sizeof(b3SolverConstraint)); + memset(¤tConstraintRow[j], 0, sizeof(b3SolverConstraint)); currentConstraintRow[j].m_lowerLimit = -B3_INFINITY; currentConstraintRow[j].m_upperLimit = B3_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; @@ -1227,26 +1135,25 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } - bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - + bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); + bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); b3TypedConstraint::b3ConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; + info2.fps = 1.f / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; info2.m_J2linearAxis = 0; info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(b3SolverConstraint)/sizeof(b3Scalar);//check this - ///the size of b3SolverConstraint needs be a multiple of b3Scalar - b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint)); + info2.rowskip = sizeof(b3SolverConstraint) / sizeof(b3Scalar); //check this + ///the size of b3SolverConstraint needs be a multiple of b3Scalar + b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3SolverConstraint)); info2.m_constraintError = ¤tConstraintRow->m_rhs; currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; info2.m_damping = infoGlobal.m_damping; @@ -1254,47 +1161,45 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2,bodies); + constraints[i]->getInfo2(&info2, bodies); ///finalize the constraint setup - for ( j=0;j<info1.m_numConstraintRows;j++) + for (j = 0; j < info1.m_numConstraintRows; j++) { b3SolverConstraint& solverConstraint = currentConstraintRow[j]; - if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold()) + if (solverConstraint.m_upperLimit >= constraints[i]->getBreakingImpulseThreshold()) { solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); } - if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) + if (solverConstraint.m_lowerLimit <= -constraints[i]->getBreakingImpulseThreshold()) { solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); } solverConstraint.m_originalContactPoint = constraint; - - b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld; - { + b3Matrix3x3& invInertiaWorldA = inertias[constraint->getRigidBodyA()].m_invInertiaWorld; + { //b3Vector3 angularFactorA(1,1,1); const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; + solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1; //*angularFactorA; } - - b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld; - { + b3Matrix3x3& invInertiaWorldB = inertias[constraint->getRigidBodyB()].m_invInertiaWorld; + { const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint->getRigidBodyB().getAngularFactor(); + solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2; //*constraint->getRigidBodyB().getAngularFactor(); } { //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal //because it gets multiplied iMJlB - b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.m_invMass; - b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal; - b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.m_invMass;//sign of normal? - b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal; + b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass; + b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal; + b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass; //sign of normal? + b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal; b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); @@ -1302,10 +1207,9 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); b3Scalar fsum = b3Fabs(sum); b3Assert(fsum > B3_EPSILON); - solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f; + solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f; } - ///fix rhs ///todo: add force/torque accelerators { @@ -1313,38 +1217,35 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel); b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; b3Scalar restitution = 0.f; - b3Scalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - b3Scalar velocityError = restitution - rel_vel * info2.m_damping; - b3Scalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - b3Scalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + b3Scalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2 + b3Scalar velocityError = restitution - rel_vel * info2.m_damping; + b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_appliedImpulse = 0.f; - } } } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; + currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows; } -#endif //DISABLE_JOINTS +#endif //DISABLE_JOINTS } - { int i; - for (i=0;i<numManifolds;i++) + for (i = 0; i < numManifolds; i++) { b3Contact4& manifold = manifoldPtr[i]; - convertContact(bodies,inertias,&manifold,infoGlobal); + convertContact(bodies, inertias, &manifold, infoGlobal); } } } -// b3ContactSolverInfo info = infoGlobal; - + // b3ContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); @@ -1353,64 +1254,63 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); else m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); { int i; - for (i=0;i<numNonContactPool;i++) + for (i = 0; i < numNonContactPool; i++) { m_orderNonContactConstraintPool[i] = i; } - for (i=0;i<numConstraintPool;i++) + for (i = 0; i < numConstraintPool; i++) { m_orderTmpConstraintPool[i] = i; } - for (i=0;i<numFrictionPool;i++) + for (i = 0; i < numFrictionPool; i++) { m_orderFrictionConstraintPool[i] = i; } } return 0.f; - } - -b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { - int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - + if (infoGlobal.m_solverMode & B3_SOLVER_RANDMIZE_ORDER) { - if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) { - - for (int j=0; j<numNonContactPool; ++j) { + for (int j = 0; j < numNonContactPool; ++j) + { int tmp = m_orderNonContactConstraintPool[j]; - int swapi = b3RandInt2(j+1); + int swapi = b3RandInt2(j + 1); m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; m_orderNonContactConstraintPool[swapi] = tmp; } - //contact/friction constraints are not solved more than - if (iteration< infoGlobal.m_numIterations) + //contact/friction constraints are not solved more than + if (iteration < infoGlobal.m_numIterations) { - for (int j=0; j<numConstraintPool; ++j) { + for (int j = 0; j < numConstraintPool; ++j) + { int tmp = m_orderTmpConstraintPool[j]; - int swapi = b3RandInt2(j+1); + int swapi = b3RandInt2(j + 1); m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; m_orderTmpConstraintPool[swapi] = tmp; } - for (int j=0; j<numFrictionPool; ++j) { + for (int j = 0; j < numFrictionPool; ++j) + { int tmp = m_orderFrictionConstraintPool[j]; - int swapi = b3RandInt2(j+1); + int swapi = b3RandInt2(j + 1); m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; m_orderFrictionConstraintPool[swapi] = tmp; } @@ -1421,173 +1321,163 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint if (infoGlobal.m_solverMode & B3_SOLVER_SIMD) { ///solve all joint constraints, using SIMD, if available - for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) { b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if (iteration < constraint.m_overrideNumSolverIterations) - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); } - if (iteration< infoGlobal.m_numIterations) + if (iteration < infoGlobal.m_numIterations) { - ///solve all contact constraints using SIMD, if available if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) { int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; + int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; - for (int c=0;c<numPoolConstraints;c++) + for (int c = 0; c < numPoolConstraints; c++) { - b3Scalar totalImpulse =0; + b3Scalar totalImpulse = 0; { const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; - resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); totalImpulse = solveManifold.m_appliedImpulse; } bool applyFriction = true; if (applyFriction) { { + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; - b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]]; - - if (totalImpulse>b3Scalar(0)) + if (totalImpulse > b3Scalar(0)) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + 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); + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } } if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) { + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]]; - b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; - - if (totalImpulse>b3Scalar(0)) + if (totalImpulse > b3Scalar(0)) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + 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); + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } } } } - } - else//B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + else //B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS { //solve the friction constraints after all contact constraints, don't interleave them int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; - for (j=0;j<numPoolConstraints;j++) + for (j = 0; j < numPoolConstraints; j++) { const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - + resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } if (!m_usePgs) averageVelocities(); - ///solve all friction constraints, using SIMD, if available int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) + for (j = 0; j < numFrictionPoolConstraints; j++) { b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - if (totalImpulse>b3Scalar(0)) + if (totalImpulse > b3Scalar(0)) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + 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); + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } } - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (j=0;j<numRollingFrictionPoolConstraints;j++) + for (j = 0; j < numRollingFrictionPoolConstraints; j++) { - b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; - if (totalImpulse>b3Scalar(0)) + if (totalImpulse > b3Scalar(0)) { - b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; + if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); } } - - - } + } } - } else + } + else { //non-SIMD version ///solve all joint constraints - for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) { b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if (iteration < constraint.m_overrideNumSolverIterations) - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); } - if (iteration< infoGlobal.m_numIterations) + if (iteration < infoGlobal.m_numIterations) { - ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j=0;j<numPoolConstraints;j++) + for (int j = 0; j < numPoolConstraints; j++) { const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } ///solve all friction constraints int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (int j=0;j<numFrictionPoolConstraints;j++) + for (int j = 0; j < numFrictionPoolConstraints; j++) { b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - if (totalImpulse>b3Scalar(0)) + if (totalImpulse > b3Scalar(0)) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } } int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;j<numRollingFrictionPoolConstraints;j++) + for (int j = 0; j < numRollingFrictionPoolConstraints; j++) { b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; - if (totalImpulse>b3Scalar(0)) + if (totalImpulse > b3Scalar(0)) { - b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; + if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); } } } @@ -1595,40 +1485,39 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint return 0.f; } - -void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { int iteration; if (infoGlobal.m_splitImpulse) { if (infoGlobal.m_solverMode & B3_SOLVER_SIMD) { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { { int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; - for (j=0;j<numPoolConstraints;j++) + for (j = 0; j < numPoolConstraints; j++) { const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } } } } else { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { { int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; - for (j=0;j<numPoolConstraints;j++) + for (j = 0; j < numPoolConstraints; j++) { const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); } } } @@ -1636,100 +1525,95 @@ void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedCon } } -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("solveGroupCacheFriendlyIterations"); { ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(constraints,numConstraints,infoGlobal); + solveGroupCacheFriendlySplitImpulseIterations(constraints, numConstraints, infoGlobal); - int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + for (int iteration = 0; iteration < maxIterations; iteration++) //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) - { - - solveSingleIteration(iteration, constraints,numConstraints,infoGlobal); - + { + solveSingleIteration(iteration, constraints, numConstraints, infoGlobal); if (!m_usePgs) { averageVelocities(); } } - } return 0.f; } -void b3PgsJacobiSolver::averageVelocities() +void b3PgsJacobiSolver::averageVelocities() { B3_PROFILE("averaging"); //average the velocities int numBodies = m_bodyCount.size(); m_deltaLinearVelocities.resize(0); - m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); m_deltaAngularVelocities.resize(0); - m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); - for (int i=0;i<m_tmpSolverBodyPool.size();i++) + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) { if (!m_tmpSolverBodyPool[i].m_invMass.isZero()) { int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; - m_deltaLinearVelocities[orgBodyIndex]+=m_tmpSolverBodyPool[i].getDeltaLinearVelocity(); - m_deltaAngularVelocities[orgBodyIndex]+=m_tmpSolverBodyPool[i].getDeltaAngularVelocity(); + m_deltaLinearVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaLinearVelocity(); + m_deltaAngularVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaAngularVelocity(); } } - - for (int i=0;i<m_tmpSolverBodyPool.size();i++) + + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) { int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; if (!m_tmpSolverBodyPool[i].m_invMass.isZero()) { - b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]); - - b3Scalar factor = 1.f/b3Scalar(m_bodyCount[orgBodyIndex]); - - m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex]*factor; - m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex]*factor; + b3Scalar factor = 1.f / b3Scalar(m_bodyCount[orgBodyIndex]); + + m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex] * factor; + m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex] * factor; } } } -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i,j; + int i, j; if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) { - for (j=0;j<numPoolConstraints;j++) + for (j = 0; j < numPoolConstraints; j++) { const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; - b3ContactPoint* pt = (b3ContactPoint*) solveManifold.m_originalContactPoint; + b3ContactPoint* pt = (b3ContactPoint*)solveManifold.m_originalContactPoint; b3Assert(pt); pt->m_appliedImpulse = solveManifold.m_appliedImpulse; - // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; // printf("pt->m_appliedImpulseLateral1 = %f\n", f); pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) { - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; } //do a callback here? } } numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) + for (j = 0; j < numPoolConstraints; j++) { const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint; @@ -1739,15 +1623,14 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdA]; b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdB]; - fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*bodyA->m_linearFactor/infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*bodyB->m_linearFactor/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* bodyA->m_angularFactor*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal* bodyB->m_angularFactor*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; - + fb->m_appliedForceBodyA += solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyA->m_linearFactor / infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += -solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyB->m_linearFactor / infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * bodyA->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal * bodyB->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; } constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); - if (b3Fabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + if (b3Fabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold()) { constr->setEnabled(false); } @@ -1755,7 +1638,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie { B3_PROFILE("write back velocities and transforms"); - for ( i=0;i<m_tmpSolverBodyPool.size();i++) + for (i = 0; i < m_tmpSolverBodyPool.size(); i++) { int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; //b3Assert(i==bodyIndex); @@ -1772,12 +1655,13 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie { body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; - } else + } + else { - b3Scalar factor = 1.f/b3Scalar(m_bodyCount[bodyIndex]); + b3Scalar factor = 1.f / b3Scalar(m_bodyCount[bodyIndex]); - b3Vector3 deltaLinVel = m_deltaLinearVelocities[bodyIndex]*factor; - b3Vector3 deltaAngVel = m_deltaAngularVelocities[bodyIndex]*factor; + b3Vector3 deltaLinVel = m_deltaLinearVelocities[bodyIndex] * factor; + b3Vector3 deltaAngVel = m_deltaAngularVelocities[bodyIndex] * factor; //printf("body %d\n",bodyIndex); //printf("deltaLinVel = %f,%f,%f\n",deltaLinVel.getX(),deltaLinVel.getY(),deltaLinVel.getZ()); //printf("deltaAngVel = %f,%f,%f\n",deltaAngVel.getX(),deltaAngVel.getY(),deltaAngVel.getZ()); @@ -1785,7 +1669,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie body->m_linVel += deltaLinVel; body->m_angVel += deltaAngVel; } - + if (infoGlobal.m_splitImpulse) { body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); @@ -1797,7 +1681,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie } } - m_tmpSolverContactConstraintPool.resizeNoInitialize(0); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); @@ -1807,9 +1690,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie return 0.f; } - - -void b3PgsJacobiSolver::reset() +void b3PgsJacobiSolver::reset() { m_btSeed2 = 0; }
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h index d2ca307fab..5b616541d9 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h @@ -1,11 +1,9 @@ #ifndef B3_PGS_JACOBI_SOLVER #define B3_PGS_JACOBI_SOLVER - struct b3Contact4; struct b3ContactPoint; - class b3Dispatcher; #include "b3TypedConstraint.h" @@ -18,132 +16,118 @@ struct b3InertiaData; class b3PgsJacobiSolver { - protected: - b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool; - b3ConstraintArray m_tmpSolverContactConstraintPool; - b3ConstraintArray m_tmpSolverNonContactConstraintPool; - b3ConstraintArray m_tmpSolverContactFrictionConstraintPool; - b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; - - b3AlignedObjectArray<int> m_orderTmpConstraintPool; - b3AlignedObjectArray<int> m_orderNonContactConstraintPool; - b3AlignedObjectArray<int> m_orderFrictionConstraintPool; + b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool; + b3ConstraintArray m_tmpSolverContactConstraintPool; + b3ConstraintArray m_tmpSolverNonContactConstraintPool; + b3ConstraintArray m_tmpSolverContactFrictionConstraintPool; + b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + + b3AlignedObjectArray<int> m_orderTmpConstraintPool; + b3AlignedObjectArray<int> m_orderNonContactConstraintPool; + b3AlignedObjectArray<int> m_orderFrictionConstraintPool; b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool; - - b3AlignedObjectArray<int> m_bodyCount; - b3AlignedObjectArray<int> m_bodyCountCheck; - - b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities; - b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities; - bool m_usePgs; - void averageVelocities(); + b3AlignedObjectArray<int> m_bodyCount; + b3AlignedObjectArray<int> m_bodyCountCheck; + + b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities; + b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities; + + bool m_usePgs; + void averageVelocities(); - int m_maxOverrideNumSolverIterations; + int m_maxOverrideNumSolverIterations; - int m_numSplitImpulseRecoveries; + int m_numSplitImpulseRecoveries; - b3Scalar getContactProcessingThreshold(b3Contact4* contact) + b3Scalar getContactProcessingThreshold(b3Contact4* contact) { return 0.02f; } - void setupFrictionConstraint( b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, - b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, - b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, - b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.); - void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, - b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, - b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, - b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); - - b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); - b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f); + void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.); + b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.); + b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f); void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, - b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, - const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, + b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, + const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, b3Vector3& rel_pos1, b3Vector3& rel_pos2); - void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, - b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal); + void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction - unsigned long m_btSeed2; + unsigned long m_btSeed2; - b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution); - void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal); - + void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal); - void resolveSplitPenetrationSIMD( - b3SolverBody& bodyA,b3SolverBody& bodyB, - const b3SolverConstraint& contactConstraint); + void resolveSplitPenetrationSIMD( + b3SolverBody& bodyA, b3SolverBody& bodyB, + const b3SolverConstraint& contactConstraint); - void resolveSplitPenetrationImpulseCacheFriendly( - b3SolverBody& bodyA,b3SolverBody& bodyB, - const b3SolverConstraint& contactConstraint); + void resolveSplitPenetrationImpulseCacheFriendly( + b3SolverBody& bodyA, b3SolverBody& bodyB, + const b3SolverConstraint& contactConstraint); //internal method - int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); - void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject); + int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias); + void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject); - void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); - - void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); - -protected: + void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); - virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); - virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); +protected: + virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); - virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); + virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); + b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal); public: - B3_DECLARE_ALIGNED_ALLOCATOR(); - + b3PgsJacobiSolver(bool usePgs); virtual ~b3PgsJacobiSolver(); -// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts); - void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); + // void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts); + void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); - b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); ///clear internal cached data and reset random seed - virtual void reset(); - + virtual void reset(); + unsigned long b3Rand2(); - int b3RandInt2 (int n); + int b3RandInt2(int n); - void setRandSeed(unsigned long seed) + void setRandSeed(unsigned long seed) { m_btSeed2 = seed; } - unsigned long getRandSeed() const + unsigned long getRandSeed() const { return m_btSeed2; } - - - - }; -#endif //B3_PGS_JACOBI_SOLVER - +#endif //B3_PGS_JACOBI_SOLVER diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp index 02c11db320..cfa7c7dd11 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -13,21 +13,14 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "b3Point2PointConstraint.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include <new> - - - - -b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB) -:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_flags(0) +b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB) + : b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0) { - } /* @@ -40,22 +33,18 @@ m_useSolveConstraintObsolete(false) } */ - -void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) { - getInfo1NonVirtual(info,bodies); + getInfo1NonVirtual(info, bodies); } -void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) { - info->m_numConstraintRows = 3; - info->nub = 3; + info->m_numConstraintRows = 3; + info->nub = 3; } - - - -void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies) +void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies) { b3Transform trA; trA.setIdentity(); @@ -67,143 +56,135 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo trB.setOrigin(bodies[m_rbB].m_pos); trB.setRotation(bodies[m_rbB].m_quat); - getInfo2NonVirtual(info, trA,trB); + getInfo2NonVirtual(info, trA, trB); } -void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) +void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) { - - //retrieve matrices + //retrieve matrices // anchor points in global coordinates with respect to body PORs. - - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - b3Vector3 a1 = body0_trans.getBasis()*getPivotInA(); + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip + 1] = 1; + info->m_J1linearAxis[2 * info->rowskip + 2] = 1; + + b3Vector3 a1 = body0_trans.getBasis() * getPivotInA(); //b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA()); { b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip); b3Vector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } - + if (info->m_J2linearAxis) { info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; + info->m_J2linearAxis[info->rowskip + 1] = -1; + info->m_J2linearAxis[2 * info->rowskip + 2] = -1; } - - b3Vector3 a2 = body1_trans.getBasis()*getPivotInB(); - + + b3Vector3 a2 = body1_trans.getBasis() * getPivotInB(); + { - // b3Vector3 a2n = -a2; + // b3Vector3 a2n = -a2; b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } - - - // set right hand side + // set right hand side b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; - b3Scalar k = info->fps * currERP; - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + b3Scalar k = info->fps * currERP; + int j; + for (j = 0; j < 3; j++) + { + info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } - if(m_flags & B3_P2P_FLAGS_CFM) + } + if (m_flags & B3_P2P_FLAGS_CFM) { - for (j=0; j<3; j++) + for (j = 0; j < 3; j++) { - info->cfm[j*info->rowskip] = m_cfm; + info->cfm[j * info->rowskip] = m_cfm; } } - b3Scalar impulseClamp = m_setting.m_impulseClamp;// - for (j=0; j<3; j++) - { + b3Scalar impulseClamp = m_setting.m_impulseClamp; // + for (j = 0; j < 3; j++) + { if (m_setting.m_impulseClamp > 0) { - info->m_lowerLimit[j*info->rowskip] = -impulseClamp; - info->m_upperLimit[j*info->rowskip] = impulseClamp; + info->m_lowerLimit[j * info->rowskip] = -impulseClamp; + info->m_upperLimit[j * info->rowskip] = impulseClamp; } } info->m_damping = m_setting.m_damping; - } - - -void b3Point2PointConstraint::updateRHS(b3Scalar timeStep) +void b3Point2PointConstraint::updateRHS(b3Scalar timeStep) { (void)timeStep; - } -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis) { - if(axis != -1) + if (axis != -1) { b3AssertConstrParams(0); } else { - switch(num) + switch (num) { - case B3_CONSTRAINT_ERP : - case B3_CONSTRAINT_STOP_ERP : - m_erp = value; + case B3_CONSTRAINT_ERP: + case B3_CONSTRAINT_STOP_ERP: + m_erp = value; m_flags |= B3_P2P_FLAGS_ERP; break; - case B3_CONSTRAINT_CFM : - case B3_CONSTRAINT_STOP_CFM : - m_cfm = value; + case B3_CONSTRAINT_CFM: + case B3_CONSTRAINT_STOP_CFM: + m_cfm = value; m_flags |= B3_P2P_FLAGS_CFM; break; - default: + default: b3AssertConstrParams(0); } } } ///return the local value of parameter -b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const +b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const { b3Scalar retVal(B3_INFINITY); - if(axis != -1) + if (axis != -1) { b3AssertConstrParams(0); } else { - switch(num) + switch (num) { - case B3_CONSTRAINT_ERP : - case B3_CONSTRAINT_STOP_ERP : + case B3_CONSTRAINT_ERP: + case B3_CONSTRAINT_STOP_ERP: b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP); - retVal = m_erp; + retVal = m_erp; break; - case B3_CONSTRAINT_CFM : - case B3_CONSTRAINT_STOP_CFM : + case B3_CONSTRAINT_CFM: + case B3_CONSTRAINT_STOP_CFM: b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM); - retVal = m_cfm; + retVal = m_cfm; break; - default: + default: b3AssertConstrParams(0); } } return retVal; } - diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h index 681b487334..14762a3e35 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -22,26 +22,24 @@ subject to the following restrictions: class b3RigidBody; - #ifdef B3_USE_DOUBLE_PRECISION -#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData -#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData" +#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData +#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData" #else -#define b3Point2PointConstraintData b3Point2PointConstraintFloatData -#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData" -#endif //B3_USE_DOUBLE_PRECISION +#define b3Point2PointConstraintData b3Point2PointConstraintFloatData +#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData" +#endif //B3_USE_DOUBLE_PRECISION -struct b3ConstraintSetting +struct b3ConstraintSetting { - b3ConstraintSetting() : - m_tau(b3Scalar(0.3)), - m_damping(b3Scalar(1.)), - m_impulseClamp(b3Scalar(0.)) + b3ConstraintSetting() : m_tau(b3Scalar(0.3)), + m_damping(b3Scalar(1.)), + m_impulseClamp(b3Scalar(0.)) { } - b3Scalar m_tau; - b3Scalar m_damping; - b3Scalar m_impulseClamp; + b3Scalar m_tau; + b3Scalar m_damping; + b3Scalar m_impulseClamp; }; enum b3Point2PointFlags @@ -51,47 +49,45 @@ enum b3Point2PointFlags }; /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space -B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint +B3_ATTRIBUTE_ALIGNED16(class) +b3Point2PointConstraint : public b3TypedConstraint { #ifdef IN_PARALLELL_SOLVER public: #endif - - b3Vector3 m_pivotInA; - b3Vector3 m_pivotInB; - - int m_flags; - b3Scalar m_erp; - b3Scalar m_cfm; - -public: - B3_DECLARE_ALIGNED_ALLOCATOR(); + b3Vector3 m_pivotInA; + b3Vector3 m_pivotInB; - b3ConstraintSetting m_setting; + int m_flags; + b3Scalar m_erp; + b3Scalar m_cfm; - b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB); +public: + B3_DECLARE_ALIGNED_ALLOCATOR(); - //b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA); + b3ConstraintSetting m_setting; + b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB); + //b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA); - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies); + virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies); - void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans); + void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& body0_trans, const b3Transform& body1_trans); - void updateRHS(b3Scalar timeStep); + void updateRHS(b3Scalar timeStep); - void setPivotA(const b3Vector3& pivotA) + void setPivotA(const b3Vector3& pivotA) { m_pivotInA = pivotA; } - void setPivotB(const b3Vector3& pivotB) + void setPivotB(const b3Vector3& pivotB) { m_pivotInB = pivotB; } @@ -106,34 +102,32 @@ public: return m_pivotInB; } - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, b3Scalar value, int axis = -1); + virtual void setParam(int num, b3Scalar value, int axis = -1); ///return the local value of parameter - virtual b3Scalar getParam(int num, int axis = -1) const; + virtual b3Scalar getParam(int num, int axis = -1) const; -// virtual int calculateSerializeBufferSize() const; + // virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) -// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const; - - + // virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct b3Point2PointConstraintFloatData +struct b3Point2PointConstraintFloatData { - b3TypedConstraintData m_typeConstraintData; - b3Vector3FloatData m_pivotInA; - b3Vector3FloatData m_pivotInB; + b3TypedConstraintData m_typeConstraintData; + b3Vector3FloatData m_pivotInA; + b3Vector3FloatData m_pivotInB; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct b3Point2PointConstraintDoubleData +struct b3Point2PointConstraintDoubleData { - b3TypedConstraintData m_typeConstraintData; - b3Vector3DoubleData m_pivotInA; - b3Vector3DoubleData m_pivotInB; + b3TypedConstraintData m_typeConstraintData; + b3Vector3DoubleData m_pivotInA; + b3Vector3DoubleData m_pivotInB; }; /* @@ -156,4 +150,4 @@ B3_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, } */ -#endif //B3_POINT2POINTCONSTRAINT_H +#endif //B3_POINT2POINTCONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h index 0049317d98..196d0e5793 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef B3_SOLVER_BODY_H #define B3_SOLVER_BODY_H - #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Matrix3x3.h" @@ -26,110 +25,104 @@ subject to the following restrictions: ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision #ifdef B3_USE_SSE #define USE_SIMD 1 -#endif // - +#endif // #ifdef USE_SIMD -struct b3SimdScalar +struct b3SimdScalar { - B3_FORCE_INLINE b3SimdScalar() + B3_FORCE_INLINE b3SimdScalar() { - } - B3_FORCE_INLINE b3SimdScalar(float fl) - :m_vec128 (_mm_set1_ps(fl)) + B3_FORCE_INLINE b3SimdScalar(float fl) + : m_vec128(_mm_set1_ps(fl)) { } - B3_FORCE_INLINE b3SimdScalar(__m128 v128) - :m_vec128(v128) + B3_FORCE_INLINE b3SimdScalar(__m128 v128) + : m_vec128(v128) { } - union - { - __m128 m_vec128; - float m_floats[4]; - float x,y,z,w; - int m_ints[4]; - b3Scalar m_unusedPadding; + union { + __m128 m_vec128; + float m_floats[4]; + float x, y, z, w; + int m_ints[4]; + b3Scalar m_unusedPadding; }; - B3_FORCE_INLINE __m128 get128() + B3_FORCE_INLINE __m128 get128() { return m_vec128; } - B3_FORCE_INLINE const __m128 get128() const + B3_FORCE_INLINE const __m128 get128() const { return m_vec128; } - B3_FORCE_INLINE void set128(__m128 v128) + B3_FORCE_INLINE void set128(__m128 v128) { m_vec128 = v128; } - B3_FORCE_INLINE operator __m128() - { - return m_vec128; - } - B3_FORCE_INLINE operator const __m128() const - { - return m_vec128; + B3_FORCE_INLINE operator __m128() + { + return m_vec128; } - - B3_FORCE_INLINE operator float() const - { - return m_floats[0]; + B3_FORCE_INLINE operator const __m128() const + { + return m_vec128; } + B3_FORCE_INLINE operator float() const + { + return m_floats[0]; + } }; ///@brief Return the elementwise product of two b3SimdScalar -B3_FORCE_INLINE b3SimdScalar -operator*(const b3SimdScalar& v1, const b3SimdScalar& v2) +B3_FORCE_INLINE b3SimdScalar +operator*(const b3SimdScalar& v1, const b3SimdScalar& v2) { - return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); + return b3SimdScalar(_mm_mul_ps(v1.get128(), v2.get128())); } ///@brief Return the elementwise product of two b3SimdScalar -B3_FORCE_INLINE b3SimdScalar -operator+(const b3SimdScalar& v1, const b3SimdScalar& v2) +B3_FORCE_INLINE b3SimdScalar +operator+(const b3SimdScalar& v1, const b3SimdScalar& v2) { - return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128())); + return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128())); } - #else #define b3SimdScalar b3Scalar #endif ///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody +B3_ATTRIBUTE_ALIGNED16(struct) +b3SolverBody { B3_DECLARE_ALIGNED_ALLOCATOR(); - b3Transform m_worldTransform; - b3Vector3 m_deltaLinearVelocity; - b3Vector3 m_deltaAngularVelocity; - b3Vector3 m_angularFactor; - b3Vector3 m_linearFactor; - b3Vector3 m_invMass; - b3Vector3 m_pushVelocity; - b3Vector3 m_turnVelocity; - b3Vector3 m_linearVelocity; - b3Vector3 m_angularVelocity; - - union - { - void* m_originalBody; - int m_originalBodyIndex; + b3Transform m_worldTransform; + b3Vector3 m_deltaLinearVelocity; + b3Vector3 m_deltaAngularVelocity; + b3Vector3 m_angularFactor; + b3Vector3 m_linearFactor; + b3Vector3 m_invMass; + b3Vector3 m_pushVelocity; + b3Vector3 m_turnVelocity; + b3Vector3 m_linearVelocity; + b3Vector3 m_angularVelocity; + + union { + void* m_originalBody; + int m_originalBodyIndex; }; int padding[3]; - - void setWorldTransform(const b3Transform& worldTransform) + void setWorldTransform(const b3Transform& worldTransform) { m_worldTransform = worldTransform; } @@ -138,45 +131,42 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody { return m_worldTransform; } - - B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + + B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const { if (m_originalBody) - velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos); else - velocity.setValue(0,0,0); + velocity.setValue(0, 0, 0); } - B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const + B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const { if (m_originalBody) - angVel =m_angularVelocity+m_deltaAngularVelocity; + angVel = m_angularVelocity + m_deltaAngularVelocity; else - angVel.setValue(0,0,0); + angVel.setValue(0, 0, 0); } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude) { if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude) + B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude) { if (m_originalBody) { - m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - - const b3Vector3& getDeltaLinearVelocity() const { return m_deltaLinearVelocity; @@ -187,20 +177,19 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody return m_deltaAngularVelocity; } - const b3Vector3& getPushVelocity() const + const b3Vector3& getPushVelocity() const { return m_pushVelocity; } - const b3Vector3& getTurnVelocity() const + const b3Vector3& getTurnVelocity() const { return m_turnVelocity; } - //////////////////////////////////////////////// ///some internal methods, don't use them - + b3Vector3& internalGetDeltaLinearVelocity() { return m_deltaLinearVelocity; @@ -225,7 +214,7 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody { m_invMass = invMass; } - + b3Vector3& internalGetPushVelocity() { return m_pushVelocity; @@ -236,67 +225,57 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody return m_turnVelocity; } - B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const { - velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos); } - B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const + B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const { - angVel = m_angularVelocity+m_deltaAngularVelocity; + angVel = m_angularVelocity + m_deltaAngularVelocity; } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude) { //if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - - - - void writebackVelocity() + void writebackVelocity() { //if (m_originalBody>=0) { - m_linearVelocity +=m_deltaLinearVelocity; + m_linearVelocity += m_deltaLinearVelocity; m_angularVelocity += m_deltaAngularVelocity; - + //m_originalBody->setCompanionId(-1); } } - - void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) + void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) { - (void) timeStep; + (void)timeStep; if (m_originalBody) { m_linearVelocity += m_deltaLinearVelocity; m_angularVelocity += m_deltaAngularVelocity; - + //correct the position/orientation based on push/turn recovery b3Transform newTransform; - if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0) + if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0) { - // b3Quaternion orn = m_worldTransform.getRotation(); - b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); + // b3Quaternion orn = m_worldTransform.getRotation(); + b3TransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform); m_worldTransform = newTransform; } //m_worldTransform.setRotation(orn); //m_originalBody->setCompanionId(-1); } } - - - }; -#endif //B3_SOLVER_BODY_H - - +#endif //B3_SOLVER_BODY_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h index bce83d4608..4927ae4288 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef B3_SOLVER_CONSTRAINT_H #define B3_SOLVER_CONSTRAINT_H - #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Matrix3x3.h" //#include "b3JacobianEntry.h" @@ -25,56 +24,50 @@ subject to the following restrictions: //#define NO_FRICTION_TANGENTIALS 1 #include "b3SolverBody.h" - ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint +B3_ATTRIBUTE_ALIGNED16(struct) +b3SolverConstraint { B3_DECLARE_ALIGNED_ALLOCATOR(); - b3Vector3 m_relpos1CrossNormal; - b3Vector3 m_contactNormal; + b3Vector3 m_relpos1CrossNormal; + b3Vector3 m_contactNormal; - b3Vector3 m_relpos2CrossNormal; + b3Vector3 m_relpos2CrossNormal; //b3Vector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal - b3Vector3 m_angularComponentA; - b3Vector3 m_angularComponentB; - - mutable b3SimdScalar m_appliedPushImpulse; - mutable b3SimdScalar m_appliedImpulse; + b3Vector3 m_angularComponentA; + b3Vector3 m_angularComponentB; + + mutable b3SimdScalar m_appliedPushImpulse; + mutable b3SimdScalar m_appliedImpulse; int m_padding1; int m_padding2; - b3Scalar m_friction; - b3Scalar m_jacDiagABInv; - b3Scalar m_rhs; - b3Scalar m_cfm; - - b3Scalar m_lowerLimit; - b3Scalar m_upperLimit; - b3Scalar m_rhsPenetration; - union - { - void* m_originalContactPoint; - b3Scalar m_unusedPadding4; + b3Scalar m_friction; + b3Scalar m_jacDiagABInv; + b3Scalar m_rhs; + b3Scalar m_cfm; + + b3Scalar m_lowerLimit; + b3Scalar m_upperLimit; + b3Scalar m_rhsPenetration; + union { + void* m_originalContactPoint; + b3Scalar m_unusedPadding4; }; - int m_overrideNumSolverIterations; - int m_frictionIndex; + int m_overrideNumSolverIterations; + int m_frictionIndex; int m_solverBodyIdA; int m_solverBodyIdB; - - enum b3SolverConstraintType + enum b3SolverConstraintType { B3_SOLVER_CONTACT_1D = 0, B3_SOLVER_FRICTION_1D }; }; -typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray; - - -#endif //B3_SOLVER_CONSTRAINT_H - - +typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray; +#endif //B3_SOLVER_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp index 699c481d64..885e277d8c 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp @@ -13,53 +13,46 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "b3TypedConstraint.h" //#include "Bullet3Common/b3Serializer.h" - #define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f) - - -b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB) -:b3TypedObject(type), -m_userConstraintType(-1), -m_userConstraintPtr((void*)-1), -m_breakingImpulseThreshold(B3_INFINITY), -m_isEnabled(true), -m_needsFeedback(false), -m_overrideNumSolverIterations(-1), -m_rbA(rbA), -m_rbB(rbB), -m_appliedImpulse(b3Scalar(0.)), -m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE), -m_jointFeedback(0) +b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB) + : b3TypedObject(type), + m_userConstraintType(-1), + m_userConstraintPtr((void*)-1), + m_breakingImpulseThreshold(B3_INFINITY), + m_isEnabled(true), + m_needsFeedback(false), + m_overrideNumSolverIterations(-1), + m_rbA(rbA), + m_rbB(rbB), + m_appliedImpulse(b3Scalar(0.)), + m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE), + m_jointFeedback(0) { } - - - b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact) { - if(lowLim > uppLim) + if (lowLim > uppLim) { return b3Scalar(1.0f); } - else if(lowLim == uppLim) + else if (lowLim == uppLim) { return b3Scalar(0.0f); } b3Scalar lim_fact = b3Scalar(1.0f); b3Scalar delta_max = vel / timeFact; - if(delta_max < b3Scalar(0.0f)) + if (delta_max < b3Scalar(0.0f)) { - if((pos >= lowLim) && (pos < (lowLim - delta_max))) + if ((pos >= lowLim) && (pos < (lowLim - delta_max))) { lim_fact = (lowLim - pos) / delta_max; } - else if(pos < lowLim) + else if (pos < lowLim) { lim_fact = b3Scalar(0.0f); } @@ -68,13 +61,13 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal lim_fact = b3Scalar(1.0f); } } - else if(delta_max > b3Scalar(0.0f)) + else if (delta_max > b3Scalar(0.0f)) { - if((pos <= uppLim) && (pos > (uppLim - delta_max))) + if ((pos <= uppLim) && (pos > (uppLim - delta_max))) { lim_fact = (uppLim - pos) / delta_max; } - else if(pos > uppLim) + else if (pos > uppLim) { lim_fact = b3Scalar(0.0f); } @@ -85,18 +78,16 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal } else { - lim_fact = b3Scalar(0.0f); + lim_fact = b3Scalar(0.0f); } return lim_fact; } - - void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor) { m_halfRange = (high - low) / 2.0f; m_center = b3NormalizeAngle(low + m_halfRange); - m_softness = _softness; + m_softness = _softness; m_biasFactor = _biasFactor; m_relaxationFactor = _relaxationFactor; } @@ -113,7 +104,7 @@ void b3AngularLimit::test(const b3Scalar angle) if (deviation < -m_halfRange) { m_solveLimit = true; - m_correction = - (deviation + m_halfRange); + m_correction = -(deviation + m_halfRange); m_sign = +1.0f; } else if (deviation > m_halfRange) @@ -125,7 +116,6 @@ void b3AngularLimit::test(const b3Scalar angle) } } - b3Scalar b3AngularLimit::getError() const { return m_correction * m_sign; diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h index cf9cec0d5e..f74aec4d3c 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef B3_TYPED_CONSTRAINT_H #define B3_TYPED_CONSTRAINT_H - #include "Bullet3Common/b3Scalar.h" #include "b3SolverConstraint.h" @@ -25,7 +24,7 @@ class b3Serializer; //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility enum b3TypedConstraintType { - B3_POINT2POINT_CONSTRAINT_TYPE=3, + B3_POINT2POINT_CONSTRAINT_TYPE = 3, B3_HINGE_CONSTRAINT_TYPE, B3_CONETWIST_CONSTRAINT_TYPE, B3_D6_CONSTRAINT_TYPE, @@ -37,92 +36,86 @@ enum b3TypedConstraintType B3_MAX_CONSTRAINT_TYPE }; - enum b3ConstraintParams { - B3_CONSTRAINT_ERP=1, + B3_CONSTRAINT_ERP = 1, B3_CONSTRAINT_STOP_ERP, B3_CONSTRAINT_CFM, B3_CONSTRAINT_STOP_CFM }; #if 1 - #define b3AssertConstrParams(_par) b3Assert(_par) +#define b3AssertConstrParams(_par) b3Assert(_par) #else - #define b3AssertConstrParams(_par) +#define b3AssertConstrParams(_par) #endif - -B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback +B3_ATTRIBUTE_ALIGNED16(struct) +b3JointFeedback { - b3Vector3 m_appliedForceBodyA; - b3Vector3 m_appliedTorqueBodyA; - b3Vector3 m_appliedForceBodyB; - b3Vector3 m_appliedTorqueBodyB; + b3Vector3 m_appliedForceBodyA; + b3Vector3 m_appliedTorqueBodyA; + b3Vector3 m_appliedForceBodyB; + b3Vector3 m_appliedTorqueBodyB; }; - struct b3RigidBodyData; - ///TypedConstraint is the baseclass for Bullet constraints and vehicles -B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject +B3_ATTRIBUTE_ALIGNED16(class) +b3TypedConstraint : public b3TypedObject { - int m_userConstraintType; + int m_userConstraintType; - union - { - int m_userConstraintId; + union { + int m_userConstraintId; void* m_userConstraintPtr; }; - b3Scalar m_breakingImpulseThreshold; - bool m_isEnabled; - bool m_needsFeedback; - int m_overrideNumSolverIterations; + b3Scalar m_breakingImpulseThreshold; + bool m_isEnabled; + bool m_needsFeedback; + int m_overrideNumSolverIterations; - - b3TypedConstraint& operator=(b3TypedConstraint& other) + b3TypedConstraint& operator=(b3TypedConstraint& other) { b3Assert(0); - (void) other; + (void)other; return *this; } protected: - int m_rbA; - int m_rbB; - b3Scalar m_appliedImpulse; - b3Scalar m_dbgDrawSize; - b3JointFeedback* m_jointFeedback; + int m_rbA; + int m_rbB; + b3Scalar m_appliedImpulse; + b3Scalar m_dbgDrawSize; + b3JointFeedback* m_jointFeedback; ///internal method used by the constraint solver, don't use them directly b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact); - public: - B3_DECLARE_ALIGNED_ALLOCATOR(); - virtual ~b3TypedConstraint() {}; - b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB); + virtual ~b3TypedConstraint(){}; + b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB); - struct b3ConstraintInfo1 { - int m_numConstraintRows,nub; + struct b3ConstraintInfo1 + { + int m_numConstraintRows, nub; }; - - - struct b3ConstraintInfo2 { + struct b3ConstraintInfo2 + { // integrator parameters: frames per second (1/stepsize), default error // reduction parameter (0..1). - b3Scalar fps,erp; + b3Scalar fps, erp; // for the first and second body, pointers to two (linear and angular) // n*3 jacobian sub matrices, stored by rows. these matrices will have // been initialized to 0 on entry. if the second body is zero then the // J2xx pointers may be 0. - b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis; // elements to jump from one row to the next in J's int rowskip; @@ -130,24 +123,24 @@ public: // right hand sides of the equation J*v = c + cfm * lambda. cfm is the // "constraint force mixing" vector. c is set to zero on entry, cfm is // set to a constant value (typically very small or zero) value on entry. - b3Scalar *m_constraintError,*cfm; + b3Scalar *m_constraintError, *cfm; // lo and hi limits for variables (set to -/+ infinity on entry). - b3Scalar *m_lowerLimit,*m_upperLimit; + b3Scalar *m_lowerLimit, *m_upperLimit; // findex vector for variables. see the LCP solver interface for a // description of what this does. this is set to -1 on entry. // note that the returned indexes are relative to the first index of // the constraint. - int *findex; + int* findex; // number of solver iterations int m_numIterations; //damping of the velocity - b3Scalar m_damping; + b3Scalar m_damping; }; - int getOverrideNumSolverIterations() const + int getOverrideNumSolverIterations() const { return m_overrideNumSolverIterations; } @@ -159,59 +152,55 @@ public: m_overrideNumSolverIterations = overideNumIterations; } - ///internal method used by the constraint solver, don't use them directly - virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep) + virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep) { - (void)ca; - (void)solverBodyA; - (void)solverBodyB; - (void)timeStep; + (void)ca; + (void)solverBodyA; + (void)solverBodyB; + (void)timeStep; } - + ///internal method used by the constraint solver, don't use them directly - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)=0; + virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies) = 0; ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)=0; + virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies) = 0; ///internal method used by the constraint solver, don't use them directly - void internalSetAppliedImpulse(b3Scalar appliedImpulse) + void internalSetAppliedImpulse(b3Scalar appliedImpulse) { m_appliedImpulse = appliedImpulse; } ///internal method used by the constraint solver, don't use them directly - b3Scalar internalGetAppliedImpulse() + b3Scalar internalGetAppliedImpulse() { return m_appliedImpulse; } - - b3Scalar getBreakingImpulseThreshold() const + b3Scalar getBreakingImpulseThreshold() const { - return m_breakingImpulseThreshold; + return m_breakingImpulseThreshold; } - void setBreakingImpulseThreshold(b3Scalar threshold) + void setBreakingImpulseThreshold(b3Scalar threshold) { m_breakingImpulseThreshold = threshold; } - bool isEnabled() const + bool isEnabled() const { return m_isEnabled; } - void setEnabled(bool enabled) + void setEnabled(bool enabled) { - m_isEnabled=enabled; + m_isEnabled = enabled; } - ///internal method used by the constraint solver, don't use them directly - virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/,b3SolverBody& /*bodyB*/,b3Scalar /*timeStep*/) {}; + virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){}; - int getRigidBodyA() const { return m_rbA; @@ -221,8 +210,7 @@ public: return m_rbB; } - - int getRigidBodyA() + int getRigidBodyA() { return m_rbA; } @@ -233,15 +221,15 @@ public: int getUserConstraintType() const { - return m_userConstraintType ; + return m_userConstraintType; } - void setUserConstraintType(int userConstraintType) + void setUserConstraintType(int userConstraintType) { m_userConstraintType = userConstraintType; }; - void setUserConstraintId(int uid) + void setUserConstraintId(int uid) { m_userConstraintId = uid; } @@ -251,17 +239,17 @@ public: return m_userConstraintId; } - void setUserConstraintPtr(void* ptr) + void setUserConstraintPtr(void* ptr) { m_userConstraintPtr = ptr; } - void* getUserConstraintPtr() + void* getUserConstraintPtr() { return m_userConstraintPtr; } - void setJointFeedback(b3JointFeedback* jointFeedback) + void setJointFeedback(b3JointFeedback * jointFeedback) { m_jointFeedback = jointFeedback; } @@ -276,37 +264,36 @@ public: return m_jointFeedback; } - int getUid() const { - return m_userConstraintId; - } + return m_userConstraintId; + } - bool needsFeedback() const + bool needsFeedback() const { return m_needsFeedback; } ///enableFeedback will allow to read the applied linear and angular impulse ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information - void enableFeedback(bool needsFeedback) + void enableFeedback(bool needsFeedback) { m_needsFeedback = needsFeedback; } - ///getAppliedImpulse is an estimated total applied impulse. + ///getAppliedImpulse is an estimated total applied impulse. ///This feedback could be used to determine breaking constraints or playing sounds. - b3Scalar getAppliedImpulse() const + b3Scalar getAppliedImpulse() const { b3Assert(m_needsFeedback); return m_appliedImpulse; } - b3TypedConstraintType getConstraintType () const + b3TypedConstraintType getConstraintType() const { return b3TypedConstraintType(m_objectType); } - + void setDbgDrawSize(b3Scalar dbgDrawSize) { m_dbgDrawSize = dbgDrawSize; @@ -316,35 +303,34 @@ public: return m_dbgDrawSize; } - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, b3Scalar value, int axis = -1) = 0; + virtual void setParam(int num, b3Scalar value, int axis = -1) = 0; ///return the local value of parameter - virtual b3Scalar getParam(int num, int axis = -1) const = 0; - -// virtual int calculateSerializeBufferSize() const; + virtual b3Scalar getParam(int num, int axis = -1) const = 0; + + // virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) //virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const; - }; -// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits +// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits // all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI]) B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians) { - if(angleLowerLimitInRadians >= angleUpperLimitInRadians) + if (angleLowerLimitInRadians >= angleUpperLimitInRadians) { return angleInRadians; } - else if(angleInRadians < angleLowerLimitInRadians) + else if (angleInRadians < angleLowerLimitInRadians) { b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians)); b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians)); return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI); } - else if(angleInRadians > angleUpperLimitInRadians) + else if (angleInRadians > angleUpperLimitInRadians) { b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians)); b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians)); @@ -356,6 +342,7 @@ B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar } } +// clang-format off ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct b3TypedConstraintData { @@ -379,17 +366,18 @@ struct b3TypedConstraintData }; +// clang-format on + /*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const { return sizeof(b3TypedConstraintData); } */ - class b3AngularLimit { private: - b3Scalar + b3Scalar m_center, m_halfRange, m_softness, @@ -404,15 +392,16 @@ private: public: /// Default constructor initializes limit as inactive, allowing free constraint movement b3AngularLimit() - :m_center(0.0f), - m_halfRange(-1.0f), - m_softness(0.9f), - m_biasFactor(0.3f), - m_relaxationFactor(1.0f), - m_correction(0.0f), - m_sign(0.0f), - m_solveLimit(false) - {} + : m_center(0.0f), + m_halfRange(-1.0f), + m_softness(0.9f), + m_biasFactor(0.3f), + m_relaxationFactor(1.0f), + m_correction(0.0f), + m_sign(0.0f), + m_solveLimit(false) + { + } /// Sets all limit's parameters. /// When low > high limit becomes inactive. @@ -441,13 +430,13 @@ public: return m_relaxationFactor; } - /// Returns correction value evaluated when test() was invoked + /// Returns correction value evaluated when test() was invoked inline b3Scalar getCorrection() const { return m_correction; } - /// Returns sign value evaluated when test() was invoked + /// Returns sign value evaluated when test() was invoked inline b3Scalar getSign() const { return m_sign; @@ -475,9 +464,6 @@ public: b3Scalar getLow() const; b3Scalar getHigh() const; - }; - - -#endif //B3_TYPED_CONSTRAINT_H +#endif //B3_TYPED_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp index fbc84cc28d..f1080d9d5e 100644 --- a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp +++ b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp @@ -11,7 +11,6 @@ #include "Bullet3Dynamics/shared/b3ContactConstraint4.h" #include "Bullet3Dynamics/shared/b3Inertia.h" - struct b3CpuRigidBodyPipelineInternalData { b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies; @@ -22,7 +21,6 @@ struct b3CpuRigidBodyPipelineInternalData b3CpuNarrowPhase* m_np; b3Config m_config; }; - b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config) { @@ -39,49 +37,43 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline() void b3CpuRigidBodyPipeline::updateAabbWorldSpace() { - - for (int i=0;i<this->getNumBodies();i++) + for (int i = 0; i < this->getNumBodies(); i++) { b3RigidBodyData* body = &m_data->m_rigidBodies[i]; b3Float4 position = body->m_pos; - b3Quat orientation = body->m_quat; + b3Quat orientation = body->m_quat; int collidableIndex = body->m_collidableIdx; b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex); int shapeIndex = collidable.m_shapeIndex; - - if (shapeIndex>=0) - { - + if (shapeIndex >= 0) + { b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex); b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i]; - float margin=0.f; - b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec); - m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0); + float margin = 0.f; + b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec); + m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0); } } } -void b3CpuRigidBodyPipeline::computeOverlappingPairs() +void b3CpuRigidBodyPipeline::computeOverlappingPairs() { int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); m_data->m_bp->calculateOverlappingPairs(); numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); - printf("numPairs=%d\n",numPairs); + printf("numPairs=%d\n", numPairs); } void b3CpuRigidBodyPipeline::computeContactPoints() { - b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray(); - - m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies); + m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies); } -void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime) +void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime) { - //update world space aabb's updateAabbWorldSpace(); @@ -92,73 +84,71 @@ void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime) computeContactPoints(); //solve contacts - + //update transforms integrate(deltaTime); - - } - -static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, - const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1) +static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, + const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1) { return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1); } - -static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1, - b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1) +static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1, + b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1) { linear = -n; angular0 = -b3Cross(r0, n); angular1 = b3Cross(r1, n); } - - -static inline void b3SolveContact(b3ContactConstraint4& cs, - const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) +static inline void b3SolveContact(b3ContactConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) { - - b3Vector3 dLinVelA; dLinVelA.setZero(); - b3Vector3 dAngVelA; dAngVelA.setZero(); - b3Vector3 dLinVelB; dLinVelB.setZero(); - b3Vector3 dAngVelB; dAngVelB.setZero(); - - for(int ic=0; ic<4; ic++) + b3Vector3 dLinVelA; + dLinVelA.setZero(); + b3Vector3 dAngVelA; + dAngVelA.setZero(); + b3Vector3 dLinVelB; + dLinVelB.setZero(); + b3Vector3 dAngVelB; + dAngVelB.setZero(); + + for (int ic = 0; ic < 4; ic++) { // dont necessary because this makes change to 0 - if( cs.m_jacCoeffInv[ic] == 0.f ) continue; + if (cs.m_jacCoeffInv[ic] == 0.f) continue; { b3Vector3 angular0, angular1, linear; b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; - b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 ); + b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1); - float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic]; + float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB) + + cs.m_b[ic]; rambdaDt *= cs.m_jacCoeffInv[ic]; { float prevSum = cs.m_appliedRambdaDt[ic]; float updated = prevSum; updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[ic] ); - updated = b3Min( updated, maxRambdaDt[ic] ); + updated = b3Max(updated, minRambdaDt[ic]); + updated = b3Min(updated, maxRambdaDt[ic]); rambdaDt = updated - prevSum; cs.m_appliedRambdaDt[ic] = updated; } - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + b3Vector3 linImp0 = invMassA * linear * rambdaDt; + b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt; + b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt; + b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt; #ifdef _WIN32 - b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp0.getX())); b3Assert(_finite(linImp1.getX())); #endif { @@ -169,53 +159,46 @@ static inline void b3SolveContact(b3ContactConstraint4& cs, } } } - - } - - - - -static inline void b3SolveFriction(b3ContactConstraint4& cs, - const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) +static inline void b3SolveFriction(b3ContactConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) { - - if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; + if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return; const b3Vector3& center = (const b3Vector3&)cs.m_center; b3Vector3 n = -(const b3Vector3&)cs.m_linear; b3Vector3 tangent[2]; - b3PlaneSpace1 (n, tangent[0],tangent[1]); + b3PlaneSpace1(n, tangent[0], tangent[1]); b3Vector3 angular0, angular1, linear; b3Vector3 r0 = center - posA; b3Vector3 r1 = center - posB; - for(int i=0; i<2; i++) + for (int i = 0; i < 2; i++) { - b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 ); + b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1); float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ); + linVelA, angVelA, linVelB, angVelB); rambdaDt *= cs.m_fJacCoeffInv[i]; - { - float prevSum = cs.m_fAppliedRambdaDt[i]; - float updated = prevSum; - updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[i] ); - updated = b3Min( updated, maxRambdaDt[i] ); - rambdaDt = updated - prevSum; - cs.m_fAppliedRambdaDt[i] = updated; - } + { + float prevSum = cs.m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max(updated, minRambdaDt[i]); + updated = b3Min(updated, maxRambdaDt[i]); + rambdaDt = updated - prevSum; + cs.m_fAppliedRambdaDt[i] = updated; + } - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + b3Vector3 linImp0 = invMassA * linear * rambdaDt; + b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt; + b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt; + b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt; #ifdef _WIN32 b3Assert(_finite(linImp0.getX())); b3Assert(_finite(linImp1.getX())); @@ -226,57 +209,45 @@ static inline void b3SolveFriction(b3ContactConstraint4& cs, angVelB += angImp1; } - { // angular damping for point constraint - b3Vector3 ab = ( posB - posA ).normalized(); - b3Vector3 ac = ( center - posA ).normalized(); - if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { // angular damping for point constraint + b3Vector3 ab = (posB - posA).normalized(); + b3Vector3 ac = (center - posA).normalized(); + if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) { - float angNA = b3Dot( n, angVelA ); - float angNB = b3Dot( n, angVelB ); + float angNA = b3Dot(n, angVelA); + float angNB = b3Dot(n, angVelB); - angVelA -= (angNA*0.1f)*n; - angVelB -= (angNB*0.1f)*n; + angVelA -= (angNA * 0.1f) * n; + angVelB -= (angNB * 0.1f) * n; } } - } - - - - -struct b3SolveTask// : public ThreadPool::Task +struct b3SolveTask // : public ThreadPool::Task { - b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies, - b3AlignedObjectArray<b3Inertia>& shapes, + b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies, + b3AlignedObjectArray<b3Inertia>& shapes, b3AlignedObjectArray<b3ContactConstraint4>& constraints, int start, int nConstraints, int maxNumBatches, - b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx - ) - : m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), - m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx), -m_start( start ), - m_nConstraints( nConstraints ), - m_solveFriction( true ), - m_maxNumBatches(maxNumBatches) - {} - - unsigned short int getType(){ return 0; } + b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx) + : m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches) + { + } + + unsigned short int getType() { return 0; } void run(int tIdx) { b3AlignedObjectArray<int> usedBodies; //printf("run..............\n"); - - for (int bb=0;bb<m_maxNumBatches;bb++) + for (int bb = 0; bb < m_maxNumBatches; bb++) { usedBodies.resize(0); - for(int ic=m_nConstraints-1; ic>=0; ic--) + for (int ic = m_nConstraints - 1; ic >= 0; ic--) //for(int ic=0; ic<m_nConstraints; ic++) { - int i = m_start + ic; if (m_constraints[i].m_batchIdx != bb) continue; @@ -298,87 +269,80 @@ m_start( start ), //printf("ic(b)=%d, localBatch=%d\n",ic,localBatch); } #endif - if (aIdx==10) + if (aIdx == 10) { //printf("ic(a)=%d, localBatch=%d\n",ic,localBatch); } - if (usedBodies.size()<(aIdx+1)) + if (usedBodies.size() < (aIdx + 1)) { - usedBodies.resize(aIdx+1,0); + usedBodies.resize(aIdx + 1, 0); } - - if (usedBodies.size()<(bIdx+1)) + + if (usedBodies.size() < (bIdx + 1)) { - usedBodies.resize(bIdx+1,0); + usedBodies.resize(bIdx + 1, 0); } if (bodyA.m_invMass) { - b3Assert(usedBodies[aIdx]==0); + b3Assert(usedBodies[aIdx] == 0); usedBodies[aIdx]++; } - + if (bodyB.m_invMass) { - b3Assert(usedBodies[bIdx]==0); + b3Assert(usedBodies[bIdx] == 0); usedBodies[bIdx]++; } - - if( !m_solveFriction ) + if (!m_solveFriction) { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; - - b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt ); + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; + b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt); } else { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; float sum = 0; - for(int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { - sum +=m_constraints[i].m_appliedRambdaDt[j]; + sum += m_constraints[i].m_appliedRambdaDt[j]; } frictionCoeff = 0.7f; - for(int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { - maxRambdaDt[j] = frictionCoeff*sum; + maxRambdaDt[j] = frictionCoeff * sum; minRambdaDt[j] = -maxRambdaDt[j]; } - b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt ); - + b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt); } } if (m_wgUsedBodies) { - if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size()) + if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size()) { m_wgUsedBodies[m_curWgidx].resize(usedBodies.size()); } - for (int i=0;i<usedBodies.size();i++) + for (int i = 0; i < usedBodies.size(); i++) { if (usedBodies[i]) { //printf("cell %d uses body %d\n", m_curWgidx,i); - m_wgUsedBodies[m_curWgidx][i]=1; + m_wgUsedBodies[m_curWgidx][i] = 1; } } } - } - - - } b3AlignedObjectArray<b3RigidBodyData>& m_bodies; @@ -397,24 +361,22 @@ void b3CpuRigidBodyPipeline::solveContactConstraints() int m_nIterations = 4; b3AlignedObjectArray<b3ContactConstraint4> contactConstraints; -// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts(); + // const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts(); int n = contactConstraints.size(); //convert contacts... - - int maxNumBatches = 250; - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0); + b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0); task.m_solveFriction = false; task.run(0); } - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0); + b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0); task.m_solveFriction = true; task.run(0); } @@ -422,53 +384,51 @@ void b3CpuRigidBodyPipeline::solveContactConstraints() void b3CpuRigidBodyPipeline::integrate(float deltaTime) { - float angDamping=0.f; - b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0); + float angDamping = 0.f; + b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0); //integrate transforms (external forces/gravity should be moved into constraint solver) - for (int i=0;i<m_data->m_rigidBodies.size();i++) + for (int i = 0; i < m_data->m_rigidBodies.size(); i++) { - b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration); + b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration); } - } -int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData) +int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData) { b3RigidBodyData body; int bodyIndex = m_data->m_rigidBodies.size(); - body.m_invMass = mass ? 1.f/mass : 0.f; - body.m_angVel.setValue(0,0,0); + body.m_invMass = mass ? 1.f / mass : 0.f; + body.m_angVel.setValue(0, 0, 0); body.m_collidableIdx = collidableIndex; body.m_frictionCoeff = 0.3f; - body.m_linVel.setValue(0,0,0); - body.m_pos.setValue(position[0],position[1],position[2]); - body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + body.m_linVel.setValue(0, 0, 0); + body.m_pos.setValue(position[0], position[1], position[2]); + body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]); body.m_restituitionCoeff = 0.f; m_data->m_rigidBodies.push_back(body); - - if (collidableIndex>=0) + if (collidableIndex >= 0) { b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand(); b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex); - b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); - b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); - + b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]); + b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]); + b3Scalar margin = 0.01f; b3Transform t; t.setIdentity(); - t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); - t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); - b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec); - - m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1); -// b3Vector3 aabbMin,aabbMax; - // m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax); + t.setOrigin(b3MakeVector3(position[0], position[1], position[2])); + t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3])); + b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec); - } else + m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1); + // b3Vector3 aabbMin,aabbMax; + // m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax); + } + else { b3Error("registerPhysicsInstance using invalid collidableIndex\n"); } @@ -476,13 +436,12 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po return bodyIndex; } - const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const { return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0; } -int b3CpuRigidBodyPipeline::getNumBodies() const +int b3CpuRigidBodyPipeline::getNumBodies() const { return m_data->m_rigidBodies.size(); } diff --git a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h index 2f3c2ae77e..9c65419f26 100644 --- a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h +++ b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h @@ -16,52 +16,47 @@ subject to the following restrictions: #ifndef B3_CPU_RIGIDBODY_PIPELINE_H #define B3_CPU_RIGIDBODY_PIPELINE_H - - #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h" class b3CpuRigidBodyPipeline { protected: - struct b3CpuRigidBodyPipelineInternalData* m_data; + struct b3CpuRigidBodyPipelineInternalData* m_data; int allocateCollidable(); public: - - b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config); virtual ~b3CpuRigidBodyPipeline(); - virtual void stepSimulation(float deltaTime); - virtual void integrate(float timeStep); - virtual void updateAabbWorldSpace(); - virtual void computeOverlappingPairs(); - virtual void computeContactPoints(); - virtual void solveContactConstraints(); - - int registerConvexPolyhedron(class b3ConvexUtility* convex); - - int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData); - void writeAllInstancesToGpu(); - void copyConstraintsToHost(); - void setGravity(const float* grav); - void reset(); - - int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold); + virtual void stepSimulation(float deltaTime); + virtual void integrate(float timeStep); + virtual void updateAabbWorldSpace(); + virtual void computeOverlappingPairs(); + virtual void computeContactPoints(); + virtual void solveContactConstraints(); + + int registerConvexPolyhedron(class b3ConvexUtility* convex); + + int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData); + void writeAllInstancesToGpu(); + void copyConstraintsToHost(); + void setGravity(const float* grav); + void reset(); + + int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold); int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold); void removeConstraintByUid(int uid); - void addConstraint(class b3TypedConstraint* constraint); - void removeConstraint(b3TypedConstraint* constraint); + void addConstraint(class b3TypedConstraint* constraint); + void removeConstraint(b3TypedConstraint* constraint); - void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults); + void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults); const struct b3RigidBodyData* getBodyBuffer() const; - int getNumBodies() const; - + int getNumBodies() const; }; -#endif //B3_CPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file +#endif //B3_CPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h index 68cf65e312..cf2eed0e7c 100644 --- a/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h +++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h @@ -5,30 +5,27 @@ typedef struct b3ContactConstraint4 b3ContactConstraint4_t; - struct b3ContactConstraint4 { - - b3Float4 m_linear;//normal? + b3Float4 m_linear; //normal? b3Float4 m_worldPos[4]; - b3Float4 m_center; // friction + b3Float4 m_center; // friction float m_jacCoeffInv[4]; float m_b[4]; float m_appliedRambdaDt[4]; - float m_fJacCoeffInv[2]; // friction - float m_fAppliedRambdaDt[2]; // friction + float m_fJacCoeffInv[2]; // friction + float m_fAppliedRambdaDt[2]; // friction unsigned int m_bodyA; unsigned int m_bodyB; - int m_batchIdx; + int m_batchIdx; unsigned int m_paddings; - }; //inline void setFrictionCoeff(float value) { m_linear[3] = value; } -inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) +inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) { - return constraint->m_linear.w; + return constraint->m_linear.w; } -#endif //B3_CONTACT_CONSTRAINT5_H +#endif //B3_CONTACT_CONSTRAINT5_H diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h index 805a2bd3ea..3e72f1c3f2 100644 --- a/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h +++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h @@ -4,89 +4,84 @@ #include "Bullet3Dynamics/shared/b3ContactConstraint4.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" - -void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q); - void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q) +void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q); +void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q) { - if (b3Fabs(n.z) > 0.70710678f) { - // choose p in y-z plane - float a = n.y*n.y + n.z*n.z; - float k = 1.f/sqrt(a); - p[0].x = 0; - p[0].y = -n.z*k; - p[0].z = n.y*k; - // set q = n x p - q[0].x = a*k; - q[0].y = -n.x*p[0].z; - q[0].z = n.x*p[0].y; - } - else { - // choose p in x-y plane - float a = n.x*n.x + n.y*n.y; - float k = 1.f/sqrt(a); - p[0].x = -n.y*k; - p[0].y = n.x*k; - p[0].z = 0; - // set q = n x p - q[0].x = -n.z*p[0].y; - q[0].y = n.z*p[0].x; - q[0].z = a*k; - } + if (b3Fabs(n.z) > 0.70710678f) + { + // choose p in y-z plane + float a = n.y * n.y + n.z * n.z; + float k = 1.f / sqrt(a); + p[0].x = 0; + p[0].y = -n.z * k; + p[0].z = n.y * k; + // set q = n x p + q[0].x = a * k; + q[0].y = -n.x * p[0].z; + q[0].z = n.x * p[0].y; + } + else + { + // choose p in x-y plane + float a = n.x * n.x + n.y * n.y; + float k = 1.f / sqrt(a); + p[0].x = -n.y * k; + p[0].y = n.x * k; + p[0].z = 0; + // set q = n x p + q[0].x = -n.z * p[0].y; + q[0].y = n.z * p[0].x; + q[0].z = a * k; + } } - - -void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1) +void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1) { - *linear = b3MakeFloat4(n.x,n.y,n.z,0.f); + *linear = b3MakeFloat4(n.x, n.y, n.z, 0.f); *angular0 = b3Cross3(r0, n); *angular1 = -b3Cross3(r1, n); } - -float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0, - b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 ) +float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0, + b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1) { return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1); } - float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1, - float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1) + float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1) { // linear0,1 are normlized - float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0; - float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0); - float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1; - float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1); - return -1.f/(jmj0+jmj1+jmj2+jmj3); + float jmj0 = invMass0; //b3Dot3F4(linear0, linear0)*invMass0; + float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0); + float jmj2 = invMass1; //b3Dot3F4(linear1, linear1)*invMass1; + float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1); + return -1.f / (jmj0 + jmj1 + jmj2 + jmj3); } - -void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA, - b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, - __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff, - b3ContactConstraint4_t* dstC ) +void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA, + b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, + __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff, + b3ContactConstraint4_t* dstC) { dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit); dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit); - float dtInv = 1.f/dt; - for(int ic=0; ic<4; ic++) + float dtInv = 1.f / dt; + for (int ic = 0; ic < 4; ic++) { dstC->m_appliedRambdaDt[ic] = 0.f; } dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f; - dstC->m_linear = src->m_worldNormalOnB; - dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() ); - for(int ic=0; ic<4; ic++) + dstC->m_linear.w = 0.7f; //src->getFrictionCoeff() ); + for (int ic = 0; ic < 4; ic++) { b3Float4 r0 = src->m_worldPosB[ic] - posA; b3Float4 r1 = src->m_worldPosB[ic] - posB; - if( ic >= src->m_worldNormalOnB.w )//npoints + if (ic >= src->m_worldNormalOnB.w) //npoints { dstC->m_jacCoeffInv[ic] = 0.f; continue; @@ -98,56 +93,56 @@ void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4Co setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1); dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, - invMassA, &invInertiaA, invMassB, &invInertiaB ); + invMassA, &invInertiaA, invMassB, &invInertiaB); relVelN = calcRelVel(linear, -linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB); + linVelA, angVelA, linVelB, angVelB); - float e = 0.f;//src->getRestituitionCoeff(); - if( relVelN*relVelN < 0.004f ) e = 0.f; + float e = 0.f; //src->getRestituitionCoeff(); + if (relVelN * relVelN < 0.004f) e = 0.f; - dstC->m_b[ic] = e*relVelN; + dstC->m_b[ic] = e * relVelN; //float penetration = src->m_worldPosB[ic].w; - dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv; + dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv; dstC->m_appliedRambdaDt[ic] = 0.f; } } - if( src->m_worldNormalOnB.w > 0 )//npoints - { // prepare friction - b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f); - for(int i=0; i<src->m_worldNormalOnB.w; i++) + if (src->m_worldNormalOnB.w > 0) //npoints + { // prepare friction + b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f); + for (int i = 0; i < src->m_worldNormalOnB.w; i++) center += src->m_worldPosB[i]; center /= (float)src->m_worldNormalOnB.w; b3Float4 tangent[2]; - b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]); - + b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]); + b3Float4 r[2]; r[0] = center - posA; r[1] = center - posB; - for(int i=0; i<2; i++) + for (int i = 0; i < 2; i++) { b3Float4 linear, angular0, angular1; setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1); dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1, - invMassA, &invInertiaA, invMassB, &invInertiaB ); + invMassA, &invInertiaA, invMassB, &invInertiaB); dstC->m_fAppliedRambdaDt[i] = 0.f; } dstC->m_center = center; } - for(int i=0; i<4; i++) + for (int i = 0; i < 4; i++) { - if( i<src->m_worldNormalOnB.w ) + if (i < src->m_worldNormalOnB.w) { dstC->m_worldPos[i] = src->m_worldPosB[i]; } else { - dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f); + dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f); } } } diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h index 96fe9f8b39..602a1335aa 100644 --- a/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h +++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h @@ -11,5 +11,4 @@ struct b3Inertia b3Mat3x3 m_initInvInertia; }; - -#endif //B3_INERTIA_H
\ No newline at end of file +#endif //B3_INERTIA_H
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h index e96f90d3f3..56d9118f95 100644 --- a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h +++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h @@ -2,11 +2,8 @@ #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" - - -inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) +inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) { - if (bodies[nodeID].m_invMass != 0.f) { float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); @@ -18,27 +15,27 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod bodies[nodeID].m_angVel.x *= angularDamping; bodies[nodeID].m_angVel.y *= angularDamping; bodies[nodeID].m_angVel.z *= angularDamping; - + b3Float4 angvel = bodies[nodeID].m_angVel; float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); - + //limit the angular motion - if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) + if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) { fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; } - if(fAngle < 0.001f) + if (fAngle < 0.001f) { // use Taylor's expansions of sync function - axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); + axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); + axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle); } - + b3Quat dorn; dorn.x = axis.x; dorn.y = axis.y; @@ -47,23 +44,21 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod b3Quat orn0 = bodies[nodeID].m_quat; b3Quat predictedOrn = b3QuatMul(dorn, orn0); predictedOrn = b3QuatNormalized(predictedOrn); - bodies[nodeID].m_quat=predictedOrn; + bodies[nodeID].m_quat = predictedOrn; } - //linear velocity - bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep; - + //linear velocity + bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep; + //apply gravity bodies[nodeID].m_linVel += gravityAcceleration * timeStep; - } - } -inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) +inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) { float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); - - if( (body->m_invMass != 0.f)) + + if ((body->m_invMass != 0.f)) { //angular velocity { @@ -72,23 +67,23 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt body->m_angVel.x *= angularDamping; body->m_angVel.y *= angularDamping; body->m_angVel.z *= angularDamping; - + b3Float4 angvel = body->m_angVel; float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); //limit the angular motion - if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) + if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) { fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; } - if(fAngle < 0.001f) + if (fAngle < 0.001f) { // use Taylor's expansions of sync function - axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); + axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); + axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle); } b3Quat dorn; dorn.x = axis.x; @@ -99,15 +94,13 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt b3Quat predictedOrn = b3QuatMul(dorn, orn0); predictedOrn = b3QuatNormalized(predictedOrn); - body->m_quat=predictedOrn; + body->m_quat = predictedOrn; } //apply gravity body->m_linVel += gravityAcceleration * timeStep; - //linear velocity - body->m_pos += body->m_linVel * timeStep; - + //linear velocity + body->m_pos += body->m_linVel * timeStep; } - } |