public void QuaternionDCreateFromYawPitchRollTest2() { const double step = 35.0f; for (double yawAngle = -720.0f; yawAngle <= 720.0f; yawAngle += step) { for (double pitchAngle = -720.0f; pitchAngle <= 720.0f; pitchAngle += step) { for (double rollAngle = -720.0f; rollAngle <= 720.0f; rollAngle += step) { double yawRad = MathHelper.ToRadians(yawAngle); double pitchRad = MathHelper.ToRadians(pitchAngle); double rollRad = MathHelper.ToRadians(rollAngle); QuaternionD yaw = QuaternionD.CreateFromAxisAngle(Vector3D.UnitY, yawRad); QuaternionD pitch = QuaternionD.CreateFromAxisAngle(Vector3D.UnitX, pitchRad); QuaternionD roll = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, rollRad); QuaternionD expected = yaw * pitch * roll; QuaternionD actual = QuaternionD.CreateFromYawPitchRoll(yawRad, pitchRad, rollRad); Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.QuaternionDCreateFromYawPitchRollTest2 Yaw:{yawAngle} Pitch:{pitchAngle} Roll:{rollAngle} did not return the expected value: expected {expected} actual {actual}"); } } } }
public void Rotation() { var positionAndOrientation = new MyPositionAndOrientation( new SerializableVector3D(10.0d, -10.0d, -2.5d), new SerializableVector3(0.0f, 0.0f, -1.0f), new SerializableVector3(0.0f, 1.0f, 0.0f)); // -90 around Z var quaternion = Quaternion.CreateFromYawPitchRoll(0, 0, -VRageMath.MathHelper.PiOver2); var o = positionAndOrientation.ToQuaternion() * quaternion; var on = Quaternion.Normalize(o); Matrix m = on.ToMatrix(); m = Matrix.Round(ref m); var p = new MyPositionAndOrientation(m); var quaternion2 = QuaternionD.CreateFromYawPitchRoll(0, 0, -Math.PI / 2); var o2 = positionAndOrientation.ToQuaternionD() * quaternion2; var on2 = QuaternionD.Normalize(o2); var p2 = new MyPositionAndOrientation(on2.ToMatrixD()); var quaternion3 = new System.Windows.Media.Media3D.Quaternion(new System.Windows.Media.Media3D.Vector3D(0, 0, 1), -90d); var x3 = positionAndOrientation.ToQuaternionD(); var o3 = new System.Windows.Media.Media3D.Quaternion(x3.X, x3.Y, x3.Z, x3.W) * quaternion3; var on3 = o3; on3.Normalize(); double num = on3.X * on3.X; double num3 = on3.Z * on3.Z; double num4 = on3.X * on3.Y; double num5 = on3.Z * on3.W; double num8 = on3.Y * on3.Z; double num9 = on3.X * on3.W; var M21 = (2.0d * (num4 - num5)); var M22 = (1.0d - 2.0d * (num3 + num)); var M23 = (2.0d * (num8 + num9)); var up3 = new Vector3D(M21, M22, M23); var fwd = new SerializableVector3(0.0f, 0.0f, -1.0f); var up = new SerializableVector3(1.0f, 0.0f, 0.0f); Assert.AreEqual(fwd.X, p.Forward.X, "Forward.X Should Equal"); Assert.AreEqual(fwd.Y, p.Forward.Y, "Forward.Y Should Equal"); Assert.AreEqual(fwd.Z, p.Forward.Z, "Forward.Z Should Equal"); Assert.AreEqual(up.X, p.Up.X, "Up.X Should Equal"); Assert.AreEqual(up.Y, p.Up.Y, "Up.Y Should Equal"); Assert.AreEqual(up.Z, p.Up.Z, "Up.Z Should Equal"); }
public void QuaternionDCreateFromYawPitchRollTest1() { double yawAngle = MathHelper.ToRadians(30.0f); double pitchAngle = MathHelper.ToRadians(40.0f); double rollAngle = MathHelper.ToRadians(50.0f); QuaternionD yaw = QuaternionD.CreateFromAxisAngle(Vector3D.UnitY, yawAngle); QuaternionD pitch = QuaternionD.CreateFromAxisAngle(Vector3D.UnitX, pitchAngle); QuaternionD roll = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, rollAngle); QuaternionD expected = yaw * pitch * roll; QuaternionD actual = QuaternionD.CreateFromYawPitchRoll(yawAngle, pitchAngle, rollAngle); Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.QuaternionDCreateFromYawPitchRollTest1 did not return the expected value: expected {expected} actual {actual}"); }