public static tfloat Determinant(Matrix44 m)
{
tfloat v1 = m.m22 * m.m33 - m.m23 * m.m32;
tfloat v2 = m.m23 * m.m31 - m.m21 * m.m33;
tfloat v3 = m.m21 * m.m32 - m.m22 * m.m31;
tfloat t1 = m.m11 * v1;
tfloat t2 = m.m12 * v2;
tfloat t3 = m.m13 * v3;
tfloat x1 = m.m00 * (t1 + t2 + t3);
tfloat v4 = m.m23 * m.m30 - m.m20 * m.m33;
tfloat v5 = m.m20 * m.m32 - m.m22 * m.m30;
t1 = m.m10 * v1;
t2 = m.m12 * v4;
t3 = m.m13 * v5;
tfloat x2 = -m.m01 * (t1 + t2 + t3);
tfloat v6 = m.m20 * m.m31 - m.m21 * m.m30;
t1 = m.m10 * (-v2);
t2 = m.m11 * v4;
t3 = m.m13 * v6;
tfloat x3 = m.m02 * (t1 + t2 + t3);
t1 = m.m10 * v3;
t2 = m.m11 * (-v5);
t3 = m.m12 * v6;
tfloat x4 = -m.m03 * (t1 + t2 + t3);
return(x1 + x2 + x3 + x4);
}
public tfloat this [int index] {
get {
if (index == 0)
{
return(this.x);
}
if (index == 1)
{
return(this.y);
}
throw new IndexOutOfRangeException("Vector2 index Invalid");
}
set {
if (index == 0)
{
this.x = value;
}
else if (index == 1)
{
this.y = value;
}
else
{
throw new IndexOutOfRangeException("Vector2 index Invalid");
}
}
}
public void Scale(Vector4 scale)
{
this.x *= scale.x;
this.y *= scale.y;
this.z *= scale.z;
this.w *= scale.w;
}
private static Vector3 MakePositive(Vector3 degrees)
{
tfloat negativeFlip = -0.0001f * Math.RAD2DEG;
tfloat positiveFlip = 360.0f + negativeFlip;
if (degrees.x < negativeFlip)
{
degrees.x += 360.0f;
}
else if (degrees.x > positiveFlip)
{
degrees.x -= 360.0f;
}
if (degrees.y < negativeFlip)
{
degrees.y += 360.0f;
}
else if (degrees.y > positiveFlip)
{
degrees.y -= 360.0f;
}
if (degrees.z < negativeFlip)
{
degrees.z += 360.0f;
}
else if (degrees.z > positiveFlip)
{
degrees.z -= 360.0f;
}
return(degrees);
}
public Vector4(tfloat x, tfloat y)
{
this.x = x;
this.y = y;
this.z = Math.ZERO;
this.w = Math.ZERO;
}
/// <summary>
/// Input euler angle is rad
/// </summary>
/// <param name="someEulerAngles"></param>
/// <param name="order"></param>
/// <returns></returns>
private static Quaternion EulerToQuaternion(Vector3 someEulerAngles,
RotationOrder order = RotationOrder.kOrderUnityDefault)
{
tfloat cX = qCos(someEulerAngles.x / 2.0f);
tfloat sX = qSin(someEulerAngles.x / 2.0f);
tfloat cY = qCos(someEulerAngles.y / 2.0f);
tfloat sY = qSin(someEulerAngles.y / 2.0f);
tfloat cZ = qCos(someEulerAngles.z / 2.0f);
tfloat sZ = qSin(someEulerAngles.z / 2.0f);
Quaternion qX = new Quaternion(sX, 0.0F, 0.0F, cX);
Quaternion qY = new Quaternion(0.0F, sY, 0.0F, cY);
Quaternion qZ = new Quaternion(0.0F, 0.0F, sZ, cZ);
Quaternion ret = Quaternion.identity;
switch (order)
{
case RotationOrder.kOrderZYX: CreateQuaternionFromAxisQuaternions(qX, qY, qZ, out ret); break;
case RotationOrder.kOrderYZX: CreateQuaternionFromAxisQuaternions(qX, qZ, qY, out ret); break;
case RotationOrder.kOrderXZY: CreateQuaternionFromAxisQuaternions(qY, qZ, qX, out ret); break;
case RotationOrder.kOrderZXY: CreateQuaternionFromAxisQuaternions(qY, qX, qZ, out ret); break;
case RotationOrder.kOrderYXZ: CreateQuaternionFromAxisQuaternions(qZ, qX, qY, out ret); break;
case RotationOrder.kOrderXYZ: CreateQuaternionFromAxisQuaternions(qZ, qY, qX, out ret); break;
}
return(ret);
}
public Quaternion(Quaternion qua)
{
this.x = qua.x;
this.y = qua.y;
this.z = qua.z;
this.w = qua.w;
}
public Vector4(tfloat x, tfloat y, tfloat z, tfloat w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public void Set(tfloat x, tfloat y, tfloat z, tfloat w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public void Set(float inX, float inY, float inZ, float inW)
{
x = inX;
y = inY;
z = inZ;
w = inW;
}
public Quaternion(tfloat x, tfloat y, tfloat z, tfloat w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public static Vector3 ClampMagnitude(Vector3 vector, tfloat maxLength)
{
if (vector.sqrMagnitude > maxLength * maxLength)
{
return(vector.normalized * maxLength);
}
return(vector);
}
public void FromToRotation(Vector3 from, Vector3 to, ref Matrix33 mtx)
{
Vector3 v = Vector3.Cross(from, to);
tfloat e = Vector3.Dot(from, to);
if (e > (Math.ONE - Math.Epsilon))
{
mtx[0, 0] = Math.ONE; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ZERO;
mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ONE; mtx[0, 2] = Math.ZERO;
mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ONE;
}
else if (e < (new tfloat(-1) * Math.ONE + Math.Epsilon))
{
Vector3 up, left = Vector3.ZERO;
tfloat invlen;
tfloat fxx, fyy, fzz, fxy, fxz, fyz;
tfloat uxx, uyy, uzz, uxy, uxz, uyz;
tfloat lxx, lyy, lzz, lxy, lxz, lyz;
left[0] = Math.ZERO; left[1] = from[2]; left[2] = -from[1];
if (Vector3.Dot(left, left) < Math.Epsilon)
{
left[0] = -from[2]; left[1] = Math.ZERO; left[2] = from[0];
}
invlen = Math.ONE / Math.Sqrt(Vector3.Dot(left, left));
left[0] *= invlen;
left[1] *= invlen;
left[2] *= invlen;
up = Vector3.Cross(left, from);
fxx = -from[0] * from[0]; fyy = -from[1] * from[1]; fzz = -from[2] * from[2];
fxy = -from[0] * from[1]; fxz = -from[0] * from[2]; fyz = -from[1] * from[2];
uxx = up[0] * up[0]; uyy = up[1] * up[1]; uzz = up[2] * up[2];
uxy = up[0] * up[1]; uxz = up[0] * up[2]; uyz = up[1] * up[2];
lxx = -left[0] * left[0]; lyy = -left[1] * left[1]; lzz = -left[2] * left[2];
lxy = -left[0] * left[1]; lxz = -left[0] * left[2]; lyz = -left[1] * left[2];
/* symmetric matrix */
mtx[0, 0] = fxx + uxx + lxx; mtx[0, 1] = fxy + uxy + lxy; mtx[0, 2] = fxz + uxz + lxz;
mtx[1, 0] = mtx[0, 1]; mtx[1, 1] = fyy + uyy + lyy; mtx[1, 2] = fyz + uyz + lyz;
mtx[2, 0] = mtx[0, 2]; mtx[2, 1] = mtx[1, 2]; mtx[2, 2] = fzz + uzz + lzz;
}
else
{
tfloat hvx, hvz, hvxy, hvxz, hvyz;
tfloat h = (Math.ONE - e) / Vector3.Dot(v, v);
hvx = h * v[0];
hvz = h * v[2];
hvxy = hvx * v[1];
hvxz = hvx * v[2];
hvyz = hvz * v[1];
mtx[0, 0] = e + hvx * v[0]; mtx[0, 1] = hvxy - v[2]; mtx[0, 2] = hvxz + v[1];
mtx[1, 0] = hvxy + v[2]; mtx[1, 1] = e + h * v[1] * v[1]; mtx[1, 2] = hvyz - v[0];
mtx[2, 0] = hvxz - v[1]; mtx[2, 1] = hvyz + v[0]; mtx[2, 2] = e + hvz * v[2];
}
}
public static Quaternion operator *(Quaternion lhs, Quaternion rhs)
{
tfloat tempx = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y;
tfloat tempy = lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z;
tfloat tempz = lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x;
tfloat tempw = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z;
Quaternion result = new Quaternion(tempx, tempy, tempz, tempw);
return(result);
}
public static Vector4 Normalize(Vector4 a)
{
tfloat num = Vector4.Magnitude(a);
if (num > Math.Epsilon)
{
return(a / num);
}
return(Vector4.ZERO);
}
// Return angle in degree [0 - 360]
public static tfloat ToDegAngle(tfloat y, tfloat x)
{
tfloat angle = Atan2(y, x) * Rad2Deg;
if (angle < 0)
{
angle += 360;
}
return(angle);
}
public static Vector3 Project(Vector3 vector, Vector3 onNormal)
{
tfloat num = Vector3.Dot(onNormal, onNormal);
if (num < Math.Epsilon)
{
return(Vector3.ZERO);
}
return(onNormal * Vector3.Dot(vector, onNormal) / num);
}
public static Vector3 Normalize(Vector3 value)
{
tfloat num = Vector3.Magnitude(value);
if (num > Math.Epsilon)
{
return(value / num);
}
return(Vector3.ZERO);
}
public static Vector4 MoveTowards(Vector4 current, Vector4 target, tfloat maxDistanceDelta)
{
Vector4 a = target - current;
tfloat magnitude = a.magnitude;
if (magnitude <= maxDistanceDelta || magnitude == Math.ZERO)
{
return(target);
}
return(current + a / magnitude * maxDistanceDelta);
}
public void Normalize()
{
tfloat magnitude = this.magnitude;
if (magnitude > Math.Epsilon)
{
this /= magnitude;
}
else
{
this = Vector2.ZERO;
}
}
public static Matrix44 Perspective(tfloat fov, tfloat aspect, tfloat zNear, tfloat zFar)
{
Matrix44 result = new Matrix44();
tfloat radians = Math.DEG2RAD * (fov / Math.TWO);
tfloat cotangent = Math.Cos(radians) / Math.Sin(radians);
tfloat deltaZ = zNear - zFar;
result [0, 0] = cotangent / aspect; result [0, 1] = Math.ZERO; result [0, 2] = Math.ZERO; result [0, 3] = Math.ZERO;
result [1, 0] = Math.ZERO; result [1, 1] = cotangent; result [1, 2] = Math.ZERO; result [1, 3] = Math.ZERO;
result [2, 0] = Math.ZERO; result [2, 1] = Math.ZERO; result [2, 2] = (zFar + zNear) / deltaZ; result [2, 3] = Math.TWO * zNear * zFar / deltaZ;
result [3, 0] = Math.ZERO; result [3, 1] = Math.ZERO; result [3, 2] = -Math.ONE; result [3, 3] = Math.ZERO;
return(result);
}
public void Normalize()
{
tfloat num = Vector4.Magnitude(this);
if (num > Math.Epsilon)
{
this /= num;
}
else
{
this = Vector4.ZERO;
}
}
public static Quaternion NormalizeSafe(ref Quaternion q)
{
tfloat mag = Magnitude(ref q);
if (mag < Math.Epsilon)
{
return(Quaternion.identity);
}
else
{
return(q / mag);
}
}
public Vector3 MultiplyPoint(Vector3 v)
{
Vector3 result;
result.x = this.m00 * v.x + this.m01 * v.y + this.m02 * v.z + this.m03;
result.y = this.m10 * v.x + this.m11 * v.y + this.m12 * v.z + this.m13;
result.z = this.m20 * v.x + this.m21 * v.y + this.m22 * v.z + this.m23;
tfloat num = this.m30 * v.x + this.m31 * v.y + this.m32 * v.z + this.m33;
num = Math.ONE / num;
result.x *= num;
result.y *= num;
result.z *= num;
return(result);
}
public static tfloat Clamp01(tfloat a)
{
if (a > 1.0f)
{
return(1.0f);
}
else if (a < 0.0f)
{
return(0.0f);
}
else
{
return(a);
}
}
public tfloat this[int index]
{
get
{
switch (index)
{
case 0:
return(this.x);
case 1:
return(this.y);
case 2:
return(this.z);
case 3:
return(this.w);
default:
throw new IndexOutOfRangeException("Invalid Quaternion index!");
}
}
set
{
switch (index)
{
case 0:
this.x = value;
break;
case 1:
this.y = value;
break;
case 2:
this.z = value;
break;
case 3:
this.w = value;
break;
default:
throw new IndexOutOfRangeException("Invalid Quaternion index!");
}
}
}
public static Matrix44 Ortho(tfloat left, tfloat right, tfloat bottom, tfloat top, tfloat zNear, tfloat zFar)
{
Matrix44 result = identity;
tfloat deltax = right - left;
tfloat deltay = top - bottom;
tfloat deltaz = zFar - zNear;
result[0, 0] = Math.TWO / deltax;
result[0, 3] = -(right + left) / deltax;
result[1, 1] = Math.TWO / deltay;
result[1, 3] = -(top + bottom) / deltay;
result[2, 2] = -Math.TWO / deltaz;
result[2, 3] = -(zFar + zNear) / deltaz;
return(result);
}
public static void MatrixToQuaternion(Matrix33 mtx, ref Quaternion q)
{
tfloat fTrace = mtx[0, 0] + mtx[1, 1] + mtx[2, 2];
tfloat fRoot;
if (fTrace > Math.ZERO)
{
fRoot = Math.Sqrt(fTrace + Math.ONE); // 2w
q.w = 0.5f * fRoot;
fRoot = 0.5f / fRoot; // 1/(4w)
q.x = (mtx[2, 1] - mtx[1, 2]) * fRoot;
q.y = (mtx[0, 2] - mtx[2, 0]) * fRoot;
q.z = (mtx[1, 0] - mtx[0, 1]) * fRoot;
}
else
{
int[] next = new int[3] {
1, 2, 0
};
int i = 0;
if (mtx[1, 1] > mtx[0, 0])
{
i = 1;
}
if (mtx[2, 2] > mtx[i, i])
{
i = 2;
}
int j = next[i];
int k = next[j];
fRoot = Math.Sqrt(mtx[i, i] - mtx[j, j] - mtx[k, k] + Math.ONE);
q[i] = 0.5f / fRoot;
q.w = (mtx[k, j] - mtx[j, k]) * fRoot;
q[j] = (mtx[j, i] + mtx[i, j]) * fRoot;
q[k] = (mtx[k, i] + mtx[i, k]) * fRoot;
}
q = Normalize(q);
}
public static Vector3 operator *(Quaternion rotation, Vector3 point)
{
tfloat num = rotation.x * Math.TWO;
tfloat num2 = rotation.y * Math.TWO;
tfloat num3 = rotation.z * Math.TWO;
tfloat num4 = rotation.x * num;
tfloat num5 = rotation.y * num2;
tfloat num6 = rotation.z * num3;
tfloat num7 = rotation.x * num2;
tfloat num8 = rotation.x * num3;
tfloat num9 = rotation.y * num3;
tfloat num10 = rotation.w * num;
tfloat num11 = rotation.w * num2;
tfloat num12 = rotation.w * num3;
Vector3 result;
result.x = (Math.ONE - (num5 + num6)) * point.x + (num7 - num12) * point.y + (num8 + num11) * point.z;
result.y = (num7 + num12) * point.x + (Math.ONE - (num4 + num6)) * point.y + (num9 - num10) * point.z;
result.z = (num8 - num11) * point.x + (num9 + num10) * point.y + (Math.ONE - (num4 + num5)) * point.z;
return(result);
}
public static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection)
{
tfloat lhsMag = fromDirection.magnitude;
tfloat rhsMag = toDirection.magnitude;
if (lhsMag < Math.Epsilon || rhsMag < Math.Epsilon)
{
return(identity);
}
//nomarl///
Vector3 lhs = fromDirection / lhsMag;
Vector3 rhs = toDirection / rhsMag;
Matrix33 mtx = new Matrix33();
mtx = mtx.SetFromToRotation(lhs, rhs);
Quaternion q = new Quaternion();
MatrixToQuaternion(mtx, ref q);
return(q);
}