// 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
}
}