//-----------------------------------------------------------------------
public void Update()
{
//This method will be executed each iteration of the script
for (int i = 0; i < 2; i++)
{
Sixense.GetNewestData(i, out Controller[i]);
// Convert quaternions to clock-wise Euler angles
float q0 = Controller[i].rot_quat0;
float q1 = Controller[i].rot_quat1;
float q2 = Controller[i].rot_quat2;
float q3 = Controller[i].rot_quat3;
Angles[i].yaw = -(float)Math.Atan2(2 * q1 * q3 - 2 * q0 * q2, 1 - 2 * q1 * q1 - 2 * q2 * q2);
Angles[i].pitch = (float)Math.Atan2(2 * q0 * q3 - 2 * q1 * q2, 1 - 2 * q0 * q0 - 2 * q2 * q2);
Angles[i].roll = -(float)Math.Asin(2 * q0 * q1 + 2 * q2 * q3);
// !!! Roll seems messed up. At +-90 degree roll it starts to roll back down to 0 and
// pitch simultaneously !!! I don't know if this is a bug with the Hydra or with the
// calculations of my Euler angles
}
}
//-----------------------------------------------------------------------
/*
* public object CreateGlobal() {
*
* var globals = new HydraPluginGlobal[2];
* globals[0] = new HydraPluginGlobal(0, this);
* globals[1] = new HydraPluginGlobal(1, this);
* return globals;
* }
*/
//-----------------------------------------------------------------------
public void Start()
{
int r = Sixense.Init();
if (r == Sixense.SUCCESS)
{
int attempts = 0;
int base_found = 0;
while (base_found == 0 && attempts < 2)
{
Thread.Sleep(1000);
base_found = Sixense.IsBaseConnected(0);
}
if (base_found == 0)
{
Sixense.Exit();
throw new Exception("Hydra not attached");
}
Controller = new Sixense.ControllerData[2];
Controller[0] = new Sixense.ControllerData();
Controller[1] = new Sixense.ControllerData();
Angles = new Sixense.ControllerAngles[2];
Angles[0] = new Sixense.ControllerAngles();
Angles[1] = new Sixense.ControllerAngles();
r = Sixense.SetActiveBase(0);
//return null;
}
else
{
throw new Exception("Failed to initialize Hydra");
}
}
//-----------------------------------------------------------------------
public void Stop()
{
Sixense.Exit();
}
