public static void Multiply(ref SCNQuaternion left, ref SCNQuaternion right, out SCNQuaternion result)
{
result = new SCNQuaternion(
right.W * left.Xyz + left.W * right.Xyz + SCNVector3.Cross(left.Xyz, right.Xyz),
left.W * right.W - SCNVector3.Dot(left.Xyz, right.Xyz));
}
public static SCNQuaternion Slerp(SCNQuaternion q1, SCNQuaternion q2, float blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == 0.0f)
{
if (q2.LengthSquared == 0.0f)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == 0.0f)
{
return(q1);
}
pfloat cosHalfAngle = q1.W * q2.W + SCNVector3.Dot(q1.Xyz, q2.Xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < 0.0f)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
float halfAngle = (float)System.Math.Acos(cosHalfAngle);
float sinHalfAngle = (float)System.Math.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
blendB = (float)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
SCNQuaternion result = new SCNQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
public static SCNQuaternion Mult(SCNQuaternion left, SCNQuaternion right)
{
return(new SCNQuaternion(
right.W * left.Xyz + left.W * right.Xyz + SCNVector3.Cross(left.Xyz, right.Xyz),
left.W * right.W - SCNVector3.Dot(left.Xyz, right.Xyz)));
}