protected Matrix44 GetTransformFromParts(Vector3 localTranslation, Matrix33 localRotation)
{
Matrix44 transform = new Matrix44
{
// Translation part
m14 = localTranslation.x,
m24 = localTranslation.y,
m34 = localTranslation.z,
// Rotation part
m11 = localRotation.m11,
m12 = localRotation.m12,
m13 = localRotation.m13,
m21 = localRotation.m21,
m22 = localRotation.m22,
m23 = localRotation.m23,
m31 = localRotation.m31,
m32 = localRotation.m32,
m33 = localRotation.m33,
// Set final row
m41 = 0,
m42 = 0,
m43 = 0,
m44 = 1
};
return(transform);
}
public static UVector3 ToEulerAngles(this Matrix33 matrix3X3)
{
var sy = Mathf.Sqrt(Mathf.Pow(matrix3X3.m11, 2) + Mathf.Pow(matrix3X3.m21, 2));
var singular = sy < 1e-6;
UVector3 angles;
if (!singular)
{
angles = new UVector3
{
x = Mathf.Atan2(matrix3X3.m32, matrix3X3.m33),
y = Mathf.Atan2(-matrix3X3.m31, sy),
z = Mathf.Atan2(matrix3X3.m21, matrix3X3.m11)
};
}
else
{
angles = new Vector2
{
x = Mathf.Atan2(-matrix3X3.m23, matrix3X3.m22),
y = Mathf.Atan2(-matrix3X3.m31, sy)
};
}
return(angles * 180);
}
public override void Read(NiHeader header, BinaryReader reader)
{
base.Read(header, reader);
this.center = Utils.ReadVector3(reader);
this.size = Utils.ReadVector3(reader);
this.rotation = Utils.ReadMatrix33(reader);
}
public void TestIdentity()
{
Matrix33 nativeMatrix = Matrix33.CreateIdentity();
Matrix3x3 managedmatrix = Matrix3x3.Identity;
// Compare native vs managed identity
Assert.That(nativeMatrix.m00, Is.EqualTo(managedmatrix[0, 0]));
Assert.That(nativeMatrix.m01, Is.EqualTo(managedmatrix[0, 1]));
Assert.That(nativeMatrix.m02, Is.EqualTo(managedmatrix[0, 2]));
Assert.That(nativeMatrix.m10, Is.EqualTo(managedmatrix[1, 0]));
Assert.That(nativeMatrix.m11, Is.EqualTo(managedmatrix[1, 1]));
Assert.That(nativeMatrix.m12, Is.EqualTo(managedmatrix[1, 2]));
Assert.That(nativeMatrix.m20, Is.EqualTo(managedmatrix[2, 0]));
Assert.That(nativeMatrix.m21, Is.EqualTo(managedmatrix[2, 1]));
Assert.That(nativeMatrix.m22, Is.EqualTo(managedmatrix[2, 2]));
// Compare managed to the real values of the identity
Assert.That(managedmatrix[0, 0], Is.EqualTo(1));
Assert.That(managedmatrix[0, 1], Is.EqualTo(0));
Assert.That(managedmatrix[0, 2], Is.EqualTo(0));
Assert.That(managedmatrix[1, 0], Is.EqualTo(0));
Assert.That(managedmatrix[1, 1], Is.EqualTo(1));
Assert.That(managedmatrix[1, 2], Is.EqualTo(0));
Assert.That(managedmatrix[2, 0], Is.EqualTo(0));
Assert.That(managedmatrix[2, 1], Is.EqualTo(0));
Assert.That(managedmatrix[2, 2], Is.EqualTo(1));
}
internal static DemoOption FromReader(ushort size, BinaryReader reader)
{
Validate(size);
var result = new DemoOption
{
State = (States)reader.ReadUInt32(),
BatteryPercentage = reader.ReadUInt32(),
Theta = reader.ReadSingle() / 1000f,
Phi = reader.ReadSingle() / 1000f,
Psi = reader.ReadSingle() / 1000f,
Altitude = reader.ReadInt32() / 1000,
Vx = reader.ReadSingle() / 1000f,
Vy = reader.ReadSingle() / 1000f,
Vz = reader.ReadSingle() / 1000f,
FrameNumber = reader.ReadUInt32(),
_detectionCameraRotation = Matrix33.FromReader(reader),
_detectionCameraTrans = Vector.FromReader(reader),
DetectionTagIndex = reader.ReadUInt32(),
DetectionCameraType = reader.ReadUInt32(),
_droneCameraRotation = Matrix33.FromReader(reader),
_droneCameraTrans = Vector.FromReader(reader)
};
return(result);
}
/// <summary>
/// Get the symmetric inverse of this matrix as a 3-by-3.
/// Returns the zero matrix if singular.
/// </summary>
/// <param name="mm"></param>
public void GetSymInverse33(ref Matrix33 matrix)
{
float det = Vector3.Dot(EX, Vector3.Cross(EY, EZ));
if (det != 0.0f)
{
det = 1.0f / det;
}
float a11 = EX.X, a12 = EY.X, a13 = EZ.X;
float a22 = EY.Y, a23 = EZ.Y;
float a33 = EZ.Z;
matrix.EX.X = det * (a22 * a33 - a23 * a23);
matrix.EX.Y = det * (a13 * a23 - a12 * a33);
matrix.EX.Z = det * (a12 * a23 - a13 * a22);
matrix.EY.X = matrix.EX.Y;
matrix.EY.Y = det * (a11 * a33 - a13 * a13);
matrix.EY.Z = det * (a13 * a12 - a11 * a23);
matrix.EZ.X = matrix.EX.Z;
matrix.EZ.Y = matrix.EY.Z;
matrix.EZ.Z = det * (a11 * a22 - a12 * a12);
}
/// <summary>
/// Writes the results of common matrix math. For testing purposes.
/// </summary>
/// <param name="localRotation">The matrix that the math functions will be applied to.</param>
private void WriteMatrices(Matrix33 localRotation)
{
localRotation.WriteMatrix33("Regular");
localRotation.Inverse().WriteMatrix33("Inverse");
localRotation.Conjugate().WriteMatrix33("Conjugate");
localRotation.ConjugateTranspose().WriteMatrix33("Conjugate Transpose");
}
public void RotatePoint270()
{
var m = Matrix33.Rotate(Algorithms.ToRadians(270));
var a = new Cartesian(100.0, 100.0);
var b = m.Transform(a);
Assert.That(IsApproximately(b.X, 100.0, 0.001));
Assert.That(IsApproximately(b.Y, -100.0, 0.001));
}
public void MultiplyIdentity()
{
var a = new Matrix33();
var b = new Matrix33();
var c = Matrix33.Multiply(a, b);
Assert.That(c.Equals(a));
Assert.That(c.Equals(b));
}
public void Identity()
{
double[,] id = new double[3, 3];
id[0, 0] = id[1, 1] = id[2, 2] = 1.0;
Matrix33 a = new Matrix33(id);
Matrix33 b = new Matrix33();
Assert.That(b.Equals(a));
}
public void TranslatePoint2()
{
var d = new Cartesian(5.0, 10.0);
var m = Matrix33.Translate(d);
var a = new Cartesian(1.0, 2.0);
var b = m.Transform(a);
Assert.That(b.X, Is.EqualTo(6.0));
Assert.That(b.Y, Is.EqualTo(12.0));
}
public void GetTransformFromParts_TwoParameters()
{
Matrix44 actual = new Matrix44();
Vector3 vector3 = GetTestVector3();
Matrix33 rotation = GetTestMatrix33();
Matrix44 matrix = actual.GetTransformFromParts(vector3, rotation);
Assert.AreEqual(0.11f, matrix.m11);
}
public void ScalePoint()
{
var d = new Cartesian(0.5, 0.25);
var m = Matrix33.Scale(d);
var a = new Cartesian(100.0, 100.0);
var b = m.Transform(a);
Assert.That(b.X, Is.EqualTo(50.0));
Assert.That(b.Y, Is.EqualTo(25.0));
}
public static void NifStream(Matrix33 val, OStream s, NifInfo info)
{
for (var c = 0; c < 3; ++c)
{
for (var r = 0; r < 3; ++r)
{
WriteFloat(val[r][c], s);
}
}
}
public NiAVObject()
{
this.flags = (ushort)14;
this.flags2 = (ushort)0;
this.translation = new Vector3(0.0f, 0.0f, 0.0f);
this.rotation = new Matrix33(true);
this.scale = 1f;
this.numProperties = 0U;
this.properties = new List <int>();
this.collisionObject = -1;
}
public void ScaleAndTranslatePoint3()
{
var tp = new Cartesian(500.0, 500.0);
var sp = new Cartesian(300.0 / 6000.0, -300.0 / 6000.0);
var m = Matrix33.CreateMatrix(sp, tp, 0.0);
var a = new Cartesian(0.0, 6000.0);
var b = m.Transform(a);
Assert.That(b.X, Is.EqualTo(500.0));
Assert.That(b.Y, Is.EqualTo(200.0));
}
public void RotateScaleAndTranslatePoint()
{
var tp = new Cartesian(500.0, 500.0);
var sp = new Cartesian(500.0 / 1000.0, -500.0 / 1000.0);
var m = Matrix33.CreateMatrix(sp, tp, Algorithms.ToRadians(30.0));
var a = new Cartesian(500.0, 100.0);
var b = m.Transform(a);
Assert.That(IsApproximately(b.X, 691.506, 0.001));
Assert.That(IsApproximately(b.Y, 331.699, 0.001));
}
public void ScaleAndTranslatePoint()
{
var tp = new Cartesian(5.0, 10.0);
var sp = new Cartesian(0.5, 0.25);
var m = Matrix33.CreateMatrix(sp, tp, 0.0);
var a = new Cartesian(100.0, 100.0);
var b = m.Transform(a);
Assert.That(b.X, Is.EqualTo(55.0));
Assert.That(b.Y, Is.EqualTo(35.0));
}
//Matrix33
public static void NifStream(out Matrix33 val, IStream s, NifInfo info)
{
val = new Matrix33();
for (var c = 0; c < 3; ++c)
{
for (var r = 0; r < 3; ++r)
{
val[r][c] = ReadFloat(s);
}
}
}
public void MultMatrix33(Matrix33 mtx)
{
if (MtxMode == NDSMatrixMode.Position || MtxMode == NDSMatrixMode.Position_Vector)
{
CurPosMtx = CurPosMtx * new Matrix44(mtx, 0, 0, 0, new Vector4(0, 0, 0, 1));
}
if (MtxMode == NDSMatrixMode.Position_Vector)
{
CurDirMtx = CurDirMtx * new Matrix44(mtx, 0, 0, 0, new Vector4(0, 0, 0, 1));
}
}
/// <summary>
/// Read TransformMatrix from the file.
/// </summary>
/// <param name="reader"></param>
public void ReadFromFile(BinaryReader reader)
{
Vector3 m1 = new Vector3(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
float x = reader.ReadSingle();
Vector3 m2 = new Vector3(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
float y = reader.ReadSingle();
Vector3 m3 = new Vector3(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
float z = reader.ReadSingle();
Rotation = new Matrix33(m1, m2, m3, true);
Position = new Vector3(x, y, z);
}
/// <summary>
/// Get the inverse of this matrix as a 2-by-2.
/// Returns the zero matrix if singular.
/// </summary>
/// <returns></returns>
public void GetInverse22(ref Matrix33 matrix)
{
float a = EX.X, b = EY.X, c = EX.Y, d = EY.Y;
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
matrix.EX.X = det * d; matrix.EY.X = -det * b; matrix.EX.Z = 0.0f;
matrix.EX.Y = -det * c; matrix.EY.Y = det * a; matrix.EY.Z = 0.0f;
matrix.EZ.X = 0.0f; matrix.EZ.Y = 0.0f; matrix.EZ.Z = 0.0f;
}
public void ConversionFromManagedToNative()
{
Matrix3x3 managedmatrix = new Matrix3x3(0, 1, 2, 10, 11, 12, 20, 21, 22);
Matrix33 nativeMatrix = managedmatrix;
Assert.That(nativeMatrix.m00, Is.EqualTo(managedmatrix[0, 0]));
Assert.That(nativeMatrix.m01, Is.EqualTo(managedmatrix[0, 1]));
Assert.That(nativeMatrix.m02, Is.EqualTo(managedmatrix[0, 2]));
Assert.That(nativeMatrix.m10, Is.EqualTo(managedmatrix[1, 0]));
Assert.That(nativeMatrix.m11, Is.EqualTo(managedmatrix[1, 1]));
Assert.That(nativeMatrix.m12, Is.EqualTo(managedmatrix[1, 2]));
Assert.That(nativeMatrix.m20, Is.EqualTo(managedmatrix[2, 0]));
Assert.That(nativeMatrix.m21, Is.EqualTo(managedmatrix[2, 1]));
Assert.That(nativeMatrix.m22, Is.EqualTo(managedmatrix[2, 2]));
}
/// <summary>
/// Creates new instance of <see cref="EulerAngles"/> struct.
/// </summary>
/// <param name="matrix">Matrix that defines new instance.</param>
public EulerAngles(Matrix33 matrix)
{
// Assert matrix being orthonormal.
this.Roll = (float)Math.Asin(Math.Max(-1.0f, Math.Min(1.0f, -matrix.M20)));
if (Math.Abs(Math.Abs(this.Roll) - (float)(Math.PI * 0.5)) < 0.01f)
{
this.Pitch = 0;
this.Yaw = (float)Math.Atan2(-matrix.M01, matrix.M11);
}
else
{
this.Pitch = (float)Math.Atan2(matrix.M21, matrix.M22);
this.Yaw = (float)Math.Atan2(matrix.M10, matrix.M00);
}
}
public void Translate()
{
var d = new Cartesian(1.23, 2.34);
double[,] m = new double[3, 3];
m[0, 0] = m[1, 1] = m[2, 2] = 1.0;
m[0, 2] = d.X;
m[1, 2] = d.Y;
Matrix33 a = new Matrix33(m);
Matrix33 b = Matrix33.Translate(d);
Assert.That(b.Equals(a));
}
private Matrix33 GetTestMatrix33()
{
Matrix33 matrix = new Matrix33();
matrix.m11 = 0.11f;
matrix.m12 = 0.12f;
matrix.m13 = 0.13f;
matrix.m21 = 0.21f;
matrix.m22 = 0.22f;
matrix.m23 = 0.23f;
matrix.m31 = 0.31f;
matrix.m32 = 0.32f;
matrix.m33 = 0.33f;
return(matrix);
}
public void TestNativeManagedEqualityVec3by3()
{
//managed
Vec3 right = new Vec3(1f, 0f, 0f);
Vec3 forward = new Vec3(0f, 1f, 0f);
Vec3 up = new Vec3(0f, 0f, 1f);
#pragma warning disable 618 //disable warning as usage is intended to test obsolete constructor
Quaternion quat = new Quaternion(right, forward, up);
#pragma warning restore 618 //enable warning
//native
Matrix33 matrix33 = Matrix33.CreateFromVectors(right, forward, up);
Quat quat2 = Quat.CreateQuatFromMatrix(matrix33);
Assert.IsTrue(quat == quat2);
}
public void TestIdentity()
{
Assert.That(true, Is.EqualTo(Matrix3x3.Identity == Matrix33.CreateIdentity()));
Matrix3x3 managedmatrix = Matrix3x3.Identity;
Assert.That(true, Is.EqualTo(managedmatrix == Matrix3x3.Identity));
Assert.That(managedmatrix.m00, Is.EqualTo(1));
Assert.That(managedmatrix.m01, Is.EqualTo(0));
Assert.That(managedmatrix.m02, Is.EqualTo(0));
Assert.That(managedmatrix.m10, Is.EqualTo(0));
Assert.That(managedmatrix.m11, Is.EqualTo(1));
Assert.That(managedmatrix.m12, Is.EqualTo(0));
Assert.That(managedmatrix.m20, Is.EqualTo(0));
Assert.That(managedmatrix.m21, Is.EqualTo(0));
Assert.That(managedmatrix.m22, Is.EqualTo(1));
}
public void Multiply(ref Matrix33 a)
{
var t00 = (m00 * a.m00) + (m01 * a.m10) + (m02 * a.m20);
var t01 = (m00 * a.m01) + (m01 * a.m11) + (m02 * a.m21);
var t02 = (m00 * a.m02) + (m01 * a.m12) + (m02 * a.m22);
var t10 = (m10 * a.m00) + (m11 * a.m10) + (m12 * a.m20);
var t11 = (m10 * a.m01) + (m11 * a.m11) + (m12 * a.m21);
var t12 = (m10 * a.m02) + (m11 * a.m12) + (m12 * a.m22);
m00 = t00;
m01 = t01;
m02 = t02;
m10 = t10;
m11 = t11;
m12 = t12;
}
// a*bで初期化
public Matrix23(ref Matrix23 a, ref Matrix33 b)
{
var t00 = (a.m00 * b.m00) + (a.m01 * b.m10) + (a.m02 * b.m20);
var t01 = (a.m00 * b.m01) + (a.m01 * b.m11) + (a.m02 * b.m21);
var t02 = (a.m00 * b.m02) + (a.m01 * b.m12) + (a.m02 * b.m22);
var t10 = (a.m10 * b.m00) + (a.m11 * b.m10) + (a.m12 * b.m20);
var t11 = (a.m10 * b.m01) + (a.m11 * b.m11) + (a.m12 * b.m21);
var t12 = (a.m10 * b.m02) + (a.m11 * b.m12) + (a.m12 * b.m22);
m00 = t00;
m01 = t01;
m02 = t02;
m10 = t10;
m11 = t11;
m12 = t12;
}
public void ConversionFromManagedToNative()
{
Matrix3x3 managedmatrix = new Matrix3x3(0, 1, 2, 10, 11, 12, 20, 21, 22);
Matrix33 nativeMatrix = managedmatrix;
Assert.That(true, Is.EqualTo(managedmatrix == nativeMatrix));
Assert.That(nativeMatrix.m00, Is.EqualTo(managedmatrix.m00));
Assert.That(nativeMatrix.m01, Is.EqualTo(managedmatrix.m01));
Assert.That(nativeMatrix.m02, Is.EqualTo(managedmatrix.m02));
Assert.That(nativeMatrix.m10, Is.EqualTo(managedmatrix.m10));
Assert.That(nativeMatrix.m11, Is.EqualTo(managedmatrix.m11));
Assert.That(nativeMatrix.m12, Is.EqualTo(managedmatrix.m12));
Assert.That(nativeMatrix.m20, Is.EqualTo(managedmatrix.m20));
Assert.That(nativeMatrix.m21, Is.EqualTo(managedmatrix.m21));
Assert.That(nativeMatrix.m22, Is.EqualTo(managedmatrix.m22));
Assert.IsTrue(managedmatrix == nativeMatrix);
}
/// <summary>
/// Creates new quaternion that represents rotation defined by given 3x3 matrix.
/// </summary>
/// <param name="m">Matrix that defines the rotation.</param>
/// <returns>
/// Quaternion that represents rotation defined by given 3x3 matrix.
/// </returns>
public static Quaternion FromMatrix33(Matrix33 m)
{
float s, p, tr = m.M00 + m.M11 + m.M22;
//check the diagonal
if (tr > (float)0.0)
{
s = (float)Math.Sqrt(tr + 1.0f); p = 0.5f / s;
return new Quaternion(s * 0.5f, (m.M21 - m.M12) * p, (m.M02 - m.M20) * p, (m.M10 - m.M01) * p);
}
//diagonal is negative. now we have to find the biggest element on the diagonal
//check if "M00" is the biggest element
if ((m.M00 >= m.M11) && (m.M00 >= m.M22))
{
s = (float)Math.Sqrt(m.M00 - m.M11 - m.M22 + 1.0f); p = 0.5f / s;
return new Quaternion((m.M21 - m.M12) * p, s * 0.5f, (m.M10 + m.M01) * p, (m.M20 + m.M02) * p);
}
//check if "M11" is the biggest element
if ((m.M11 >= m.M00) && (m.M11 >= m.M22))
{
s = (float)Math.Sqrt(m.M11 - m.M22 - m.M00 + 1.0f); p = 0.5f / s;
return new Quaternion((m.M02 - m.M20) * p, (m.M01 + m.M10) * p, s * 0.5f, (m.M21 + m.M12) * p);
}
//check if "M22" is the biggest element
if ((m.M22 >= m.M00) && (m.M22 >= m.M11))
{
s = (float)Math.Sqrt(m.M22 - m.M00 - m.M11 + 1.0f); p = 0.5f / s;
return new Quaternion((m.M10 - m.M01) * p, (m.M02 + m.M20) * p, (m.M12 + m.M21) * p, s * 0.5f);
}
return Quaternion.Identity; // if it ends here, then we have no valid rotation matrix
}
/// <summary>
/// Creates new instance of <see cref="Quaternion"/> struct.
/// </summary>
/// <param name="matrix">3x3 matrix that represents rotation.</param>
public Quaternion(Matrix33 matrix)
{
this = FromMatrix33(matrix);
}
