public void TestQuaternionsSlerp5()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(0.0, 355.0 / 180 * Math.PI, 0.0);
Quaternions q2 = new Quaternions();
q2.AxisAngle = new MCvPoint3D64f(0.0, 5.0 / 180 * Math.PI, 0.0);
double epsilon = 1.0e-12;
double x = 0, y = 0, z = 0;
Quaternions q = q1.Slerp(q2, 0.5);
q.GetEuler(ref x, ref y, ref z);
EmguAssert.IsFalse(double.IsNaN(x));
EmguAssert.IsFalse(double.IsNaN(y));
EmguAssert.IsFalse(double.IsNaN(z));
double deltaDegree = Math.Abs(y / Math.PI * 180.0 - 0.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
q = q2.Slerp(q1, 0.5);
q.GetEuler(ref x, ref y, ref z);
EmguAssert.IsFalse(double.IsNaN(x));
EmguAssert.IsFalse(double.IsNaN(y));
EmguAssert.IsFalse(double.IsNaN(z));
deltaDegree = Math.Abs(y / Math.PI * 180.0 - 0.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
}
public void TestQuaternion3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = q1.AxisAngle;
double epsilon = 1.0e-8;
EmguAssert.IsTrue(Math.Abs(q1.W - q2.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.X - q2.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Y - q2.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Z - q2.Z) < epsilon);
RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.X, q1.AxisAngle.Y, q1.AxisAngle.Z });
Mat m1 = rVec.RotationMatrix;
Matrix <double> m2 = new Matrix <double>(3, 3);
q1.GetRotationMatrix(m2);
Matrix <double> diff = new Matrix <double>(3, 3);
CvInvoke.AbsDiff(m1, m2, diff);
double norm = CvInvoke.Norm(diff, Emgu.CV.CvEnum.NormType.C);
EmguAssert.IsTrue(norm < epsilon);
Quaternions q4 = q1 * Quaternions.Empty;
//EmguAssert.IsTrue(q4.Equals(q1));
}
public void TestQuaternionsSlerp3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(0.0, 30.0 / 180 * Math.PI, 0.0);
Quaternions q2 = new Quaternions();
q2.AxisAngle = new MCvPoint3D64f(0.0, 40.0 / 180 * Math.PI, 0.0);
double epsilon = 1.0e-12;
double x = 0, y = 0, z = 0;
Quaternions q = q1.Slerp(q2, 0.5);
q.GetEuler(ref x, ref y, ref z);
double deltaDegree = Math.Abs(y / Math.PI * 180.0 - 35.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
q = q1.Slerp(q2, 0.8);
q.GetEuler(ref x, ref y, ref z);
deltaDegree = Math.Abs(y / Math.PI * 180.0 - 38.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
q = q1.Slerp(q2, 0.15);
q.GetEuler(ref x, ref y, ref z);
deltaDegree = Math.Abs(y / Math.PI * 180.0 - 31.5);
EmguAssert.IsTrue(deltaDegree <= epsilon);
}
public void TestQuaternionsSlerp1()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
double epsilon = 1.0e-12;
Quaternions q = q1.Slerp(q2, 0.0);
EmguAssert.IsTrue(Math.Abs(q1.W - q.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.X - q.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Y - q.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Z - q.Z) < epsilon);
q = q1.Slerp(q2, 1.0);
EmguAssert.IsTrue(Math.Abs(q2.W - q.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q2.X - q.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q2.Y - q.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q2.Z - q.Z) < 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);
}
/// <summary>
/// Writes TransformSyncData into a writer.
/// </summary>
/// <param name="writer"></param>
/// <param name="syncData"></param>
public static void WriteTransformSyncData(this NetworkWriter writer, TransformSyncData syncData)
{
//SyncProperties.
SyncProperties sp = (SyncProperties)syncData.SyncProperties;
writer.WriteByte(syncData.SyncProperties);
//NetworkIdentity.
//Get compression level for netIdentity.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Id1))
{
writer.WriteByte((byte)syncData.NetworkIdentity);
}
else if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Id2))
{
writer.WriteUInt16((ushort)syncData.NetworkIdentity);
}
else
{
writer.WriteUInt32(syncData.NetworkIdentity);
}
//ComponentIndex.
writer.WriteByte(syncData.ComponentIndex);
//Position.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Position))
{
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.CompressSmall))
{
WriteCompressedVector3(writer, syncData.Position);
}
else
{
writer.WriteVector3(syncData.Position);
}
}
//Rotation.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Rotation))
{
writer.WriteUInt32(Quaternions.CompressQuaternion(syncData.Rotation));
}
//Scale.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Scale))
{
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.CompressSmall))
{
WriteCompressedVector3(writer, syncData.Scale);
}
else
{
writer.WriteVector3(syncData.Scale);
}
}
//Platform.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Platform))
{
writer.WriteUInt32(syncData.PlatformNetId);
}
}
public override bool OnSerialize(NetworkWriter writer, bool initialState)
{
if (initialState)
{
writer.WriteVector3(TargetTransform.GetPosition(base.UseLocalSpace));
writer.WriteUInt32(Quaternions.CompressQuaternion(TargetTransform.GetRotation(base.UseLocalSpace)));
writer.WriteVector3(TargetTransform.GetScale());
}
return(base.OnSerialize(writer, initialState));
}
public override void OnDeserialize(NetworkReader reader, bool initialState)
{
if (initialState)
{
TargetTransform.SetPosition(base.UseLocalSpace, reader.ReadVector3());
TargetTransform.SetRotation(base.UseLocalSpace, Quaternions.DecompressQuaternion(reader.ReadUInt32()));
TargetTransform.SetScale(reader.ReadVector3());
}
base.OnDeserialize(reader, initialState);
}
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);
}
/// <summary>
/// Reads a compressed quaternion from a reader.
/// </summary>
/// <param name="reader"></param>
/// <returns></returns>
public static Quaternion ReadCompressedQuaternion(NetworkReader reader)
{
byte largest = reader.ReadByte();
short a = 0, b = 0, c = 0;
if (!Quaternions.UseLargestOnly(largest))
{
a = reader.ReadInt16();
b = reader.ReadInt16();
c = reader.ReadInt16();
}
return(Quaternions.DecompressQuaternion(largest, a, b, c));
}
/// <summary>
/// Writes a compressed quaternion to a writer.
/// </summary>
/// <param name="writer"></param>
/// <param name="rotation"></param>
public static void WriteCompressedQuaternion(NetworkWriter writer, Quaternion rotation)
{
byte largest;
short a, b, c;
Quaternions.CompressQuaternion(rotation, out largest, out a, out b, out c);
writer.WriteByte(largest);
if (!Quaternions.UseLargestOnly(largest))
{
writer.WriteInt16(a);
writer.WriteInt16(b);
writer.WriteInt16(c);
}
}
public override string ToString()
{
return(string.Format("{{ Index: {0}, Name: {1}, Gender: {2}, Quaternions: [{3}], Colors: [{4}] }}",
Index,
Name,
Gender,
Quaternions == null
? null
: Quaternions.Aggregate(string.Empty,
(s, n) => string.IsNullOrEmpty(s) ? n.ToString() : s + ", " + n),
Colors == null
? null
: Colors.Aggregate(string.Empty,
(s, n) => string.IsNullOrEmpty(s) ? n.ToString() : s + ", " + n)));
}
public override void readFromXml(XmlNode xmlNode)
{
XmlNode centerNode = xmlNode.SelectSingleNode(CENTER);
String[] parts = centerNode.InnerText.Split(',');
if (parts.Length == 3)
{
center = new Point3(float.Parse(parts[0]), float.Parse(parts[1]), float.Parse(parts[2]));
}
XmlNode quaternionsNode = xmlNode.SelectSingleNode(QUATERNIONS);
parts = quaternionsNode.InnerText.Split(',');
if (parts.Length == 4)
{
quaternion = new Quaternions(float.Parse(parts[0]), float.Parse(parts[1]), float.Parse(parts[2]), float.Parse(parts[3]));
}
}
public override bool OnSerialize(NetworkWriter writer, bool initialState)
{
if (initialState)
{
/* If root then no need to send transform data as that's already
* handled in the spawn message. */
if (transform.root == null)
{
return(base.OnSerialize(writer, initialState));
}
writer.WriteVector3(TargetTransform.GetPosition(base.UseLocalSpace));
writer.WriteUInt32(Quaternions.CompressQuaternion(TargetTransform.GetRotation(base.UseLocalSpace)));
writer.WriteVector3(TargetTransform.GetScale());
}
return(base.OnSerialize(writer, initialState));
}
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 override void OnDeserialize(NetworkReader reader, bool initialState)
{
if (initialState)
{
/* If root then no need to read transform data as that's already
* handled in the spawn message. */
if (transform.root == null)
{
base.OnDeserialize(reader, initialState);
return;
}
TargetTransform.SetPosition(base.UseLocalSpace, reader.ReadVector3());
TargetTransform.SetRotation(base.UseLocalSpace, Quaternions.DecompressQuaternion(reader.ReadUInt32()));
TargetTransform.SetScale(reader.ReadVector3());
}
base.OnDeserialize(reader, initialState);
}
public void TestAxisAngleCompose()
{
MCvPoint3D64f angle1 = new MCvPoint3D64f(4.1652539565753417e-022, -9.4229054916424228e-022, 5.1619136559035708e-008);
MCvPoint3D64f angle2 = new MCvPoint3D64f(4.3209729769679014e-023, 3.2042397847543764e-023, -6.4083339340765912e-008);
Quaternions q1 = new Quaternions();
q1.AxisAngle = angle1;
Quaternions q2 = new Quaternions();
q2.AxisAngle = angle2;
Quaternions q = q1 * q2;
MCvPoint3D64f angle = q.AxisAngle;
EmguAssert.AreNotEqual(double.NaN, angle.X, "Invalid value x");
EmguAssert.AreNotEqual(double.NaN, angle.Y, "Invalid value y");
EmguAssert.AreNotEqual(double.NaN, angle.Z, "Invalid value z");
}
//------------------------------------------------------------------------------------------------------------------------------------
// Backwards Compatibility Methods
#region BackwardsComp
/// <summary> Manually assign IMU Correction for old firmware versions. </summary>
private void SetupWrist()
{
if (this.glove != null && this.glove.gloveData.dataLoaded)
{
string ID = this.gloveData.deviceID;
if (ID.Contains("220102"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quaternions.FromEuler(Mathf.PI, 0, Mathf.PI / 2.0f)); //correction for glove 1
SenseGlove_Debugger.Log("Red Glove Compensation");
}
string[] gloveVersion = this.gloveData.firmwareVersion.Split('.');
if (gloveVersion[0][0] == 'v')
{
gloveVersion[0] = gloveVersion[0].Substring(1);
} //if there is a v in front of it, remove this.
int mainVersion = int.Parse(gloveVersion[0]);
int subVersion = int.Parse(gloveVersion[1]);
if (mainVersion <= 2 && subVersion <= 19)
{
if (ID.Contains("120206"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quaternions.FromEuler(Mathf.PI / 2.0f, 0, Mathf.PI)); //correction for glove 1
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
else if (ID.Contains("120101"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quaternions.FromEuler(Mathf.PI, 0, 0)); //correction for glove 1
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
else if (ID.Contains("120203"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quaternions.FromEuler(0, 0, Mathf.PI / 2.0f)); //correction?
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
else if (ID.Contains("120307") || ID.Contains("120304") || ID.Contains("120310") || ID.Contains("120309") || ID.Contains("120311") || ID.Contains("120312"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quaternions.FromEuler(0, 0, Mathf.PI)); //correction for glove 7 & 4?
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
}
}
}
public static void Main(string[] args)
{
var managed_stopwatch = new Stopwatch();
var directx_stopwatch = new Stopwatch();
var quaternions = Quaternions.ToArray();
var vectors = Vectors.ToArray();
AppDomain.CurrentDomain.Load("DirectXOperations");
{
managed_stopwatch.Start();
for (int i = 0; i < Count; ++i)
{
var matrix = Matrix3D.Identity;
matrix.Rotate(quaternions[i]);
matrix.Translate(vectors[i]);
}
managed_stopwatch.Stop();
}
{
directx_stopwatch.Start();
for (int i = 0; i < Count; ++i)
{
quaternions[i].ComposeTransform(ref vectors[i]);
}
directx_stopwatch.Stop();
}
Console.WriteLine($"Managed run: { managed_stopwatch.Elapsed }");
Console.WriteLine($"DirectX run: { directx_stopwatch.Elapsed }");
Console.ReadKey(true);
}
public CubeLocationMark(int frameNo, Point3 center, Quaternions quaternion) : base(frameNo)
{
this.center = center;
this.quaternion = quaternion;
}
public void TestQuaternionsSlerp5()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(0.0, 355.0 / 180 * Math.PI, 0.0);
Quaternions q2 = new Quaternions();
q2.AxisAngle = new MCvPoint3D64f(0.0, 5.0 / 180 * Math.PI, 0.0);
double epsilon = 1.0e-12;
double x = 0, y = 0, z = 0;
Quaternions q = q1.Slerp(q2, 0.5);
q.GetEuler(ref x, ref y, ref z);
EmguAssert.IsFalse(double.IsNaN(x));
EmguAssert.IsFalse(double.IsNaN(y));
EmguAssert.IsFalse(double.IsNaN(z));
double deltaDegree = Math.Abs(y / Math.PI * 180.0 - 0.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
q = q2.Slerp(q1, 0.5);
q.GetEuler(ref x, ref y, ref z);
EmguAssert.IsFalse(double.IsNaN(x));
EmguAssert.IsFalse(double.IsNaN(y));
EmguAssert.IsFalse(double.IsNaN(z));
deltaDegree = Math.Abs(y / Math.PI * 180.0 - 0.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
}
public int Multiply(Quaternions x, Quaternions y)
{
return table[x][y];
}
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 TestQuaternion3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = q1.AxisAngle;
double epsilon = 1.0e-12;
Assert.Less(Math.Abs(q1.W - q2.W), epsilon);
Assert.Less(Math.Abs(q1.X - q2.X), epsilon);
Assert.Less(Math.Abs(q1.Y - q2.Y), epsilon);
Assert.Less(Math.Abs(q1.Z - q2.Z), epsilon);
RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.x, q1.AxisAngle.y, q1.AxisAngle.z });
Matrix<double> m1 = rVec.RotationMatrix;
Matrix<double> m2 = new Matrix<double>(3, 3);
q1.GetRotationMatrix(m2);
Matrix<double> diff = new Matrix<double>(3, 3);
CvInvoke.cvAbsDiff(m1, m2, diff);
double norm = CvInvoke.cvNorm(diff, IntPtr.Zero, Emgu.CV.CvEnum.NORM_TYPE.CV_C, IntPtr.Zero);
Assert.Less(norm, epsilon);
}
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 TestQuaternion3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = q1.AxisAngle;
double epsilon = 1.0e-8;
EmguAssert.IsTrue(Math.Abs(q1.W - q2.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.X - q2.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Y - q2.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Z - q2.Z) < epsilon);
RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.X, q1.AxisAngle.Y, q1.AxisAngle.Z });
Mat m1 = rVec.RotationMatrix;
Matrix<double> m2 = new Matrix<double>(3, 3);
q1.GetRotationMatrix(m2);
Matrix<double> diff = new Matrix<double>(3, 3);
CvInvoke.AbsDiff(m1, m2, diff);
double norm = CvInvoke.Norm(diff, Emgu.CV.CvEnum.NormType.C);
EmguAssert.IsTrue(norm < epsilon);
Quaternions q4 = q1 * Quaternions.Empty;
//EmguAssert.IsTrue(q4.Equals(q1));
}
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 TestAxisAngleCompose()
{
MCvPoint3D64f angle1 = new MCvPoint3D64f(4.1652539565753417e-022, -9.4229054916424228e-022, 5.1619136559035708e-008);
MCvPoint3D64f angle2 = new MCvPoint3D64f(4.3209729769679014e-023, 3.2042397847543764e-023, -6.4083339340765912e-008);
Quaternions q1 = new Quaternions();
q1.AxisAngle = angle1;
Quaternions q2 = new Quaternions();
q2.AxisAngle = angle2;
Quaternions q = q1 * q2;
MCvPoint3D64f angle = q.AxisAngle;
EmguAssert.AreNotEqual(double.NaN, angle.X, "Invalid value x");
EmguAssert.AreNotEqual(double.NaN, angle.Y, "Invalid value y");
EmguAssert.AreNotEqual(double.NaN, angle.Z, "Invalid value z");
}
public void TestQuaternionsSlerp1()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
double epsilon = 1.0e-12;
Quaternions q = q1.Slerp(q2, 0.0);
EmguAssert.IsTrue(Math.Abs(q1.W - q.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.X - q.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Y - q.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Z - q.Z) < epsilon);
q = q1.Slerp(q2, 1.0);
EmguAssert.IsTrue(Math.Abs(q2.W - q.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q2.X - q.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q2.Y - q.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q2.Z - q.Z) < epsilon);
}
public void TestQuaternionsSlerp3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(0.0, 30.0 / 180 * Math.PI, 0.0);
Quaternions q2 = new Quaternions();
q2.AxisAngle = new MCvPoint3D64f(0.0, 40.0 / 180 * Math.PI, 0.0);
double epsilon = 1.0e-12;
double x = 0, y = 0, z = 0;
Quaternions q = q1.Slerp(q2, 0.5);
q.GetEuler(ref x, ref y, ref z);
double deltaDegree = Math.Abs(y / Math.PI * 180.0 - 35.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
q = q1.Slerp(q2, 0.8);
q.GetEuler(ref x, ref y, ref z);
deltaDegree = Math.Abs(y / Math.PI * 180.0 - 38.0);
EmguAssert.IsTrue(deltaDegree <= epsilon);
q = q1.Slerp(q2, 0.15);
q.GetEuler(ref x, ref y, ref z);
deltaDegree = Math.Abs(y / Math.PI * 180.0 - 31.5);
EmguAssert.IsTrue(deltaDegree <= 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));
}
/// <summary>
/// Converts reader data into a new TransformSyncData.
/// </summary>
/// <param name="reader"></param>
/// <returns></returns>
public static TransformSyncData ReadTransformSyncData(this NetworkReader reader)
{
TransformSyncData syncData = new TransformSyncData();
//Sync properties.
SyncProperties sp = (SyncProperties)reader.ReadByte();
syncData.SyncProperties = (byte)sp;
//NetworkIdentity.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Id1))
{
syncData.NetworkIdentity = reader.ReadByte();
}
else if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Id2))
{
syncData.NetworkIdentity = reader.ReadUInt16();
}
else
{
syncData.NetworkIdentity = reader.ReadUInt32();
}
//ComponentIndex.
syncData.ComponentIndex = reader.ReadByte();
//Position.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Position))
{
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.CompressSmall))
{
syncData.Position = ReadCompressedVector3(reader);
}
else
{
syncData.Position = reader.ReadVector3();
}
}
//Rotation.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Rotation))
{
syncData.Rotation = Quaternions.DecompressQuaternion(reader.ReadUInt32());
}
//scale.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Scale))
{
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.CompressSmall))
{
syncData.Scale = ReadCompressedVector3(reader);
}
else
{
syncData.Scale = reader.ReadVector3();
}
}
//Platformed.
if (EnumContains.SyncPropertiesContains(sp, SyncProperties.Platform))
{
syncData.PlatformNetId = reader.ReadUInt32();
}
return(syncData);
}