From 3d7f1555865a981b7144becfc58d3f3f34362f5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= Date: Wed, 9 Mar 2022 21:15:53 +0100 Subject: Remove unused Bullet module and thirdparty code It has been disabled in `master` since one year (#45852) and our plan is for Bullet, and possibly other thirdparty physics engines, to be implemented via GDExtension so that they can be selected by the users who need them. --- .../ConstraintSolver/b3ContactSolverInfo.h | 149 -- .../ConstraintSolver/b3FixedConstraint.cpp | 103 -- .../ConstraintSolver/b3FixedConstraint.h | 34 - .../ConstraintSolver/b3Generic6DofConstraint.cpp | 737 --------- .../ConstraintSolver/b3Generic6DofConstraint.h | 517 ------ .../ConstraintSolver/b3JacobianEntry.h | 150 -- .../ConstraintSolver/b3PgsJacobiSolver.cpp | 1696 -------------------- .../ConstraintSolver/b3PgsJacobiSolver.h | 133 -- .../ConstraintSolver/b3Point2PointConstraint.cpp | 190 --- .../ConstraintSolver/b3Point2PointConstraint.h | 153 -- .../ConstraintSolver/b3SolverBody.h | 281 ---- .../ConstraintSolver/b3SolverConstraint.h | 73 - .../ConstraintSolver/b3TypedConstraint.cpp | 151 -- .../ConstraintSolver/b3TypedConstraint.h | 469 ------ 14 files changed, 4836 deletions(-) delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp delete mode 100644 thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h (limited to 'thirdparty/bullet/Bullet3Dynamics/ConstraintSolver') diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h deleted file mode 100644 index 049c9116fd..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h +++ /dev/null @@ -1,149 +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/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp deleted file mode 100644 index ace4b18388..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp +++ /dev/null @@ -1,103 +0,0 @@ - -#include "b3FixedConstraint.h" -#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" -#include "Bullet3Common/b3TransformUtil.h" -#include - -b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB) - : b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB) -{ - m_pivotInA = frameInA.getOrigin(); - m_pivotInB = frameInB.getOrigin(); - m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse(); -} - -b3FixedConstraint::~b3FixedConstraint() -{ -} - -void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) -{ - info->m_numConstraintRows = 6; - info->nub = 6; -} - -void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies) -{ - //fix the 3 linear degrees of freedom - - const b3Vector3& worldPosA = bodies[m_rbA].m_pos; - const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat; - const b3Vector3& worldPosB = bodies[m_rbB].m_pos; - const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat; - - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip + 1] = 1; - info->m_J1linearAxis[2 * info->rowskip + 2] = 1; - - b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA); - { - b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip); - b3Vector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); - } - - if (info->m_J2linearAxis) - { - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip + 1] = -1; - info->m_J2linearAxis[2 * info->rowskip + 2] = -1; - } - - b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB); - - { - // b3Vector3 a2n = -a2; - b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip); - a2.getSkewSymmetricMatrix(angular0, angular1, angular2); - } - - // set right hand side for the linear dofs - b3Scalar k = info->fps * info->erp; - b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA); - int j; - for (j = 0; j < 3; j++) - { - info->m_constraintError[j * info->rowskip] = linearError[j]; - //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } - - //fix the 3 angular degrees of freedom - - int start_row = 3; - int s = info->rowskip; - int start_index = start_row * s; - - // 3 rows to make body rotations equal - info->m_J1angularAxis[start_index] = 1; - info->m_J1angularAxis[start_index + s + 1] = 1; - info->m_J1angularAxis[start_index + s * 2 + 2] = 1; - if (info->m_J2angularAxis) - { - info->m_J2angularAxis[start_index] = -1; - info->m_J2angularAxis[start_index + s + 1] = -1; - info->m_J2angularAxis[start_index + s * 2 + 2] = -1; - } - - // set right hand side for the angular dofs - - b3Vector3 diff; - b3Scalar angle; - b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse(); - - b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle); - diff *= -angle; - for (j = 0; j < 3; j++) - { - info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j]; - } -} \ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h deleted file mode 100644 index 64809666e4..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h +++ /dev/null @@ -1,34 +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/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp deleted file mode 100644 index 0d5bb2014b..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp +++ /dev/null @@ -1,737 +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 - -#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->fps; - info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; - } - } - if (limit) - { - b3Scalar k = info->fps * limot->m_stopERP; - if (!rotational) - { - info->m_constraintError[srow] += k * limot->m_currentLimitError; - } - else - { - info->m_constraintError[srow] += -k * limot->m_currentLimitError; - } - info->cfm[srow] = limot->m_stopCFM; - if (limot->m_loLimit == limot->m_hiLimit) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -B3_INFINITY; - info->m_upperLimit[srow] = B3_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = B3_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -B3_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - b3Scalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); - //make sure that if no body -> angVelB == zero vec - // if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); - //make sure that if no body -> angVelB == zero vec - // if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - b3Scalar newc = -limot->m_bounce * vel; - if (newc > info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - b3Scalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else - return 0; -} - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). -///If no axis is provided, it uses the default axis for this constraint. -void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis) -{ - if ((axis >= 0) && (axis < 3)) - { - switch (num) - { - case B3_CONSTRAINT_STOP_ERP: - m_linearLimits.m_stopERP[axis] = value; - m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); - break; - case B3_CONSTRAINT_STOP_CFM: - m_linearLimits.m_stopCFM[axis] = value; - m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); - break; - case B3_CONSTRAINT_CFM: - m_linearLimits.m_normalCFM[axis] = value; - m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); - break; - default: - b3AssertConstrParams(0); - } - } - else if ((axis >= 3) && (axis < 6)) - { - switch (num) - { - case B3_CONSTRAINT_STOP_ERP: - m_angularLimits[axis - 3].m_stopERP = value; - m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); - break; - case B3_CONSTRAINT_STOP_CFM: - m_angularLimits[axis - 3].m_stopCFM = value; - m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); - break; - case B3_CONSTRAINT_CFM: - m_angularLimits[axis - 3].m_normalCFM = value; - m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); - break; - default: - b3AssertConstrParams(0); - } - } - else - { - b3AssertConstrParams(0); - } -} - -///return the local value of parameter -b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const -{ - b3Scalar retVal = 0; - if ((axis >= 0) && (axis < 3)) - { - switch (num) - { - case B3_CONSTRAINT_STOP_ERP: - b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_stopERP[axis]; - break; - case B3_CONSTRAINT_STOP_CFM: - b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_stopCFM[axis]; - break; - case B3_CONSTRAINT_CFM: - b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_normalCFM[axis]; - break; - default: - b3AssertConstrParams(0); - } - } - else if ((axis >= 3) && (axis < 6)) - { - switch (num) - { - case B3_CONSTRAINT_STOP_ERP: - b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_stopERP; - break; - case B3_CONSTRAINT_STOP_CFM: - b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_stopCFM; - break; - case B3_CONSTRAINT_CFM: - b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_normalCFM; - break; - default: - b3AssertConstrParams(0); - } - } - else - { - b3AssertConstrParams(0); - } - return retVal; -} - -void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies) -{ - b3Vector3 zAxis = axis1.normalized(); - b3Vector3 yAxis = axis2.normalized(); - b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system - - b3Transform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - - // now get constraint frame in local coordinate systems - m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW; - m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW; - - calculateTransforms(bodies); -} diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h deleted file mode 100644 index 1597809db3..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ /dev/null @@ -1,517 +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 = 6.0f; - m_maxLimitForce = 300.0f; - m_loLimit = 1.0f; - m_hiLimit = -1.0f; - m_normalCFM = 0.f; - m_stopERP = 0.2f; - m_stopCFM = 0.f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - } - - b3RotationalLimitMotor(const b3RotationalLimitMotor& limot) - { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_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
-
    -
  • 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.
    • - -
  • 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.
    • - -
  • Angulars limits have these possible ranges: - - - - - - - - - - - - - - - - - - -
    AXISMIN ANGLEMAX ANGLE
    X-PIPI
    Y-PI/2PI/2
    Z-PIPI
    -
    • -
- -*/ -B3_ATTRIBUTE_ALIGNED16(class) -b3Generic6DofConstraint : public b3TypedConstraint -{ -protected: - //! relative_frames - //!@{ - b3Transform m_frameInA; //!< the constraint space w.r.t body A - b3Transform m_frameInB; //!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - // b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints - // b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - b3TranslationalLimitMotor m_linearLimits; - //!@} - - //! hinge_parameters - //!@{ - b3RotationalLimitMotor m_angularLimits[3]; - //!@} - -protected: - //! temporal variables - //!@{ - b3Transform m_calculatedTransformA; - b3Transform m_calculatedTransformB; - b3Vector3 m_calculatedAxisAngleDiff; - b3Vector3 m_calculatedAxis[3]; - b3Vector3 m_calculatedLinearDiff; - b3Scalar m_timeStep; - b3Scalar m_factA; - b3Scalar m_factB; - bool m_hasStaticBody; - - b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; - - //!@} - - b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) - { - b3Assert(0); - (void)other; - return *this; - } - - int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB); - - int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB); - - // tests linear limits - void calculateLinearInfo(); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - -public: - B3_DECLARE_ALIGNED_ALLOCATOR(); - - b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies); - - //! Calcs global transform of the offsets - /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. - \sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo - */ - void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies); - - void calculateTransforms(const b3RigidBodyData* bodies); - - //! Gets the global transform of the offset for body A - /*! - \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. - */ - const b3Transform& getCalculatedTransformA() const - { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. - */ - const b3Transform& getCalculatedTransformB() const - { - return m_calculatedTransformB; - } - - const b3Transform& getFrameOffsetA() const - { - return m_frameInA; - } - - const b3Transform& getFrameOffsetB() const - { - return m_frameInB; - } - - b3Transform& getFrameOffsetA() - { - return m_frameInA; - } - - b3Transform& getFrameOffsetB() - { - return m_frameInB; - } - - virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - - void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - - virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies); - - void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies); - - void updateRHS(b3Scalar timeStep); - - //! Get the rotation axis in global coordinates - b3Vector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. - */ - b3Scalar getAngle(int axis_index) const; - - //! Get the relative position of the constraint pivot - /*! - \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. - */ - b3Scalar getRelativePivotPosition(int axis_index) const; - - void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies); - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const b3Vector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } - - void getLinearLowerLimit(b3Vector3 & linearLower) - { - linearLower = m_linearLimits.m_lowerLimit; - } - - void setLinearUpperLimit(const b3Vector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } - - void getLinearUpperLimit(b3Vector3 & linearUpper) - { - linearUpper = m_linearLimits.m_upperLimit; - } - - void setAngularLowerLimit(const b3Vector3& angularLower) - { - for (int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]); - } - - void getAngularLowerLimit(b3Vector3 & angularLower) - { - for (int i = 0; i < 3; i++) - angularLower[i] = m_angularLimits[i].m_loLimit; - } - - void setAngularUpperLimit(const b3Vector3& angularUpper) - { - for (int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]); - } - - void getAngularUpperLimit(b3Vector3 & angularUpper) - { - for (int i = 0; i < 3; i++) - angularUpper[i] = m_angularLimits[i].m_hiLimit; - } - - //! Retrieves the angular limit informacion - b3RotationalLimitMotor* getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - b3TranslationalLimitMotor* getTranslationalLimitMotor() - { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, b3Scalar lo, b3Scalar hi) - { - if (axis < 3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { - lo = b3NormalizeAngle(lo); - hi = b3NormalizeAngle(hi); - m_angularLimits[axis - 3].m_loLimit = lo; - m_angularLimits[axis - 3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) - { - if (limitIndex < 3) - { - return m_linearLimits.isLimited(limitIndex); - } - return m_angularLimits[limitIndex - 3].isLimited(); - } - - virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable - - int get_limit_motor_info2(b3RotationalLimitMotor * limot, - const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, - b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false); - - // access for UseFrameOffset - bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } - void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, b3Scalar value, int axis = -1); - ///return the local value of parameter - virtual b3Scalar getParam(int num, int axis = -1) const; - - void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies); -}; - -#endif //B3_GENERIC_6DOF_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h deleted file mode 100644 index 13269debf6..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h +++ /dev/null @@ -1,150 +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/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp deleted file mode 100644 index b7050b1070..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ /dev/null @@ -1,1696 +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 -#include "Bullet3Common/b3StackAlloc.h" - -//#include "b3SolverBody.h" -//#include "b3SolverConstraint.h" -#include "Bullet3Common/b3AlignedObjectArray.h" -#include //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 -#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(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 - -void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal, - b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, - b3Vector3& rel_pos1, b3Vector3& rel_pos2) -{ - const b3Vector3& pos1 = cp.getPositionWorldOnA(); - const b3Vector3& pos2 = cp.getPositionWorldOnB(); - - b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex]; - b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex]; - - // b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - // b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - b3Vector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? getInvInertiaTensorWorld(&inertias[bodyA->m_originalBodyIndex]) * torqueAxis0 : b3MakeVector3(0, 0, 0); - b3Vector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? getInvInertiaTensorWorld(&inertias[bodyB->m_originalBodyIndex]) * -torqueAxis1 : b3MakeVector3(0, 0, 0); - - b3Scalar scaledDenom; - { -#ifdef COMPUTE_IMPULSE_DENOM - b3Scalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB); - b3Scalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB); -#else - b3Vector3 vec; - b3Scalar denom0 = 0.f; - b3Scalar denom1 = 0.f; - if (rb0) - { - vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - b3Scalar denom; - if (m_usePgs) - { - scaledDenom = denom = relaxation / (denom0 + denom1); - } - else - { - denom = relaxation / (denom0 + denom1); - - b3Scalar countA = rb0->m_invMass ? b3Scalar(m_bodyCount[bodyA->m_originalBodyIndex]) : 1.f; - b3Scalar countB = rb1->m_invMass ? b3Scalar(m_bodyCount[bodyB->m_originalBodyIndex]) : 1.f; - scaledDenom = relaxation / (denom0 * countA + denom1 * countB); - } - solverConstraint.m_jacDiagABInv = denom; - } - - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - - b3Scalar restitution = 0.f; - b3Scalar penetration = cp.getDistance() + infoGlobal.m_linearSlop; - - { - b3Vector3 vel1, vel2; - - vel1 = rb0 ? getVelocityInLocalPoint(rb0, rel_pos1) : b3MakeVector3(0, 0, 0); - vel2 = rb1 ? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0, 0, 0); - - // b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - solverConstraint.m_friction = cp.m_combinedFriction; - - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= b3Scalar(0.)) - { - restitution = 0.f; - }; - } - - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); - if (rb1) - bodyB->internalApplyImpulse(solverConstraint.m_contactNormal * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(b3Scalar)solverConstraint.m_appliedImpulse); - } - else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0 ? bodyA->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? bodyA->m_angularVelocity : b3MakeVector3(0, 0, 0)); - b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1 ? bodyB->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? bodyB->m_angularVelocity : b3MakeVector3(0, 0, 0)); - b3Scalar rel_vel = vel1Dotn + vel2Dotn; - - b3Scalar positionalError = 0.f; - b3Scalar velocityError = restitution - rel_vel; // * damping; - - b3Scalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - if (penetration > 0) - { - positionalError = 0; - - velocityError -= penetration / infoGlobal.m_timeStep; - } - else - { - positionalError = -penetration * erp / infoGlobal.m_timeStep; - } - - b3Scalar penetrationImpulse = positionalError * scaledDenom; //solverConstraint.m_jacDiagABInv; - b3Scalar velocityImpulse = velocityError * scaledDenom; //solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - } - else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } -} - -void b3PgsJacobiSolver::setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal) -{ - b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - { - b3SolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (bodies[bodyA->m_originalBodyIndex].m_invMass) - bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); - if (bodies[bodyB->m_originalBodyIndex].m_invMass) - bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint1.m_angularComponentB, -(b3Scalar)frictionConstraint1.m_appliedImpulse); - } - else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; - if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (bodies[bodyA->m_originalBodyIndex].m_invMass) - bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse); - if (bodies[bodyB->m_originalBodyIndex].m_invMass) - bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint2.m_angularComponentB, -(b3Scalar)frictionConstraint2.m_appliedImpulse); - } - else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } - } -} - -void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal) -{ - b3RigidBodyData *colObj0 = 0, *colObj1 = 0; - - int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(), bodies, inertias); - int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(), bodies, inertias); - - // b3RigidBody* bodyA = b3RigidBody::upcast(colObj0); - // b3RigidBody* bodyB = b3RigidBody::upcast(colObj1); - - b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - ///avoid collision response between two static objects - if (solverBodyA->m_invMass.isZero() && solverBodyB->m_invMass.isZero()) - return; - - int rollingFriction = 1; - int numContacts = getNumContacts(manifold); - for (int j = 0; j < numContacts; j++) - { - b3ContactPoint cp; - getContactPoint(manifold, j, cp); - - if (cp.getDistance() <= getContactProcessingThreshold(manifold)) - { - b3Vector3 rel_pos1; - b3Vector3 rel_pos2; - b3Scalar relaxation; - b3Scalar rel_vel; - b3Vector3 vel; - - int frictionIndex = m_tmpSolverContactConstraintPool.size(); - b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); - // b3RigidBody* rb0 = b3RigidBody::upcast(colObj0); - // b3RigidBody* rb1 = b3RigidBody::upcast(colObj1); - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - - solverConstraint.m_originalContactPoint = &cp; - - setupContactConstraint(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); - - // const b3Vector3& pos1 = cp.getPositionWorldOnA(); - // const b3Vector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints - - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - - b3Vector3 angVelA, angVelB; - solverBodyA->getAngularVelocity(angVelA); - solverBodyB->getAngularVelocity(angVelB); - b3Vector3 relAngVel = angVelB - angVelA; - - if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0)) - { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length() > infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - if (relAngVel.length() > 0.001) - addRollingFrictionConstraint(bodies, inertias, relAngVel, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - } - else - { - addRollingFrictionConstraint(bodies, inertias, cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - b3Vector3 axis0, axis1; - b3PlaneSpace1(cp.m_normalWorldOnB, axis0, axis1); - if (axis0.length() > 0.001) - addRollingFrictionConstraint(bodies, inertias, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - if (axis1.length() > 0.001) - addRollingFrictionConstraint(bodies, inertias, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - } - } - - ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame - ///based on the relative linear velocity. - ///If the relative velocity it zero, it will automatically compute a friction direction. - - ///You can also enable two friction directions, using the B3_SOLVER_USE_2_FRICTION_DIRECTIONS. - ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. - /// - ///If you choose B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. - /// - ///The user can manually override the friction directions for certain contacts using a contact callback, - ///and set the cp.m_lateralFrictionInitialized to true - ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) - ///this will give a conveyor belt effect - /// - if (!(infoGlobal.m_solverMode & B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > B3_EPSILON) - { - cp.m_lateralFrictionDir1 *= 1.f / b3Sqrt(lat_rel_vel); - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize(); //?? - addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - } - - addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - } - else - { - b3PlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2); - - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - } - - addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - { - cp.m_lateralFrictionInitialized = true; - } - } - } - else - { - addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1); - - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); - - setFrictionConstraintImpulse(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - } - } - } -} - -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) -{ - B3_PROFILE("solveGroupCacheFriendlySetup"); - - m_maxOverrideNumSolverIterations = 0; - - m_tmpSolverBodyPool.resize(0); - - m_bodyCount.resize(0); - m_bodyCount.resize(numBodies, 0); - m_bodyCountCheck.resize(0); - m_bodyCountCheck.resize(numBodies, 0); - - m_deltaLinearVelocities.resize(0); - m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); - m_deltaAngularVelocities.resize(0); - m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); - - //int totalBodies = 0; - - for (int i = 0; i < numConstraints; i++) - { - int bodyIndexA = constraints[i]->getRigidBodyA(); - int bodyIndexB = constraints[i]->getRigidBodyB(); - if (m_usePgs) - { - m_bodyCount[bodyIndexA] = -1; - m_bodyCount[bodyIndexB] = -1; - } - else - { - //didn't implement joints with Jacobi version yet - b3Assert(0); - } - } - for (int i = 0; i < numManifolds; i++) - { - int bodyIndexA = manifoldPtr[i].getBodyA(); - int bodyIndexB = manifoldPtr[i].getBodyB(); - if (m_usePgs) - { - m_bodyCount[bodyIndexA] = -1; - m_bodyCount[bodyIndexB] = -1; - } - else - { - if (bodies[bodyIndexA].m_invMass) - { - //m_bodyCount[bodyIndexA]+=manifoldPtr[i].getNPoints(); - m_bodyCount[bodyIndexA]++; - } - else - m_bodyCount[bodyIndexA] = -1; - - if (bodies[bodyIndexB].m_invMass) - // m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints(); - m_bodyCount[bodyIndexB]++; - else - m_bodyCount[bodyIndexB] = -1; - } - } - - if (1) - { - int j; - for (j = 0; j < numConstraints; j++) - { - b3TypedConstraint* constraint = constraints[j]; - - constraint->internalSetAppliedImpulse(0.0f); - } - } - - //b3RigidBody* rb0=0,*rb1=0; - //if (1) - { - { - int totalNumRows = 0; - int i; - - m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); - //calculate the total number of contraint rows - for (i = 0; i < numConstraints; i++) - { - b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - b3JointFeedback* fb = constraints[i]->getJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA.setZero(); - fb->m_appliedTorqueBodyA.setZero(); - fb->m_appliedForceBodyB.setZero(); - fb->m_appliedTorqueBodyB.setZero(); - } - - if (constraints[i]->isEnabled()) - { - } - if (constraints[i]->isEnabled()) - { - constraints[i]->getInfo1(&info1, bodies); - } - else - { - info1.m_numConstraintRows = 0; - info1.nub = 0; - } - totalNumRows += info1.m_numConstraintRows; - } - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); - -#ifndef DISABLE_JOINTS - ///setup the b3SolverConstraints - int currentRow = 0; - - for (i = 0; i < numConstraints; i++) - { - const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - - if (info1.m_numConstraintRows) - { - b3Assert(currentRow < totalNumRows); - - b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; - b3TypedConstraint* constraint = constraints[i]; - - b3RigidBodyData& rbA = bodies[constraint->getRigidBodyA()]; - //b3RigidBody& rbA = constraint->getRigidBodyA(); - // b3RigidBody& rbB = constraint->getRigidBodyB(); - b3RigidBodyData& rbB = bodies[constraint->getRigidBodyB()]; - - int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(), bodies, inertias); - int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(), bodies, inertias); - - b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; - - int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) - m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - - int j; - for (j = 0; j < info1.m_numConstraintRows; j++) - { - memset(¤tConstraintRow[j], 0, sizeof(b3SolverConstraint)); - currentConstraintRow[j].m_lowerLimit = -B3_INFINITY; - currentConstraintRow[j].m_upperLimit = B3_INFINITY; - currentConstraintRow[j].m_appliedImpulse = 0.f; - currentConstraintRow[j].m_appliedPushImpulse = 0.f; - currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; - currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; - currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; - } - - bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); - bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); - bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); - bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); - bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); - bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); - bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); - bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); - - b3TypedConstraint::b3ConstraintInfo2 info2; - info2.fps = 1.f / infoGlobal.m_timeStep; - info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; - info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = 0; - info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(b3SolverConstraint) / sizeof(b3Scalar); //check this - ///the size of b3SolverConstraint needs be a multiple of b3Scalar - b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3SolverConstraint)); - info2.m_constraintError = ¤tConstraintRow->m_rhs; - currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; - info2.m_damping = infoGlobal.m_damping; - info2.cfm = ¤tConstraintRow->m_cfm; - info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; - info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; - info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2, bodies); - - ///finalize the constraint setup - for (j = 0; j < info1.m_numConstraintRows; j++) - { - b3SolverConstraint& solverConstraint = currentConstraintRow[j]; - - if (solverConstraint.m_upperLimit >= constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); - } - - if (solverConstraint.m_lowerLimit <= -constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); - } - - solverConstraint.m_originalContactPoint = constraint; - - b3Matrix3x3& invInertiaWorldA = inertias[constraint->getRigidBodyA()].m_invInertiaWorld; - { - //b3Vector3 angularFactorA(1,1,1); - const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1; //*angularFactorA; - } - - b3Matrix3x3& invInertiaWorldB = inertias[constraint->getRigidBodyB()].m_invInertiaWorld; - { - const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2; //*constraint->getRigidBodyB().getAngularFactor(); - } - - { - //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal - //because it gets multiplied iMJlB - b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass; - b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal; - b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass; //sign of normal? - b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal; - - b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); - sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - sum += iMJlB.dot(solverConstraint.m_contactNormal); - sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - b3Scalar fsum = b3Fabs(sum); - b3Assert(fsum > B3_EPSILON); - solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f; - } - - ///fix rhs - ///todo: add force/torque accelerators - { - b3Scalar rel_vel; - b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel); - b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel); - - rel_vel = vel1Dotn + vel2Dotn; - - b3Scalar restitution = 0.f; - b3Scalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2 - b3Scalar velocityError = restitution - rel_vel * info2.m_damping; - b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; - b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; - solverConstraint.m_appliedImpulse = 0.f; - } - } - } - currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows; - } -#endif //DISABLE_JOINTS - } - - { - int i; - - for (i = 0; i < numManifolds; i++) - { - b3Contact4& manifold = manifoldPtr[i]; - convertContact(bodies, inertias, &manifold, infoGlobal); - } - } - } - - // b3ContactSolverInfo info = infoGlobal; - - int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); - else - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); - - m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); - { - int i; - for (i = 0; i < numNonContactPool; i++) - { - m_orderNonContactConstraintPool[i] = i; - } - for (i = 0; i < numConstraintPool; i++) - { - m_orderTmpConstraintPool[i] = i; - } - for (i = 0; i < numFrictionPool; i++) - { - m_orderFrictionConstraintPool[i] = i; - } - } - - return 0.f; -} - -b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) -{ - int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - if (infoGlobal.m_solverMode & B3_SOLVER_RANDMIZE_ORDER) - { - if (1) // uncomment this for a bit less random ((iteration & 7) == 0) - { - for (int j = 0; j < numNonContactPool; ++j) - { - int tmp = m_orderNonContactConstraintPool[j]; - int swapi = b3RandInt2(j + 1); - m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; - m_orderNonContactConstraintPool[swapi] = tmp; - } - - //contact/friction constraints are not solved more than - if (iteration < infoGlobal.m_numIterations) - { - for (int j = 0; j < numConstraintPool; ++j) - { - int tmp = m_orderTmpConstraintPool[j]; - int swapi = b3RandInt2(j + 1); - m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; - m_orderTmpConstraintPool[swapi] = tmp; - } - - for (int j = 0; j < numFrictionPool; ++j) - { - int tmp = m_orderFrictionConstraintPool[j]; - int swapi = b3RandInt2(j + 1); - m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; - m_orderFrictionConstraintPool[swapi] = tmp; - } - } - } - } - - if (infoGlobal.m_solverMode & B3_SOLVER_SIMD) - { - ///solve all joint constraints, using SIMD, if available - for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) - { - b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; - if (iteration < constraint.m_overrideNumSolverIterations) - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); - } - - if (iteration < infoGlobal.m_numIterations) - { - ///solve all contact constraints using SIMD, if available - if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; - - for (int c = 0; c < numPoolConstraints; c++) - { - b3Scalar totalImpulse = 0; - - { - const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; - resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - totalImpulse = solveManifold.m_appliedImpulse; - } - bool applyFriction = true; - if (applyFriction) - { - { - b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; - - if (totalImpulse > b3Scalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - } - - if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) - { - b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]]; - - if (totalImpulse > b3Scalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - } - } - } - } - else //B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS - { - //solve the friction constraints after all contact constraints, don't interleave them - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - - for (j = 0; j < numPoolConstraints; j++) - { - const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - - if (!m_usePgs) - averageVelocities(); - - ///solve all friction constraints, using SIMD, if available - - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j = 0; j < numFrictionPoolConstraints; j++) - { - b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse > b3Scalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - } - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (j = 0; j < numRollingFrictionPoolConstraints; j++) - { - b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; - b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; - if (totalImpulse > b3Scalar(0)) - { - b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; - if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); - } - } - } - } - } - else - { - //non-SIMD version - ///solve all joint constraints - for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) - { - b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; - if (iteration < constraint.m_overrideNumSolverIterations) - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); - } - - if (iteration < infoGlobal.m_numIterations) - { - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j = 0; j < numPoolConstraints; j++) - { - const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (int j = 0; j < numFrictionPoolConstraints; j++) - { - b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse > b3Scalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - } - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j = 0; j < numRollingFrictionPoolConstraints; j++) - { - b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; - b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; - if (totalImpulse > b3Scalar(0)) - { - b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; - if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); - } - } - } - } - return 0.f; -} - -void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) -{ - int iteration; - if (infoGlobal.m_splitImpulse) - { - if (infoGlobal.m_solverMode & B3_SOLVER_SIMD) - { - for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) - { - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j = 0; j < numPoolConstraints; j++) - { - const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - } - } - } - else - { - for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) - { - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j = 0; j < numPoolConstraints; j++) - { - const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - } - } - } - } - } -} - -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) -{ - B3_PROFILE("solveGroupCacheFriendlyIterations"); - - { - ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(constraints, numConstraints, infoGlobal); - - int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - - for (int iteration = 0; iteration < maxIterations; iteration++) - //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) - { - solveSingleIteration(iteration, constraints, numConstraints, infoGlobal); - - if (!m_usePgs) - { - averageVelocities(); - } - } - } - return 0.f; -} - -void b3PgsJacobiSolver::averageVelocities() -{ - B3_PROFILE("averaging"); - //average the velocities - int numBodies = m_bodyCount.size(); - - m_deltaLinearVelocities.resize(0); - m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); - m_deltaAngularVelocities.resize(0); - m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0)); - - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - if (!m_tmpSolverBodyPool[i].m_invMass.isZero()) - { - int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; - m_deltaLinearVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaLinearVelocity(); - m_deltaAngularVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaAngularVelocity(); - } - } - - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; - - if (!m_tmpSolverBodyPool[i].m_invMass.isZero()) - { - b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]); - - b3Scalar factor = 1.f / b3Scalar(m_bodyCount[orgBodyIndex]); - - m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex] * factor; - m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex] * factor; - } - } -} - -b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal) -{ - B3_PROFILE("solveGroupCacheFriendlyFinish"); - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i, j; - - if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) - { - for (j = 0; j < numPoolConstraints; j++) - { - const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; - b3ContactPoint* pt = (b3ContactPoint*)solveManifold.m_originalContactPoint; - b3Assert(pt); - pt->m_appliedImpulse = solveManifold.m_appliedImpulse; - // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - // printf("pt->m_appliedImpulseLateral1 = %f\n", f); - pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); - if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; - } - //do a callback here? - } - } - - numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); - for (j = 0; j < numPoolConstraints; j++) - { - const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; - b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint; - b3JointFeedback* fb = constr->getJointFeedback(); - if (fb) - { - b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdA]; - b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdB]; - - fb->m_appliedForceBodyA += solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyA->m_linearFactor / infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += -solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyB->m_linearFactor / infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * bodyA->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal * bodyB->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; - } - - constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); - if (b3Fabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold()) - { - constr->setEnabled(false); - } - } - - { - B3_PROFILE("write back velocities and transforms"); - for (i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; - //b3Assert(i==bodyIndex); - - b3RigidBodyData* body = &bodies[bodyIndex]; - if (body->m_invMass) - { - if (infoGlobal.m_splitImpulse) - m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); - else - m_tmpSolverBodyPool[i].writebackVelocity(); - - if (m_usePgs) - { - body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; - body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; - } - else - { - b3Scalar factor = 1.f / b3Scalar(m_bodyCount[bodyIndex]); - - b3Vector3 deltaLinVel = m_deltaLinearVelocities[bodyIndex] * factor; - b3Vector3 deltaAngVel = m_deltaAngularVelocities[bodyIndex] * factor; - //printf("body %d\n",bodyIndex); - //printf("deltaLinVel = %f,%f,%f\n",deltaLinVel.getX(),deltaLinVel.getY(),deltaLinVel.getZ()); - //printf("deltaAngVel = %f,%f,%f\n",deltaAngVel.getX(),deltaAngVel.getY(),deltaAngVel.getZ()); - - body->m_linVel += deltaLinVel; - body->m_angVel += deltaAngVel; - } - - if (infoGlobal.m_splitImpulse) - { - body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); - b3Quaternion orn; - orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation(); - body->m_quat = orn; - } - } - } - } - - m_tmpSolverContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); - - m_tmpSolverBodyPool.resizeNoInitialize(0); - return 0.f; -} - -void b3PgsJacobiSolver::reset() -{ - m_btSeed2 = 0; -} \ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h deleted file mode 100644 index 5b616541d9..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h +++ /dev/null @@ -1,133 +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 m_tmpSolverBodyPool; - b3ConstraintArray m_tmpSolverContactConstraintPool; - b3ConstraintArray m_tmpSolverNonContactConstraintPool; - b3ConstraintArray m_tmpSolverContactFrictionConstraintPool; - b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; - - b3AlignedObjectArray m_orderTmpConstraintPool; - b3AlignedObjectArray m_orderNonContactConstraintPool; - b3AlignedObjectArray m_orderFrictionConstraintPool; - b3AlignedObjectArray m_tmpConstraintSizesPool; - - b3AlignedObjectArray m_bodyCount; - b3AlignedObjectArray m_bodyCountCheck; - - b3AlignedObjectArray m_deltaLinearVelocities; - b3AlignedObjectArray m_deltaAngularVelocities; - - bool m_usePgs; - void averageVelocities(); - - int m_maxOverrideNumSolverIterations; - - int m_numSplitImpulseRecoveries; - - b3Scalar getContactProcessingThreshold(b3Contact4* contact) - { - return 0.02f; - } - void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, - b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, - b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, - b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.); - - void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, - b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, - b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, - b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.); - - b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.); - b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f); - - void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, - b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, - const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, - b3Vector3& rel_pos1, b3Vector3& rel_pos2); - - void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, - b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal); - - ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction - unsigned long m_btSeed2; - - b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution); - - void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal); - - void resolveSplitPenetrationSIMD( - b3SolverBody& bodyA, b3SolverBody& bodyB, - const b3SolverConstraint& contactConstraint); - - void resolveSplitPenetrationImpulseCacheFriendly( - b3SolverBody& bodyA, b3SolverBody& bodyB, - const b3SolverConstraint& contactConstraint); - - //internal method - int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias); - void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject); - - void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); - - void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint); - -protected: - virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); - - virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); - virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); - b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); - - virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal); - -public: - B3_DECLARE_ALIGNED_ALLOCATOR(); - - b3PgsJacobiSolver(bool usePgs); - virtual ~b3PgsJacobiSolver(); - - // void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts); - void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); - - b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); - - ///clear internal cached data and reset random seed - virtual void reset(); - - unsigned long b3Rand2(); - - int b3RandInt2(int n); - - void setRandSeed(unsigned long seed) - { - m_btSeed2 = seed; - } - unsigned long getRandSeed() const - { - return m_btSeed2; - } -}; - -#endif //B3_PGS_JACOBI_SOLVER diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp deleted file mode 100644 index cfa7c7dd11..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp +++ /dev/null @@ -1,190 +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 - -b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB) - : b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0) -{ -} - -/* -b3Point2PointConstraint::b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA) -:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), -m_flags(0), -m_useSolveConstraintObsolete(false) -{ - -} -*/ - -void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) -{ - getInfo1NonVirtual(info, bodies); -} - -void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies) -{ - info->m_numConstraintRows = 3; - info->nub = 3; -} - -void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies) -{ - b3Transform trA; - trA.setIdentity(); - trA.setOrigin(bodies[m_rbA].m_pos); - trA.setRotation(bodies[m_rbA].m_quat); - - b3Transform trB; - trB.setIdentity(); - trB.setOrigin(bodies[m_rbB].m_pos); - trB.setRotation(bodies[m_rbB].m_quat); - - getInfo2NonVirtual(info, trA, trB); -} - -void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) -{ - //retrieve matrices - - // anchor points in global coordinates with respect to body PORs. - - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip + 1] = 1; - info->m_J1linearAxis[2 * info->rowskip + 2] = 1; - - b3Vector3 a1 = body0_trans.getBasis() * getPivotInA(); - //b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA()); - - { - b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip); - b3Vector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); - } - - if (info->m_J2linearAxis) - { - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip + 1] = -1; - info->m_J2linearAxis[2 * info->rowskip + 2] = -1; - } - - b3Vector3 a2 = body1_trans.getBasis() * getPivotInB(); - - { - // b3Vector3 a2n = -a2; - b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip); - a2.getSkewSymmetricMatrix(angular0, angular1, angular2); - } - - // set right hand side - b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; - b3Scalar k = info->fps * currERP; - int j; - for (j = 0; j < 3; j++) - { - info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); - //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } - if (m_flags & B3_P2P_FLAGS_CFM) - { - for (j = 0; j < 3; j++) - { - info->cfm[j * info->rowskip] = m_cfm; - } - } - - b3Scalar impulseClamp = m_setting.m_impulseClamp; // - for (j = 0; j < 3; j++) - { - if (m_setting.m_impulseClamp > 0) - { - info->m_lowerLimit[j * info->rowskip] = -impulseClamp; - info->m_upperLimit[j * info->rowskip] = impulseClamp; - } - } - info->m_damping = m_setting.m_damping; -} - -void b3Point2PointConstraint::updateRHS(b3Scalar timeStep) -{ - (void)timeStep; -} - -///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). -///If no axis is provided, it uses the default axis for this constraint. -void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis) -{ - if (axis != -1) - { - b3AssertConstrParams(0); - } - else - { - switch (num) - { - case B3_CONSTRAINT_ERP: - case B3_CONSTRAINT_STOP_ERP: - m_erp = value; - m_flags |= B3_P2P_FLAGS_ERP; - break; - case B3_CONSTRAINT_CFM: - case B3_CONSTRAINT_STOP_CFM: - m_cfm = value; - m_flags |= B3_P2P_FLAGS_CFM; - break; - default: - b3AssertConstrParams(0); - } - } -} - -///return the local value of parameter -b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const -{ - b3Scalar retVal(B3_INFINITY); - if (axis != -1) - { - b3AssertConstrParams(0); - } - else - { - switch (num) - { - case B3_CONSTRAINT_ERP: - case B3_CONSTRAINT_STOP_ERP: - b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP); - retVal = m_erp; - break; - case B3_CONSTRAINT_CFM: - case B3_CONSTRAINT_STOP_CFM: - b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM); - retVal = m_cfm; - break; - default: - b3AssertConstrParams(0); - } - } - return retVal; -} diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h deleted file mode 100644 index 14762a3e35..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h +++ /dev/null @@ -1,153 +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/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h deleted file mode 100644 index 196d0e5793..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h +++ /dev/null @@ -1,281 +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/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h deleted file mode 100644 index 4927ae4288..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h +++ /dev/null @@ -1,73 +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 b3ConstraintArray; - -#endif //B3_SOLVER_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp deleted file mode 100644 index 885e277d8c..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp +++ /dev/null @@ -1,151 +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/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h deleted file mode 100644 index f74aec4d3c..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h +++ /dev/null @@ -1,469 +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; - } -} - -// clang-format off -///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; - -}; - -// clang-format on - -/*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 -- cgit v1.2.3