コード例 #1
0
        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);
        }
コード例 #2
0
        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));
        }
コード例 #3
0
        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);
        }
コード例 #4
0
        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);
        }
コード例 #5
0
        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);
        }
コード例 #6
0
        /// <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);
            }
        }
コード例 #7
0
 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));
 }
コード例 #8
0
 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);
 }
コード例 #9
0
        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);
        }
コード例 #10
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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);

      }
コード例 #11
0
        /// <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));
        }
コード例 #12
0
        /// <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);
            }
        }
コード例 #13
0
 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)));
 }
コード例 #14
0
ファイル: CubeLocationMark.cs プロジェクト: tuandnvn/ecat
        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]));
            }
        }
コード例 #15
0
        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));
        }
コード例 #16
0
        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));
        }
コード例 #17
0
        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);
        }
コード例 #18
0
        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");
        }
コード例 #19
0
    //------------------------------------------------------------------------------------------------------------------------------------
    // 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");
                }
            }
        }
    }
コード例 #20
0
        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);
        }
コード例 #21
0
ファイル: CubeLocationMark.cs プロジェクト: tuandnvn/ecat
 public CubeLocationMark(int frameNo, Point3 center, Quaternions quaternion) : base(frameNo)
 {
     this.center = center;
     this.quaternion = quaternion;
 }
コード例 #22
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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);
      }
コード例 #23
0
ファイル: Multiplier.cs プロジェクト: ajlopez/TddRocks
 public int Multiply(Quaternions x, Quaternions y)
 {
     return table[x][y];
 }
コード例 #24
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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);

      }
コード例 #25
0
ファイル: AutoTestVarious.cs プロジェクト: samuto/UnityOpenCV
        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);
        }
コード例 #26
0
ファイル: AutoTestVarious.cs プロジェクト: samuto/UnityOpenCV
        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));
        }
コード例 #27
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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));
      }
コード例 #28
0
ファイル: AutoTestVarious.cs プロジェクト: samuto/UnityOpenCV
        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);
        }
コード例 #29
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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");
      }
コード例 #30
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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);

      }
コード例 #31
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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);
      }
コード例 #32
0
ファイル: AutoTestQuaternions.cs プロジェクト: Delaley/emgucv
      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));

      }
コード例 #33
0
        /// <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);
        }