public override void Stop()
{
if (isReading)
{
Sixense.Exit();
}
}
private void InitHydraRead()
{
if (Sixense.Init() == Sixense.SUCCESS)
{
int attempts = 0;
int base_found = 0;
while (base_found == 0 && attempts++ < 2)
{
base_found = Sixense.IsBaseConnected(0);
if (base_found == 0)
{
Thread.Sleep(1000);
}
}
if (base_found == 0)
{
Sixense.Exit();
throw new Exception("Hydra not attached");
}
Sixense.SetActiveBase(0);
}
else
{
throw new Exception("Failed to initialize Hydra");
}
}
public override void Stop()
{
if (isReading && readInitlized)
{
Sixense.Exit();
}
}
public override void DoBeforeNextExecute()
{
if (isWriting)
{
hydraSpoof.Write(EmulatedData);
}
if (isReading)
{
if (!readInitlized)
{
InitHydraRead();
readInitlized = true;
}
//This method will be executed each iteration of the script
for (int i = 0; i < 2; i++)
{
var lastSequence = Controller[i].sequence_number;
Sixense.GetNewestData(i, out Controller[i]);
if (lastSequence == Controller[i].sequence_number)
{
continue;
}
//getEulerAngles() from sixense_math.cpp
float h, p, r;
float A, B;
B = Controller[i].rot_mat_12;
p = (float)Math.Asin(B);
A = (float)Math.Cos(p);
if (Math.Abs(A) > 0.005f)
{
h = (float)Math.Atan2(-Controller[i].rot_mat_02 / A, Controller[i].rot_mat_22 / A); // atan2( D, C )
r = (float)Math.Atan2(-Controller[i].rot_mat_10 / A, Controller[i].rot_mat_11 / A); // atan2( F, E )
}
else
{
h = 0;
r = (float)Math.Atan2(Controller[i].rot_mat_21, Controller[i].rot_mat_20); // atan2( F, E ) when B=0, D=1
}
Angles[i].yaw = -h;
Angles[i].pitch = p;
Angles[i].roll = -r;
OnUpdate();
}
}
}
public override void DoBeforeNextExecute()
{
if (isWriting)
{
hydraSpoof.Write(EmulatedData);
}
if (isReading)
{
if (!readInitlized)
{
InitHydraRead();
readInitlized = true;
}
//This method will be executed each iteration of the script
for (int i = 0; i < 2; i++)
{
var lastSequence = Controller[i].sequence_number;
Sixense.GetNewestData(i, out Controller[i]);
if (lastSequence == Controller[i].sequence_number)
{
continue;
}
// 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);
OnUpdate();
// !!! 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 override void DoBeforeNextExecute()
{
//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 override Action 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 enableHemisphereTracking()
{
Sixense.AutoEnableHemisphereTracking(index);
}
//-----------------------------------------------------------------------
public override void Stop()
{
Sixense.Exit();
}
