diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp | 43 |
1 files changed, 18 insertions, 25 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp index b009f41aec..42ed1fbb87 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp @@ -13,43 +13,38 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btUniversalConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - - #define UNIV_EPS btScalar(0.01f) - // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2) -: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) + : btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), + 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 = m_axis1.normalize(); btVector3 yAxis = m_axis2.normalize(); - btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + btVector3 xAxis = yAxis.cross(zAxis); // 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; @@ -58,30 +53,28 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, setLinearLowerLimit(btVector3(0., 0., 0.)); setLinearUpperLimit(btVector3(0., 0., 0.)); setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); - setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); + setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); } -void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +void btUniversalConstraint::setAxis(const btVector3& axis1, const btVector3& axis2) { - m_axis1 = axis1; - m_axis2 = axis2; + m_axis1 = axis1; + m_axis2 = axis2; btVector3 zAxis = axis1.normalized(); btVector3 yAxis = axis2.normalized(); - btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + btVector3 xAxis = yAxis.cross(zAxis); // 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(m_anchor); // now get constraint frame in local coordinate systems m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; - calculateTransforms(); + calculateTransforms(); } - - |