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