/// <summary> Calculate a 3D position of the wrist, based on an existing tracking hardware and its location. </summary>
/// <param name="trackedObject"></param>
/// <param name="hardware"></param>
/// <param name="wristPos"></param>
/// <param name="wristRot"></param>
/// <returns></returns>
public virtual bool GetWristLocation(Transform trackedObject, SGCore.PosTrackingHardware hardware, out Vector3 wristPos, out Quaternion wristRot)
{
#if UNFINISHED_FEATURES
if (CVDataAvailable())
{
wristPos = SG.Util.SG_Conversions.ToUnityPosition(this.lastCVData.wristWorldPosition);
wristRot = SG.Util.SG_Conversions.ToUnityQuaternion(this.lastCVData.wristWorldRotation);
return(true);
}
#endif
if (this.lastGlove != null)
{
SGCore.Kinematics.Vect3D trackedPos = SG.Util.SG_Conversions.ToPosition(trackedObject.position, true);
SGCore.Kinematics.Quat trackedRot = SG.Util.SG_Conversions.ToQuaternion(trackedObject.rotation);
SGCore.Kinematics.Vect3D wPos; SGCore.Kinematics.Quat wRot;
lastGlove.GetWristLocation(trackedPos, trackedRot, hardware, out wPos, out wRot);
wristPos = SG.Util.SG_Conversions.ToUnityPosition(wPos);
wristRot = SG.Util.SG_Conversions.ToUnityQuaternion(wRot);
return(true);
}
wristPos = trackedObject.position;
wristRot = trackedObject.rotation;
return(false);
}
/// <summary> Convert an array of Unity Vector3 positions into a Vect3D Position array used by SGCore. </summary>
/// <param name="pos"></param>
/// <param name="scale">scale m to mm</param>
/// <returns></returns>
public static SGCore.Kinematics.Vect3D[] ToPosition(Vector3[] pos, bool scale = true)
{
SGCore.Kinematics.Vect3D[] res = new SGCore.Kinematics.Vect3D[pos.Length];
for (int f = 0; f < pos.Length; f++)
{
res[f] = SG_Conversions.ToPosition(pos[f], scale);
}
return(res);
}
/// <summary> Convert from a Unity Vector3 position to a Vect3D used by SGCore. </summary>
/// <param name="pos"></param>
/// <param name="scale">scale m to mm</param>
/// <returns></returns>
public static SGCore.Kinematics.Vect3D ToPosition(Vector3 pos, bool scale = true)
{
SGCore.Kinematics.Vect3D res = new SGCore.Kinematics.Vect3D(pos.x, pos.z, pos.y);
if (scale)
{
res.Scale(1000);
} //from m to mm
return(res);
}
//-------------------------------------------------------------------------------------------------------------------------
// Positions
/// <summary> Convert a Vect3D position taken from SGCore into a Vector3 Unity Position. </summary>
/// <param name="pos"></param>
/// <param name="scale">Scale from mm to m</param>
/// <returns></returns>
public static Vector3 ToUnityPosition(SGCore.Kinematics.Vect3D pos, bool scale = true)
{
Vector3 res = new Vector3(pos.x, pos.z, pos.y);
if (scale)
{
res /= 1000f;
}
return(res);
}
//--------------------------------------------------------------------------------------------------------
// Tracking
/// <summary> Calculates the 3D position of the glove hardware origin. Mostly used in representing a 3D model of the hardware. If you wish to know where the hand is; use GetWristLocation instead. </summary>
/// <param name="trackedObject"></param>
/// <param name="hardware"></param>
/// <param name="wristPos"></param>
/// <param name="wristRot"></param>
/// <returns></returns>
public virtual bool GetGloveLocation(Transform trackedObject, SGCore.PosTrackingHardware hardware, out Vector3 glovePos, out Quaternion gloveRot)
{
if (this.lastGlove != null)
{
SGCore.Kinematics.Vect3D trackedPos = SG.Util.SG_Conversions.ToPosition(trackedObject.position, true);
SGCore.Kinematics.Quat trackedRot = SG.Util.SG_Conversions.ToQuaternion(trackedObject.rotation);
SGCore.Kinematics.Vect3D wPos; SGCore.Kinematics.Quat wRot;
lastGlove.GetGloveLocation(trackedPos, trackedRot, hardware, out wPos, out wRot);
glovePos = SG.Util.SG_Conversions.ToUnityPosition(wPos);
gloveRot = SG.Util.SG_Conversions.ToUnityQuaternion(wRot);
return(true);
}
glovePos = trackedObject.position;
gloveRot = trackedObject.rotation;
return(false);
}
/// <summary> Generates a BasicHandModel based on the Joint Transforms of this hand. </summary>
/// <returns></returns>
private SGCore.Kinematics.BasicHandModel GenerateHandModel()
{
bool right = this.handSide != HandSide.LeftHand;
float[][] handLenghts = new float[5][];
SGCore.Kinematics.Vect3D[] startPositions = new SGCore.Kinematics.Vect3D[5];
Transform[][] fingerJoints = FingerJoints;
SGCore.Kinematics.BasicHandModel defaultH = SGCore.Kinematics.BasicHandModel.Default(right);
for (int f = 0; f < 5; f++)
{
// SGCore.Kinematics.Vect3D lastPos = fingerJoints[f].Length > 0 ? SG_Util.ToPosition(fingerJoints[f][0].position, true)
// : defaultH.GetJointPosition((SGCore.Finger)f);
SGCore.Kinematics.Vect3D lastPos = fingerJoints[f].Length > 0 ? RelativePosition(fingerJoints[f][0], this.wristTransform)
: defaultH.GetJointPosition((SGCore.Finger)f);
startPositions[f] = lastPos; //the first position is the starting position.
handLenghts[f] = new float[3];
float[] defaultL = handLenghts[f] = defaultH.GetFingerLengths((SGCore.Finger)f);
for (int j = 1; j < 4; j++)
{
if (fingerJoints[f].Length > j)
{
SGCore.Kinematics.Vect3D currPos = RelativePosition(fingerJoints[f][j], this.wristTransform);
handLenghts[f][j - 1] = (currPos.x - lastPos.x); //converts from m to mm
lastPos = currPos; //update
}
else
{
handLenghts[f][j - 1] = defaultL[j - 1];
}
}
}
SGCore.Kinematics.BasicHandModel HM = new SGCore.Kinematics.BasicHandModel(right, handLenghts, startPositions);
//Debug.Log("Collected HandModelInfo: " + HM.ToString(true));
return(HM);
}
//-------------------------------------------------------------------------------------------------------------------------
// Euler Rotations
/// <summary> Convert a set of euler angles from SGCore into one that can be used by Unity. </summary>
/// <param name="euler"></param>
/// <returns></returns>
public static Vector3 ToUnityEuler(SGCore.Kinematics.Vect3D euler)
{
euler = SGCore.Kinematics.Values.Degrees(euler);
return(new Vector3(-euler.x, -euler.z, -euler.y));
}
