static void _LerpRotateBasis( out Matrix3x3 basis, ref Vector3 axis, float cos, float rate )
{
if( rate <= IKEpsilon ) {
basis = Matrix3x3.identity;
return;
}
if( rate <= 1.0f - IKEpsilon ) {
float acos = (cos >= 1.0f - IKEpsilon) ? 0.0f : ((cos <= -1.0f - IKEpsilon) ? (180.0f * Mathf.Deg2Rad) : (float)System.Math.Acos( (float)cos ));
cos = (float)System.Math.Cos( (float)(acos * rate) );
}
SAFBIKMatSetAxisAngle( out basis, ref axis, cos );
}