public static Mat4 FromPosOriScale(Vec3 pos, Mat3 ori, double scale)
{
return new Mat4
{
values = new double[]
{
ori[0] * scale, ori[3] * scale, ori[6] * scale, pos.x,
ori[1] * scale, ori[4] * scale, ori[7] * scale, pos.y,
ori[2] * scale, ori[5] * scale, ori[8] * scale, pos.z,
0, 0, 0, 1
}
};
}
// Creates a rotation around a point
// TODO: check whether this actually works or not !!!
public static Mat4 RotationAroundPoint(Mat3 rot, Vec3 point)
{
Mat3 ident = Mat3.Identity;
return Mat4.FromPositionAndOrientation((rot * point), ident) * Mat4.FromMat3(rot) * Mat4.FromPositionAndOrientation(-(point), ident);
}
// A 4x4 matrix representing an object at a specific position, with a specific orientation
public static Mat4 FromPositionAndOrientation(Vec3 pos, Mat3 mat)
{
return new Mat4
{
values = new double[]
{
mat[0], mat[3], mat[6], pos.x,
mat[1], mat[4], mat[7], pos.y,
mat[2], mat[5], mat[8], pos.z,
0, 0, 0, 1
}
};
}
// Generates a 4x4 matrix with no translation/perspective components, and with rotation/scaling/skew component taken from the specified 3x3 matrix
public static Mat4 FromMat3(Mat3 mat)
{
return new Mat4
{
values = new double[]
{
mat[0], mat[1], mat[2], 0,
mat[3], mat[4], mat[5], 0,
mat[6], mat[7], mat[8], 0,
0, 0, 0, 1
}
};
}
// Get an orthonormal matrix as close to the given matrix as possible
public static Mat3 Normalize(Mat3 mat)
{
Vec3 a = new Vec3 { x = mat[0], y = mat[1], z = mat[2] };
Vec3 b = new Vec3 { x = mat[3], y = mat[4], z = mat[5] };
Vec3 c = Vec3.Cross(a, b);
a = a * (1.0 / a.ComputeMagnitude());
return new Mat3 { values = new double[] { a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z } };
}
// Does a proper inverse (as opposed to a transpose, which conveniently happens to be the same as inverse IF we're using an orthonormal matrix)
public static Mat3 Invert(Mat3 matrix)
{
/*
double det = matrix.Determinant, inv = 1.0 / det, ninv = -inv;
double[] v = matrix.values; // for convenience (maybe also faster?)
return new Mat3
{
values = new double[]
{
(v[4] * v[8] - v[5] * v[7]) * inv,
(v[3] * v[8] - v[5] * v[6]) * ninv,
(v[3] * v[7] - v[4] * v[6]) * inv,
(v[1] * v[8] - v[2] * v[7]) * ninv,
(v[0] * v[8] - v[2] * v[6]) * inv,
(v[0] * v[7] - v[1] * v[6]) * ninv,
(v[1] * v[5] - v[2] * v[4]) * inv,
(v[0] * v[5] - v[2] * v[3]) * ninv,
(v[0] * v[4] - v[1] * v[3]) * inv
}
};
*/
double inv = 1.0 / matrix.Determinant;
Vec3 x0 = new Vec3 { x = matrix[0], y = matrix[3], z = matrix[6] };
Vec3 x1 = new Vec3 { x = matrix[1], y = matrix[4], z = matrix[7] };
Vec3 x2 = new Vec3 { x = matrix[2], y = matrix[5], z = matrix[8] };
Vec3 y0 = Vec3.Cross(x1, x2);
Vec3 y1 = Vec3.Cross(x2, x0);
Vec3 y2 = Vec3.Cross(x0, x1);
return new Mat3
{
values = new double[]
{
y0.x * inv, y0.y * inv, y0.z * inv,
y1.x * inv, y1.y * inv, y1.z * inv,
y2.x * inv, y2.y * inv, y2.z * inv
}
};
}
// Creates a rotation around a point
// TODO: check whether this actually works or not !!!
public static Mat4 RotationAroundPoint(Mat3 rot, Vec3 point)
{
Mat3 ident = Mat3.Identity;
return(Mat4.FromPositionAndOrientation((rot * point), ident) * Mat4.FromMat3(rot) * Mat4.FromPositionAndOrientation(-(point), ident));
}