void Update()
{
StateMutex.WaitOne();
if (Bodies_ != null && BodyAdjustments_ != null)
{
foreach (System.Collections.Generic.KeyValuePair <string, Body_> kv in Bodies_)
{
bool adjustmentMade = false;
foreach (System.Collections.Generic.KeyValuePair <string, Body_> adjustment in BodyAdjustments_)
{
if (kv.Key == adjustment.Key)
{
InputBroker.SetPosition(kv.Key, new Vector3(
kv.Value.translation.x + adjustment.Value.translation.x,
kv.Value.translation.y + adjustment.Value.translation.y,
kv.Value.translation.z + adjustment.Value.translation.z));
Quaternion q = new Quaternion(kv.Value.quaternion.x, kv.Value.quaternion.y, kv.Value.quaternion.z, kv.Value.quaternion.w) *
Quaternion.Euler(adjustment.Value.euler.p, adjustment.Value.euler.h, adjustment.Value.euler.r);
InputBroker.SetRotation(kv.Key, q);
adjustmentMade = true;
}
if (!adjustmentMade)
{
InputBroker.SetPosition(kv.Key, new Vector3(kv.Value.translation.x, kv.Value.translation.y, kv.Value.translation.z));
Quaternion q = new Quaternion(kv.Value.quaternion.x, kv.Value.quaternion.y, kv.Value.quaternion.z, kv.Value.quaternion.w);
InputBroker.SetRotation(kv.Key, q);
}
}
}
}
StateMutex.ReleaseMutex();
}
// Store InputSimulator values in InputBroker
void UpdateInputBroker()
{
// Update RiftDK1's Vicon
InputBroker.SetPosition(RiftDK1Vicon, new Vector3(RiftDK1X, RiftDK1Y, RiftDK1Z));
Quaternion OculusRiftQuaternion = Quaternion.Euler(RiftDK1P, RiftDK1H, RiftDK1R);
InputBroker.SetRotation(RiftDK1Vicon, OculusRiftQuaternion);
// Update RightGlove's Vicon
InputBroker.SetPosition(RightGloveVicon, new Vector3(RightGloveX, RightGloveY, RightGloveZ));
Quaternion RightGloveQuaternion = Quaternion.Euler(RightGloveP, RightGloveH, RightGloveR);
InputBroker.SetRotation(RightGloveVicon, RightGloveQuaternion);
// Update RightWiimote's Vicon
InputBroker.SetPosition(RightWiimoteVicon, new Vector3(RightWiimoteX, RightWiimoteY, RightWiimoteZ));
Quaternion RightWiimoteQuaternion = Quaternion.Euler(RightWiimoteP, RightWiimoteH, RightWiimoteR);
InputBroker.SetRotation(RightWiimoteVicon, RightWiimoteQuaternion);
// Update RightWiimote's Buttons
InputBroker.SetKey(RightWiimoteName + ":Up", RightWiimoteUp);
InputBroker.SetKey(RightWiimoteName + ":Down", RightWiimoteDown);
InputBroker.SetKey(RightWiimoteName + ":Left", RightWiimoteLeft);
InputBroker.SetKey(RightWiimoteName + ":Right", RightWiimoteRight);
InputBroker.SetKey(RightWiimoteName + ":A", RightWiimoteA);
InputBroker.SetKey(RightWiimoteName + ":B", RightWiimoteB);
InputBroker.SetKey(RightWiimoteName + ":Minus", RightWiimoteMinus);
InputBroker.SetKey(RightWiimoteName + ":Plus", RightWiimotePlus);
InputBroker.SetKey(RightWiimoteName + ":One", RightWiimote1);
InputBroker.SetKey(RightWiimoteName + ":Two", RightWiimote2);
// Update LeftGlove's Vicon
InputBroker.SetPosition(LeftGloveVicon, new Vector3(LeftGloveX, LeftGloveY, LeftGloveZ));
Quaternion LeftGloveQuaternion = Quaternion.Euler(LeftGloveP, LeftGloveH, LeftGloveR);
InputBroker.SetRotation(LeftGloveVicon, LeftGloveQuaternion);
// Update LeftWiimote's Vicon
InputBroker.SetPosition(LeftWiimoteVicon, new Vector3(LeftWiimoteX, LeftWiimoteY, LeftWiimoteZ));
Quaternion LeftWiimoteQuaternion = Quaternion.Euler(LeftWiimoteP, LeftWiimoteH, LeftWiimoteR);
InputBroker.SetRotation(LeftWiimoteVicon, LeftWiimoteQuaternion);
// Update LeftWiimote's Buttons
InputBroker.SetKey(LeftWiimoteName + ":Up", LeftWiimoteUp);
InputBroker.SetKey(LeftWiimoteName + ":Down", LeftWiimoteDown);
InputBroker.SetKey(LeftWiimoteName + ":Left", LeftWiimoteLeft);
InputBroker.SetKey(LeftWiimoteName + ":Right", LeftWiimoteRight);
InputBroker.SetKey(LeftWiimoteName + ":A", LeftWiimoteA);
InputBroker.SetKey(LeftWiimoteName + ":B", LeftWiimoteB);
InputBroker.SetKey(LeftWiimoteName + ":Minus", LeftWiimoteMinus);
InputBroker.SetKey(LeftWiimoteName + ":Plus", LeftWiimotePlus);
InputBroker.SetKey(LeftWiimoteName + ":One", LeftWiimote1);
InputBroker.SetKey(LeftWiimoteName + ":Two", LeftWiimote2);
// Update RightShoe's Vicon
InputBroker.SetPosition(RightShoeVicon, new Vector3(RightShoeX, RightShoeY, RightShoeZ));
Quaternion RightShoeQuaternion = Quaternion.Euler(RightShoeP, RightShoeH, RightShoeR);
InputBroker.SetRotation(RightShoeVicon, RightShoeQuaternion);
// Update LeftShoe's Vicon
InputBroker.SetPosition(LeftShoeVicon, new Vector3(LeftShoeX, LeftShoeY, LeftShoeZ));
Quaternion LeftShoeQuaternion = Quaternion.Euler(LeftShoeP, LeftShoeH, LeftShoeR);
InputBroker.SetRotation(LeftShoeVicon, LeftShoeQuaternion);
}