diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp | 66 |
1 files changed, 25 insertions, 41 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index 6f765884ec0..8baf52bcd19 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -17,26 +17,23 @@ subject to the following restrictions: #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - -btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) +btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) { - init(); + init(); } - btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) - : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB) + : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB) { - init(); + init(); } - void btGeneric6DofSpringConstraint::init() { m_objectType = D6_SPRING_CONSTRAINT_TYPE; - for(int i = 0; i < 6; i++) + for (int i = 0; i < 6; i++) { m_springEnabled[i] = false; m_equilibriumPoint[i] = btScalar(0.f); @@ -45,12 +42,11 @@ void btGeneric6DofSpringConstraint::init() } } - void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) { btAssert((index >= 0) && (index < 6)); m_springEnabled[index] = onOff; - if(index < 3) + if (index < 3) { m_linearLimits.m_enableMotor[index] = onOff; } @@ -60,44 +56,38 @@ void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) } } - - void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) { btAssert((index >= 0) && (index < 6)); m_springStiffness[index] = stiffness; } - void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) { btAssert((index >= 0) && (index < 6)); m_springDamping[index] = damping; } - void btGeneric6DofSpringConstraint::setEquilibriumPoint() { calculateTransforms(); int i; - for( i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; } - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; } } - - void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) { btAssert((index >= 0) && (index < 6)); calculateTransforms(); - if(index < 3) + if (index < 3) { m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; } @@ -113,15 +103,14 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val) m_equilibriumPoint[index] = val; } - void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info) { // it is assumed that calculateTransforms() have been called before this call int i; //btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { - if(m_springEnabled[i]) + if (m_springEnabled[i]) { // get current position of constraint btScalar currPos = m_calculatedLinearDiff[i]; @@ -130,28 +119,27 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf // spring force is (delta * m_stiffness) according to Hooke's Law btScalar force = delta * m_springStiffness[i]; btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); - m_linearLimits.m_targetVelocity[i] = velFactor * force; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + m_linearLimits.m_targetVelocity[i] = velFactor * force; + m_linearLimits.m_maxMotorForce[i] = btFabs(force); } } - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { - if(m_springEnabled[i + 3]) + if (m_springEnabled[i + 3]) { // get current position of constraint btScalar currPos = m_calculatedAxisAngleDiff[i]; // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i+3]; + btScalar delta = currPos - m_equilibriumPoint[i + 3]; // spring force is (-delta * m_stiffness) according to Hooke's Law - btScalar force = -delta * m_springStiffness[i+3]; - btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); + btScalar force = -delta * m_springStiffness[i + 3]; + btScalar velFactor = info->fps * m_springDamping[i + 3] / btScalar(info->m_numIterations); m_angularLimits[i].m_targetVelocity = velFactor * force; - m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; + m_angularLimits[i].m_maxMotorForce = btFabs(force); } } } - void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) { // this will be called by constraint solver at the constraint setup stage @@ -161,25 +149,21 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) btGeneric6DofConstraint::getInfo2(info); } - -void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2) +void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1, const btVector3& 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]); // now get constraint frame in local coordinate systems m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; - calculateTransforms(); + calculateTransforms(); } - - - |