public static Vector3 SlerpUnclamped(Vector3 p, Vector3 q, float t)
{
bool errorFlag1 = false;
bool errorFlag2 = false;
CheckUnitVector(p, ref errorFlag1);
CheckUnitVector(q, ref errorFlag2);
if (errorFlag1)
{
p = p.Normalized;
}
if (errorFlag2)
{
q = q.Normalized;
}
float cosine = (p.x * q.x) + (p.y * q.y) + (p.z * q.z);
cosine = MathHelpers.Clamp(-1, 1, cosine);
if (MathHelpers.IsEqual(cosine - 1, 0.000001f, MathHelpers.Precision.Precision_6))
{
// use lerp
Vector3 result = LerpUnclamped(p, q, t);
return(result.Normalized);
}
float radians = (float)Math.Acos(cosine);
float scale_0 = (float)Math.Sin((1 - t) * radians);
float scale_1 = (float)Math.Sin(t * radians);
Vector3 result2 = (p * scale_0 + q * scale_1) / (float)Math.Sin(radians);
return(result2.Normalized);
}
public static Vector2 SlerpUnclamped(Vector2 p, Vector2 q, float t)
{
CheckUnitVector(p, "p is not unit vector :" + p.ToString());
CheckUnitVector(q, "q is not unit vector :" + q.ToString());
float cosine = (p.x * q.x) + (p.y * q.y);
if (MathHelpers.IsEqual(cosine, 0.999999f, MathHelpers.Precision.Precision_6)) // vectors are almost parallel
{
// use lerp
Vector2 result = Lerp(p, q, t);
return(result.Normalized);
}
float radians = (float)Math.Acos(cosine);
float scale_0 = (float)Math.Sin((1 - t) * radians);
float scale_1 = (float)Math.Sin(t * radians);
Vector2 result2 = (p * scale_0 + q * scale_1) / (float)Math.Sin(radians);
return(result2.Normalized);
}
public bool Equals(Quaternion other)
{
return(MathHelpers.IsEqual(_v.x, other.x) && MathHelpers.IsEqual(_v.y, other.y) && MathHelpers.IsEqual(_v.z, other.z) && MathHelpers.IsEqual(_w, other.w));
}
public bool Equals(Vector3 other)
{
return(MathHelpers.IsEqual(_x, other.x) && MathHelpers.IsEqual(_y, other.y) && MathHelpers.IsEqual(_z, other.z));
}
public bool Equals(Matrix3x4 other)
{
return(MathHelpers.IsEqual(m00, other.m00) && MathHelpers.IsEqual(m01, other.m01) && MathHelpers.IsEqual(m02, other.m02) && MathHelpers.IsEqual(m03, other.m03) &&
MathHelpers.IsEqual(m10, other.m10) && MathHelpers.IsEqual(m11, other.m11) && MathHelpers.IsEqual(m12, other.m12) && MathHelpers.IsEqual(m13, other.m13) &&
MathHelpers.IsEqual(m20, other.m20) && MathHelpers.IsEqual(m21, other.m21) && MathHelpers.IsEqual(m22, other.m22) && MathHelpers.IsEqual(m23, other.m23));
}