public static SBSQuaternion Integrate(SBSQuaternion rotation, SBSVector3 angularVelocity, float dt)
{
SBSQuaternion spin = new SBSQuaternion(angularVelocity.x, angularVelocity.y, angularVelocity.z, 0.0f) * rotation;
spin.ScaleBy(0.5f * dt);
SBSQuaternion output = rotation + spin;
output.Normalize();
return(output);
}
public static void Slerp(SBSQuaternion q0, SBSQuaternion q1, float t, out SBSQuaternion o)
#endif
{
float w1 = q0.w, x1 = q0.x, y1 = q0.y, z1 = q0.z,
w2 = q1.w, x2 = q1.x, y2 = q1.y, z2 = q1.z,
dot = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
if (dot < 0.0f)
{
dot = -dot;
w2 = -w2;
x2 = -x2;
y2 = -y2;
z2 = -z2;
}
if (dot < 0.95f)
{
float angle = SBSMath.Acos(dot),
s = 1.0f / SBSMath.Sin(angle),
s1 = SBSMath.Sin(angle * (1.0f - t)) * s,
s2 = SBSMath.Sin(angle * t) * s;
o.w = w1 * s1 + w2 * s2;
o.x = x1 * s1 + x2 * s2;
o.y = y1 * s1 + y2 * s2;
o.z = z1 * s1 + z2 * s2;
}
else
{
o.w = w1 + t * (w2 - w1);
o.x = x1 + t * (x2 - x1);
o.y = y1 + t * (y2 - y1);
o.z = z1 + t * (z2 - z1);
o.Normalize();
}
}
public static void Slerp(SBSQuaternion q0, SBSQuaternion q1, float t, out SBSQuaternion o)
#endif
{
float w1 = q0.w, x1 = q0.x, y1 = q0.y, z1 = q0.z,
w2 = q1.w, x2 = q1.x, y2 = q1.y, z2 = q1.z,
dot = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
if (dot < 0.0f) {
dot = -dot;
w2 = -w2;
x2 = -x2;
y2 = -y2;
z2 = -z2;
}
if (dot < 0.95f) {
float angle = SBSMath.Acos(dot),
s = 1.0f / SBSMath.Sin(angle),
s1 = SBSMath.Sin(angle * (1.0f - t)) * s,
s2 = SBSMath.Sin(angle * t) * s;
o.w = w1 * s1 + w2 * s2;
o.x = x1 * s1 + x2 * s2;
o.y = y1 * s1 + y2 * s2;
o.z = z1 * s1 + z2 * s2;
} else {
o.w = w1 + t * (w2 - w1);
o.x = x1 + t * (x2 - x1);
o.y = y1 + t * (y2 - y1);
o.z = z1 + t * (z2 - z1);
o.Normalize();
}
}
