public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix3x3 result)
{
float qX2 = quaternion.X + quaternion.X;
float qY2 = quaternion.Y + quaternion.Y;
float qZ2 = quaternion.Z + quaternion.Z;
float XX = qX2 * quaternion.X;
float YY = qY2 * quaternion.Y;
float ZZ = qZ2 * quaternion.Z;
float XY = qX2 * quaternion.Y;
float XZ = qX2 * quaternion.Z;
float XW = qX2 * quaternion.W;
float YZ = qY2 * quaternion.Z;
float YW = qY2 * quaternion.W;
float ZW = qZ2 * quaternion.W;
result.X = new Vector3(
1 - YY - ZZ,
XY + ZW,
XZ - YW);
result.Y = new Vector3(
XY - ZW,
1 - XX - ZZ,
YZ + XW);
result.Z = new Vector3(
XZ + YW,
YZ - XW,
1 - XX - YY);
}
public static void CreateScale(ref Vector3 scale, out Matrix3x3 linearTransform)
{
linearTransform.X = new Vector3(scale.X, 0, 0);
linearTransform.Y = new Vector3(0, scale.Y, 0);
linearTransform.Z = new Vector3(0, 0, scale.Z);
}
public static void Transpose(ref Matrix3x3 m, out Matrix3x3 transposed)
{
var xy = m.X.Y;
var xz = m.X.Z;
var yz = m.Y.Z;
transposed.X = new Vector3(m.X.X, m.Y.X, m.Z.X);
transposed.Y = new Vector3(xy, m.Y.Y, m.Z.Y);
transposed.Z = new Vector3(xz, yz, m.Z.Z);
}
public static void CreateFromMatrix(ref Matrix matrix, out Matrix3x3 matrix3x3)
{
matrix3x3.X = new Vector3(matrix.X.X, matrix.X.Y, matrix.X.Z);
matrix3x3.Y = new Vector3(matrix.Y.X, matrix.Y.Y, matrix.Y.Z);
matrix3x3.Z = new Vector3(matrix.Z.X, matrix.Z.Y, matrix.Z.Z);
}
public static void TransformTranspose(ref Vector3 v, ref Matrix3x3 m, out Vector3 result)
{
result = new Vector3(
Vector3.Dot(v, m.X),
Vector3.Dot(v, m.Y),
Vector3.Dot(v, m.Z));
}
public static unsafe void Transpose(Matrix3x3* m, Matrix3x3* transposed)
{
Transpose((M*)m, (M*)transposed);
}
public static void Multiply(ref Matrix3x3 a, ref Matrix3x3 b, out Matrix3x3 result)
{
var bX = b.X;
var bY = b.Y;
{
var x = new Vector3(a.X.X);
var y = new Vector3(a.X.Y);
var z = new Vector3(a.X.Z);
result.X = x * bX + y * bY + z * b.Z;
}
{
var x = new Vector3(a.Y.X);
var y = new Vector3(a.Y.Y);
var z = new Vector3(a.Y.Z);
result.Y = x * bX + y * bY + z * b.Z;
}
{
var x = new Vector3(a.Z.X);
var y = new Vector3(a.Z.Y);
var z = new Vector3(a.Z.Z);
result.Z = x * bX + y * bY + z * b.Z;
}
}
public static void Transform(ref Vector3 v, ref Matrix3x3 m, out Vector3 result)
{
var x = new Vector3(v.X);
var y = new Vector3(v.Y);
var z = new Vector3(v.Z);
result = m.X * x + m.Y * y + m.Z * z;
}
public static unsafe void Invert(Matrix3x3* m, Matrix3x3* inverse)
{
var mScalar = (M*)m;
var inverseScalar = (M*)inverse;
var m11 = mScalar->M22 * mScalar->M33 - mScalar->M32 * mScalar->M23;
var m21 = mScalar->M23 * mScalar->M31 - mScalar->M33 * mScalar->M21;
var m31 = mScalar->M21 * mScalar->M32 - mScalar->M31 * mScalar->M22;
var determinantInverse = 1f / (m11 * mScalar->M11 + m21 * mScalar->M12 + m31 * mScalar->M13);
var m12 = mScalar->M32 * mScalar->M13 - mScalar->M12 * mScalar->M33;
var m22 = mScalar->M33 * mScalar->M11 - mScalar->M13 * mScalar->M31;
var m32 = mScalar->M31 * mScalar->M12 - mScalar->M11 * mScalar->M32;
var m13 = mScalar->M12 * mScalar->M23 - mScalar->M22 * mScalar->M13;
var m23 = mScalar->M13 * mScalar->M21 - mScalar->M23 * mScalar->M11;
var m33 = mScalar->M11 * mScalar->M22 - mScalar->M21 * mScalar->M12;
inverseScalar->M11 = m11 * determinantInverse;
inverseScalar->M21 = m21 * determinantInverse;
inverseScalar->M31 = m31 * determinantInverse;
inverseScalar->M12 = m12 * determinantInverse;
inverseScalar->M22 = m22 * determinantInverse;
inverseScalar->M32 = m32 * determinantInverse;
inverseScalar->M13 = m13 * determinantInverse;
inverseScalar->M23 = m23 * determinantInverse;
inverseScalar->M33 = m33 * determinantInverse;
}
public static void Invert(ref Matrix3x3 m, out Matrix3x3 inverse)
{
//Current implementation of cross far from optimal without shuffles, and even then this has some room for improvement.
//Inverts should be really rare, so it's not too concerning. Use the scalar version when possible until ryujit improves (and we improve this implementation).
Vector3 yz, zx, xy;
Vector3x.Cross(ref m.Y, ref m.Z, out yz);
Vector3x.Cross(ref m.Z, ref m.X, out zx);
Vector3x.Cross(ref m.X, ref m.Y, out xy);
var inverseDeterminant = 1f / Vector3.Dot(m.X, yz);
inverse.X = yz * inverseDeterminant;
inverse.Y = zx * inverseDeterminant;
inverse.Z = xy * inverseDeterminant;
Transpose(ref inverse, out inverse);
}
///<summary>
/// Constructs a new affine transform.
///</summary>
///<param name="linearTransform">The linear transform component.</param>
///<param name="translation">Translation component of the transform.</param>
public AffineTransform(ref Matrix3x3 linearTransform, ref Vector3 translation)
{
LinearTransform = linearTransform;
Translation = translation;
}
///<summary>
/// Constructs a new affine transform.
///</summary>
///<param name="translation">Translation to use in the transform.</param>
public AffineTransform(ref Vector3 translation)
{
LinearTransform = Matrix3x3.Identity;
Translation = translation;
}
///<summary>
/// Constructs a new affine transform.
///</summary>
///<param name="linearTransform">The linear transform component.</param>
///<param name="translation">Translation component of the transform.</param>
public AffineTransform(Matrix3x3 linearTransform, Vector3 translation)
: this(ref linearTransform, ref translation)
{
}
