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.Approximately(cosine, 1, 0.000001f))
{
// use lerp
var result = LerpUnclamped(p, q, t);
return(result.Normalized);
}
var radians = (float)Math.Acos(cosine);
var scale_0 = (float)Math.Sin((1 - t) * radians);
var scale_1 = (float)Math.Sin(t * radians);
Vector3 result2 = (p * scale_0 + q * scale_1) / (float)Math.Sin(radians);
return(result2.Normalized);
}
public bool Equals(Matrix4x4 obj)
{
return(MathHelpers.Approximately(m00, obj.m00) && MathHelpers.Approximately(m01, obj.m01) && MathHelpers.Approximately(m02, obj.m02) && MathHelpers.Approximately(m03, obj.m03) &&
MathHelpers.Approximately(m10, obj.m10) && MathHelpers.Approximately(m11, obj.m11) && MathHelpers.Approximately(m12, obj.m12) && MathHelpers.Approximately(m13, obj.m13) &&
MathHelpers.Approximately(m20, obj.m20) && MathHelpers.Approximately(m21, obj.m21) && MathHelpers.Approximately(m22, obj.m22) && MathHelpers.Approximately(m23, obj.m23) &&
MathHelpers.Approximately(m30, obj.m30) && MathHelpers.Approximately(m31, obj.m31) && MathHelpers.Approximately(m32, obj.m32) && MathHelpers.Approximately(m33, obj.m33));
}
public void SetLookOrientation(Vector3 forward, Vector3 up)
{
Vector3 xAxis;
Vector3 yAxis;
Vector3 zAxis;
Vector3 upVector = up.Normalized;
if (forward.IsNearlyZero())
{
Matrix3x3 managedMatrix = Matrix3x3.Identity;
this = new Quaternion(managedMatrix);
return;
}
yAxis = forward.Normalized;
if (MathHelpers.Approximately(0f, yAxis.x) && MathHelpers.Approximately(0f, yAxis.y) && (up == Vector3.Up))
{
upVector = new Vector3(-yAxis.z, 0f, 0f);
}
xAxis = (yAxis.Cross(upVector)).Normalized;
zAxis = (xAxis.Cross(yAxis)).Normalized;
var matrix = new Matrix3x3(xAxis, yAxis, zAxis);
this = new Quaternion(matrix);
}
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.Approximately(cosine, 0.999999f, 0.000001f)) // vectors are almost parallel
{
// use lerp
var result = Lerp(p, q, t);
return(result.Normalized);
}
var radians = (float)Math.Acos(cosine);
var scale_0 = (float)Math.Sin((1 - t) * radians);
var 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(Vector3 other)
{
return(MathHelpers.Approximately(_x, other.x) && MathHelpers.Approximately(_y, other.y) && MathHelpers.Approximately(_z, other.z));
}
public bool Equals(Quaternion other)
{
return(MathHelpers.Approximately(_v.x, other.x) && MathHelpers.Approximately(_v.y, other.y) && MathHelpers.Approximately(_v.z, other.z) && MathHelpers.Approximately(_w, other.w));
}
public bool Equals(Matrix3x3 other)
{
return(MathHelpers.Approximately(m00, other.m00) && MathHelpers.Approximately(m01, other.m01) && MathHelpers.Approximately(m02, other.m02) &&
MathHelpers.Approximately(m10, other.m10) && MathHelpers.Approximately(m11, other.m11) && MathHelpers.Approximately(m12, other.m12) &&
MathHelpers.Approximately(m20, other.m20) && MathHelpers.Approximately(m21, other.m21) && MathHelpers.Approximately(m22, other.m22));
}