author Erwin Coumans Sun, 5 Aug 2007 17:44:32 +0000 (17:44 +0000) committer Erwin Coumans Sun, 5 Aug 2007 17:44:32 +0000 (17:44 +0000)

index ad7fc3a269e875fa0f938663dfc0ab1a6eb9493b..27e3098754958e2ce7c723bc73e5fe39d609a095 100644 (file)
@@ -17,176 +17,58 @@ subject to the following restrictions:
#include "btHingeConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
-#include "LinearMath/btSimdMinMax.h"
#include <new>

-
btHingeConstraint::btHingeConstraint():
m_enableAngularMotor(false)
{
}

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-                                                                        btVector3& axisInA,btVector3& axisInB)
-                                                                        :btTypedConstraint(rbA,rbB),
-                                                                        m_angularOnly(false),
-                                                                        m_enableAngularMotor(false)
-{
-       m_rbAFrame.getOrigin() = pivotInA;
-
-       // since no frame is given, assume this to be zero angle and just pick rb transform axis
-       btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
-       btScalar projection = rbAxisA1.dot(axisInA);
-       if (projection > SIMD_EPSILON)
-               rbAxisA1 = rbAxisA1*projection - axisInA;
-        else
-               rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-
-       btVector3 rbAxisA2 = rbAxisA1.cross(axisInA);
-
-       m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-                                                                       rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-                                                                       rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
-
-       btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-       btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-       btVector3 rbAxisB2 =  rbAxisB1.cross(axisInB);
-
-
-       m_rbBFrame.getOrigin() = pivotInB;
-       m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
-                                                                       rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
-                                                                       rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
-
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30);
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
-
-}
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false)
-{
-
-       // since no frame is given, assume this to be zero angle and just pick rb transform axis
-       // fixed axis in worldspace
-       btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
-       btScalar projection = rbAxisA1.dot(axisInA);
-       if (projection > SIMD_EPSILON)
-               rbAxisA1 = rbAxisA1*projection - axisInA;
-       else
-               rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-
-       btVector3 rbAxisA2 = axisInA.cross(rbAxisA1);
-
-       m_rbAFrame.getOrigin() = pivotInA;
-       m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-                                                                       rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-                                                                       rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
-
-
-       btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
-
-       btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-       btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-       btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
-
-
-       m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
-       m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-                                                                       rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-                                                                       rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30);
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
-}
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
-                                                                    const btTransform& rbAFrame, const btTransform& rbBFrame)
-:btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+                                                                btVector3& axisInA,btVector3& axisInB)
+:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_axisInA(axisInA),
+m_axisInB(-axisInB),
m_angularOnly(false),
m_enableAngularMotor(false)
{
-       // flip axis
-       m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30);
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
-}

+}

-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
-:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
+:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_axisInA(axisInA),
+//fixed axis in worldspace
+m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
m_angularOnly(false),
m_enableAngularMotor(false)
{
-       // flip axis
-       m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
-
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30);
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
+
}

void   btHingeConstraint::buildJacobian()
{
m_appliedImpulse = btScalar(0.);

+       btVector3       normal(0,0,0);
+
if (!m_angularOnly)
{
-               btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-               btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-               btVector3 relPos = pivotBInW - pivotAInW;
-
-               btVector3 normal[3];
-               if (relPos.length2() > SIMD_EPSILON)
-               {
-                       normal[0] = relPos.normalized();
-               }
-               else
-               {
-                       normal[0].setValue(btScalar(1.0),0,0);
-               }
-
-               btPlaneSpace1(normal[0], normal[1], normal[2]);
-
for (int i=0;i<3;i++)
{
+                       normal[i] = 1;
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                               pivotAInW - m_rbA.getCenterOfMassPosition(),
-                               pivotBInW - m_rbB.getCenterOfMassPosition(),
-                               normal[i],
+                               m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+                               m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+                               normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
+                       normal[i] = 0;
}
}

@@ -197,12 +79,12 @@ void       btHingeConstraint::buildJacobian()
btVector3 jointAxis0local;
btVector3 jointAxis1local;

-       btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+       btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);

-       getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+       getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
-       btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+       btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;

new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
@@ -223,71 +105,44 @@ void      btHingeConstraint::buildJacobian()
m_rbB.getInvInertiaDiagLocal());

-       // Compute limit information
-       btScalar hingeAngle = getHingeAngle();
-
-       //set bias, sign, clear accumulator
-       m_correction = btScalar(0.);
-       m_limitSign = btScalar(0.);
-       m_solveLimit = false;
-       m_accLimitImpulse = btScalar(0.);
-
-       if (m_lowerLimit < m_upperLimit)
-       {
-               if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-               {
-                       m_correction = (m_lowerLimit - hingeAngle);
-                       m_limitSign = 1.0f;
-                       m_solveLimit = true;
-               }
-               else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-               {
-                       m_correction = m_upperLimit - hingeAngle;
-                       m_limitSign = -1.0f;
-                       m_solveLimit = true;
-               }
-       }
-
-       //Compute K = J*W*J' for hinge axis
-       btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-       m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-                                    getRigidBodyB().computeAngularImpulseDenominator(axisA));

}

void   btHingeConstraint::solveConstraint(btScalar     timeStep)
{

-       btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-       btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+       btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+       btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;

+       btVector3 normal(0,0,0);
btScalar tau = btScalar(0.3);
btScalar damping = btScalar(1.);

//linear part
if (!m_angularOnly)
{
-               btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
-               btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-               btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-               btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-               btVector3 vel = vel1 - vel2;
-
for (int i=0;i<3;i++)
{
-                       const btVector3& normal = m_jac[i].m_linearJointAxis;
+                       normal[i] = 1;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

+                       btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+                       btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+                       btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+                       btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+                       btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
//positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-                       btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+                       btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
m_appliedImpulse += impulse;
btVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+
+                       normal[i] = 0;
}
}

@@ -296,8 +151,8 @@ void        btHingeConstraint::solveConstraint(btScalar     timeStep)
///solve angular part

// get axes in world space
-               btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-               btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+               btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+               btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;

const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
@@ -319,7 +174,7 @@ void        btHingeConstraint::solveConstraint(btScalar     timeStep)
getRigidBodyB().computeAngularImpulseDenominator(normal);
// scale for mass and relaxation
//todo:  expose this 0.9 factor to developer
-                               velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+                               velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9);
}

//solve angular positional correction
@@ -335,28 +190,10 @@ void      btHingeConstraint::solveConstraint(btScalar     timeStep)

m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
-
-                       // solve limit
-                       if (m_solveLimit)
-                       {
-                               btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
-
-                               btScalar impulseMag = amplitude * m_kHinge;
-
-                               // Clamp the accumulated impulse
-                               btScalar temp = m_accLimitImpulse;
-                               m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, 0.0f );
-                               impulseMag = m_accLimitImpulse - temp;
-
-
-                               btVector3 impulse = axisA * impulseMag * m_limitSign;
-                               m_rbA.applyTorqueImpulse(impulse);
-                               m_rbB.applyTorqueImpulse(-impulse);
-                       }
}

//apply motor
-               if (m_enableAngularMotor)
+               if (m_enableAngularMotor)
{
//todo: add limits too
btVector3 angularLimit(0,0,0);
@@ -367,7 +204,10 @@ void       btHingeConstraint::solveConstraint(btScalar     timeStep)
btScalar desiredMotorVel = m_motorTargetVelocity;
btScalar motor_relvel = desiredMotorVel - projRelVel;

-                       btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+                       btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+                                       getRigidBodyB().computeAngularImpulseDenominator(axisA);
+
+                       btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;;
//todo: should clip against accumulated impulse
btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
@@ -387,11 +227,3 @@ void       btHingeConstraint::updateRHS(btScalar   timeStep)

}

-btScalar btHingeConstraint::getHingeAngle()
-{
-       const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-       const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-       const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
-
-       return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
-}
index 4b42d3d2acda89ef3a4820f5f3817566fa4109ae..5c1ceafbc5bd92372ea31be2d968e5fd64317aa2 100644 (file)
@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/

-/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
-
#ifndef HINGECONSTRAINT_H
#define HINGECONSTRAINT_H

@@ -24,6 +22,7 @@ subject to the following restrictions:

class btRigidBody;

+
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/// axis defines the orientation of the hinge axis
class btHingeConstraint : public btTypedConstraint
@@ -31,40 +30,22 @@ class btHingeConstraint : public btTypedConstraint
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor

-       btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
-       btTransform m_rbBFrame;
-
-       btScalar        m_motorTargetVelocity;
-       btScalar        m_maxMotorImpulse;
-
-       btScalar        m_limitSoftness;
-       btScalar        m_biasFactor;
-       btScalar    m_relaxationFactor;
-
-       btScalar    m_lowerLimit;
-       btScalar    m_upperLimit;
-
-       btScalar        m_kHinge;
-
-       btScalar        m_limitSign;
-       btScalar        m_correction;
-
-       btScalar        m_accLimitImpulse;
+       btVector3       m_pivotInA;
+       btVector3       m_pivotInB;
+       btVector3       m_axisInA;
+       btVector3       m_axisInB;

bool            m_angularOnly;
-       bool            m_enableAngularMotor;
-       bool            m_solveLimit;

+       btScalar                m_motorTargetVelocity;
+       btScalar                m_maxMotorImpulse;
+       bool            m_enableAngularMotor;

public:

-       btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+       btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB);

btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
-
-       btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
-
-       btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);

btHingeConstraint();

@@ -95,33 +76,6 @@ public:
m_maxMotorImpulse = maxMotorImpulse;
}

-       void    setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
-       {
-               m_lowerLimit = low;
-               m_upperLimit = high;
-
-               m_limitSoftness =  _softness;
-               m_biasFactor = _biasFactor;
-               m_relaxationFactor = _relaxationFactor;
-
-       }
-
-       btScalar getHingeAngle();
-
-
-       const btTransform& getAFrame() { return m_rbAFrame; };
-       const btTransform& getBFrame() { return m_rbBFrame; };
-
-       inline int getSolveLimit()
-       {
-               return m_solveLimit;
-       }
-
-       inline btScalar getLimitSign()
-       {
-               return m_limitSign;
-       }
-
};

#endif //HINGECONSTRAINT_H