/// <summary>
/// creates a new matrix from a rotation matrix and a translation vector
/// </summary>
/// <param name="rot"></param>
/// <param name="trans"></param>
public Matrix4(Matrix3 rot, Vector3 trans)
{
A1 = rot.A1; B1 = rot.B1; C1 = rot.C1; D1 = trans.X;
A2 = rot.A2; B2 = rot.B2; C2 = rot.C2; D2 = trans.Y;
A3 = rot.A3; B3 = rot.B3; C3 = rot.C3; D3 = trans.Z;
A4 = 0; B4 = 0; C4 = 0; D4 = 1.0f;
}
/// <summary>
/// creates a matrix from a Matrix3 object
/// </summary>
/// <param name="m">3x3 matrix</param>
/// <returns>4x4 matrix</returns>
public static Matrix4 From(Matrix3 m)
{
return new Matrix4(m.A1, m.A2, m.A3, 0, m.B1, m.B2, m.B3, 0, m.C1, m.C2, m.C3, 0, 0, 0, 0, 1);
}
/// <summary>
/// get 3x3 matrix from 4x4 matrix without specified column and row
/// </summary>
/// <param name="source">source matrix (4x4)</param>
/// <param name="column">column to remove</param>
/// <param name="row">row to remove</param>
/// <returns>3x3 matrix from 4x4 matrix without specified column and row</returns>
public static Matrix3 Minor(Matrix4 source, int column, int row)
{
int r = 0;
Matrix3 result = new Matrix3();
for (int iRow = 0; iRow < 4; iRow++)
{
int c = 0;
if (iRow != row)
{
for (int iColumn = 0; iColumn < 4; iColumn++)
{
if (iColumn != column)
{
result[c, r] = source[iColumn, iRow];
c++;
}
}
r++;
}
}
return result;
}
/// <summary>
/// returns the transposed matrix
/// </summary>
/// <param name="matrix">matrix to transpose</param>
/// <returns>the transposed matrix</returns>
public static Matrix3 Transpose(Matrix3 matrix)
{
Matrix3 m = new Matrix3();
m.A1 = matrix.A1; m.A2 = matrix.B1; m.A3 = matrix.C1;
m.B1 = matrix.A2; m.B2 = matrix.B2; m.B3 = matrix.C2;
m.C1 = matrix.A3; m.C2 = matrix.B3; m.C3 = matrix.C3;
return m;
}
/// <summary>
/// spherical interpolation between to matrices
/// </summary>
/// <param name="a">first matrix</param>
/// <param name="b">second matrix</param>
/// <param name="time">interpolation time [0..1]</param>
/// <returns>interpolated matrix</returns>
public static Matrix3 Slerp(Matrix3 a, Matrix3 b, float time)
{
Quaternion qa = a.Quaternion;
Quaternion qb = b.Quaternion;
Quaternion interpolated = Quaternion.Slerp(qa, qb, time);
Matrix3 result = interpolated.Matrix;
return result;
}
/// <summary>
/// builds scale matrix
/// </summary>
/// <param name="vec">value to scale</param>
/// <returns>scale matrix</returns>
public static Matrix3 Scaling(Vector3 vec)
{
Matrix3 m = new Matrix3();
m.A1 = vec.X;
m.B2 = vec.Y;
m.C3 = vec.Z;
return m;
}
/// <summary>
/// builds a rotation matrix for a given vector and an angle
/// </summary>
/// <param name="vec">to rotate around</param>
/// <param name="angle">rotation angle</param>
/// <returns>rotation matrix</returns>
public static Matrix3 Rotation(Vector3 vec, float angle)
{
float c = Math.Trigonometry.Cos(angle);
float s = Math.Trigonometry.Sin(angle);
float t = 1 - c;
Matrix3 m =
new Matrix3(t * vec.X * vec.X + c, t * vec.X * vec.Y - s * vec.Z, t * vec.X * vec.Z + s * vec.Y,
t * vec.X * vec.Y + s * vec.Z, t * vec.Y * vec.Y + c, t * vec.Y * vec.Z - s * vec.X,
t * vec.X * vec.Z - s * vec.Y, t * vec.Y * vec.Z + s * vec.X, t * vec.Z * vec.Z + c);
return Matrix3.Transpose(m);
}
/// <summary>
/// returns the rotation matrix for alpha, beta and gamma
/// </summary>
/// <param name="alpha">around x-Axis: yaw</param>
/// <param name="beta">around y-Axis: pitch</param>
/// <param name="gamma">around z-Axis: role</param>
public static Matrix3 Rotation(float alpha, float beta, float gamma)
{
// some constants
float sa = Math.Trigonometry.Sin(alpha);
float ca = Math.Trigonometry.Cos(alpha);
float sb = Math.Trigonometry.Sin(beta);
float cb = Math.Trigonometry.Cos(beta);
float sg = Math.Trigonometry.Sin(gamma);
float cg = Math.Trigonometry.Cos(gamma);
float sbsa = sb * sa;
float sbca = sb * ca;
Matrix3 m = new Matrix3();
m.A1 = cg * cb;
m.A2 = sg * cb;
m.A3 = -sb;
m.B1 = cg * sbsa - sg * ca;
m.B2 = sg * sbsa + cg * ca;
m.B3 = cb * sa;
m.C1 = cg * sbca + sg * sa;
m.C2 = sg * sbca - cg * sa;
m.C3 = cb * ca;
return m;
}
