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
);
}
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 ();
}
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);
}
}
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.
}
public static float AngleFromQuaternion(Quat q)
{
return(2.0f * (float)System.Math.Acos(q.w));
}
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);
}
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;
}