public void TestQuaternions1()
{
Quaternions q = new Quaternions();
double epsilon = 1.0e-10;
Matrix <double> point = new Matrix <double>(3, 1);
point.SetRandNormal(new MCvScalar(), new MCvScalar(20));
using (Matrix <double> pt1 = new Matrix <double>(3, 1))
using (Matrix <double> pt2 = new Matrix <double>(3, 1))
using (Matrix <double> pt3 = new Matrix <double>(3, 1))
{
double x1 = 1.0, y1 = 0.2, z1 = 0.1;
double x2 = 0.0, y2 = 0.0, z2 = 0.0;
q.SetEuler(x1, y1, z1);
q.GetEuler(ref x2, ref y2, ref z2);
EmguAssert.IsTrue(
Math.Abs(x2 - x1) < epsilon &&
Math.Abs(y2 - y1) < epsilon &&
Math.Abs(z2 - z1) < epsilon);
q.RotatePoints(point, pt1);
Matrix <double> rMat = new Matrix <double>(3, 3);
q.GetRotationMatrix(rMat);
CvInvoke.Gemm(rMat, point, 1.0, null, 0.0, pt2, Emgu.CV.CvEnum.GemmType.Default);
CvInvoke.AbsDiff(pt1, pt2, pt3);
EmguAssert.IsTrue(
pt3[0, 0] < epsilon &&
pt3[1, 0] < epsilon &&
pt3[2, 0] < epsilon);
}
double rotationAngle = 0.2;
q.SetEuler(rotationAngle, 0.0, 0.0);
EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon);
q.SetEuler(0.0, rotationAngle, 0.0);
EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon);
q.SetEuler(0.0, 0.0, rotationAngle);
EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon);
q = q * q;
EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - rotationAngle) < epsilon);
q.SetEuler(0.2, 0.1, 0.05);
double t = q.RotationAngle;
q = q * q;
EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - t) < epsilon);
}
public void TestQuaternions1()
{
Quaternions q = new Quaternions();
double epsilon = 1.0e-10;
Matrix<double> point = new Matrix<double>(3, 1);
point.SetRandNormal(new MCvScalar(), new MCvScalar(20));
using (Matrix<double> pt1 = new Matrix<double>(3, 1))
using (Matrix<double> pt2 = new Matrix<double>(3, 1))
using (Matrix<double> pt3 = new Matrix<double>(3, 1))
{
double x1 = 1.0, y1 = 0.2, z1 = 0.1;
double x2 = 0.0, y2 = 0.0, z2 = 0.0;
q.SetEuler(x1, y1, z1);
q.GetEuler(ref x2, ref y2, ref z2);
EmguAssert.IsTrue(
Math.Abs(x2 - x1) < epsilon &&
Math.Abs(y2 - y1) < epsilon &&
Math.Abs(z2 - z1) < epsilon);
q.RotatePoints(point, pt1);
Matrix<double> rMat = new Matrix<double>(3, 3);
q.GetRotationMatrix(rMat);
CvInvoke.Gemm(rMat, point, 1.0, null, 0.0, pt2, Emgu.CV.CvEnum.GemmType.Default);
CvInvoke.AbsDiff(pt1, pt2, pt3);
EmguAssert.IsTrue(
pt3[0, 0] < epsilon &&
pt3[1, 0] < epsilon &&
pt3[2, 0] < epsilon);
}
double rotationAngle = 0.2;
q.SetEuler(rotationAngle, 0.0, 0.0);
EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon);
q.SetEuler(0.0, rotationAngle, 0.0);
EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon);
q.SetEuler(0.0, 0.0, rotationAngle);
EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon);
q = q * q;
EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - rotationAngle) < epsilon);
q.SetEuler(0.2, 0.1, 0.05);
double t = q.RotationAngle;
q = q * q;
EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - t) < epsilon);
}
public void TestQuaternionEulerAngleAndRotate()
{
double epsilon = 1.0e-12;
Random r = new Random();
Quaternions q1 = new Quaternions();
double roll1 = r.NextDouble(), pitch1 = r.NextDouble(), yaw1 = r.NextDouble();
double roll2 = 0, pitch2 = 0, yaw2 = 0;
q1.SetEuler(roll1, pitch1, yaw1);
q1.GetEuler(ref roll2, ref pitch2, ref yaw2);
EmguAssert.IsTrue(Math.Abs(roll1 - roll2) < epsilon);
EmguAssert.IsTrue(Math.Abs(pitch1 - pitch2) < epsilon);
EmguAssert.IsTrue(Math.Abs(yaw1 - yaw2) < epsilon);
Quaternions q2 = new Quaternions();
q2.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
MCvPoint3D64f p = new MCvPoint3D64f(r.NextDouble() * 10, r.NextDouble() * 10, r.NextDouble() * 10);
MCvPoint3D64f delta = (q1 * q2).RotatePoint(p) - q1.RotatePoint(q2.RotatePoint(p));
EmguAssert.IsTrue(delta.X < epsilon);
EmguAssert.IsTrue(delta.Y < epsilon);
EmguAssert.IsTrue(delta.Z < epsilon);
}
public void TestQuaternionsMultiplicationPerformance()
{
Quaternions q = new Quaternions();
Random r = new Random();
q.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
Stopwatch watch = Stopwatch.StartNew();
Quaternions sum = Quaternions.Empty;
for (int i = 0; i < 1000000; i++)
{
sum *= q;
}
watch.Stop();
EmguAssert.WriteLine(String.Format("Time used: {0} milliseconds", watch.ElapsedMilliseconds));
}
public void TestQuaternionsPerformance()
{
Quaternions q = new Quaternions();
Random r = new Random();
q.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
Stopwatch watch = Stopwatch.StartNew();
double counter = 0.0;
for (int i = 0; i < 1000000; i++)
{
Quaternions q2 = q * q;
counter += q2.W;
}
watch.Stop();
Trace.WriteLine(String.Format("Time used: {0} milliseconds", watch.ElapsedMilliseconds));
}
public void TestQuaternion2()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
MCvPoint3D64f p = new MCvPoint3D64f(r.NextDouble() * 10, r.NextDouble() * 10, r.NextDouble() * 10);
MCvPoint3D64f delta = (q1 * q2).RotatePoint(p) - q1.RotatePoint(q2.RotatePoint(p));
double epsilon = 1.0e-8;
Assert.Less(delta.x, epsilon);
Assert.Less(delta.y, epsilon);
Assert.Less(delta.z, epsilon);
}
public void TestQuaternionsMultiplicationPerformance()
{
Quaternions q = new Quaternions();
Random r = new Random();
q.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
Stopwatch watch = Stopwatch.StartNew();
Quaternions sum = Quaternions.Empty;
for (int i = 0; i < 1000000; i++)
{
sum *= q;
}
watch.Stop();
EmguAssert.WriteLine(String.Format("Time used: {0} milliseconds", watch.ElapsedMilliseconds));
}
public void TestQuaternionEulerAngleAndRotate()
{
double epsilon = 1.0e-12;
Random r = new Random();
Quaternions q1 = new Quaternions();
double roll1 = r.NextDouble(), pitch1 = r.NextDouble(), yaw1 = r.NextDouble();
double roll2 = 0, pitch2 = 0, yaw2 = 0;
q1.SetEuler(roll1, pitch1, yaw1);
q1.GetEuler(ref roll2, ref pitch2, ref yaw2);
EmguAssert.IsTrue(Math.Abs(roll1 - roll2) < epsilon);
EmguAssert.IsTrue(Math.Abs(pitch1 - pitch2) < epsilon);
EmguAssert.IsTrue(Math.Abs(yaw1 - yaw2) < epsilon);
Quaternions q2 = new Quaternions();
q2.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
MCvPoint3D64f p = new MCvPoint3D64f(r.NextDouble() * 10, r.NextDouble() * 10, r.NextDouble() * 10);
MCvPoint3D64f delta = (q1 * q2).RotatePoint(p) - q1.RotatePoint(q2.RotatePoint(p));
EmguAssert.IsTrue(delta.X < epsilon);
EmguAssert.IsTrue(delta.Y < epsilon);
EmguAssert.IsTrue(delta.Z < epsilon);
}