public void calcNormal( out btVector3 normal )
{
btVector3 tmp;
btVector3 tmp2;
m_vertices2.Sub( ref m_vertices1, out tmp );
m_vertices3.Sub( ref m_vertices1, out tmp2 );
tmp.cross( ref tmp2, out normal );
normal.normalize();
}
protected virtual void IntegrateTransforms(float timeStep)
{
BulletGlobals.StartProfile("integrateTransforms");
IndexedMatrix predictedTrans;
int length = m_nonStaticRigidBodies.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("IntegrateTransforms [{0}]", length);
}
#endif
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.SetHitFraction(1f);
if (body.IsActive() && (!body.IsStaticOrKinematicObject()))
{
body.PredictIntegratedTransform(timeStep, out predictedTrans);
float squareMotion = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
//if (body.GetCcdSquareMotionThreshold() != 0 && body.GetCcdSquareMotionThreshold() < squareMotion)
if (GetDispatchInfo().m_useContinuous&& body.GetCcdSquareMotionThreshold() != 0.0f && body.GetCcdSquareMotionThreshold() < squareMotion)
{
BulletGlobals.StartProfile("CCD motion clamping");
if (body.GetCollisionShape().IsConvex())
{
gNumClampedCcdMotions++;
using (ClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get())
{
sweepResults.Initialize(body, body.GetWorldTransform()._origin, predictedTrans._origin, GetBroadphase().GetOverlappingPairCache(), GetDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
SphereShape tmpSphere = BulletGlobals.SphereShapePool.Get();
tmpSphere.Initialize(body.GetCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
sweepResults.m_allowedPenetration = GetDispatchInfo().GetAllowedCcdPenetration();
sweepResults.m_collisionFilterGroup = body.GetBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.GetBroadphaseProxy().m_collisionFilterMask;
IndexedMatrix modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans._basis = body.GetWorldTransform()._basis;
modifiedPredictedTrans._origin = predictedTrans._origin;
ConvexSweepTest(tmpSphere, body.GetWorldTransform(), modifiedPredictedTrans, sweepResults, 0f);
if (sweepResults.HasHit() && (sweepResults.m_closestHitFraction < 1.0f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body.SetHitFraction(sweepResults.m_closestHitFraction);
body.PredictIntegratedTransform(timeStep * body.GetHitFraction(), out predictedTrans);
body.SetHitFraction(0.0f);
body.ProceedToTransform(ref predictedTrans);
#if false
btVector3 linVel = body.getLinearVelocity();
float maxSpeed = body.getCcdMotionThreshold() / getSolverInfo().m_timeStep;
float maxSpeedSqr = maxSpeed * maxSpeed;
if (linVel.LengthSquared() > maxSpeedSqr)
{
linVel.normalize();
linVel *= maxSpeed;
body.setLinearVelocity(linVel);
float ms2 = body.getLinearVelocity().LengthSquared();
body.predictIntegratedTransform(timeStep, predictedTrans);
float sm2 = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
float smt = body.getCcdSquareMotionThreshold();
printf("sm2=%f\n", sm2);
}
#else
//response between two dynamic objects without friction, assuming 0 penetration depth
float appliedImpulse = 0.0f;
float depth = 0.0f;
appliedImpulse = ContactConstraint.ResolveSingleCollision(body, sweepResults.m_hitCollisionObject, ref sweepResults.m_hitPointWorld, ref sweepResults.m_hitNormalWorld, GetSolverInfo(), depth);
#endif
continue;
}
BulletGlobals.SphereShapePool.Free(tmpSphere);
}
}
BulletGlobals.StopProfile();
}
body.ProceedToTransform(ref predictedTrans);
}
}
}
}
bool collide( ref btVector3 sphereCenter, out btVector3 point, out btVector3 resultNormal, out double depth, double timeOfImpact, double contactBreakingThreshold )
{
//btVector3[] vertices = m_triangle.getVertexPtr( 0 );
double radius = m_sphere.getRadius();
double radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal;
btVector3.btCross2Del( ref m_triangle.m_vertices2, ref m_triangle.m_vertices1
, ref m_triangle.m_vertices3, ref m_triangle.m_vertices1
, out normal );
normal.normalize();
btVector3 p1ToCentre; sphereCenter.Sub( ref m_triangle.m_vertices1, out p1ToCentre );
double distanceFromPlane = p1ToCentre.dot( ref normal );
if( distanceFromPlane < btScalar.BT_ZERO )
{
//triangle facing the other way
distanceFromPlane *= btScalar.BT_NEG_ONE;
normal *= btScalar.BT_NEG_ONE;
}
bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
// Check for contact / intersection
bool hasContact = false;
btVector3 contactPoint = btVector3.Zero;
if( isInsideContactPlane )
{
if( pointInTriangle(
ref m_triangle.m_vertices1
, ref m_triangle.m_vertices2
, ref m_triangle.m_vertices3
, ref normal
, ref sphereCenter
) )
{
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
contactPoint = sphereCenter - normal * distanceFromPlane;
}
else
{
// Could be inside one of the contact capsules
double contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
btVector3 nearestOnEdge;
for( int i = 0; i < m_triangle.getNumEdges(); i++ )
{
btVector3 pa;
btVector3 pb;
m_triangle.getEdge( i, out pa, out pb );
double distanceSqr = SegmentSqrDistance( ref pa, ref pb, ref sphereCenter, out nearestOnEdge );
if( distanceSqr < contactCapsuleRadiusSqr )
{
// Yep, we're inside a capsule
hasContact = true;
contactPoint = nearestOnEdge;
}
}
}
}
if( hasContact )
{
btVector3 contactToCentre; sphereCenter.Sub( ref contactPoint, out contactToCentre );
double distanceSqr = contactToCentre.length2();
if( distanceSqr < radiusWithThreshold * radiusWithThreshold )
{
if( distanceSqr > btScalar.SIMD_EPSILON )
{
double distance = btScalar.btSqrt( distanceSqr );
resultNormal = contactToCentre;
resultNormal.normalize();
point = contactPoint;
depth = -( radius - distance );
}
else
{
resultNormal = normal;
point = contactPoint;
depth = -radius;
}
return true;
}
}
depth = 0;
point = btVector3.Zero;
resultNormal = btVector3.Zero;
return false;
}
/*
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 );
}
}
}
void adjustSwingAxisToUseEllipseNormal( ref btVector3 vSwingAxis )
{
// the swing axis is computed as the "twist-free" cone rotation,
// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
// so, if we're outside the limits, the closest way back inside the cone isn't
// along the vector back to the center. better (and more stable) to use the ellipse normal.
// convert swing axis to direction from center to surface of ellipse
// (ie. rotate 2D vector by PI/2)
double y = -vSwingAxis.z;
double z = vSwingAxis.y;
// do the math...
if( Math.Abs( z ) > btScalar.SIMD_EPSILON ) // avoid division by 0. and we don't need an update if z == 0.
{
// compute gradient/normal of ellipse surface at current "point"
double grad = y / z;
grad *= m_swingSpan2 / m_swingSpan1;
// adjust y/z to represent normal at point (instead of vector to point)
if( y > 0 )
y = Math.Abs( grad * z );
else
y = -Math.Abs( grad * z );
// convert ellipse direction back to swing axis
vSwingAxis.z = ( -y );
vSwingAxis.y = ( z );
vSwingAxis.normalize();
}
}
// given a twist rotation in constraint space, (pre: cone must already be removed)
// this method computes its corresponding angle and axis.
void computeTwistLimitInfo( ref btQuaternion qTwist,
out double twistAngle, // out
out btVector3 vTwistAxis ) // out
{
btQuaternion qMinTwist = qTwist;
twistAngle = qTwist.getAngle();
if( twistAngle > btScalar.SIMD_PI ) // long way around. flip quat and recalculate.
{
qTwist.inverse( out qMinTwist );
twistAngle = qMinTwist.getAngle();
}
if( twistAngle < 0 )
{
// this should never happen
#if false
Debug.Assert(false);
#endif
}
vTwistAxis = new btVector3( qMinTwist.x, qMinTwist.y, qMinTwist.z );
if( twistAngle > btScalar.SIMD_EPSILON )
vTwistAxis.normalize();
}
// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void computeConeLimitInfo( ref btQuaternion qCone,
out double swingAngle, // out
out btVector3 vSwingAxis, // out
out double swingLimit ) // out
{
swingAngle = qCone.getAngle();
if( swingAngle > btScalar.SIMD_EPSILON )
{
vSwingAxis = new btVector3( qCone.x, qCone.y, qCone.z );
vSwingAxis.normalize();
#if false
// non-zero twist?! this should never happen.
Debug.Assert(Math.Abs(vSwingAxis.x) <= btScalar.SIMD_EPSILON));
#endif
// Compute limit for given swing. tricky:
// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
// For starters, compute the direction from center to surface of ellipse.
// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
double xEllipse = vSwingAxis.y;
double yEllipse = -vSwingAxis.z;
// Now, we use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
if( Math.Abs( xEllipse ) > btScalar.SIMD_EPSILON )
{
double surfaceSlope2 = ( yEllipse * yEllipse ) / ( xEllipse * xEllipse );
double norm = 1 / ( m_swingSpan2 * m_swingSpan2 );
norm += surfaceSlope2 / ( m_swingSpan1 * m_swingSpan1 );
double swingLimit2 = ( 1 + surfaceSlope2 ) / norm;
swingLimit = btScalar.btSqrt( swingLimit2 );
}
// test!
/*swingLimit = m_swingSpan2;
if (Math.Abs(vSwingAxis.z) > btScalar.SIMD_EPSILON)
{
double mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
double sinphi = m_swingSpan2 / sqrt(mag_2);
double phi = asin(sinphi);
double theta = atan2(Math.Abs(vSwingAxis.y),Math.Abs(vSwingAxis.z));
double alpha = 3.14159f - theta - phi;
double sinalpha = sin(alpha);
swingLimit = m_swingSpan1 * sinphi/sinalpha;
}*/
}
else //if( swingAngle < 0 )
{
vSwingAxis = btVector3.xAxis;
swingLimit = 0;
// this should never happen!
#if false
Debug.Assert(false);
#endif
}
}
