public static float Distance(SBSVector3 v0, SBSVector3 v1)
{
float dx = v1.x - v0.x;
float dy = v1.y - v0.y;
float dz = v1.z - v0.z;
float dd = dx * dx + dy * dy + dz * dz;
return(SBSMath.Sqrt(dd));
}
public static SBSQuaternion Slerp(SBSQuaternion q0, SBSQuaternion q1, float t)
{
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;
w1 = w1 * s1 + w2 * s2;
x1 = x1 * s1 + x2 * s2;
y1 = y1 * s1 + y2 * s2;
z1 = z1 * s1 + z2 * s2;
}
else
{
w1 += t * (w2 - w1);
x1 += t * (x2 - x1);
y1 += t * (y2 - y1);
z1 += t * (z2 - z1);
float oom = x1 * x1 + y1 * y1 + z1 * z1 + w1 * w1;
if (oom < SBSMath.SqrEpsilon)
{
return(new SBSQuaternion(0.0f, 0.0f, 0.0f, 1.0f));
}
oom = 1.0f / SBSMath.Sqrt(oom);
x1 *= oom;
y1 *= oom;
z1 *= oom;
w1 *= oom;
}
return(new SBSQuaternion(x1, y1, z1, w1));
}
public void Normalize()
{
float oom = x * x + y * y + z * z + w * w;
if (oom < SBSMath.SqrEpsilon)
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
w = 1.0f;
return;
}
oom = 1.0f / SBSMath.Sqrt(oom);
x *= oom;
y *= oom;
z *= oom;
w *= oom;
}
public static SBSQuaternion LookRotation(SBSVector3 forward, SBSVector3 up)
{
#if UNITY_FLASH
forward = forward.normalized;
#else
forward.Normalize();
#endif
SBSVector3 right = SBSVector3.Cross(up.normalized, forward).normalized;
up = SBSVector3.Cross(forward, right);
float w = SBSMath.Sqrt(1.0f + right.x + up.y + forward.z) * 0.5f,
oo4w = 1.0f / (4.0f * w),
x = (forward.y - up.z) * oo4w,
y = (right.z - forward.x) * oo4w,
z = (up.x - right.y) * oo4w;
return(new SBSQuaternion(x, y, z, w));
}
public float Normalize()
{
float m = x * x + y * y + z * z;
if (m < SBSMath.SqrEpsilon)
{
return(0.0f);
}
m = SBSMath.Sqrt(m);
float oom = 1.0f / m;
x *= oom;
y *= oom;
z *= oom;
return(m);
}