diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/ConstraintSolver')
14 files changed, 5172 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h new file mode 100644 index 0000000000..7a12257b33 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h @@ -0,0 +1,159 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CONTACT_SOLVER_INFO +#define B3_CONTACT_SOLVER_INFO + +#include "Bullet3Common/b3Scalar.h" + +enum b3SolverMode +{ + B3_SOLVER_RANDMIZE_ORDER = 1, + B3_SOLVER_FRICTION_SEPARATE = 2, + B3_SOLVER_USE_WARMSTARTING = 4, + B3_SOLVER_USE_2_FRICTION_DIRECTIONS = 16, + B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, + B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, + B3_SOLVER_CACHE_FRIENDLY = 128, + B3_SOLVER_SIMD = 256, + B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, + B3_SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 +}; + +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; + + +}; + +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_restitution = b3Scalar(0.); + m_maxErrorReduction = b3Scalar(20.); + m_numIterations = 10; + m_erp = b3Scalar(0.2); + m_erp2 = b3Scalar(0.8); + m_globalCfm = b3Scalar(0.); + m_sor = b3Scalar(1.); + m_splitImpulse = true; + m_splitImpulsePenetrationThreshold = -.04f; + m_splitImpulseTurnErp = 0.1f; + m_linearSlop = b3Scalar(0.0); + 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. + } +}; + +///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]; + +}; +///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]; +}; + + + +#endif //B3_CONTACT_SOLVER_INFO diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp new file mode 100644 index 0000000000..5e11e74935 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp @@ -0,0 +1,108 @@ + +#include "b3FixedConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#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) +{ + m_pivotInA = frameInA.getOrigin(); + m_pivotInB = frameInB.getOrigin(); + m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); + +} + +b3FixedConstraint::~b3FixedConstraint () +{ +} + + +void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + info->m_numConstraintRows = 6; + info->nub = 6; +} + +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 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; + + 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 a1neg = -a1; + 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; + } + + b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB); + + { + // 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); + } + + // 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]; + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + + //fix the 3 angular degrees of freedom + + int start_row = 3; + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->m_J1angularAxis[start_index] = 1; + info->m_J1angularAxis[start_index + s + 1] = 1; + info->m_J1angularAxis[start_index + s*2+2] = 1; + if ( info->m_J2angularAxis) + { + info->m_J2angularAxis[start_index] = -1; + info->m_J2angularAxis[start_index + s+1] = -1; + info->m_J2angularAxis[start_index + s*2+2] = -1; + } + + + // set right hand side for the angular dofs + + 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]; + } + +}
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h new file mode 100644 index 0000000000..e884a82912 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h @@ -0,0 +1,35 @@ + +#ifndef B3_FIXED_CONSTRAINT_H +#define B3_FIXED_CONSTRAINT_H + +#include "b3TypedConstraint.h" + +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); + + virtual ~b3FixedConstraint(); + + + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies); + + virtual void setParam(int num, b3Scalar value, int axis = -1) + { + b3Assert(0); + } + virtual b3Scalar getParam(int num, int axis = -1) const + { + b3Assert(0); + return 0.f; + } + +}; + +#endif //B3_FIXED_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp new file mode 100644 index 0000000000..168a773d56 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp @@ -0,0 +1,807 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +/* +2007-09-09 +Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + +#include "b3Generic6DofConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include "Bullet3Common/b3TransformUtil.h" +#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) +{ + 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; + 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) +{ + // // 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); + 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)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -B3_HALF_PI; + xyz[2] = b3Scalar(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = B3_HALF_PI; + xyz[2] = 0.0; + } + return false; +} + +//////////////////////////// b3RotationalLimitMotor //////////////////////////////////// + +int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value) +{ + if(m_loLimit>m_hiLimit) + { + 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; + return 1; + } + else if (test_value> m_hiLimit) + { + 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; + return 2; + }; + + 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) + { + 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; + return 2; + } + else if (test_value> hiLimit) + { + m_currentLimit[limitIndex] = 1;//High limit violation + m_currentLimitError[limitIndex] = test_value - hiLimit; + return 1; + }; + + 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); + // 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]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + b3Vector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + + 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); + return tr; +} + +void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies) +{ + b3Transform transA; + b3Transform transB; + transA = getCenterOfMassTransform(bodies[m_rbA]); + transB = getCenterOfMassTransform(bodies[m_rbB]); + calculateTransforms(transA,transB,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 + 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)) + { + m_factA = miB / miS; + } + else + { + m_factA = b3Scalar(0.5f); + } + m_factB = b3Scalar(1.0f) - m_factA; + } +} + + + + + + + +bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index) +{ + b3Scalar angle = m_calculatedAxisAngleDiff[axis_index]; + angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); + m_angularLimits[axis_index].m_currentPosition = angle; + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); +} + + + + +void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + //prepare constraint + 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++) + { + if(m_linearLimits.needApplyForce(i)) + { + info->m_numConstraintRows++; + info->nub--; + } + } + //test angular limits + for (i=0;i<3 ;i++ ) + { + if(testAngularLimitMotor(i)) + { + info->m_numConstraintRows++; + info->nub--; + } + } +// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows); +} + +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) +{ + + b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]); + b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]); + const b3Vector3& linVelA = bodies[m_rbA].m_linVel; + const b3Vector3& linVelB = bodies[m_rbB].m_linVel; + 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); + } + 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); + } + +} + + +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); + + int 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); + } + 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); + } +} + + + +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; + //solve linear limits + b3RotationalLimitMotor limot; + for (int i=0;i<3 ;i++ ) + { + 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]; + 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) + { + 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) + { + rotAllowed = 0; + } + 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); + } + } + } + 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) +{ + b3Generic6DofConstraint * d6constraint = this; + int row = row_offset; + //solve angular limits + for (int i=0;i<3 ;i++ ) + { + 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)) + { + m_angularLimits[i].m_normalCFM = info->cfm[0]; + } + if(!(flags & B3_6DOF_FLAGS_CFM_STOP)) + { + m_angularLimits[i].m_stopCFM = info->cfm[0]; + } + 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); + } + } + + return row; +} + + + + +void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep) +{ + (void)timeStep; + +} + + +void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies) +{ + m_frameInA = frameA; + m_frameInB = frameB; + + calculateTransforms(bodies); +} + + + +b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const +{ + return m_calculatedAxis[axis_index]; +} + + +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)) + { + weight = b3Scalar(1.0); + } + else + { + weight = imA / (imA + imB); + } + const b3Vector3& pA = m_calculatedTransformA.getOrigin(); + const b3Vector3& pB = m_calculatedTransformB.getOrigin(); + m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight); + 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++) + { + 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) +{ + 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]; + } + if (J2) + { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + if((!rotational)) + { + if (m_useOffsetForConstraintFrame) + { + b3Vector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // get its projection to constraint axis + b3Vector3 projB = ax1 * relB.dot(ax1); + // get vector directed from bodyB to constraint axis (and orthogonal to it) + b3Vector3 orthoB = relB - projB; + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + b3Vector3 projA = ax1 * relA.dot(ax1); + b3Vector3 orthoA = relA - projA; + // get desired offset between frames A and B along constraint axis + b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; + // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis + b3Vector3 totalDist = projA + ax1 * desiredOffs - projB; + // get offset vectors relA and relB + relA = orthoA + totalDist * m_factA; + relB = orthoB - totalDist * m_factB; + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + 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 + { + 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]; + + 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]; + } + } + // 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) + { + 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); + info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + } + } + if(limit) + { + b3Scalar k = info->fps * limot->m_stopERP; + if(!rotational) + { + info->m_constraintError[srow] += k * limot->m_currentLimitError; + } + else + { + 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]) + info->m_constraintError[srow] = newc; + } + } + 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; +} + + + + + + + ///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)) + { + switch(num) + { + 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 : + m_linearLimits.m_stopCFM[axis] = value; + m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + 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 : + b3AssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + 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 : + 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 : + m_angularLimits[axis - 3].m_normalCFM = value; + m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + default : + b3AssertConstrParams(0); + } + } + else + { + b3AssertConstrParams(0); + } +} + + ///return the local value of parameter +b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const +{ + b3Scalar retVal = 0; + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + 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 : + 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 : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_linearLimits.m_normalCFM[axis]; + break; + default : + b3AssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + 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 : + 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 : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_angularLimits[axis - 3].m_normalCFM; + break; + default : + b3AssertConstrParams(0); + } + } + else + { + b3AssertConstrParams(0); + } + return retVal; +} + + + +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 + + 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]); + + // 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 new file mode 100644 index 0000000000..084d36055c --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -0,0 +1,550 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +b3Generic6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + +#ifndef B3_GENERIC_6DOF_CONSTRAINT_H +#define B3_GENERIC_6DOF_CONSTRAINT_H + +#include "Bullet3Common/b3Vector3.h" +#include "b3JacobianEntry.h" +#include "b3TypedConstraint.h" + +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 = 0.1f; + 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_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; + } + + + + //! Is limited + 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; + } + + //! calculates error + /*! + calculates m_currentLimit and m_currentLimitError. + */ + int testLimitValue(b3Scalar test_value); + + //! apply the correction impulses for two bodies + 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 + //!@} + 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_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; + + 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++) + { + m_enableMotor[i] = other.m_enableMotor[i]; + m_targetVelocity[i] = other.m_targetVelocity[i]; + m_maxMotorForce[i] = other.m_maxMotorForce[i]; + } + } + + //! 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; + } + 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); + + +}; + +enum b36DofFlags +{ + B3_6DOF_FLAGS_CFM_NORM = 1, + B3_6DOF_FLAGS_CFM_STOP = 2, + B3_6DOF_FLAGS_ERP_STOP = 4 +}; +#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 +/*! +b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. +currently this limit supports rotational motors<br> +<ul> +<li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method. +At this moment translational motors are not supported. May be in the future. </li> + +<li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit. +This is accessible through b3Generic6DofConstraint.getLimitMotor method, +This brings support for limit parameters and motors. </li> + +<li> Angulars limits have these possible ranges: +<table border=1 > +<tr> + <td><b>AXIS</b></td> + <td><b>MIN ANGLE</b></td> + <td><b>MAX ANGLE</b></td> +</tr><tr> + <td>X</td> + <td>-PI</td> + <td>PI</td> +</tr><tr> + <td>Y</td> + <td>-PI/2</td> + <td>PI/2</td> +</tr><tr> + <td>Z</td> + <td>-PI</td> + <td>PI</td> +</tr> +</table> +</li> +</ul> + +*/ +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 + //!@} + + //! 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]; + //!@} + + +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 + + bool m_useLinearReferenceFrameA; + bool m_useOffsetForConstraintFrame; + + int m_flags; + + //!@} + + b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) + { + b3Assert(0); + (void) other; + return *this; + } + + + 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(); + + + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + 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 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; + } + + //! 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; + } + + b3Transform & getFrameOffsetB() + { + return m_frameInB; + } + + + + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + + void getInfo1NonVirtual (b3ConstraintInfo1* info,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); + + //! Get the rotation axis in global coordinates + b3Vector3 getAxis(int axis_index) const; + + //! Get the relative Euler angle + /*! + \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. + */ + 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); + + //! 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); + + void setLinearLowerLimit(const b3Vector3& linearLower) + { + m_linearLimits.m_lowerLimit = linearLower; + } + + void getLinearLowerLimit(b3Vector3& linearLower) + { + linearLower = m_linearLimits.m_lowerLimit; + } + + void setLinearUpperLimit(const b3Vector3& linearUpper) + { + m_linearLimits.m_upperLimit = linearUpper; + } + + void getLinearUpperLimit(b3Vector3& linearUpper) + { + linearUpper = m_linearLimits.m_upperLimit; + } + + void setAngularLowerLimit(const b3Vector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]); + } + + void getAngularLowerLimit(b3Vector3& angularLower) + { + 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++) + m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]); + } + + void getAngularUpperLimit(b3Vector3& angularUpper) + { + 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 + { + lo = b3NormalizeAngle(lo); + hi = b3NormalizeAngle(hi); + m_angularLimits[axis-3].m_loLimit = lo; + m_angularLimits[axis-3].m_hiLimit = hi; + } + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + bool isLimited(int limitIndex) + { + if(limitIndex<3) + { + return m_linearLimits.isLimited(limitIndex); + + } + return m_angularLimits[limitIndex-3].isLimited(); + } + + 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); + + // 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). + ///If no axis is provided, it uses the default axis for this constraint. + 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); + + + + +}; + + + + + +#endif //B3_GENERIC_6DOF_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h new file mode 100644 index 0000000000..a55168eb38 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h @@ -0,0 +1,155 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_JACOBIAN_ENTRY_H +#define B3_JACOBIAN_ENTRY_H + +#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 +// if you only are interested in angular part, just feed massInvA and massInvB zero + +/// 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 +{ +public: + b3JacobianEntry() {}; + //constraint between two different rigidbodies + b3JacobianEntry( + const b3Matrix3x3& world2A, + const b3Matrix3x3& world2B, + const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + const b3Vector3& jointAxis, + const b3Vector3& inertiaInvA, + const b3Scalar massInvA, + const b3Vector3& inertiaInvB, + const b3Scalar massInvB) + :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_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + b3Assert(m_Adiag > b3Scalar(0.0)); + } + + //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.))) + { + 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); + + 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) + { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + b3Assert(m_Adiag > b3Scalar(0.0)); + } + + //constraint on one rigidbody + b3JacobianEntry( + const b3Matrix3x3& world2A, + const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + const b3Vector3& jointAxis, + const b3Vector3& inertiaInvA, + const b3Scalar massInvA) + :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_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + b3Assert(m_Adiag > b3Scalar(0.0)); + } + + 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 + { + const b3JacobianEntry& jacA = *this; + b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + b3Scalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + 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 + { + 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 lin1 = massInvB * lin; + 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) + { + b3Vector3 linrel = linvelA - linvelB; + 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]; + return rel_vel2 + B3_EPSILON; + } +//private: + + 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; + +}; + +#endif //B3_JACOBIAN_ENTRY_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp new file mode 100644 index 0000000000..de729d4556 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -0,0 +1,1815 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2012 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +//enable B3_SOLVER_DEBUG if you experience solver crashes +//#define B3_SOLVER_DEBUG +//#define COMPUTE_IMPULSE_DENOM 1 +//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. + +//#define DISABLE_JOINTS + +#include "b3PgsJacobiSolver.h" +#include "Bullet3Common/b3MinMax.h" +#include "b3TypedConstraint.h" +#include <new> +#include "Bullet3Common/b3StackAlloc.h" + +//#include "b3SolverBody.h" +//#include "b3SolverConstraint.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#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) +{ + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +} + +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) +{ + return inertia->m_invInertiaWorld; +} + + + +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) +{ + return rb->m_linVel; +} + +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) +{ + return rb->m_angVel; +} + +static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos) +{ + //we also calculate lin/ang velocity for kinematic objects + return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); + +} + +struct b3ContactPoint +{ + 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() + { + return m_positionWorldOnA; + } + b3Vector3 getPositionWorldOnB() + { + return m_positionWorldOnB; + } + b3Scalar getDistance() + { + return m_distance; + } +}; + +void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut) +{ + pointOut.m_appliedImpulse = 0.f; + pointOut.m_appliedImpulseLateral1 = 0.f; + pointOut.m_appliedImpulseLateral2 = 0.f; + pointOut.m_combinedFriction = contact->getFrictionCoeff(); + pointOut.m_combinedRestitution = contact->getRestituitionCoeff(); + pointOut.m_combinedRollingFriction = 0.f; + pointOut.m_contactCFM1 = 0.f; + pointOut.m_contactCFM2 = 0.f; + pointOut.m_contactMotion1 = 0.f; + pointOut.m_contactMotion2 = 0.f; + pointOut.m_distance = contact->getPenetration(contactIndex);//??0.01f + b3Vector3 normalOnB = contact->m_worldNormalOnB; + normalOnB.normalize();//is this needed? + + 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; +} + +int getNumContacts(b3Contact4* contact) +{ + return contact->getNPoints(); +} + +b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs) +: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) +{ + 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_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; + 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); + + 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) +{ + + 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; +} + + + + + + + + + +#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 ) +{ + __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 + +// Project Gauss Seidel or the equivalent Sequential Impulse +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 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)); +#else + resolveSingleConstraintRowGeneric(body1,body2,c); +#endif +} + +// Project Gauss Seidel or the equivalent Sequential Impulse + 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()); + +// 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; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + 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); +} + + 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 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)); +#else + resolveSingleConstraintRowLowerLimit(body1,body2,c); +#endif +} + +// Project Gauss Seidel or the equivalent Sequential Impulse + 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()); + + 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; + 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); +} + + +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); + } +} + + void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +{ +#ifdef USE_SIMD + if (!c.m_rhsPenetration) + return; + + 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 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)); +#else + resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); +#endif +} + + + +unsigned long b3PgsJacobiSolver::b3Rand2() +{ + 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) +{ + // seems good; xor-fold and modulus + const unsigned long un = static_cast<unsigned long>(n); + unsigned long r = b3Rand2(); + + // 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) { + r ^= (r >> 16); + if (un <= 0x00000100UL) { + r ^= (r >> 8); + if (un <= 0x00000010UL) { + r ^= (r >> 4); + if (un <= 0x00000004UL) { + r ^= (r >> 2); + if (un <= 0x00000002UL) { + r ^= (r >> 1); + } + } + } + } + } + + return (int) (r % un); +} + + + +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); + + if (rb) + { + solverBody->m_worldTransform = getWorldTransform(rb); + 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_linearVelocity = getLinearVelocity(rb); + solverBody->m_angularVelocity = getAngularVelocity(rb); + } else + { + solverBody->m_worldTransform.setIdentity(); + 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); + } + + +} + + + + + + +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) +{ + + + solverConstraint.m_contactNormal = normalAxis; + b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + + b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex]; + b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex]; + + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + + solverConstraint.m_friction = cp.m_combinedFriction; + solverConstraint.m_originalContactPoint = 0; + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + 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); + } + { + 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); + } + + b3Scalar scaledDenom; + + { + b3Vector3 vec; + b3Scalar denom0 = 0.f; + b3Scalar denom1 = 0.f; + if (body0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = body0->m_invMass + normalAxis.dot(vec); + } + if (body1) + { + 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 + { + 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); + } + + 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)); + + rel_vel = vel1Dotn+vel2Dotn; + +// b3Scalar positionalError = 0.f; + + 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& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + 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) + +{ + b3Vector3 normalAxis=b3MakeVector3(0,0,0); + + + solverConstraint.m_contactNormal = normalAxis; + b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + + b3RigidBodyData* body0 = &bodies[m_tmpSolverBodyPool[solverBodyIdA].m_originalBodyIndex]; + b3RigidBodyData* body1 = &bodies[m_tmpSolverBodyPool[solverBodyIdB].m_originalBodyIndex]; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + + solverConstraint.m_friction = cp.m_combinedRollingFriction; + solverConstraint.m_originalContactPoint = 0; + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + b3Vector3 ftorqueAxis1 = -normalAxis1; + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + 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); + } + + + { + 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; + } + + { + + + 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)); + + rel_vel = vel1Dotn+vel2Dotn; + +// b3Scalar positionalError = 0.f; + + 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& 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); + return solverConstraint; +} + + +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_bodyCount[bodyIndex]<0) + { + curIndex = m_tmpSolverBodyPool.size(); + b3SolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(bodyIndex,&solverBody,&body); + solverBody.m_originalBodyIndex = bodyIndex; + m_bodyCount[bodyIndex] = curIndex; + } else + { + curIndex = m_bodyCount[bodyIndex]; + } + } else + { + b3Assert(m_bodyCount[bodyIndex]>0); + m_bodyCountCheck[bodyIndex]++; + curIndex = m_tmpSolverBodyPool.size(); + b3SolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(bodyIndex,&solverBody,&body); + solverBody.m_originalBodyIndex = bodyIndex; + } + + 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) +{ + + const b3Vector3& pos1 = cp.getPositionWorldOnA(); + const b3Vector3& pos2 = cp.getPositionWorldOnB(); + + b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + 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(); + + 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); + + 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; + + vel1 = rb0? getVelocityInLocalPoint(rb0,rel_pos1) : b3MakeVector3(0,0,0); + vel2 = rb1? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0,0,0); + + // b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0); + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + + solverConstraint.m_friction = cp.m_combinedFriction; + + + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= b3Scalar(0.)) + { + restitution = 0.f; + }; + } + + + ///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; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + 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; + } + + 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) +{ + + 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); + 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 + { + frictionConstraint1.m_appliedImpulse = 0.f; + } + } + + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + 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; + if (bodies[bodyA->m_originalBodyIndex].m_invMass) + 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 + { + frictionConstraint2.m_appliedImpulse = 0.f; + } + } +} + + + + +void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal) +{ + b3RigidBodyData* colObj0=0,*colObj1=0; + + + int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias); + int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(),bodies,inertias); + +// 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 numContacts = getNumContacts(manifold); + for (int j=0;j<numContacts;j++) + { + + b3ContactPoint cp; + getContactPoint(manifold,j,cp); + + if (cp.getDistance() <= getContactProcessingThreshold(manifold)) + { + b3Vector3 rel_pos1; + b3Vector3 rel_pos2; + b3Scalar relaxation; + b3Scalar rel_vel; + b3Vector3 vel; + + int frictionIndex = m_tmpSolverContactConstraintPool.size(); + b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); +// 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); + +// const b3Vector3& pos1 = cp.getPositionWorldOnA(); +// const b3Vector3& pos2 = cp.getPositionWorldOnB(); + + /////setup the friction constraints + + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + + b3Vector3 angVelA,angVelB; + solverBodyA->getAngularVelocity(angVelA); + solverBodyB->getAngularVelocity(angVelB); + b3Vector3 relAngVel = angVelB-angVelA; + + if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + { + //only a single rollingFriction per manifold + rollingFriction--; + 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 + { + 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 + ///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, + ///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 + /// + if (!(infoGlobal.m_solverMode & B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + 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_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); + + } + + 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); + + 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_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 + { + 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); + + 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) +{ + B3_PROFILE("solveGroupCacheFriendlySetup"); + + + m_maxOverrideNumSolverIterations = 0; + + + + m_tmpSolverBodyPool.resize(0); + + + m_bodyCount.resize(0); + m_bodyCount.resize(numBodies,0); + m_bodyCountCheck.resize(0); + m_bodyCountCheck.resize(numBodies,0); + + m_deltaLinearVelocities.resize(0); + m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + m_deltaAngularVelocities.resize(0); + m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + + //int totalBodies = 0; + + 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 + { + //didn't implement joints with Jacobi version yet + b3Assert(0); + } + + } + 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 + { + if (bodies[bodyIndexA].m_invMass) + { + //m_bodyCount[bodyIndexA]+=manifoldPtr[i].getNPoints(); + m_bodyCount[bodyIndexA]++; + } + else + m_bodyCount[bodyIndexA]=-1; + + if (bodies[bodyIndexB].m_invMass) + // m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints(); + m_bodyCount[bodyIndexB]++; + else + m_bodyCount[bodyIndexB]=-1; + } + + } + + + + if (1) + { + int j; + for (j=0;j<numConstraints;j++) + { + b3TypedConstraint* constraint = constraints[j]; + + constraint->internalSetAppliedImpulse(0.0f); + } + } + + //b3RigidBody* rb0=0,*rb1=0; + //if (1) + { + { + + int totalNumRows = 0; + int i; + + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + //calculate the total number of contraint rows + for (i=0;i<numConstraints;i++) + { + b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + b3JointFeedback* fb = constraints[i]->getJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + + if (constraints[i]->isEnabled()) + { + } + if (constraints[i]->isEnabled()) + { + constraints[i]->getInfo1(&info1,bodies); + } else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + + +#ifndef DISABLE_JOINTS + ///setup the b3SolverConstraints + int currentRow = 0; + + for (i=0;i<numConstraints;i++) + { + const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + + if (info1.m_numConstraintRows) + { + b3Assert(currentRow<totalNumRows); + + b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; + b3TypedConstraint* constraint = constraints[i]; + + 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]; + + + + + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + + int j; + for ( j=0;j<info1.m_numConstraintRows;j++) + { + memset(¤tConstraintRow[j],0,sizeof(b3SolverConstraint)); + currentConstraintRow[j].m_lowerLimit = -B3_INFINITY; + currentConstraintRow[j].m_upperLimit = B3_INFINITY; + currentConstraintRow[j].m_appliedImpulse = 0.f; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + 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); + + + b3TypedConstraint::b3ConstraintInfo2 info2; + 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.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; + info2.m_damping = infoGlobal.m_damping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; + constraints[i]->getInfo2(&info2,bodies); + + ///finalize the constraint setup + for ( j=0;j<info1.m_numConstraintRows;j++) + { + b3SolverConstraint& solverConstraint = currentConstraintRow[j]; + + if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = 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; + { + + //b3Vector3 angularFactorA(1,1,1); + const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; + solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; + } + + b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld; + { + + const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + 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; + + b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + 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; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + b3Scalar rel_vel; + 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; + + 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; + solverConstraint.m_appliedImpulse = 0.f; + + } + } + } + currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; + } +#endif //DISABLE_JOINTS + } + + + { + int i; + + for (i=0;i<numManifolds;i++) + { + b3Contact4& manifold = manifoldPtr[i]; + convertContact(bodies,inertias,&manifold,infoGlobal); + } + } + } + +// b3ContactSolverInfo info = infoGlobal; + + + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + + ///@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); + else + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); + + m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); + { + int i; + for (i=0;i<numNonContactPool;i++) + { + m_orderNonContactConstraintPool[i] = i; + } + for (i=0;i<numConstraintPool;i++) + { + m_orderTmpConstraintPool[i] = 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) +{ + + 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) + { + + for (int j=0; j<numNonContactPool; ++j) { + int tmp = m_orderNonContactConstraintPool[j]; + 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) + { + for (int j=0; j<numConstraintPool; ++j) { + int tmp = m_orderTmpConstraintPool[j]; + int swapi = b3RandInt2(j+1); + m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; + m_orderTmpConstraintPool[swapi] = tmp; + } + + for (int j=0; j<numFrictionPool; ++j) { + int tmp = m_orderFrictionConstraintPool[j]; + int swapi = b3RandInt2(j+1); + m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; + m_orderFrictionConstraintPool[swapi] = tmp; + } + } + } + } + + if (infoGlobal.m_solverMode & B3_SOLVER_SIMD) + { + ///solve all joint constraints, using SIMD, if available + 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); + } + + 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; + + for (int c=0;c<numPoolConstraints;c++) + { + b3Scalar totalImpulse =0; + + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; + 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]]; + + if (totalImpulse>b3Scalar(0)) + { + 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); + } + } + + if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) + { + + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + + if (totalImpulse>b3Scalar(0)) + { + 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); + } + } + } + } + + } + 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++) + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + 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++) + { + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse>b3Scalar(0)) + { + 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); + } + } + + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (j=0;j<numRollingFrictionPoolConstraints;j++) + { + + b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + if (totalImpulse>b3Scalar(0)) + { + 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); + } + } + + + } + } + } else + { + //non-SIMD version + ///solve all joint constraints + 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); + } + + if (iteration< infoGlobal.m_numIterations) + { + + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + 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); + } + ///solve all friction constraints + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + 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)) + { + 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); + } + } + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + 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)) + { + 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); + } + } + } + } + return 0.f; +} + + +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++) + { + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int 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); + } + } + } + } + else + { + for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + { + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int 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); + } + } + } + } + } +} + +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); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + + for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) + { + + solveSingleIteration(iteration, constraints,numConstraints,infoGlobal); + + + if (!m_usePgs) + { + averageVelocities(); + } + } + + } + return 0.f; +} + +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_deltaAngularVelocities.resize(0); + m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + + 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(); + } + } + + 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 b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal) +{ + B3_PROFILE("solveGroupCacheFriendlyFinish"); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int i,j; + + if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) + { + for (j=0;j<numPoolConstraints;j++) + { + const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; + b3ContactPoint* pt = (b3ContactPoint*) solveManifold.m_originalContactPoint; + b3Assert(pt); + pt->m_appliedImpulse = solveManifold.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; + } + //do a callback here? + } + } + + numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; + b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint; + b3JointFeedback* fb = constr->getJointFeedback(); + if (fb) + { + 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; + + } + + constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); + if (b3Fabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } + } + + { + B3_PROFILE("write back velocities and transforms"); + for ( i=0;i<m_tmpSolverBodyPool.size();i++) + { + int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; + //b3Assert(i==bodyIndex); + + b3RigidBodyData* body = &bodies[bodyIndex]; + if (body->m_invMass) + { + if (infoGlobal.m_splitImpulse) + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + else + m_tmpSolverBodyPool[i].writebackVelocity(); + + if (m_usePgs) + { + body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; + body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; + } else + { + b3Scalar factor = 1.f/b3Scalar(m_bodyCount[bodyIndex]); + + 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()); + + body->m_linVel += deltaLinVel; + body->m_angVel += deltaAngVel; + } + + if (infoGlobal.m_splitImpulse) + { + body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); + b3Quaternion orn; + orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation(); + body->m_quat = orn; + } + } + } + } + + + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + return 0.f; +} + + + +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 new file mode 100644 index 0000000000..d2ca307fab --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h @@ -0,0 +1,149 @@ +#ifndef B3_PGS_JACOBI_SOLVER +#define B3_PGS_JACOBI_SOLVER + + +struct b3Contact4; +struct b3ContactPoint; + + +class b3Dispatcher; + +#include "b3TypedConstraint.h" +#include "b3ContactSolverInfo.h" +#include "b3SolverBody.h" +#include "b3SolverConstraint.h" + +struct b3RigidBodyData; +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<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(); + + int m_maxOverrideNumSolverIterations; + + int m_numSplitImpulseRecoveries; + + 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 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, + b3Vector3& rel_pos1, b3Vector3& rel_pos2); + + 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; + + + b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution); + + void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal); + + + void resolveSplitPenetrationSIMD( + 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); + + 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: + + virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,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); + + 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(); + + unsigned long b3Rand2(); + + int b3RandInt2 (int n); + + void setRandSeed(unsigned long seed) + { + m_btSeed2 = seed; + } + unsigned long getRandSeed() const + { + return m_btSeed2; + } + + + + +}; + +#endif //B3_PGS_JACOBI_SOLVER + diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp new file mode 100644 index 0000000000..02c11db320 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -0,0 +1,209 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#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,const b3Vector3& pivotInA) +:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_flags(0), +m_useSolveConstraintObsolete(false) +{ + +} +*/ + + +void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + getInfo1NonVirtual(info,bodies); +} + +void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + info->m_numConstraintRows = 3; + info->nub = 3; +} + + + + +void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies) +{ + b3Transform trA; + trA.setIdentity(); + trA.setOrigin(bodies[m_rbA].m_pos); + trA.setRotation(bodies[m_rbA].m_quat); + + b3Transform trB; + trB.setIdentity(); + trB.setOrigin(bodies[m_rbB].m_pos); + trB.setRotation(bodies[m_rbB].m_quat); + + getInfo2NonVirtual(info, trA,trB); +} + +void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) +{ + + //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(); + //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 a1neg = -a1; + 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; + } + + b3Vector3 a2 = body1_trans.getBasis()*getPivotInB(); + + { + // 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); + } + + + + // 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]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + if(m_flags & B3_P2P_FLAGS_CFM) + { + for (j=0; j<3; j++) + { + info->cfm[j*info->rowskip] = m_cfm; + } + } + + 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_damping = m_setting.m_damping; + +} + + + +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). +///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) + { + b3AssertConstrParams(0); + } + else + { + switch(num) + { + 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; + m_flags |= B3_P2P_FLAGS_CFM; + break; + default: + b3AssertConstrParams(0); + } + } +} + +///return the local value of parameter +b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const +{ + b3Scalar retVal(B3_INFINITY); + if(axis != -1) + { + b3AssertConstrParams(0); + } + else + { + switch(num) + { + case B3_CONSTRAINT_ERP : + case B3_CONSTRAINT_STOP_ERP : + b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP); + retVal = m_erp; + break; + case B3_CONSTRAINT_CFM : + case B3_CONSTRAINT_STOP_CFM : + b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM); + retVal = m_cfm; + break; + default: + b3AssertConstrParams(0); + } + } + return retVal; +} + diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h new file mode 100644 index 0000000000..681b487334 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -0,0 +1,159 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_POINT2POINTCONSTRAINT_H +#define B3_POINT2POINTCONSTRAINT_H + +#include "Bullet3Common/b3Vector3.h" +//#include "b3JacobianEntry.h" +#include "b3TypedConstraint.h" + +class b3RigidBody; + + +#ifdef B3_USE_DOUBLE_PRECISION +#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData +#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData" +#else +#define b3Point2PointConstraintData b3Point2PointConstraintFloatData +#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData" +#endif //B3_USE_DOUBLE_PRECISION + +struct b3ConstraintSetting +{ + b3ConstraintSetting() : + m_tau(b3Scalar(0.3)), + m_damping(b3Scalar(1.)), + m_impulseClamp(b3Scalar(0.)) + { + } + b3Scalar m_tau; + b3Scalar m_damping; + b3Scalar m_impulseClamp; +}; + +enum b3Point2PointFlags +{ + B3_P2P_FLAGS_ERP = 1, + B3_P2P_FLAGS_CFM = 2 +}; + +/// 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 +{ +#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(); + + 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); + + void getInfo1NonVirtual (b3ConstraintInfo1* 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 updateRHS(b3Scalar timeStep); + + void setPivotA(const b3Vector3& pivotA) + { + m_pivotInA = pivotA; + } + + void setPivotB(const b3Vector3& pivotB) + { + m_pivotInB = pivotB; + } + + const b3Vector3& getPivotInA() const + { + return m_pivotInA; + } + + const b3Vector3& getPivotInB() const + { + return m_pivotInB; + } + + ///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); + ///return the local value of parameter + virtual b3Scalar getParam(int num, int axis = -1) 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; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3Point2PointConstraintFloatData +{ + b3TypedConstraintData m_typeConstraintData; + b3Vector3FloatData m_pivotInA; + b3Vector3FloatData m_pivotInB; +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3Point2PointConstraintDoubleData +{ + b3TypedConstraintData m_typeConstraintData; + b3Vector3DoubleData m_pivotInA; + b3Vector3DoubleData m_pivotInB; +}; + +/* +B3_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const +{ + return sizeof(b3Point2PointConstraintData); + +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +B3_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const +{ + b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer; + + b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); + m_pivotInA.serialize(p2pData->m_pivotInA); + m_pivotInB.serialize(p2pData->m_pivotInB); + + return b3Point2PointConstraintDataName; +} +*/ + +#endif //B3_POINT2POINTCONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h new file mode 100644 index 0000000000..0049317d98 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h @@ -0,0 +1,302 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_SOLVER_BODY_H +#define B3_SOLVER_BODY_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" + +#include "Bullet3Common/b3AlignedAllocator.h" +#include "Bullet3Common/b3TransformUtil.h" + +///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 // + + +#ifdef USE_SIMD + +struct b3SimdScalar +{ + B3_FORCE_INLINE b3SimdScalar() + { + + } + + B3_FORCE_INLINE b3SimdScalar(float fl) + :m_vec128 (_mm_set1_ps(fl)) + { + } + + 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; + }; + B3_FORCE_INLINE __m128 get128() + { + return m_vec128; + } + + B3_FORCE_INLINE const __m128 get128() const + { + return m_vec128; + } + + 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 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) +{ + 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) +{ + 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_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; + }; + + int padding[3]; + + + void setWorldTransform(const b3Transform& worldTransform) + { + m_worldTransform = worldTransform; + } + + const b3Transform& getWorldTransform() const + { + return m_worldTransform; + } + + 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); + else + velocity.setValue(0,0,0); + } + + B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const + { + if (m_originalBody) + angVel =m_angularVelocity+m_deltaAngularVelocity; + else + 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) + { + if (m_originalBody) + { + 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) + { + if (m_originalBody) + { + m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + const b3Vector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const b3Vector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const b3Vector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const b3Vector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + b3Vector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + b3Vector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const b3Vector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const b3Vector3& internalGetInvMass() const + { + return m_invMass; + } + + void internalSetInvMass(const b3Vector3& invMass) + { + m_invMass = invMass; + } + + b3Vector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + b3Vector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + } + + B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const + { + 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) + { + //if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + + void writebackVelocity() + { + //if (m_originalBody>=0) + { + m_linearVelocity +=m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //m_originalBody->setCompanionId(-1); + } + } + + + void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) + { + (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) + { + // 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 + + diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h new file mode 100644 index 0000000000..bce83d4608 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h @@ -0,0 +1,80 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_SOLVER_CONSTRAINT_H +#define B3_SOLVER_CONSTRAINT_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" +//#include "b3JacobianEntry.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +//#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_DECLARE_ALIGNED_ALLOCATOR(); + + b3Vector3 m_relpos1CrossNormal; + b3Vector3 m_contactNormal; + + 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; + 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; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + int m_solverBodyIdA; + int m_solverBodyIdB; + + + enum b3SolverConstraintType + { + B3_SOLVER_CONTACT_1D = 0, + B3_SOLVER_FRICTION_1D + }; +}; + +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 new file mode 100644 index 0000000000..699c481d64 --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp @@ -0,0 +1,161 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#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) +{ +} + + + + +b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact) +{ + if(lowLim > uppLim) + { + return b3Scalar(1.0f); + } + 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((pos >= lowLim) && (pos < (lowLim - delta_max))) + { + lim_fact = (lowLim - pos) / delta_max; + } + else if(pos < lowLim) + { + lim_fact = b3Scalar(0.0f); + } + else + { + lim_fact = b3Scalar(1.0f); + } + } + else if(delta_max > b3Scalar(0.0f)) + { + if((pos <= uppLim) && (pos > (uppLim - delta_max))) + { + lim_fact = (uppLim - pos) / delta_max; + } + else if(pos > uppLim) + { + lim_fact = b3Scalar(0.0f); + } + else + { + lim_fact = b3Scalar(1.0f); + } + } + else + { + 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_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; +} + +void b3AngularLimit::test(const b3Scalar angle) +{ + m_correction = 0.0f; + m_sign = 0.0f; + m_solveLimit = false; + + if (m_halfRange >= 0.0f) + { + b3Scalar deviation = b3NormalizeAngle(angle - m_center); + if (deviation < -m_halfRange) + { + m_solveLimit = true; + m_correction = - (deviation + m_halfRange); + m_sign = +1.0f; + } + else if (deviation > m_halfRange) + { + m_solveLimit = true; + m_correction = m_halfRange - deviation; + m_sign = -1.0f; + } + } +} + + +b3Scalar b3AngularLimit::getError() const +{ + return m_correction * m_sign; +} + +void b3AngularLimit::fit(b3Scalar& angle) const +{ + if (m_halfRange > 0.0f) + { + b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center); + if (!b3Equal(relativeAngle, m_halfRange)) + { + if (relativeAngle > 0.0f) + { + angle = getHigh(); + } + else + { + angle = getLow(); + } + } + } +} + +b3Scalar b3AngularLimit::getLow() const +{ + return b3NormalizeAngle(m_center - m_halfRange); +} + +b3Scalar b3AngularLimit::getHigh() const +{ + return b3NormalizeAngle(m_center + m_halfRange); +} diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h new file mode 100644 index 0000000000..cf9cec0d5e --- /dev/null +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -0,0 +1,483 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2010 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_TYPED_CONSTRAINT_H +#define B3_TYPED_CONSTRAINT_H + + +#include "Bullet3Common/b3Scalar.h" +#include "b3SolverConstraint.h" + +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_HINGE_CONSTRAINT_TYPE, + B3_CONETWIST_CONSTRAINT_TYPE, + B3_D6_CONSTRAINT_TYPE, + B3_SLIDER_CONSTRAINT_TYPE, + B3_CONTACT_CONSTRAINT_TYPE, + B3_D6_SPRING_CONSTRAINT_TYPE, + B3_GEAR_CONSTRAINT_TYPE, + B3_FIXED_CONSTRAINT_TYPE, + B3_MAX_CONSTRAINT_TYPE +}; + + +enum b3ConstraintParams +{ + B3_CONSTRAINT_ERP=1, + B3_CONSTRAINT_STOP_ERP, + B3_CONSTRAINT_CFM, + B3_CONSTRAINT_STOP_CFM +}; + +#if 1 + #define b3AssertConstrParams(_par) b3Assert(_par) +#else + #define b3AssertConstrParams(_par) +#endif + + +B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback +{ + 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 +{ + int m_userConstraintType; + + union + { + int m_userConstraintId; + void* m_userConstraintPtr; + }; + + b3Scalar m_breakingImpulseThreshold; + bool m_isEnabled; + bool m_needsFeedback; + int m_overrideNumSolverIterations; + + + b3TypedConstraint& operator=(b3TypedConstraint& other) + { + b3Assert(0); + (void) other; + return *this; + } + +protected: + 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); + + struct b3ConstraintInfo1 { + int m_numConstraintRows,nub; + }; + + + + struct b3ConstraintInfo2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + 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; + + // elements to jump from one row to the next in J's + int rowskip; + + // 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; + + // lo and hi limits for variables (set to -/+ infinity on entry). + 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; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + b3Scalar m_damping; + }; + + int getOverrideNumSolverIterations() const + { + return m_overrideNumSolverIterations; + } + + ///override the number of constraint solver iterations used to solve this constraint + ///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations + void setOverrideNumSolverIterations(int overideNumIterations) + { + 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) + { + (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; + + ///internal method used by the constraint solver, don't use them directly + 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) + { + m_appliedImpulse = appliedImpulse; + } + ///internal method used by the constraint solver, don't use them directly + b3Scalar internalGetAppliedImpulse() + { + return m_appliedImpulse; + } + + + b3Scalar getBreakingImpulseThreshold() const + { + return m_breakingImpulseThreshold; + } + + void setBreakingImpulseThreshold(b3Scalar threshold) + { + m_breakingImpulseThreshold = threshold; + } + + bool isEnabled() const + { + return m_isEnabled; + } + + void setEnabled(bool 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*/) {}; + + + int getRigidBodyA() const + { + return m_rbA; + } + int getRigidBodyB() const + { + return m_rbB; + } + + + int getRigidBodyA() + { + return m_rbA; + } + int getRigidBodyB() + { + return m_rbB; + } + + int getUserConstraintType() const + { + return m_userConstraintType ; + } + + void setUserConstraintType(int userConstraintType) + { + m_userConstraintType = userConstraintType; + }; + + void setUserConstraintId(int uid) + { + m_userConstraintId = uid; + } + + int getUserConstraintId() const + { + return m_userConstraintId; + } + + void setUserConstraintPtr(void* ptr) + { + m_userConstraintPtr = ptr; + } + + void* getUserConstraintPtr() + { + return m_userConstraintPtr; + } + + void setJointFeedback(b3JointFeedback* jointFeedback) + { + m_jointFeedback = jointFeedback; + } + + const b3JointFeedback* getJointFeedback() const + { + return m_jointFeedback; + } + + b3JointFeedback* getJointFeedback() + { + return m_jointFeedback; + } + + + int getUid() const + { + return m_userConstraintId; + } + + 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) + { + m_needsFeedback = needsFeedback; + } + + ///getAppliedImpulse is an estimated total applied impulse. + ///This feedback could be used to determine breaking constraints or playing sounds. + b3Scalar getAppliedImpulse() const + { + b3Assert(m_needsFeedback); + return m_appliedImpulse; + } + + b3TypedConstraintType getConstraintType () const + { + return b3TypedConstraintType(m_objectType); + } + + void setDbgDrawSize(b3Scalar dbgDrawSize) + { + m_dbgDrawSize = dbgDrawSize; + } + b3Scalar getDbgDrawSize() + { + return m_dbgDrawSize; + } + + ///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; + + ///return the local value of parameter + 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 +// 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) + { + return angleInRadians; + } + 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) + { + b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians)); + b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians)); + return (diffLo < diffHi) ? (angleInRadians - B3_2_PI) : angleInRadians; + } + else + { + return angleInRadians; + } +} + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3TypedConstraintData +{ + int m_bodyA; + int m_bodyB; + char *m_name; + + int m_objectType; + int m_userConstraintType; + int m_userConstraintId; + int m_needsFeedback; + + float m_appliedImpulse; + float m_dbgDrawSize; + + int m_disableCollisionsBetweenLinkedBodies; + int m_overrideNumSolverIterations; + + float m_breakingImpulseThreshold; + int m_isEnabled; + +}; + +/*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const +{ + return sizeof(b3TypedConstraintData); +} +*/ + + +class b3AngularLimit +{ +private: + b3Scalar + m_center, + m_halfRange, + m_softness, + m_biasFactor, + m_relaxationFactor, + m_correction, + m_sign; + + bool + m_solveLimit; + +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) + {} + + /// Sets all limit's parameters. + /// When low > high limit becomes inactive. + /// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit + void set(b3Scalar low, b3Scalar high, b3Scalar _softness = 0.9f, b3Scalar _biasFactor = 0.3f, b3Scalar _relaxationFactor = 1.0f); + + /// Checks conastaint angle against limit. If limit is active and the angle violates the limit + /// correction is calculated. + void test(const b3Scalar angle); + + /// Returns limit's softness + inline b3Scalar getSoftness() const + { + return m_softness; + } + + /// Returns limit's bias factor + inline b3Scalar getBiasFactor() const + { + return m_biasFactor; + } + + /// Returns limit's relaxation factor + inline b3Scalar getRelaxationFactor() const + { + return m_relaxationFactor; + } + + /// Returns correction value evaluated when test() was invoked + inline b3Scalar getCorrection() const + { + return m_correction; + } + + /// Returns sign value evaluated when test() was invoked + inline b3Scalar getSign() const + { + return m_sign; + } + + /// Gives half of the distance between min and max limit angle + inline b3Scalar getHalfRange() const + { + return m_halfRange; + } + + /// Returns true when the last test() invocation recognized limit violation + inline bool isLimit() const + { + return m_solveLimit; + } + + /// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle + /// returned is modified so it equals to the limit closest to given angle. + void fit(b3Scalar& angle) const; + + /// Returns correction value multiplied by sign value + b3Scalar getError() const; + + b3Scalar getLow() const; + + b3Scalar getHigh() const; + +}; + + + +#endif //B3_TYPED_CONSTRAINT_H |