Cr8-xyz/blender
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

reverted to old hinge constraint, it breaks several tests/demos

Browse Source
This commit is contained in:
Erwin Coumans
2007-08-05 17:44:32 +00:00
parent d638f54c08
commit f0de3124ce
2 changed files with 52 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files

236
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp vendored
View File

@@ -17,10 +17,8 @@ subject to the following restrictions:
#include "btHingeConstraint.h" #include "btHingeConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h" #include "LinearMath/btTransformUtil.h"
#include "LinearMath/btSimdMinMax.h"
#include <new> #include <new>
btHingeConstraint::btHingeConstraint(): btHingeConstraint::btHingeConstraint():
m_enableAngularMotor(false) m_enableAngularMotor(false)
{ {
@@ -28,165 +26,49 @@ m_enableAngularMotor(false)
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB) btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(rbA,rbB), :btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_axisInA(axisInA),
m_axisInB(-axisInB),
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(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) btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false) :btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
{ m_axisInA(axisInA),
// since no frame is given, assume this to be zero angle and just pick rb transform axis
//fixed axis in worldspace //fixed axis in worldspace
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
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),
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(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),
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() void btHingeConstraint::buildJacobian()
{ {
m_appliedImpulse = btScalar(0.); m_appliedImpulse = btScalar(0.);
btVector3 normal(0,0,0);
if (!m_angularOnly) 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++) for (int i=0;i<3;i++)
{ {
normal[i] = 1;
new (&m_jac[i]) btJacobianEntry( new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(), m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(), m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal[i], normal,
m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(), m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass()); m_rbB.getInvMass());
normal[i] = 0;
} }
} }
@@ -197,12 +79,12 @@ void btHingeConstraint::buildJacobian()
btVector3 jointAxis0local; btVector3 jointAxis0local;
btVector3 jointAxis1local; 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 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; 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, new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getCenterOfMassTransform().getBasis().transpose(),
@@ -223,71 +105,44 @@ void btHingeConstraint::buildJacobian()
m_rbB.getInvInertiaDiagLocal()); 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) void btHingeConstraint::solveConstraint(btScalar timeStep)
{ {
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
btVector3 normal(0,0,0);
btScalar tau = btScalar(0.3); btScalar tau = btScalar(0.3);
btScalar damping = btScalar(1.); btScalar damping = btScalar(1.);
//linear part //linear part
if (!m_angularOnly) if (!m_angularOnly)
{ {
for (int i=0;i<3;i++)
{
normal[i] = 1;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2; btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++)
{
const btVector3& normal = m_jac[i].m_linearJointAxis;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
btScalar rel_vel; btScalar rel_vel;
rel_vel = normal.dot(vel); rel_vel = normal.dot(vel);
//positional error (zeroth order error) //positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal 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; m_appliedImpulse += impulse;
btVector3 impulse_vector = normal * impulse; btVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.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 ///solve angular part
// get axes in world space // get axes in world space
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
@@ -319,7 +174,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
getRigidBodyB().computeAngularImpulseDenominator(normal); getRigidBodyB().computeAngularImpulseDenominator(normal);
// scale for mass and relaxation // scale for mass and relaxation
//todo: expose this 0.9 factor to developer //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 //solve angular positional correction
@@ -335,24 +190,6 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
m_rbB.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 //apply motor
@@ -367,7 +204,10 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
btScalar desiredMotorVel = m_motorTargetVelocity; btScalar desiredMotorVel = m_motorTargetVelocity;
btScalar motor_relvel = desiredMotorVel - projRelVel; 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 //todo: should clip against accumulated impulse
btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; 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) );
}

60
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h vendored
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 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 #ifndef HINGECONSTRAINT_H
#define HINGECONSTRAINT_H #define HINGECONSTRAINT_H
@@ -24,6 +22,7 @@ subject to the following restrictions:
class btRigidBody; class btRigidBody;
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /// 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 /// axis defines the orientation of the hinge axis
class btHingeConstraint : public btTypedConstraint class btHingeConstraint : public btTypedConstraint
@@ -31,30 +30,16 @@ class btHingeConstraint : public btTypedConstraint
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. btVector3 m_pivotInA;
btTransform m_rbBFrame; btVector3 m_pivotInB;
btVector3 m_axisInA;
btVector3 m_axisInB;
bool m_angularOnly;
btScalar m_motorTargetVelocity; btScalar m_motorTargetVelocity;
btScalar m_maxMotorImpulse; 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;
bool m_angularOnly;
bool m_enableAngularMotor; bool m_enableAngularMotor;
bool m_solveLimit;
public: public:
@@ -62,10 +47,6 @@ public:
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); 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(); btHingeConstraint();
virtual void buildJacobian(); virtual void buildJacobian();
@@ -95,33 +76,6 @@ public:
m_maxMotorImpulse = maxMotorImpulse; 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 #endif //HINGECONSTRAINT_H