// <summary>
/// Update latest G4 sensor data
/// </summary>
internal void G4Update(ref CyberGloveUnityPlugin.G4Data gd)
{
m_G4sensorNo = ( AttachedG4SensorNo )gd.sensor_No;
if (G4HomeSet == false)
{
if (Mathf.Abs(gd.G4Pos.x) <= 0.01f && Mathf.Abs(gd.G4Pos.y) <= 0.01f && Mathf.Abs(gd.G4Pos.z) <= 0.10f)
{
}
else
{
G4Cp.Set(gd.G4Pos.x, gd.G4Pos.y, gd.G4Pos.z);
G4Cori.Set(gd.G4Rot.x, gd.G4Rot.y, gd.G4Rot.z); //yaw pitch roll order reported by G4
G4HomeSet = true;
}
}
else
{
Vector3 g4position = new Vector3(gd.G4Pos.x, gd.G4Pos.y, gd.G4Pos.z);
Vector3 g4rotation = new Vector3(gd.G4Rot.x, gd.G4Rot.y, gd.G4Rot.z);
if (Mathf.Abs(G4Cp.x) <= 0.01f && Mathf.Abs(G4Cp.y) <= 0.01f && Mathf.Abs(G4Cp.z) <= 0.01f)
{
G4Cp = g4position;
G4Cori = g4rotation;
}
else
{
Vector3 G4trans = g4position - G4Cp;
m_PalmPosition.Set(G4trans.x, G4trans.y, G4trans.z);
//Vector3 rotation = new Vector3(gd.G4Rot.x,gd.G4Rot.y,gd.G4Rot.z); //x:yaw y:pitch z:roll order reported by G4
Vector3 rotation = new Vector3(gd.G4Rot.x, 0.0f, gd.G4Rot.z);
Vector3 G4ori = G4Cori - rotation;
Quaternion G4rot_quat = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
G4rot_quat = EulerToQuat(-G4ori.y, -(pi - G4ori.z), G4ori.x);
m_PalmRotation = G4rot_quat * m_PalmRotation;
}
}
}
internal CyberGlove()
{
m_Connected = false;
m_G4Connected = false;
m_Handedness = CyberGloveHandedness.UNKNOWN;
m_G4sensorNo = AttachedG4SensorNo.UNKNOWN;
m_PalmPosition.Set(0.0f, 0.0f, 0.0f);
m_PalmRotation.Set(0.0f, 0.0f, 0.0f, 1.0f);
G4Cp.Set(0.0f, 0.0f, 0.0f);
G4Cori.Set(0.0f, 0.0f, 0.0f);
G4HomeSet = false;
for (int finger = 0; finger < 5; finger++) //5 fingers: Thumb, Index, Middle, Ring, Pinky
{
for (int joint = 0; joint < 4; joint++) //4 joints: Inner, Middle, Outer, FingerTip
{
m_fingerJointPos[finger, joint].Set(0.0f, 0.0f, 0.0f);
m_fingerJointRot_quat[finger, joint].Set(0.0f, 0.0f, 0.0f, 1.0f);
}
}
}