diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp | 29 |
1 files changed, 12 insertions, 17 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp index 4be2aabe4d..6507e1a0a7 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -13,54 +13,49 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btHinge2Constraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - - // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) -: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) + : btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), RO_XYZ), + m_anchor(anchor), + m_axis1(axis1), + m_axis2(axis2) { // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number + // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first btVector3 zAxis = axis1.normalize(); btVector3 xAxis = axis2.normalize(); - btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system + btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system btTransform frameInW; frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); + frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); frameInW.setOrigin(anchor); // now get constraint frame in local coordinate systems m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; // sei limits setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); - setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); + setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); // like front wheels of a car - setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); - setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); + setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); + setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); // enable suspension enableSpring(2, true); setStiffness(2, SIMD_PI * SIMD_PI * 4.f); setDamping(2, 0.01f); setEquilibriumPoint(); } - |