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