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)
{
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 void enableHemisphereTracking()
{
Sixense.AutoEnableHemisphereTracking(index);
}
