示例#1
0
 public static Quat MultiplyQuaternions(Quat p, Quat q)
 {
     return new Quat(
         p.w * q.x + p.x * q.w + p.y * q.z - p.z * q.y,
         p.w * q.y - p.x * q.z + p.y * q.w + p.z * q.x,
         p.w * q.z + p.x * q.y - p.y * q.x + p.z * q.w,
         p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z
     );
 }
示例#2
0
 string CreateMeshEntity(Vector position, Quat orientation, Vector scale, MeshResource mesh)
 {
     Entity entity = new Entity();
     entity["location"]["position"].Suggest(position);
     entity["location"]["orientation"].Suggest(orientation);
     entity["mesh"]["scale"].Suggest(scale);
     entity["mesh"]["uri"].Suggest(mesh.uri);
     entity["mesh"]["visible"].Suggest(mesh.visible);
     World.Instance.Add(entity);
     return entity.Guid.ToString ();
 }
示例#3
0
 public static Vector AxisFromQuaternion(Quat q)
 {
     if (q.w * q.w != 1)
     {
         return new Vector(
             q.x / ((float)System.Math.Sqrt(1 - q.w * q.w)),
             q.y / ((float)System.Math.Sqrt(1 - q.w * q.w)),
             q.z / ((float)System.Math.Sqrt(1 - q.w * q.w))
         );
     }
     else
     {
         return new Vector(0, 0, 0);
     }
 }
示例#4
0
 public static float AngleFromQuaternion(Quat q)
 {
     return 2.0f * (float)System.Math.Acos(q.w);
 }
        private void UpdateOrientation(string guid, Quat orientation, int timestamp)
        {
            var entity = World.Instance.FindEntity(guid);
            entity["location"]["orientation"].Suggest(orientation);

            // We currently ignore timestamp, but may it in the future to implement dead reckoning.
        }
示例#6
0
 public static float AngleFromQuaternion(Quat q)
 {
     return(2.0f * (float)System.Math.Acos(q.w));
 }
示例#7
0
        private void setFrameQuaternion(string jointName, string x, string y, string z)
        {
            Quat q = new Quat(0, 0, 0, 1);
            Vector xAxis = new Vector(1, 0, 0);
            Vector yAxis = new Vector(0, 1, 0);
            Vector zAxis = new Vector(0, 0, 1);

            Vector rotData = new Vector(0, 0, 0);

            double angle1 = System.Math.PI * double.Parse(x, CultureInfo.InvariantCulture) / 180.0;
            double angle2 = System.Math.PI * double.Parse(y, CultureInfo.InvariantCulture) / 180.0;
            double angle3 = System.Math.PI * double.Parse(z, CultureInfo.InvariantCulture) / 180.0;

            if (Joints[jointName].channelOrder.Contains("XrotationYrotationZrotation")) //XYZ
            {
                q = FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle1);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle2));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle3));
                rotData = new Vector((float)angle1, (float)angle2, (float)angle3);
            }
            else if (Joints[jointName].channelOrder.Contains("XrotationZrotationYrotation")) //XZY
            {
                q = FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle1);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle2));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle3));
                rotData = new Vector((float)angle1, (float)angle3, (float)angle2);
            }
            else if (Joints[jointName].channelOrder.Contains("YrotationXrotationZrotation")) //YXZ
            {
                q = FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle1);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle2));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle3));
                rotData = new Vector((float)angle2, (float)angle1, (float)angle3);
            }
            else if (Joints[jointName].channelOrder.Contains("YrotationZrotationXrotation")) //YZX
            {
                q = FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle1);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle2));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle3));
                rotData = new Vector((float)angle2, (float)angle3, (float)angle1);
            }
            else if (Joints[jointName].channelOrder.Contains("ZrotationYrotationXrotation")) //ZYX
            {
                q = FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle1);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle2));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle3));
                rotData = new Vector((float)angle3, (float)angle2, (float)angle1);
            }
            else if (Joints[jointName].channelOrder.Contains("ZrotationXrotationYrotation")) //ZXY
            {
                q = FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle1);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle2));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle3));
                rotData = new Vector((float)angle3, (float)angle1, (float)angle2);
            }

            //Console.WriteLine("quaternion: " + q.x + "," + q.y + "," + q.z + "," + q.w);
            //Console.ReadLine();

            Joints[jointName].quaternionData.Add(q);
            Joints[jointName].rotationData.Add(rotData);
        }
示例#8
0
        private void applyAdditionalJointControl(string jointName, int index, float x, float y, float z)
        {
            Quat q = new Quat(0, 0, 0, 1);
            Vector xAxis = new Vector(1, 0, 0);
            Vector yAxis = new Vector(0, 1, 0);
            Vector zAxis = new Vector(0, 0, 1);

            if (Joints[jointName].channelOrder.Contains("XrotationYrotationZrotation")) //XYZ
            {
                q = FIVES.Math.QuaternionFromAxisAngle(xAxis, x);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
            }
            else if (Joints[jointName].channelOrder.Contains("XrotationZrotationYrotation")) //XZY
            {
                q = FIVES.Math.QuaternionFromAxisAngle(xAxis, x);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
            }
            else if (Joints[jointName].channelOrder.Contains("YrotationXrotationZrotation")) //YXZ
            {
                q = FIVES.Math.QuaternionFromAxisAngle(yAxis, y);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
            }
            else if (Joints[jointName].channelOrder.Contains("YrotationZrotationXrotation")) //YZX
            {
                q = FIVES.Math.QuaternionFromAxisAngle(yAxis, y);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
            }
            else if (Joints[jointName].channelOrder.Contains("ZrotationYrotationXrotation")) //ZYX
            {
                q = FIVES.Math.QuaternionFromAxisAngle(zAxis, z);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
            }
            else if (Joints[jointName].channelOrder.Contains("ZrotationXrotationYrotation")) //ZXY
            {
                q = FIVES.Math.QuaternionFromAxisAngle(zAxis, z);
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
                q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
            }

            Joints[jointName].quaternionData[index] = q;
        }