private static CFrame invert4x4(CFrame a)
{
float[] ac = a.components();
float a14 = ac[0], a24 = ac[1], a34 = ac[2], a11 = ac[3], a12 = ac[4], a13 = ac[5], a21 = ac[6], a22 = ac[7], a23 = ac[8], a31 = ac[9], a32 = ac[10], a33 = ac[11];
float det = getDeterminant(a);
if (det == 0)
{
return(a);
}
float b11 = (a22 * a33 * m44 + a23 * a34 * m42 + a24 * a32 * m43 - a22 * a34 * m43 - a23 * a32 * m44 - a24 * a33 * m42) / det;
float b12 = (a12 * a34 * m43 + a13 * a32 * m44 + a14 * a33 * m42 - a12 * a33 * m44 - a13 * a34 * m42 - a14 * a32 * m43) / det;
float b13 = (a12 * a23 * m44 + a13 * a24 * m42 + a14 * a22 * m43 - a12 * a24 * m43 - a13 * a22 * m44 - a14 * a23 * m42) / det;
float b14 = (a12 * a24 * a33 + a13 * a22 * a34 + a14 * a23 * a32 - a12 * a23 * a34 - a13 * a24 * a32 - a14 * a22 * a33) / det;
float b21 = (a21 * a34 * m43 + a23 * a31 * m44 + a24 * a33 * m41 - a21 * a33 * m44 - a23 * a34 * m41 - a24 * a31 * m43) / det;
float b22 = (a11 * a33 * m44 + a13 * a34 * m41 + a14 * a31 * m43 - a11 * a34 * m43 - a13 * a31 * m44 - a14 * a33 * m41) / det;
float b23 = (a11 * a24 * m43 + a13 * a21 * m44 + a14 * a23 * m41 - a11 * a23 * m44 - a13 * a24 * m41 - a14 * a21 * m43) / det;
float b24 = (a11 * a23 * a34 + a13 * a24 * a31 + a14 * a21 * a33 - a11 * a24 * a33 - a13 * a21 * a34 - a14 * a23 * a31) / det;
float b31 = (a21 * a32 * m44 + a22 * a34 * m41 + a24 * a31 * m42 - a21 * a34 * m42 - a22 * a31 * m44 - a24 * a32 * m41) / det;
float b32 = (a11 * a34 * m42 + a12 * a31 * m44 + a14 * a32 * m41 - a11 * a32 * m44 - a12 * a34 * m41 - a14 * a31 * m42) / det;
float b33 = (a11 * a22 * m44 + a12 * a24 * m41 + a14 * a21 * m42 - a11 * a24 * m42 - a12 * a21 * m44 - a14 * a22 * m41) / det;
float b34 = (a11 * a24 * a32 + a12 * a21 * a34 + a14 * a22 * a31 - a11 * a22 * a34 - a12 * a24 * a31 - a14 * a21 * a32) / det;
float b41 = (a21 * a33 * m42 + a22 * a31 * m43 + a23 * a32 * m41 - a21 * a32 * m43 - a22 * a33 * m41 - a23 * a31 * m42) / det;
float b42 = (a11 * a32 * m43 + a12 * a33 * m41 + a13 * a31 * m42 - a11 * a33 * m42 - a12 * a31 * m43 - a13 * a32 * m41) / det;
float b43 = (a11 * a23 * m42 + a12 * a21 * m43 + a13 * a22 * m41 - a11 * a22 * m43 - a12 * a23 * m41 - a13 * a21 * m42) / det;
float b44 = (a11 * a22 * a33 + a12 * a23 * a31 + a13 * a21 * a32 - a11 * a23 * a32 - a12 * a21 * a33 - a13 * a22 * a31) / det;
return(new CFrame(b14, b24, b34, b11, b12, b13, b21, b22, b23, b31, b32, b33));
}
public static CFrame Angles(float x, float y, float z)
{
CFrame cfx = fromAxisAngle(RIGHT, x);
CFrame cfy = fromAxisAngle(UP, y);
CFrame cfz = fromAxisAngle(BACK, z);
return(cfx * cfy * cfz);
}
static void Main(string[] args)
{
CFrame dcf = new CFrame();
CFrame cf1 = new CFrame(1, 2, 3) * CFrame.Angles((float)Math.PI / 3, (float)Math.PI / 6, 0);
CFrame cf2 = new CFrame(-4, 5, 7.2f) * CFrame.Angles(0, (float)Math.PI / 7, -(float)Math.PI / 3);
CFrame cf = cf1 * cf2;
Vector3 v = new Vector3(10, -5, 6);
Vector3 v2 = new Vector3(12.6602535f, -0.669872522f, -1.2320509f);
Console.WriteLine(cf * cf2.inverse());
Console.WriteLine(cf1.pointToObjectSpace(v2));
Console.ReadLine();
}
private static float getDeterminant(CFrame a)
{
float[] ac = a.components();
float a14 = ac[0], a24 = ac[1], a34 = ac[2], a11 = ac[3], a12 = ac[4], a13 = ac[5], a21 = ac[6], a22 = ac[7], a23 = ac[8], a31 = ac[9], a32 = ac[10], a33 = ac[11];
float det = (a11 * a22 * a33 * m44 + a11 * a23 * a34 * m42 + a11 * a24 * a32 * m43
+ a12 * a21 * a34 * m43 + a12 * a23 * a31 * m44 + a12 * a24 * a33 * m41
+ a13 * a21 * a32 * m44 + a13 * a22 * a34 * m41 + a13 * a24 * a31 * m42
+ a14 * a21 * a33 * m42 + a14 * a22 * a31 * m43 + a14 * a23 * a32 * m41
- a11 * a22 * a34 * m43 - a11 * a23 * a32 * m44 - a11 * a24 * a33 * m42
- a12 * a21 * a33 * m44 - a12 * a23 * a34 * m41 - a12 * a24 * a31 * m43
- a13 * a21 * a34 * m42 - a13 * a22 * a31 * m44 - a13 * a24 * a32 * m41
- a14 * a21 * a32 * m43 - a14 * a22 * a33 * m41 - a14 * a23 * a31 * m42);
return(det);
}
private static CFrame lerpinternal(CFrame a, CFrame b, float t)
{
CFrame cf = a.inverse() * b;
float[] q = quaternionFromCFrame(cf);
float w = q[0], i = q[1], j = q[2], k = q[3];
float theta = (float)Math.Acos(w) * 2;
Vector3 v = new Vector3(i, j, k);
Vector3 p = a.p.Lerp(b.p, t);
if (theta != 0)
{
CFrame r = a * fromAxisAngle(v, theta * t);
return((r - r.p) + p);
}
else
{
return((a - a.p) + p);
}
}
private static float[] quaternionFromCFrame(CFrame a)
{
float[] ac = a.components();
float mx = ac[0], my = ac[1], mz = ac[2], m11 = ac[3], m12 = ac[4], m13 = ac[5], m21 = ac[6], m22 = ac[7], m23 = ac[8], m31 = ac[9], m32 = ac[10], m33 = ac[11];
float trace = m11 + m22 + m33;
float w = 1, i = 0, j = 0, k = 0;
if (trace > 0)
{
float s = (float)Math.Sqrt(1 + trace);
float r = 0.5f / s;
w = s * 0.5f; i = (m32 - m23) * r; j = (m13 - m31) * r; k = (m21 - m12) * r;
}
else
{
float big = Math.Max(Math.Max(m11, m22), m33);
if (big == m11)
{
float s = (float)Math.Sqrt(1 + m11 - m22 - m33);
float r = 0.5f / s;
w = (m32 - m23) * r; i = 0.5f * s; j = (m21 + m12) * r; k = (m13 + m31) * r;
}
else if (big == m22)
{
float s = (float)Math.Sqrt(1 - m11 + m22 - m33);
float r = 0.5f / s;
w = (m13 - m31) * r; i = (m21 + m12) * r; j = 0.5f * s; k = (m32 + m23) * r;
}
else if (big == m33)
{
float s = (float)Math.Sqrt(1 - m11 - m22 + m33);
float r = 0.5f / s;
w = (m21 - m12) * r; i = (m13 + m31) * r; j = (m32 + m23) * r; k = 0.5f * s;
}
}
return(new float[] { w, i, j, k });
}
public CFrame toObjectSpace(CFrame cf2)
{
return(this.inverse() * cf2);
}
public CFrame toWorldSpace(CFrame cf2)
{
return(this * cf2);
}
public CFrame lerp(CFrame cf2, float t)
{
return(lerpinternal(this, cf2, t));
}
