/*
void solveConstraintObsolete( btSolverBody bodyA, btSolverBody bodyB, double timeStep )
{
if( m_useSolveConstraintObsolete )
{
btVector3 pivotAInW = m_rbA.m_worldTransform * m_rbAFrame.m_origin;
btVector3 pivotBInW = m_rbB.m_worldTransform * m_rbBFrame.m_origin;
double tau = (double)( 0.3 );
//linear part
if( !m_angularOnly )
{
btVector3 rel_pos1 = pivotAInW - m_rbA.m_worldTransform.m_origin;
btVector3 rel_pos2 = pivotBInW - m_rbB.m_worldTransform.m_origin;
btVector3 vel1;
bodyA.internalGetVelocityInLocalPointObsolete( rel_pos1, vel1 );
btVector3 vel2;
bodyB.internalGetVelocityInLocalPointObsolete( rel_pos2, vel2 );
btVector3 vel = vel1 - vel2;
for( int i = 0; i < 3; i++ )
{
btIVector3 normal = m_jac[i].m_linearJointAxis;
double jacDiagABInv = btScalar.BT_ONE / m_jac[i].getDiagonal();
double rel_vel;
rel_vel = normal.dot( vel );
//positional error (zeroth order error)
double depth = -( pivotAInW - pivotBInW ).dot( normal ); //this is the error projected on the normal
double impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
btVector3 ftorqueAxis1 = rel_pos1.cross( normal );
btVector3 ftorqueAxis2 = rel_pos2.cross( normal );
bodyA.internalApplyImpulse( normal * m_rbA.getInvMass(), m_rbA.m_invInertiaTensorWorld * ftorqueAxis1, impulse );
bodyB.internalApplyImpulse( normal * m_rbB.getInvMass(), m_rbB.m_invInertiaTensorWorld * ftorqueAxis2, -impulse );
}
}
// apply motor
if( m_bMotorEnabled )
{
// compute current and predicted transforms
btTransform trACur = m_rbA.m_worldTransform;
btTransform trBCur = m_rbB.m_worldTransform;
btVector3 omegaA; bodyA.internalGetAngularVelocity( omegaA );
btVector3 omegaB; bodyB.internalGetAngularVelocity( omegaB );
btTransform trAPred; trAPred.setIdentity();
btVector3 zerovec( 0, 0, 0);
btTransformUtil::integrateTransform(
trACur, zerovec, omegaA, timeStep, trAPred );
btTransform trBPred; trBPred.setIdentity();
btTransformUtil::integrateTransform(
trBCur, zerovec, omegaB, timeStep, trBPred );
// compute desired transforms in world
btTransform trPose( m_qTarget );
btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
btTransform trADes = trBPred * trABDes;
btTransform trBDes = trAPred * trABDes.inverse();
// compute desired omegas in world
btVector3 omegaADes, omegaBDes;
btTransformUtil::calculateVelocity( trACur, trADes, timeStep, zerovec, omegaADes );
btTransformUtil::calculateVelocity( trBCur, trBDes, timeStep, zerovec, omegaBDes );
// compute delta omegas
btVector3 dOmegaA = omegaADes - omegaA;
btVector3 dOmegaB = omegaBDes - omegaB;
// compute weighted avg axis of dOmega (weighting based on inertias)
btVector3 axisA, axisB;
double kAxisAInv = 0, kAxisBInv = 0;
if( dOmegaA.length2() > btScalar.SIMD_EPSILON )
{
axisA = dOmegaA.normalized();
kAxisAInv = m_rbA.computeAngularImpulseDenominator( axisA );
}
if( dOmegaB.length2() > btScalar.SIMD_EPSILON )
{
axisB = dOmegaB.normalized();
kAxisBInv = m_rbB.computeAngularImpulseDenominator( axisB );
}
btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
static bool bDoTorque = true;
if( bDoTorque & avgAxis.length2() > btScalar.SIMD_EPSILON )
{
avgAxis.normalize();
kAxisAInv = m_rbA.computeAngularImpulseDenominator( avgAxis );
kAxisBInv = m_rbB.computeAngularImpulseDenominator( avgAxis );
double kInvCombined = kAxisAInv + kAxisBInv;
btVector3 impulse = ( kAxisAInv * dOmegaA - kAxisBInv * dOmegaB ) /
( kInvCombined * kInvCombined );
if( m_maxMotorImpulse >= 0 )
{
double fMaxImpulse = m_maxMotorImpulse;
if( m_bNormalizedMotorStrength )
fMaxImpulse = fMaxImpulse / kAxisAInv;
btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
double newUnclampedMag = newUnclampedAccImpulse.length();
if( newUnclampedMag > fMaxImpulse )
{
newUnclampedAccImpulse.normalize();
newUnclampedAccImpulse *= fMaxImpulse;
impulse = newUnclampedAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += impulse;
}
double impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag );
}
}
else if( m_damping > btScalar.SIMD_EPSILON ) // no motor: do a little damping
{
btVector3 angVelA; bodyA.internalGetAngularVelocity( angVelA );
btVector3 angVelB; bodyB.internalGetAngularVelocity( angVelB );
btVector3 relVel = angVelB - angVelA;
if( relVel.length2() > btScalar.SIMD_EPSILON )
{
btVector3 relVelAxis = relVel.normalized();
double m_kDamping = btScalar.BT_ONE /
( m_rbA.computeAngularImpulseDenominator( relVelAxis ) +
m_rbB.computeAngularImpulseDenominator( relVelAxis ) );
btVector3 impulse = m_damping * m_kDamping * relVel;
double impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag );
}
}
// joint limits
{
///solve angular part
btVector3 angVelA;
bodyA.internalGetAngularVelocity( angVelA );
btVector3 angVelB;
bodyB.internalGetAngularVelocity( angVelB );
// solve swing limit
if( m_solveSwingLimit )
{
double amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep;
double relSwingVel = ( angVelB - angVelA ).dot( m_swingAxis );
if( relSwingVel > 0 )
amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
double impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
double temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = btMax( m_accSwingLimitImpulse + impulseMag, (double)( 0.0 ) );
impulseMag = m_accSwingLimitImpulse - temp;
btVector3 impulse = m_swingAxis * impulseMag;
// don't let cone response affect twist
// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
{
btVector3 impulseTwistCouple = impulse.dot( m_twistAxisA ) * m_twistAxisA;
btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
impulse = impulseNoTwistCouple;
}
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * noTwistSwingAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * noTwistSwingAxis, -impulseMag );
}
// solve twist limit
if( m_solveTwistLimit )
{
double amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep;
double relTwistVel = ( angVelB - angVelA ).dot( m_twistAxis );
if( relTwistVel > 0 ) // only damp when moving towards limit (m_twistAxis flipping is important)
amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
double impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
double temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = btMax( m_accTwistLimitImpulse + impulseMag, (double)( 0.0 ) );
impulseMag = m_accTwistLimitImpulse - temp;
// btVector3 impulse = m_twistAxis * impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * m_twistAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * m_twistAxis, -impulseMag );
}
}
}
}
*/
/*
void calcAngleInfo()
{
m_swingCorrection = btScalar.BT_ZERO;
m_twistLimitSign = btScalar.BT_ZERO;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1 = btVector3.Zero,b1Axis2 = btVector3.Zero, b1Axis3 = btVector3.Zero;
btVector3 b2Axis1 = btVector3.Zero, b2Axis2 = btVector3.Zero;
b1Axis1 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 0 );
b2Axis1 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 0 );
double swing1 = btScalar.BT_ZERO, swing2 = btScalar.BT_ZERO;
double swx = btScalar.BT_ZERO, swy = btScalar.BT_ZERO;
double thresh = 10;
double fact;
// Get Frame into world space
if( m_swingSpan1 >= (double)( 0.05f ) )
{
b1Axis2 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 1 );
swx = b2Axis1.dot( b1Axis1 );
swy = b2Axis1.dot( b1Axis2 );
swing1 = btScalar.btAtan2Fast( swy, swx );
fact = ( swy * swy + swx * swx ) * thresh * thresh;
fact = fact / ( fact + (double)( 1.0 ) );
swing1 *= fact;
}
if( m_swingSpan2 >= (double)( 0.05f ) )
{
b1Axis3 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 2 );
swx = b2Axis1.dot( b1Axis1 );
swy = b2Axis1.dot( b1Axis3 );
swing2 = btScalar.btAtan2Fast( swy, swx );
fact = ( swy * swy + swx * swx ) * thresh * thresh;
fact = fact / ( fact + (double)( 1.0 ) );
swing2 *= fact;
}
double RMaxAngle1Sq = 1.0f / ( m_swingSpan1 * m_swingSpan1 );
double RMaxAngle2Sq = 1.0f / ( m_swingSpan2 * m_swingSpan2 );
double EllipseAngle = btScalar.btFabs( swing1 * swing1 ) * RMaxAngle1Sq + btScalar.btFabs( swing2 * swing2 ) * RMaxAngle2Sq;
if( EllipseAngle > 1.0f )
{
m_swingCorrection = EllipseAngle - 1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross( b1Axis2 * b2Axis1.dot( b1Axis2 ) + b1Axis3 * b2Axis1.dot( b1Axis3 ) );
m_swingAxis.normalize();
double swingAxisSign = ( b2Axis1.dot( b1Axis1 ) >= 0.0f ) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
}
// Twist limits
if( m_twistSpan >= btScalar.BT_ZERO )
{
btVector3 b2Axis2 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 1 );
btQuaternion rotationArc = btQuaternion.shortestArcQuat( b2Axis1, b1Axis1 );
btVector3 TwistRef = btQuaternion.quatRotate( rotationArc, b2Axis2 );
double twist = btScalar.btAtan2Fast( TwistRef.dot( b1Axis3 ), TwistRef.dot( b1Axis2 ) );
m_twistAngle = twist;
// double lockedFreeFactor = (m_twistSpan > (double)(0.05f)) ? m_limitSoftness : btScalar.BT_ZERO;
double lockedFreeFactor = ( m_twistSpan > (double)( 0.05f ) ) ? (double)( 1.0f ) : btScalar.BT_ZERO;
if( twist <= -m_twistSpan * lockedFreeFactor )
{
m_twistCorrection = -( twist + m_twistSpan );
m_solveTwistLimit = true;
m_twistAxis = ( b2Axis1 + b1Axis1 ) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
else if( twist > m_twistSpan * lockedFreeFactor )
{
m_twistCorrection = ( twist - m_twistSpan );
m_solveTwistLimit = true;
m_twistAxis = ( b2Axis1 + b1Axis1 ) * 0.5f;
m_twistAxis.normalize();
}
}
}
*/
void calcAngleInfo2( ref btTransform transA, ref btTransform transB, ref btMatrix3x3 invInertiaWorldA, ref btMatrix3x3 invInertiaWorldB )
{
m_swingCorrection = btScalar.BT_ZERO;
m_twistLimitSign = btScalar.BT_ZERO;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if( m_bMotorEnabled && ( !m_useSolveConstraintObsolete ) )
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
btTransform trPose = new btTransform( ref m_qTarget );
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btTransform trB; transB.Apply( ref m_rbBFrame, out trB );
btTransform tmp;
btTransform trAInv;
trA.inverse( out trAInv );
trB.Apply( ref trPose, out tmp );
btTransform trDeltaAB;// = trB * trPose * trA.inverse();
tmp.Apply( ref trAInv, out trDeltaAB );
btQuaternion qDeltaAB; trDeltaAB.getRotation( out qDeltaAB );
btVector3 swingAxis = new btVector3( qDeltaAB.x, qDeltaAB.y, qDeltaAB.z );
double swingAxisLen2 = swingAxis.length2();
if( btScalar.btFuzzyZero( swingAxisLen2 ) )
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
if( !btScalar.btFuzzyZero( m_swingCorrection ) )
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
btQuaternion tmpA;
transA.getRotation( out tmpA );
btQuaternion tmpAFrame;
m_rbAFrame.getRotation( out tmpAFrame );
btQuaternion qA; tmpA.Mult( ref tmpAFrame, out qA );
transB.getRotation( out tmpA );
m_rbBFrame.getRotation( out tmpAFrame );
btQuaternion qB; tmpA.Mult( ref tmpAFrame, out qB );// transB.getRotation() * m_rbBFrame.getRotation();
btQuaternion qBInv;
qB.inverse( out qBInv );
btQuaternion qAB; qBInv.Mult( ref qA, out qAB );
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist; btQuaternion.quatRotate( ref qAB, ref vTwist, out vConeNoTwist ); vConeNoTwist.normalize();
btQuaternion qABCone; btQuaternion.shortestArcQuat( ref vTwist, ref vConeNoTwist, out qABCone ); qABCone.normalize();
btQuaternion qABInv;
qABCone.inverse( out qABInv );
btQuaternion qABTwist; qABInv.Mult( ref qAB, out qABTwist ); qABTwist.normalize();
if( m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh )
{
double swingAngle, swingLimit = 0;
btVector3 swingAxis;
computeConeLimitInfo( ref qABCone, out swingAngle, out swingAxis, out swingLimit );
if( swingAngle > swingLimit * m_limitSoftness )
{
m_solveSwingLimit = true;
// compute limit ratio: 0.1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1;
if( swingAngle < swingLimit && m_limitSoftness < 1 - btScalar.SIMD_EPSILON )
{
m_swingLimitRatio = ( swingAngle - swingLimit * m_limitSoftness ) /
( swingLimit - swingLimit * m_limitSoftness );
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - ( swingLimit * m_limitSoftness );
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal( ref swingAxis );
// Calculate necessary axis & factors
btVector3 swingAxisInv;
swingAxis.Invert( out swingAxisInv );
btQuaternion.quatRotate( ref qB, ref swingAxisInv, out m_swingAxis );
m_twistAxisA.setValue( 0, 0, 0 );
m_kSwing = btScalar.BT_ONE /
( computeAngularImpulseDenominator( ref m_swingAxis, invInertiaWorldA ) +
computeAngularImpulseDenominator( ref m_swingAxis, invInertiaWorldB ) );
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
btVector3 ivA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 0 );
btVector3 jvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 1 );
btVector3 kvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 2 );
btVector3 ivB = transB.m_basis * m_rbBFrame.m_basis.getColumn( 0 );
btVector3 target;
double x = ivB.dot( ivA );
double y = ivB.dot( jvA );
double z = ivB.dot( kvA );
if( ( m_swingSpan1 < m_fixThresh ) && ( m_swingSpan2 < m_fixThresh ) )
{ // fixed. We'll need to add one more row to constraint
if( ( !btScalar.btFuzzyZero( y ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) )
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.cross( ivA );
}
}
else
{
if( m_swingSpan1 < m_fixThresh )
{ // hinge around Y axis
// if(!(btFuzzyZero(y)))
if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) )
{
m_solveSwingLimit = true;
if( m_swingSpan2 >= m_fixThresh )
{
y = (double)( 0 );
double span2 = btScalar.btAtan2( z, x );
if( span2 > m_swingSpan2 )
{
x = btScalar.btCos( m_swingSpan2 );
z = btScalar.btSin( m_swingSpan2 );
}
else if( span2 < -m_swingSpan2 )
{
x = btScalar.btCos( m_swingSpan2 );
z = -btScalar.btSin( m_swingSpan2 );
}
}
}
}
else
{ // hinge around Z axis
// if(!btFuzzyZero(z))
if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( y ) ) ) )
{
m_solveSwingLimit = true;
if( m_swingSpan1 >= m_fixThresh )
{
z = (double)( 0 );
double span1 = btScalar.btAtan2( y, x );
if( span1 > m_swingSpan1 )
{
x = btScalar.btCos( m_swingSpan1 );
y = btScalar.btSin( m_swingSpan1 );
}
else if( span1 < -m_swingSpan1 )
{
x = btScalar.btCos( m_swingSpan1 );
y = -btScalar.btSin( m_swingSpan1 );
}
}
}
}
target.x = x * ivA[0] + y * jvA[0] + z * kvA[0];
target.y = x * ivA[1] + y * jvA[1] + z * kvA[1];
target.z = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.w = 0;
target.normalize();
m_swingAxis = -ivB.cross( target );
m_swingCorrection = m_swingAxis.length();
if( !btScalar.btFuzzyZero( m_swingCorrection ) )
m_swingAxis.normalize();
}
}
if( m_twistSpan >= (double)( 0 ) )
{
btVector3 twistAxis;
computeTwistLimitInfo( ref qABTwist, out m_twistAngle, out twistAxis );
twistAxis.Invert( out twistAxis );
if( m_twistAngle > m_twistSpan * m_limitSoftness )
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1;
if( m_twistAngle < m_twistSpan && m_limitSoftness < 1 - btScalar.SIMD_EPSILON )
{
m_twistLimitRatio = ( m_twistAngle - m_twistSpan * m_limitSoftness ) /
( m_twistSpan - m_twistSpan * m_limitSoftness );
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - ( m_twistSpan * m_limitSoftness );
btQuaternion.quatRotate( ref qB, ref twistAxis, out m_twistAxis );
m_kTwist = btScalar.BT_ONE /
( computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldA ) +
computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldB ) );
}
if( m_solveSwingLimit )
{
btQuaternion.quatRotate( ref qA, ref twistAxis, out m_twistAxisA );
}
}
else
{
m_twistAngle = (double)( 0 );
}
}
}
