/// <summary>
/// Direct-Matrix-Slerp: for the sake of completeness, I have included the following expression
/// for Spherical-Linear-Interpolation without using quaternions. This is much faster then converting
/// both matrices into quaternions in order to do a quaternion slerp and then converting the slerped
/// quaternion back into a matrix.
/// This is a high-precision calculation. Given two orthonormal 3x3 matrices this function calculates
/// the shortest possible interpolation-path between the two rotations. The interpolation curve forms
/// a great arc on the rotation sphere (geodesic). Not only does Slerp follow a great arc it follows
/// the shortest great arc. Furthermore Slerp has constant angular velocity. All in all Slerp is the
/// optimal interpolation curve between two rotations.
/// STABILITY PROBLEM: There are two singularities at angle=0 and angle=PI. At 0 the interpolation-axis
/// is arbitrary, which means any axis will produce the same result because we have no rotation. Thats
/// why I'm using (1,0,0). At PI the rotations point away from each other and the interpolation-axis
/// is unpredictable. In this case I'm also using the axis (1,0,0). If the angle is ~0 or ~PI, then we
/// have to normalize a very small vector and this can cause numerical instability. The quaternion-slerp
/// has exactly the same problems. Ivo
/// </summary>
/// <param name="m"></param>
/// <param name="n"></param>
/// <param name="t"></param>
/// <example>Matrix33 slerp=Matrix33::CreateSlerp( m,n,0.333f );</example>
public void SetSlerp(Matrix34 m, Matrix34 n, float t)
{
// calculate delta-rotation between m and n (=39 flops)
Matrix33 d = new Matrix33(), i = new Matrix33();
d.M00 = m.M00 * n.M00 + m.M10 * n.M10 + m.M20 * n.M20; d.M01 = m.M00 * n.M01 + m.M10 * n.M11 + m.M20 * n.M21; d.M02 = m.M00 * n.M02 + m.M10 * n.M12 + m.M20 * n.M22;
d.M10 = m.M01 * n.M00 + m.M11 * n.M10 + m.M21 * n.M20; d.M11 = m.M01 * n.M01 + m.M11 * n.M11 + m.M21 * n.M21; d.M12 = m.M01 * n.M02 + m.M11 * n.M12 + m.M21 * n.M22;
d.M20 = d.M01 * d.M12 - d.M02 * d.M11; d.M21 = d.M02 * d.M10 - d.M00 * d.M12; d.M22 = d.M00 * d.M11 - d.M01 * d.M10;
// extract angle and axis
double cosine = MathHelpers.Clamp((d.M00 + d.M11 + d.M22 - 1.0) * 0.5, -1.0, +1.0);
double angle = Math.Atan2(Math.Sqrt(1.0 - cosine * cosine), cosine);
var axis = new Vec3(d.M21 - d.M12, d.M02 - d.M20, d.M10 - d.M01);
double l = Math.Sqrt(axis | axis); if (l > 0.00001)
{
axis /= (float)l;
}
else
{
axis = new Vec3(1, 0, 0);
}
i.SetRotationAA((float)angle * t, axis); // angle interpolation and calculation of new delta-matrix (=26 flops)
// final concatenation (=39 flops)
M00 = m.M00 * i.M00 + m.M01 * i.M10 + m.M02 * i.M20; M01 = m.M00 * i.M01 + m.M01 * i.M11 + m.M02 * i.M21; M02 = m.M00 * i.M02 + m.M01 * i.M12 + m.M02 * i.M22;
M10 = m.M10 * i.M00 + m.M11 * i.M10 + m.M12 * i.M20; M11 = m.M10 * i.M01 + m.M11 * i.M11 + m.M12 * i.M21; M12 = m.M10 * i.M02 + m.M11 * i.M12 + m.M12 * i.M22;
M20 = M01 * M12 - M02 * M11; M21 = M02 * M10 - M00 * M12; M22 = M00 * M11 - M01 * M10;
M03 = m.M03 * (1 - t) + n.M03 * t;
M13 = m.M13 * (1 - t) + n.M13 * t;
M23 = m.M23 * (1 - t) + n.M23 * t;
}
public static Matrix34 CreateFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
{
var matrix = new Matrix34();
matrix.SetFromVectors(vx, vy, vz, pos);
return(matrix);
}
public static Matrix34 CreateTranslationMat(Vec3 v)
{
var matrix = new Matrix34();
matrix.SetTranslationMat(v);
return(matrix);
}
public static Matrix34 CreateScale(Vec3 s, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetScale(s, t);
return(matrix);
}
Matrix34 GetInverted()
{
Matrix34 dst = this;
dst.Invert();
return(dst);
}
public static Matrix34 Create(Vec3 s, Quat q, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.Set(s, q, t);
return(matrix);
}
public static Matrix34 CreateRotationAA(float c, float s, Vec3 axis, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetRotationAA(c, s, axis, t);
return(matrix);
}
public static Matrix34 CreateSlerp(Matrix34 m, Matrix34 n, float t)
{
var matrix = new Matrix34();
matrix.SetSlerp(m, n, t);
return(matrix);
}
public static Matrix34 CreateRotationXYZ(Vec3 rad, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetRotationXYZ(rad, t);
return(matrix);
}
public static Matrix34 CreateRotationAA(Vec3 rot, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetRotationAA(rot, t);
return(matrix);
}
public static Matrix34 CreateIdentity()
{
var matrix = new Matrix34();
matrix.SetIdentity();
return(matrix);
}
public void SetFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
{
var m34 = new Matrix34();
m34.M00 = vx.X; m34.M01 = vy.X; m34.M02 = vz.X; m34.M03 = pos.X;
m34.M10 = vx.Y; m34.M11 = vy.Y; m34.M12 = vz.Y; m34.M13 = pos.Y;
m34.M20 = vx.Z; m34.M21 = vy.Z; m34.M22 = vz.Z; m34.M23 = pos.Z;
this = new QuatT(m34);
}
public Matrix34 GetInvertedFast()
{
var dst = new Matrix34();
dst.M00 = M00; dst.M01 = M10; dst.M02 = M20; dst.M03 = -M03 * M00 - M13 * M10 - M23 * M20;
dst.M10 = M01; dst.M11 = M11; dst.M12 = M21; dst.M13 = -M03 * M01 - M13 * M11 - M23 * M21;
dst.M20 = M02; dst.M21 = M12; dst.M22 = M22; dst.M23 = -M03 * M02 - M13 * M12 - M23 * M22;
return(dst);
}
public Matrix33(Matrix34 m)
{
M00 = m.M00;
M01 = m.M01;
M02 = m.M02;
M10 = m.M10;
M11 = m.M11;
M12 = m.M12;
M20 = m.M20;
M21 = m.M21;
M22 = m.M22;
}
public Matrix33(Matrix34 m)
{
M00 = m.M00;
M01 = m.M01;
M02 = m.M02;
M10 = m.M10;
M11 = m.M11;
M12 = m.M12;
M20 = m.M20;
M21 = m.M21;
M22 = m.M22;
}
public static Matrix34 operator *(Matrix34 l, Matrix34 r)
{
var m = new Matrix34();
m.M00 = l.M00 * r.M00 + l.M01 * r.M10 + l.M02 * r.M20;
m.M10 = l.M10 * r.M00 + l.M11 * r.M10 + l.M12 * r.M20;
m.M20 = l.M20 * r.M00 + l.M21 * r.M10 + l.M22 * r.M20;
m.M01 = l.M00 * r.M01 + l.M01 * r.M11 + l.M02 * r.M21;
m.M11 = l.M10 * r.M01 + l.M11 * r.M11 + l.M12 * r.M21;
m.M21 = l.M20 * r.M01 + l.M21 * r.M11 + l.M22 * r.M21;
m.M02 = l.M00 * r.M02 + l.M01 * r.M12 + l.M02 * r.M22;
m.M12 = l.M10 * r.M02 + l.M11 * r.M12 + l.M12 * r.M22;
m.M22 = l.M20 * r.M02 + l.M21 * r.M12 + l.M22 * r.M22;
m.M03 = l.M00 * r.M03 + l.M01 * r.M13 + l.M02 * r.M23 + l.M03;
m.M13 = l.M10 * r.M03 + l.M11 * r.M13 + l.M12 * r.M23 + l.M13;
m.M23 = l.M20 * r.M03 + l.M21 * r.M13 + l.M22 * r.M23 + l.M23;
return(m);
}
public static Matrix34 CreateRotationAA(Vec3 rot, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetRotationAA(rot, t);
return matrix;
}
public static Matrix34 CreateFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
{
var matrix = new Matrix34();
matrix.SetFromVectors(vx, vy, vz, pos);
return matrix;
}
public static Matrix34 CreateIdentity()
{
var matrix = new Matrix34();
matrix.SetIdentity();
return matrix;
}
public static Matrix34 operator *(Matrix34 l, Matrix34 r)
{
var m = new Matrix34();
m.M00 = l.M00 * r.M00 + l.M01 * r.M10 + l.M02 * r.M20;
m.M10 = l.M10 * r.M00 + l.M11 * r.M10 + l.M12 * r.M20;
m.M20 = l.M20 * r.M00 + l.M21 * r.M10 + l.M22 * r.M20;
m.M01 = l.M00 * r.M01 + l.M01 * r.M11 + l.M02 * r.M21;
m.M11 = l.M10 * r.M01 + l.M11 * r.M11 + l.M12 * r.M21;
m.M21 = l.M20 * r.M01 + l.M21 * r.M11 + l.M22 * r.M21;
m.M02 = l.M00 * r.M02 + l.M01 * r.M12 + l.M02 * r.M22;
m.M12 = l.M10 * r.M02 + l.M11 * r.M12 + l.M12 * r.M22;
m.M22 = l.M20 * r.M02 + l.M21 * r.M12 + l.M22 * r.M22;
m.M03 = l.M00 * r.M03 + l.M01 * r.M13 + l.M02 * r.M23 + l.M03;
m.M13 = l.M10 * r.M03 + l.M11 * r.M13 + l.M12 * r.M23 + l.M13;
m.M23 = l.M20 * r.M03 + l.M21 * r.M13 + l.M22 * r.M23 + l.M23;
return m;
}
public static Matrix34 Create(Vec3 s, Quat q, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.Set(s, q, t);
return matrix;
}
public static Matrix34 CreateScale(Vec3 s, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetScale(s, t);
return matrix;
}
public void SetRotationAA(float c, float s, Vec3 axis, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateRotationAA(c, s, axis));
M03 = t.X; M13 = t.Y; M23 = t.Z;
}
/*!
*
* Convert three Euler angle to mat33 (rotation order:XYZ)
* The Euler angles are assumed to be in radians.
* The translation-vector is set to zero.
*
* Example 1:
* Matrix34 m34;
* m34.SetRotationXYZ( Ang3(0.5f,0.2f,0.9f), translation );
*
* Example 2:
* Matrix34 m34=Matrix34::CreateRotationXYZ( Ang3(0.5f,0.2f,0.9f), translation );
*/
public void SetRotationXYZ(Vec3 rad, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateRotationXYZ(rad));
SetTranslation(t);
}
public void SetRotationAA(Vec3 rot, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateRotationAA(rot));
SetTranslation(t);
}
/// <summary>
/// Direct-Matrix-Slerp: for the sake of completeness, I have included the following expression
/// for Spherical-Linear-Interpolation without using quaternions. This is much faster then converting
/// both matrices into quaternions in order to do a quaternion slerp and then converting the slerped
/// quaternion back into a matrix.
/// This is a high-precision calculation. Given two orthonormal 3x3 matrices this function calculates
/// the shortest possible interpolation-path between the two rotations. The interpolation curve forms
/// a great arc on the rotation sphere (geodesic). Not only does Slerp follow a great arc it follows
/// the shortest great arc. Furthermore Slerp has constant angular velocity. All in all Slerp is the
/// optimal interpolation curve between two rotations.
/// STABILITY PROBLEM: There are two singularities at angle=0 and angle=PI. At 0 the interpolation-axis
/// is arbitrary, which means any axis will produce the same result because we have no rotation. Thats
/// why I'm using (1,0,0). At PI the rotations point away from each other and the interpolation-axis
/// is unpredictable. In this case I'm also using the axis (1,0,0). If the angle is ~0 or ~PI, then we
/// have to normalize a very small vector and this can cause numerical instability. The quaternion-slerp
/// has exactly the same problems. Ivo
/// </summary>
/// <param name="m"></param>
/// <param name="n"></param>
/// <param name="t"></param>
/// <example>Matrix33 slerp=Matrix33::CreateSlerp( m,n,0.333f );</example>
public void SetSlerp(Matrix34 m, Matrix34 n, float t)
{
// calculate delta-rotation between m and n (=39 flops)
Matrix33 d = new Matrix33(), i = new Matrix33();
d.M00 = m.M00 * n.M00 + m.M10 * n.M10 + m.M20 * n.M20; d.M01 = m.M00 * n.M01 + m.M10 * n.M11 + m.M20 * n.M21; d.M02 = m.M00 * n.M02 + m.M10 * n.M12 + m.M20 * n.M22;
d.M10 = m.M01 * n.M00 + m.M11 * n.M10 + m.M21 * n.M20; d.M11 = m.M01 * n.M01 + m.M11 * n.M11 + m.M21 * n.M21; d.M12 = m.M01 * n.M02 + m.M11 * n.M12 + m.M21 * n.M22;
d.M20 = d.M01 * d.M12 - d.M02 * d.M11; d.M21 = d.M02 * d.M10 - d.M00 * d.M12; d.M22 = d.M00 * d.M11 - d.M01 * d.M10;
// extract angle and axis
double cosine = MathHelpers.Clamp((d.M00 + d.M11 + d.M22 - 1.0) * 0.5, -1.0, +1.0);
double angle = Math.Atan2(Math.Sqrt(1.0 - cosine * cosine), cosine);
var axis = new Vec3(d.M21 - d.M12, d.M02 - d.M20, d.M10 - d.M01);
double l = Math.Sqrt(axis | axis); if (l > 0.00001) axis /= (float)l; else axis = new Vec3(1, 0, 0);
i.SetRotationAA((float)angle * t, axis); // angle interpolation and calculation of new delta-matrix (=26 flops)
// final concatenation (=39 flops)
M00 = m.M00 * i.M00 + m.M01 * i.M10 + m.M02 * i.M20; M01 = m.M00 * i.M01 + m.M01 * i.M11 + m.M02 * i.M21; M02 = m.M00 * i.M02 + m.M01 * i.M12 + m.M02 * i.M22;
M10 = m.M10 * i.M00 + m.M11 * i.M10 + m.M12 * i.M20; M11 = m.M10 * i.M01 + m.M11 * i.M11 + m.M12 * i.M21; M12 = m.M10 * i.M02 + m.M11 * i.M12 + m.M12 * i.M22;
M20 = M01 * M12 - M02 * M11; M21 = M02 * M10 - M00 * M12; M22 = M00 * M11 - M01 * M10;
M03 = m.M03 * (1 - t) + n.M03 * t;
M13 = m.M13 * (1 - t) + n.M13 * t;
M23 = m.M23 * (1 - t) + n.M23 * t;
}
public void SetFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
{
var m34 = new Matrix34();
m34.M00 = vx.X; m34.M01 = vy.X; m34.M02 = vz.X; m34.M03 = pos.X;
m34.M10 = vx.Y; m34.M11 = vy.Y; m34.M12 = vz.Y; m34.M13 = pos.Y;
m34.M20 = vx.Z; m34.M21 = vy.Z; m34.M22 = vz.Z; m34.M23 = pos.Z;
this = new QuatT(m34);
}
/*!
* Create rotation-matrix about Z axis using an angle.
* The angle is assumed to be in radians.
* The translation-vector is set to zero.
*
* Example:
* Matrix34 m34;
* m34.SetRotationZ(0.5f);
*/
public void SetRotationZ(float rad, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateRotationZ(rad));
SetTranslation(t);
}
public void SetRotationAA(Vec3 rot, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateRotationAA(rot));
SetTranslation(t);
}
public static Matrix34 CreateTranslationMat(Vec3 v)
{
var matrix = new Matrix34();
matrix.SetTranslationMat(v);
return matrix;
}
public static Matrix34 CreateRotationAA(float c, float s, Vec3 axis, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetRotationAA(c, s, axis, t);
return matrix;
}
public Quat(Matrix34 matrix)
: this(new Matrix33(matrix))
{
}
public static Matrix34 CreateRotationZ(float rad, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.SetRotationZ(rad, t);
return matrix;
}
/*!
* Create a rotation matrix around an arbitrary axis (Eulers Theorem).
* The axis is specified as an normalized Vector3. The angle is assumed to be in radians.
* This function also assumes a translation-vector and stores it in the right column.
*
* Example:
* Matrix34 m34;
* Vector3 axis=GetNormalized( Vector3(-1.0f,-0.3f,0.0f) );
* m34.SetRotationAA( 3.14314f, axis, Vector3(5,5,5) );
*/
public void SetRotationAA(float rad, Vec3 axis, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateRotationAA(rad, axis));
SetTranslation(t);
}
public static Matrix34 CreateSlerp(Matrix34 m, Matrix34 n, float t)
{
var matrix = new Matrix34();
matrix.SetSlerp(m, n, t);
return matrix;
}
public void SetScale(Vec3 s, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateScale(s));
SetTranslation(t);
}
public bool IsEquivalent(Matrix34 m, float e = 0.05f)
{
return ((Math.Abs(M00 - m.M00) <= e) && (Math.Abs(M01 - m.M01) <= e) && (Math.Abs(M02 - m.M02) <= e) && (Math.Abs(M03 - m.M03) <= e) &&
(Math.Abs(M10 - m.M10) <= e) && (Math.Abs(M11 - m.M11) <= e) && (Math.Abs(M12 - m.M12) <= e) && (Math.Abs(M13 - m.M13) <= e) &&
(Math.Abs(M20 - m.M20) <= e) && (Math.Abs(M21 - m.M21) <= e) && (Math.Abs(M22 - m.M22) <= e) && (Math.Abs(M23 - m.M23) <= e));
}
public bool IsEquivalent(Matrix34 m, float e = 0.05f)
{
return((Math.Abs(M00 - m.M00) <= e) && (Math.Abs(M01 - m.M01) <= e) && (Math.Abs(M02 - m.M02) <= e) && (Math.Abs(M03 - m.M03) <= e) &&
(Math.Abs(M10 - m.M10) <= e) && (Math.Abs(M11 - m.M11) <= e) && (Math.Abs(M12 - m.M12) <= e) && (Math.Abs(M13 - m.M13) <= e) &&
(Math.Abs(M20 - m.M20) <= e) && (Math.Abs(M21 - m.M21) <= e) && (Math.Abs(M22 - m.M22) <= e) && (Math.Abs(M23 - m.M23) <= e));
}
public void SetRotationAA(float c, float s, Vec3 axis, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateRotationAA(c, s, axis));
M03 = t.X; M13 = t.Y; M23 = t.Z;
}
public Matrix34 GetInvertedFast()
{
var dst = new Matrix34();
dst.M00 = M00; dst.M01 = M10; dst.M02 = M20; dst.M03 = -M03 * M00 - M13 * M10 - M23 * M20;
dst.M10 = M01; dst.M11 = M11; dst.M12 = M21; dst.M13 = -M03 * M01 - M13 * M11 - M23 * M21;
dst.M20 = M02; dst.M21 = M12; dst.M22 = M22; dst.M23 = -M03 * M02 - M13 * M12 - M23 * M22;
return dst;
}
public void SetScale(Vec3 s, Vec3 t = default(Vec3))
{
this = new Matrix34(Matrix33.CreateScale(s));
SetTranslation(t);
}
public QuatT(Matrix34 m)
{
Q = new Quat(m);
T = m.Translation;
}
public Quat(Matrix34 matrix)
: this(new Matrix33(matrix))
{
}
public QuatT(Matrix34 m)
{
Q = new Quat(m);
T = m.Translation;
}
public QuatT(Matrix34 m)
{
Q = new Quat(m);
T = m.GetTranslation();
}
public QuatT(Matrix34 m)
{
Q = new Quat(m);
T = m.GetTranslation();
}