public static void TransformCoordinate(ref Vector3 coord, ref Matrix4x4 transform, out Vector3 result)
{
Vector4 vector = new Vector4
{
X = (((coord.X * transform.M11) + (coord.Y * transform.M21)) + (coord.Z * transform.M31)) + transform.M41,
Y = (((coord.X * transform.M12) + (coord.Y * transform.M22)) + (coord.Z * transform.M32)) + transform.M42,
Z = (((coord.X * transform.M13) + (coord.Y * transform.M23)) + (coord.Z * transform.M33)) + transform.M43,
W = 1.0 / ((((coord.X * transform.M14) + (coord.Y * transform.M24)) + (coord.Z * transform.M34)) + transform.M44)
};
result = new Vector3(vector.X * vector.W, vector.Y * vector.W, vector.Z * vector.W);
}
public static Vector3 TransformCoordinate(Vector3 coord, Matrix4x4 transform)
{
Vector4 vector = new Vector4
{
X = (((coord.X * transform.M11) + (coord.Y * transform.M21)) + (coord.Z * transform.M31)) + transform.M41,
Y = (((coord.X * transform.M12) + (coord.Y * transform.M22)) + (coord.Z * transform.M32)) + transform.M42,
Z = (((coord.X * transform.M13) + (coord.Y * transform.M23)) + (coord.Z * transform.M33)) + transform.M43,
W = 1.0 / ((((coord.X * transform.M14) + (coord.Y * transform.M24)) + (coord.Z * transform.M34)) + transform.M44)
};
return new Vector3(vector.X * vector.W, vector.Y * vector.W, vector.Z * vector.W);
}
public static Vector4 Transform(Vector4 v, Matrix4x4 matrix)
{
Vector4 result;
result.X = v.X * matrix.M11 + v.Y * matrix.M21 + v.Z * matrix.M31 + v.W * matrix.M41;
result.Y = v.X * matrix.M12 + v.Y * matrix.M22 + v.Z * matrix.M32 + v.W * matrix.M42;
result.Z = v.X * matrix.M13 + v.Y * matrix.M23 + v.Z * matrix.M33 + v.W * matrix.M43;
result.W = v.X * matrix.M14 + v.Y * matrix.M24 + v.Z * matrix.M34 + v.W * matrix.M44;
return result;
}
public static Vector3 Transform(Vector3 v, Matrix4x4 matrix)
{
Vector3 result;
result.X = v.X * matrix.M11 + v.Y * matrix.M21 + v.Z * matrix.M31 + matrix.M41;
result.Y = v.X * matrix.M12 + v.Y * matrix.M22 + v.Z * matrix.M32 + matrix.M42;
result.Z = v.X * matrix.M13 + v.Y * matrix.M23 + v.Z * matrix.M33 + matrix.M43;
return result;
}
public static void Transpose(ref Matrix4x4 m, out Matrix4x4 transposed)
{
double intermediate = m.M12;
transposed.M12 = m.M21;
transposed.M21 = intermediate;
intermediate = m.M13;
transposed.M13 = m.M31;
transposed.M31 = intermediate;
intermediate = m.M14;
transposed.M14 = m.M41;
transposed.M41 = intermediate;
intermediate = m.M23;
transposed.M23 = m.M32;
transposed.M32 = intermediate;
intermediate = m.M24;
transposed.M24 = m.M42;
transposed.M42 = intermediate;
intermediate = m.M34;
transposed.M34 = m.M43;
transposed.M43 = intermediate;
transposed.M11 = m.M11;
transposed.M22 = m.M22;
transposed.M33 = m.M33;
transposed.M44 = m.M44;
}
public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix4x4 result)
{
double qX2 = quaternion.X + quaternion.X;
double qY2 = quaternion.Y + quaternion.Y;
double qZ2 = quaternion.Z + quaternion.Z;
double XX = qX2 * quaternion.X;
double YY = qY2 * quaternion.Y;
double ZZ = qZ2 * quaternion.Z;
double XY = qX2 * quaternion.Y;
double XZ = qX2 * quaternion.Z;
double XW = qX2 * quaternion.W;
double YZ = qY2 * quaternion.Z;
double YW = qY2 * quaternion.W;
double ZW = qZ2 * quaternion.W;
result.M11 = 1 - YY - ZZ;
result.M21 = XY - ZW;
result.M31 = XZ + YW;
result.M41 = 0;
result.M12 = XY + ZW;
result.M22 = 1 - XX - ZZ;
result.M32 = YZ - XW;
result.M42 = 0;
result.M13 = XZ - YW;
result.M23 = YZ + XW;
result.M33 = 1 - XX - YY;
result.M43 = 0;
result.M14 = 0;
result.M24 = 0;
result.M34 = 0;
result.M44 = 1;
}
public static void CreateTranslation(ref Vector3
translation, out Matrix4x4 translationMatrix)
{
translationMatrix = new Matrix4x4
{
M11 = 1,
M22 = 1,
M33 = 1,
M44 = 1,
M41 = translation.X,
M42 = translation.Y,
M43 = translation.Z
};
}
public static void TransformTranspose(ref Vector3 v, ref Matrix4x4 matrix, out Vector4 result)
{
result.X = v.X * matrix.M11 + v.Y * matrix.M12 + v.Z * matrix.M13 + matrix.M14;
result.Y = v.X * matrix.M21 + v.Y * matrix.M22 + v.Z * matrix.M23 + matrix.M24;
result.Z = v.X * matrix.M31 + v.Y * matrix.M32 + v.Z * matrix.M33 + matrix.M34;
result.W = v.X * matrix.M41 + v.Y * matrix.M42 + v.Z * matrix.M43 + matrix.M44;
}
public static void CreateScale(ref Vector3 scale, out Matrix4x4 scaleMatrix)
{
scaleMatrix = new Matrix4x4
{
M11 = scale.X,
M22 = scale.Y,
M33 = scale.Z,
M44 = 1
};
}
public static void CreateScale(double x, double y, double z, out Matrix4x4 scaleMatrix)
{
scaleMatrix = new Matrix4x4
{
M11 = x,
M22 = y,
M33 = z,
M44 = 1
};
}
public static void CreateRotationZ(double radians, out Matrix4x4 result)
{
double num2 = Math.Cos(radians);
double num = Math.Sin(radians);
result.M11 = num2;
result.M12 = num;
result.M13 = 0f;
result.M14 = 0f;
result.M21 = -num;
result.M22 = num2;
result.M23 = 0f;
result.M24 = 0f;
result.M31 = 0f;
result.M32 = 0f;
result.M33 = 1f;
result.M34 = 0f;
result.M41 = 0f;
result.M42 = 0f;
result.M43 = 0f;
result.M44 = 1f;
}
public static void CreatePerspectiveFieldOfView(double fieldOfView,
double aspectRatio, double nearClip, double farClip, out Matrix4x4 matrix)
{
double num = 1f / (Math.Tan(fieldOfView * 0.5));
double num9 = num / aspectRatio;
matrix.M11 = num9;
matrix.M12 = matrix.M13 = matrix.M14 = 0;
matrix.M22 = num;
matrix.M21 = matrix.M23 = matrix.M24 = 0;
matrix.M31 = matrix.M32 = 0;
matrix.M33 = farClip / (nearClip - farClip);
matrix.M34 = -1;
matrix.M41 = matrix.M42 = matrix.M44 = 0;
matrix.M43 = (nearClip * farClip) / (nearClip - farClip);
}
public static void CreateLookAt(ref Vector3 cameraPosition,
ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix4x4 result)
{
Vector3 vector = Vector3.Normalize(cameraPosition - cameraTarget);
Vector3 vector2 = Vector3.Normalize(Vector3.Cross(cameraUpVector, vector));
Vector3 vector3 = Vector3.Cross(vector, vector2);
result.M11 = vector2.X;
result.M12 = vector3.X;
result.M13 = vector.X;
result.M14 = 0f;
result.M21 = vector2.Y;
result.M22 = vector3.Y;
result.M23 = vector.Y;
result.M24 = 0f;
result.M31 = vector2.Z;
result.M32 = vector3.Z;
result.M33 = vector.Z;
result.M34 = 0f;
result.M41 = -Vector3.Dot(vector2, cameraPosition);
result.M42 = -Vector3.Dot(vector3, cameraPosition);
result.M43 = -Vector3.Dot(vector, cameraPosition);
result.M44 = 1f;
}
public static void TransformNormal(ref Vector3 v, ref Matrix4x4 matrix, out Vector3 result)
{
double vX = v.X, vY = v.Y, vZ = v.Z;
result.X = vX * matrix.M11 + vY * matrix.M21 + vZ * matrix.M31 + matrix.M41;
result.Y = vX * matrix.M12 + vY * matrix.M22 + vZ * matrix.M32 + matrix.M42;
result.Z = vX * matrix.M13 + vY * matrix.M23 + vZ * matrix.M33 + matrix.M43;
}
public static void Invert(ref Matrix4x4 m, out Matrix4x4 inverted)
{
double s0 = m.M11 * m.M22 - m.M21 * m.M12;
double s1 = m.M11 * m.M23 - m.M21 * m.M13;
double s2 = m.M11 * m.M24 - m.M21 * m.M14;
double s3 = m.M12 * m.M23 - m.M22 * m.M13;
double s4 = m.M12 * m.M24 - m.M22 * m.M14;
double s5 = m.M13 * m.M24 - m.M23 * m.M14;
double c5 = m.M33 * m.M44 - m.M43 * m.M34;
double c4 = m.M32 * m.M44 - m.M42 * m.M34;
double c3 = m.M32 * m.M43 - m.M42 * m.M33;
double c2 = m.M31 * m.M44 - m.M41 * m.M34;
double c1 = m.M31 * m.M43 - m.M41 * m.M33;
double c0 = m.M31 * m.M42 - m.M41 * m.M32;
double inverseDeterminant = 1.0 / (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
double m11 = m.M11;
double m12 = m.M12;
double m13 = m.M13;
double m14 = m.M14;
double m21 = m.M21;
double m22 = m.M22;
double m23 = m.M23;
double m31 = m.M31;
double m32 = m.M32;
double m33 = m.M33;
double m41 = m.M41;
double m42 = m.M42;
inverted.M11 = (m.M22 * c5 - m.M23 * c4 + m.M24 * c3) * inverseDeterminant;
inverted.M12 = (-m.M12 * c5 + m.M13 * c4 - m.M14 * c3) * inverseDeterminant;
inverted.M13 = (m.M42 * s5 - m.M43 * s4 + m.M44 * s3) * inverseDeterminant;
inverted.M14 = (-m.M32 * s5 + m.M33 * s4 - m.M34 * s3) * inverseDeterminant;
inverted.M21 = (-m.M21 * c5 + m.M23 * c2 - m.M24 * c1) * inverseDeterminant;
inverted.M22 = (m11 * c5 - m13 * c2 + m14 * c1) * inverseDeterminant;
inverted.M23 = (-m.M41 * s5 + m.M43 * s2 - m.M44 * s1) * inverseDeterminant;
inverted.M24 = (m.M31 * s5 - m.M33 * s2 + m.M34 * s1) * inverseDeterminant;
inverted.M31 = (m21 * c4 - m22 * c2 + m.M24 * c0) * inverseDeterminant;
inverted.M32 = (-m11 * c4 + m12 * c2 - m14 * c0) * inverseDeterminant;
inverted.M33 = (m.M41 * s4 - m.M42 * s2 + m.M44 * s0) * inverseDeterminant;
inverted.M34 = (-m31 * s4 + m32 * s2 - m.M34 * s0) * inverseDeterminant;
inverted.M41 = (-m21 * c3 + m22 * c1 - m23 * c0) * inverseDeterminant;
inverted.M42 = (m11 * c3 - m12 * c1 + m13 * c0) * inverseDeterminant;
inverted.M43 = (-m41 * s3 + m42 * s1 - m.M43 * s0) * inverseDeterminant;
inverted.M44 = (m31 * s3 - m32 * s1 + m33 * s0) * inverseDeterminant;
}
public static void TransformTranspose(ref Vector4 v, ref Matrix4x4 matrix, out Vector4 result)
{
double vX = v.X, vY = v.Y, vZ = v.Z, vW = v.W;
result.X = vX * matrix.M11 + vY * matrix.M12 + vZ * matrix.M13 + vW * matrix.M14;
result.Y = vX * matrix.M21 + vY * matrix.M22 + vZ * matrix.M23 + vW * matrix.M24;
result.Z = vX * matrix.M31 + vY * matrix.M32 + vZ * matrix.M33 + vW * matrix.M34;
result.W = vX * matrix.M41 + vY * matrix.M42 + vZ * matrix.M43 + vW * matrix.M44;
}
public static void Mul(ref Matrix4x4 left, ref Matrix4x4 right, out Matrix4x4 result)
{
double resultM11 = left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31 + left.M14 * right.M41;
double resultM12 = left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32 + left.M14 * right.M42;
double resultM13 = left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33 + left.M14 * right.M43;
double resultM14 = left.M11 * right.M14 + left.M12 * right.M24 + left.M13 * right.M34 + left.M14 * right.M44;
double resultM21 = left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31 + left.M24 * right.M41;
double resultM22 = left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32 + left.M24 * right.M42;
double resultM23 = left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33 + left.M24 * right.M43;
double resultM24 = left.M21 * right.M14 + left.M22 * right.M24 + left.M23 * right.M34 + left.M24 * right.M44;
double resultM31 = left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31 + left.M34 * right.M41;
double resultM32 = left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32 + left.M34 * right.M42;
double resultM33 = left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33 + left.M34 * right.M43;
double resultM34 = left.M31 * right.M14 + left.M32 * right.M24 + left.M33 * right.M34 + left.M34 * right.M44;
double resultM41 = left.M41 * right.M11 + left.M42 * right.M21 + left.M43 * right.M31 + left.M44 * right.M41;
double resultM42 = left.M41 * right.M12 + left.M42 * right.M22 + left.M43 * right.M32 + left.M44 * right.M42;
double resultM43 = left.M41 * right.M13 + left.M42 * right.M23 + left.M43 * right.M33 + left.M44 * right.M43;
double resultM44 = left.M41 * right.M14 + left.M42 * right.M24 + left.M43 * right.M34 + left.M44 * right.M44;
result.M11 = resultM11;
result.M12 = resultM12;
result.M13 = resultM13;
result.M14 = resultM14;
result.M21 = resultM21;
result.M22 = resultM22;
result.M23 = resultM23;
result.M24 = resultM24;
result.M31 = resultM31;
result.M32 = resultM32;
result.M33 = resultM33;
result.M34 = resultM34;
result.M41 = resultM41;
result.M42 = resultM42;
result.M43 = resultM43;
result.M44 = resultM44;
}
public static void TransformTranspose(ref Vector3 v, ref Matrix4x4 matrix, out Vector3 result)
{
double vX = v.X, vY = v.Y, vZ = v.Z;
result.X = vX * matrix.M11 + vY * matrix.M12 + vZ * matrix.M13 + matrix.M14;
result.Y = vX * matrix.M21 + vY * matrix.M22 + vZ * matrix.M23 + matrix.M24;
result.Z = vX * matrix.M31 + vY * matrix.M32 + vZ * matrix.M33 + matrix.M34;
}
public static void Mul(ref Matrix4x4 matrix, double scale, out Matrix4x4 result)
{
result.M11 = matrix.M11 * scale;
result.M12 = matrix.M12 * scale;
result.M13 = matrix.M13 * scale;
result.M14 = matrix.M14 * scale;
result.M21 = matrix.M21 * scale;
result.M22 = matrix.M22 * scale;
result.M23 = matrix.M23 * scale;
result.M24 = matrix.M24 * scale;
result.M31 = matrix.M31 * scale;
result.M32 = matrix.M32 * scale;
result.M33 = matrix.M33 * scale;
result.M34 = matrix.M34 * scale;
result.M41 = matrix.M41 * scale;
result.M42 = matrix.M42 * scale;
result.M43 = matrix.M43 * scale;
result.M44 = matrix.M44 * scale;
}
public static void CreateFromMatrix(ref Matrix4x4 matrix4X4, out Matrix3x3 matrix3X3)
{
matrix3X3.M11 = matrix4X4.M11;
matrix3X3.M12 = matrix4X4.M12;
matrix3X3.M13 = matrix4X4.M13;
matrix3X3.M21 = matrix4X4.M21;
matrix3X3.M22 = matrix4X4.M22;
matrix3X3.M23 = matrix4X4.M23;
matrix3X3.M31 = matrix4X4.M31;
matrix3X3.M32 = matrix4X4.M32;
matrix3X3.M33 = matrix4X4.M33;
}
public static void CreateFromAxisAngle(ref Vector3 axis, double angle, out Matrix4x4 result)
{
double xx = axis.X * axis.X;
double yy = axis.Y * axis.Y;
double zz = axis.Z * axis.Z;
double xy = axis.X * axis.Y;
double xz = axis.X * axis.Z;
double yz = axis.Y * axis.Z;
double sinAngle = Math.Sin(angle);
double oneMinusCosAngle = 1 - Math.Cos(angle);
result.M11 = 1 + oneMinusCosAngle * (xx - 1);
result.M21 = -axis.Z * sinAngle + oneMinusCosAngle * xy;
result.M31 = axis.Y * sinAngle + oneMinusCosAngle * xz;
result.M41 = 0;
result.M12 = axis.Z * sinAngle + oneMinusCosAngle * xy;
result.M22 = 1 + oneMinusCosAngle * (yy - 1);
result.M32 = -axis.X * sinAngle + oneMinusCosAngle * yz;
result.M42 = 0;
result.M13 = -axis.Y * sinAngle + oneMinusCosAngle * xz;
result.M23 = axis.X * sinAngle + oneMinusCosAngle * yz;
result.M33 = 1 + oneMinusCosAngle * (zz - 1);
result.M43 = 0;
result.M14 = 0;
result.M24 = 0;
result.M34 = 0;
result.M44 = 1;
}
