diff options
Diffstat (limited to 'thirdparty/bullet/src/Bullet3Dynamics')
22 files changed, 0 insertions, 6118 deletions
diff --git a/thirdparty/bullet/src/Bullet3Dynamics/CMakeLists.txt b/thirdparty/bullet/src/Bullet3Dynamics/CMakeLists.txt deleted file mode 100644 index 94c120d9b5..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ - -INCLUDE_DIRECTORIES( - ${BULLET_PHYSICS_SOURCE_DIR}/src -) - -SET(Bullet3Dynamics_SRCS - b3CpuRigidBodyPipeline.cpp - ConstraintSolver/b3FixedConstraint.cpp - ConstraintSolver/b3Generic6DofConstraint.cpp - ConstraintSolver/b3PgsJacobiSolver.cpp - ConstraintSolver/b3Point2PointConstraint.cpp - ConstraintSolver/b3TypedConstraint.cpp -) - -SET(Bullet3Dynamics_HDRS - b3CpuRigidBodyPipeline.h - ConstraintSolver/b3ContactSolverInfo.h - ConstraintSolver/b3FixedConstraint.h - ConstraintSolver/b3Generic6DofConstraint.h - ConstraintSolver/b3JacobianEntry.h - ConstraintSolver/b3PgsJacobiSolver.h - ConstraintSolver/b3Point2PointConstraint.h - ConstraintSolver/b3SolverBody.h - ConstraintSolver/b3SolverConstraint.h - ConstraintSolver/b3TypedConstraint.h - shared/b3ContactConstraint4.h - shared/b3ConvertConstraint4.h - shared/b3Inertia.h - shared/b3IntegrateTransforms.h -) - -ADD_LIBRARY(Bullet3Dynamics ${Bullet3Dynamics_SRCS} ${Bullet3Dynamics_HDRS}) -if (BUILD_SHARED_LIBS) - target_link_libraries(Bullet3Dynamics Bullet3Collision) -endif () -SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES VERSION ${BULLET_VERSION}) -SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES SOVERSION ${BULLET_VERSION}) - -IF (INSTALL_LIBS) - IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) - #FILES_MATCHING requires CMake 2.6 - IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) - IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - INSTALL(TARGETS Bullet3Dynamics DESTINATION .) - ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - INSTALL(TARGETS Bullet3Dynamics - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib${LIB_SUFFIX} - ARCHIVE DESTINATION lib${LIB_SUFFIX}) - INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} -DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN -".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) - ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) - - IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES FRAMEWORK true) - SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES PUBLIC_HEADER "${Bullet3Dynamics_HDRS}") - ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) -ENDIF (INSTALL_LIBS) diff --git a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h deleted file mode 100644 index 7a12257b33..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h +++ /dev/null @@ -1,159 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp deleted file mode 100644 index 5e11e74935..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp +++ /dev/null @@ -1,108 +0,0 @@ - -#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/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h deleted file mode 100644 index e884a82912..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h +++ /dev/null @@ -1,35 +0,0 @@ - -#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/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp deleted file mode 100644 index 168a773d56..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp +++ /dev/null @@ -1,807 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h deleted file mode 100644 index 084d36055c..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ /dev/null @@ -1,550 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h deleted file mode 100644 index a55168eb38..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h +++ /dev/null @@ -1,155 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp deleted file mode 100644 index de729d4556..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ /dev/null @@ -1,1815 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h deleted file mode 100644 index d2ca307fab..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h +++ /dev/null @@ -1,149 +0,0 @@ -#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/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp deleted file mode 100644 index 02c11db320..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h deleted file mode 100644 index 681b487334..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h +++ /dev/null @@ -1,159 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h deleted file mode 100644 index 0049317d98..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h +++ /dev/null @@ -1,302 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h deleted file mode 100644 index bce83d4608..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h +++ /dev/null @@ -1,80 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp deleted file mode 100644 index 699c481d64..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/* -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/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h deleted file mode 100644 index cf9cec0d5e..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h +++ /dev/null @@ -1,483 +0,0 @@ -/* -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 diff --git a/thirdparty/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/thirdparty/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp deleted file mode 100644 index fbc84cc28d..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp +++ /dev/null @@ -1,488 +0,0 @@ -#include "b3CpuRigidBodyPipeline.h" - -#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h" -#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" -#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h" -#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" -#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" -#include "Bullet3Common/b3Vector3.h" -#include "Bullet3Dynamics/shared/b3ContactConstraint4.h" -#include "Bullet3Dynamics/shared/b3Inertia.h" - - -struct b3CpuRigidBodyPipelineInternalData -{ - b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies; - b3AlignedObjectArray<b3Inertia> m_inertias; - b3AlignedObjectArray<b3Aabb> m_aabbWorldSpace; - - b3DynamicBvhBroadphase* m_bp; - b3CpuNarrowPhase* m_np; - b3Config m_config; -}; - - -b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config) -{ - m_data = new b3CpuRigidBodyPipelineInternalData; - m_data->m_np = narrowphase; - m_data->m_bp = broadphaseDbvt; - m_data->m_config = config; -} - -b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline() -{ - delete m_data; -} - -void b3CpuRigidBodyPipeline::updateAabbWorldSpace() -{ - - for (int i=0;i<this->getNumBodies();i++) - { - b3RigidBodyData* body = &m_data->m_rigidBodies[i]; - b3Float4 position = body->m_pos; - b3Quat orientation = body->m_quat; - - int collidableIndex = body->m_collidableIdx; - b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex); - int shapeIndex = collidable.m_shapeIndex; - - if (shapeIndex>=0) - { - - - b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex); - b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i]; - float margin=0.f; - b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec); - m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0); - } - } -} - -void b3CpuRigidBodyPipeline::computeOverlappingPairs() -{ - int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); - m_data->m_bp->calculateOverlappingPairs(); - numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); - printf("numPairs=%d\n",numPairs); -} - -void b3CpuRigidBodyPipeline::computeContactPoints() -{ - - b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray(); - - m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies); - -} -void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime) -{ - - //update world space aabb's - updateAabbWorldSpace(); - - //compute overlapping pairs - computeOverlappingPairs(); - - //compute contacts - computeContactPoints(); - - //solve contacts - - //update transforms - integrate(deltaTime); - - -} - - -static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, - const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1) -{ - return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1); -} - - -static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1, - b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1) -{ - linear = -n; - angular0 = -b3Cross(r0, n); - angular1 = b3Cross(r1, n); -} - - - -static inline void b3SolveContact(b3ContactConstraint4& cs, - const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) -{ - - b3Vector3 dLinVelA; dLinVelA.setZero(); - b3Vector3 dAngVelA; dAngVelA.setZero(); - b3Vector3 dLinVelB; dLinVelB.setZero(); - b3Vector3 dAngVelB; dAngVelB.setZero(); - - for(int ic=0; ic<4; ic++) - { - // dont necessary because this makes change to 0 - if( cs.m_jacCoeffInv[ic] == 0.f ) continue; - - { - b3Vector3 angular0, angular1, linear; - b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; - b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; - b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 ); - - float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic]; - rambdaDt *= cs.m_jacCoeffInv[ic]; - - { - float prevSum = cs.m_appliedRambdaDt[ic]; - float updated = prevSum; - updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[ic] ); - updated = b3Min( updated, maxRambdaDt[ic] ); - rambdaDt = updated - prevSum; - cs.m_appliedRambdaDt[ic] = updated; - } - - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; -#ifdef _WIN32 - b3Assert(_finite(linImp0.getX())); - b3Assert(_finite(linImp1.getX())); -#endif - { - linVelA += linImp0; - angVelA += angImp0; - linVelB += linImp1; - angVelB += angImp1; - } - } - } - - -} - - - - - -static inline void b3SolveFriction(b3ContactConstraint4& cs, - const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) -{ - - if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; - const b3Vector3& center = (const b3Vector3&)cs.m_center; - - b3Vector3 n = -(const b3Vector3&)cs.m_linear; - - b3Vector3 tangent[2]; - - b3PlaneSpace1 (n, tangent[0],tangent[1]); - - b3Vector3 angular0, angular1, linear; - b3Vector3 r0 = center - posA; - b3Vector3 r1 = center - posB; - for(int i=0; i<2; i++) - { - b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 ); - float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ); - rambdaDt *= cs.m_fJacCoeffInv[i]; - - { - float prevSum = cs.m_fAppliedRambdaDt[i]; - float updated = prevSum; - updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[i] ); - updated = b3Min( updated, maxRambdaDt[i] ); - rambdaDt = updated - prevSum; - cs.m_fAppliedRambdaDt[i] = updated; - } - - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; -#ifdef _WIN32 - b3Assert(_finite(linImp0.getX())); - b3Assert(_finite(linImp1.getX())); -#endif - linVelA += linImp0; - angVelA += angImp0; - linVelB += linImp1; - angVelB += angImp1; - } - - { // angular damping for point constraint - b3Vector3 ab = ( posB - posA ).normalized(); - b3Vector3 ac = ( center - posA ).normalized(); - if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) - { - float angNA = b3Dot( n, angVelA ); - float angNB = b3Dot( n, angVelB ); - - angVelA -= (angNA*0.1f)*n; - angVelB -= (angNB*0.1f)*n; - } - } - -} - - - - - -struct b3SolveTask// : public ThreadPool::Task -{ - b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies, - b3AlignedObjectArray<b3Inertia>& shapes, - b3AlignedObjectArray<b3ContactConstraint4>& constraints, - int start, int nConstraints, - int maxNumBatches, - b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx - ) - : m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), - m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx), -m_start( start ), - m_nConstraints( nConstraints ), - m_solveFriction( true ), - m_maxNumBatches(maxNumBatches) - {} - - unsigned short int getType(){ return 0; } - - void run(int tIdx) - { - b3AlignedObjectArray<int> usedBodies; - //printf("run..............\n"); - - - for (int bb=0;bb<m_maxNumBatches;bb++) - { - usedBodies.resize(0); - for(int ic=m_nConstraints-1; ic>=0; ic--) - //for(int ic=0; ic<m_nConstraints; ic++) - { - - int i = m_start + ic; - if (m_constraints[i].m_batchIdx != bb) - continue; - - float frictionCoeff = b3GetFrictionCoeff(&m_constraints[i]); - int aIdx = (int)m_constraints[i].m_bodyA; - int bIdx = (int)m_constraints[i].m_bodyB; - //int localBatch = m_constraints[i].m_batchIdx; - b3RigidBodyData& bodyA = m_bodies[aIdx]; - b3RigidBodyData& bodyB = m_bodies[bIdx]; - -#if 0 - if ((bodyA.m_invMass) && (bodyB.m_invMass)) - { - // printf("aIdx=%d, bIdx=%d\n", aIdx,bIdx); - } - if (bIdx==10) - { - //printf("ic(b)=%d, localBatch=%d\n",ic,localBatch); - } -#endif - if (aIdx==10) - { - //printf("ic(a)=%d, localBatch=%d\n",ic,localBatch); - } - if (usedBodies.size()<(aIdx+1)) - { - usedBodies.resize(aIdx+1,0); - } - - if (usedBodies.size()<(bIdx+1)) - { - usedBodies.resize(bIdx+1,0); - } - - if (bodyA.m_invMass) - { - b3Assert(usedBodies[aIdx]==0); - usedBodies[aIdx]++; - } - - if (bodyB.m_invMass) - { - b3Assert(usedBodies[bIdx]==0); - usedBodies[bIdx]++; - } - - - if( !m_solveFriction ) - { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; - - b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt ); - - } - else - { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; - - float sum = 0; - for(int j=0; j<4; j++) - { - sum +=m_constraints[i].m_appliedRambdaDt[j]; - } - frictionCoeff = 0.7f; - for(int j=0; j<4; j++) - { - maxRambdaDt[j] = frictionCoeff*sum; - minRambdaDt[j] = -maxRambdaDt[j]; - } - - b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt ); - - } - } - - if (m_wgUsedBodies) - { - if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size()) - { - m_wgUsedBodies[m_curWgidx].resize(usedBodies.size()); - } - for (int i=0;i<usedBodies.size();i++) - { - if (usedBodies[i]) - { - //printf("cell %d uses body %d\n", m_curWgidx,i); - m_wgUsedBodies[m_curWgidx][i]=1; - } - } - } - - } - - - - } - - b3AlignedObjectArray<b3RigidBodyData>& m_bodies; - b3AlignedObjectArray<b3Inertia>& m_shapes; - b3AlignedObjectArray<b3ContactConstraint4>& m_constraints; - b3AlignedObjectArray<int>* m_wgUsedBodies; - int m_curWgidx; - int m_start; - int m_nConstraints; - bool m_solveFriction; - int m_maxNumBatches; -}; - -void b3CpuRigidBodyPipeline::solveContactConstraints() -{ - int m_nIterations = 4; - - b3AlignedObjectArray<b3ContactConstraint4> contactConstraints; -// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts(); - int n = contactConstraints.size(); - //convert contacts... - - - - int maxNumBatches = 250; - - for(int iter=0; iter<m_nIterations; iter++) - { - b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0); - task.m_solveFriction = false; - task.run(0); - } - - for(int iter=0; iter<m_nIterations; iter++) - { - b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0); - task.m_solveFriction = true; - task.run(0); - } -} - -void b3CpuRigidBodyPipeline::integrate(float deltaTime) -{ - float angDamping=0.f; - b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0); - - //integrate transforms (external forces/gravity should be moved into constraint solver) - for (int i=0;i<m_data->m_rigidBodies.size();i++) - { - b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration); - } - -} - -int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData) -{ - b3RigidBodyData body; - int bodyIndex = m_data->m_rigidBodies.size(); - body.m_invMass = mass ? 1.f/mass : 0.f; - body.m_angVel.setValue(0,0,0); - body.m_collidableIdx = collidableIndex; - body.m_frictionCoeff = 0.3f; - body.m_linVel.setValue(0,0,0); - body.m_pos.setValue(position[0],position[1],position[2]); - body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); - body.m_restituitionCoeff = 0.f; - - m_data->m_rigidBodies.push_back(body); - - - if (collidableIndex>=0) - { - b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand(); - - b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex); - b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); - b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); - - b3Scalar margin = 0.01f; - b3Transform t; - t.setIdentity(); - t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); - t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); - b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec); - - m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1); -// b3Vector3 aabbMin,aabbMax; - // m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax); - - } else - { - b3Error("registerPhysicsInstance using invalid collidableIndex\n"); - } - - return bodyIndex; -} - - -const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const -{ - return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0; -} - -int b3CpuRigidBodyPipeline::getNumBodies() const -{ - return m_data->m_rigidBodies.size(); -} diff --git a/thirdparty/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h b/thirdparty/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h deleted file mode 100644 index 2f3c2ae77e..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h +++ /dev/null @@ -1,67 +0,0 @@ -/* -Copyright (c) 2013 Advanced Micro Devices, Inc. - -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. -*/ -//Originally written by Erwin Coumans - -#ifndef B3_CPU_RIGIDBODY_PIPELINE_H -#define B3_CPU_RIGIDBODY_PIPELINE_H - - - -#include "Bullet3Common/b3AlignedObjectArray.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h" - -class b3CpuRigidBodyPipeline -{ -protected: - struct b3CpuRigidBodyPipelineInternalData* m_data; - - int allocateCollidable(); - -public: - - - b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config); - virtual ~b3CpuRigidBodyPipeline(); - - virtual void stepSimulation(float deltaTime); - virtual void integrate(float timeStep); - virtual void updateAabbWorldSpace(); - virtual void computeOverlappingPairs(); - virtual void computeContactPoints(); - virtual void solveContactConstraints(); - - int registerConvexPolyhedron(class b3ConvexUtility* convex); - - int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData); - void writeAllInstancesToGpu(); - void copyConstraintsToHost(); - void setGravity(const float* grav); - void reset(); - - int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold); - int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold); - void removeConstraintByUid(int uid); - - void addConstraint(class b3TypedConstraint* constraint); - void removeConstraint(b3TypedConstraint* constraint); - - void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults); - - const struct b3RigidBodyData* getBodyBuffer() const; - - int getNumBodies() const; - -}; - -#endif //B3_CPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3Dynamics/premake4.lua b/thirdparty/bullet/src/Bullet3Dynamics/premake4.lua deleted file mode 100644 index 669336a6a1..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/premake4.lua +++ /dev/null @@ -1,15 +0,0 @@ - project "Bullet3Dynamics" - - language "C++" - - kind "StaticLib" - - includedirs { - ".." - } - - - files { - "**.cpp", - "**.h" - }
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h b/thirdparty/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h deleted file mode 100644 index 68cf65e312..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef B3_CONTACT_CONSTRAINT5_H -#define B3_CONTACT_CONSTRAINT5_H - -#include "Bullet3Common/shared/b3Float4.h" - -typedef struct b3ContactConstraint4 b3ContactConstraint4_t; - - -struct b3ContactConstraint4 -{ - - b3Float4 m_linear;//normal? - b3Float4 m_worldPos[4]; - b3Float4 m_center; // friction - float m_jacCoeffInv[4]; - float m_b[4]; - float m_appliedRambdaDt[4]; - float m_fJacCoeffInv[2]; // friction - float m_fAppliedRambdaDt[2]; // friction - - unsigned int m_bodyA; - unsigned int m_bodyB; - int m_batchIdx; - unsigned int m_paddings; - -}; - -//inline void setFrictionCoeff(float value) { m_linear[3] = value; } -inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) -{ - return constraint->m_linear.w; -} - -#endif //B3_CONTACT_CONSTRAINT5_H diff --git a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h b/thirdparty/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h deleted file mode 100644 index 805a2bd3ea..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h +++ /dev/null @@ -1,153 +0,0 @@ - - -#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" -#include "Bullet3Dynamics/shared/b3ContactConstraint4.h" -#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" - - -void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q); - void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q) -{ - if (b3Fabs(n.z) > 0.70710678f) { - // choose p in y-z plane - float a = n.y*n.y + n.z*n.z; - float k = 1.f/sqrt(a); - p[0].x = 0; - p[0].y = -n.z*k; - p[0].z = n.y*k; - // set q = n x p - q[0].x = a*k; - q[0].y = -n.x*p[0].z; - q[0].z = n.x*p[0].y; - } - else { - // choose p in x-y plane - float a = n.x*n.x + n.y*n.y; - float k = 1.f/sqrt(a); - p[0].x = -n.y*k; - p[0].y = n.x*k; - p[0].z = 0; - // set q = n x p - q[0].x = -n.z*p[0].y; - q[0].y = n.z*p[0].x; - q[0].z = a*k; - } -} - - - -void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1) -{ - *linear = b3MakeFloat4(n.x,n.y,n.z,0.f); - *angular0 = b3Cross3(r0, n); - *angular1 = -b3Cross3(r1, n); -} - - -float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0, - b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 ) -{ - return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1); -} - - -float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1, - float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1) -{ - // linear0,1 are normlized - float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0; - float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0); - float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1; - float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1); - return -1.f/(jmj0+jmj1+jmj2+jmj3); -} - - -void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA, - b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, - __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff, - b3ContactConstraint4_t* dstC ) -{ - dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit); - dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit); - - float dtInv = 1.f/dt; - for(int ic=0; ic<4; ic++) - { - dstC->m_appliedRambdaDt[ic] = 0.f; - } - dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f; - - - dstC->m_linear = src->m_worldNormalOnB; - dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() ); - for(int ic=0; ic<4; ic++) - { - b3Float4 r0 = src->m_worldPosB[ic] - posA; - b3Float4 r1 = src->m_worldPosB[ic] - posB; - - if( ic >= src->m_worldNormalOnB.w )//npoints - { - dstC->m_jacCoeffInv[ic] = 0.f; - continue; - } - - float relVelN; - { - b3Float4 linear, angular0, angular1; - setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1); - - dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, - invMassA, &invInertiaA, invMassB, &invInertiaB ); - - relVelN = calcRelVel(linear, -linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB); - - float e = 0.f;//src->getRestituitionCoeff(); - if( relVelN*relVelN < 0.004f ) e = 0.f; - - dstC->m_b[ic] = e*relVelN; - //float penetration = src->m_worldPosB[ic].w; - dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv; - dstC->m_appliedRambdaDt[ic] = 0.f; - } - } - - if( src->m_worldNormalOnB.w > 0 )//npoints - { // prepare friction - b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f); - for(int i=0; i<src->m_worldNormalOnB.w; i++) - center += src->m_worldPosB[i]; - center /= (float)src->m_worldNormalOnB.w; - - b3Float4 tangent[2]; - b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]); - - b3Float4 r[2]; - r[0] = center - posA; - r[1] = center - posB; - - for(int i=0; i<2; i++) - { - b3Float4 linear, angular0, angular1; - setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1); - - dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1, - invMassA, &invInertiaA, invMassB, &invInertiaB ); - dstC->m_fAppliedRambdaDt[i] = 0.f; - } - dstC->m_center = center; - } - - for(int i=0; i<4; i++) - { - if( i<src->m_worldNormalOnB.w ) - { - dstC->m_worldPos[i] = src->m_worldPosB[i]; - } - else - { - dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f); - } - } -} diff --git a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3Inertia.h b/thirdparty/bullet/src/Bullet3Dynamics/shared/b3Inertia.h deleted file mode 100644 index 96fe9f8b39..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3Inertia.h +++ /dev/null @@ -1,15 +0,0 @@ - - -#ifndef B3_INERTIA_H -#define B3_INERTIA_H - -#include "Bullet3Common/shared/b3Mat3x3.h" - -struct b3Inertia -{ - b3Mat3x3 m_invInertiaWorld; - b3Mat3x3 m_initInvInertia; -}; - - -#endif //B3_INERTIA_H
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/thirdparty/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h deleted file mode 100644 index e96f90d3f3..0000000000 --- a/thirdparty/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h +++ /dev/null @@ -1,113 +0,0 @@ - - -#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" - - - -inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) -{ - - if (bodies[nodeID].m_invMass != 0.f) - { - float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); - - //angular velocity - { - b3Float4 axis; - //add some hardcoded angular damping - bodies[nodeID].m_angVel.x *= angularDamping; - bodies[nodeID].m_angVel.y *= angularDamping; - bodies[nodeID].m_angVel.z *= angularDamping; - - b3Float4 angvel = bodies[nodeID].m_angVel; - - float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); - - //limit the angular motion - if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) - { - fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; - } - if(fAngle < 0.001f) - { - // use Taylor's expansions of sync function - axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); - } - - b3Quat dorn; - dorn.x = axis.x; - dorn.y = axis.y; - dorn.z = axis.z; - dorn.w = b3Cos(fAngle * timeStep * 0.5f); - b3Quat orn0 = bodies[nodeID].m_quat; - b3Quat predictedOrn = b3QuatMul(dorn, orn0); - predictedOrn = b3QuatNormalized(predictedOrn); - bodies[nodeID].m_quat=predictedOrn; - } - //linear velocity - bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep; - - //apply gravity - bodies[nodeID].m_linVel += gravityAcceleration * timeStep; - - } - -} - -inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) -{ - float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); - - if( (body->m_invMass != 0.f)) - { - //angular velocity - { - b3Float4 axis; - //add some hardcoded angular damping - body->m_angVel.x *= angularDamping; - body->m_angVel.y *= angularDamping; - body->m_angVel.z *= angularDamping; - - b3Float4 angvel = body->m_angVel; - float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); - //limit the angular motion - if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) - { - fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; - } - if(fAngle < 0.001f) - { - // use Taylor's expansions of sync function - axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); - } - b3Quat dorn; - dorn.x = axis.x; - dorn.y = axis.y; - dorn.z = axis.z; - dorn.w = b3Cos(fAngle * timeStep * 0.5f); - b3Quat orn0 = body->m_quat; - - b3Quat predictedOrn = b3QuatMul(dorn, orn0); - predictedOrn = b3QuatNormalized(predictedOrn); - body->m_quat=predictedOrn; - } - - //apply gravity - body->m_linVel += gravityAcceleration * timeStep; - - //linear velocity - body->m_pos += body->m_linVel * timeStep; - - } - -} |