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); }