public BodyJoints GetBody <T>(T source)
{
Kinect.Body kinectBody = source as Kinect.Body;
int id = (int)kinectBody.TrackingId;
Status status = Status.NotTracking;
Dictionary <JointType, Voxar.Joint> joints = new Dictionary <JointType, Voxar.Joint>();
if (kinectBody.IsTracked)
{
status = Voxar.Status.Tracking;
foreach (Kinect.Joint kinectJoint in kinectBody.Joints.Values)
{
// Debug.Log(kinectJoint.JointType);
if (kinectJoint.JointType == Kinect.JointType.HandTipLeft || kinectJoint.JointType == Kinect.JointType.HandTipRight ||
kinectJoint.JointType == Kinect.JointType.ThumbLeft || kinectJoint.JointType == Kinect.JointType.ThumbRight)
{
continue;
}
Voxar.Joint joint = GetJoint(kinectJoint);
joints.Add(joint.type, joint);
}
}
else
{
status = Voxar.Status.NotTracking;
}
Voxar.BodyJoints body = new Voxar.BodyJoints(id, status, joints);
return(body);
}
public Voxar.Joint GetJoint <T>(T source)
{
var astra = source as Astra.Joint;
Status status = Status.NotTracking;
JointType type = JointType.Head;
switch (astra.Status)
{
case Astra.JointStatus.NotTracked:
status = Status.NotTracking;
break;
case Astra.JointStatus.LowConfidence:
status = Status.Inferred;
break;
case Astra.JointStatus.Tracked:
status = Status.Tracking;
break;
}
switch (astra.Type)
{
case Astra.JointType.Head:
type = JointType.Head;
break;
case Astra.JointType.Neck:
type = JointType.Neck;
break;
case Astra.JointType.ShoulderSpine:
type = JointType.ShoulderSpine;
break;
case Astra.JointType.LeftShoulder:
type = JointType.LeftShoulder;
break;
case Astra.JointType.RightShoulder:
type = JointType.RightShoulder;
break;
case Astra.JointType.MidSpine:
type = JointType.MiddleSpine;
break;
case Astra.JointType.BaseSpine:
type = JointType.BaseSpine;
break;
case Astra.JointType.LeftHip:
type = JointType.LeftHip;
break;
case Astra.JointType.RightHip:
type = JointType.RightHip;
break;
case Astra.JointType.LeftElbow:
type = JointType.LeftElbow;
break;
case Astra.JointType.LeftWrist:
type = JointType.LeftWrist;
break;
case Astra.JointType.LeftHand:
type = JointType.LeftHand;
break;
case Astra.JointType.RightElbow:
type = JointType.RightElbow;
break;
case Astra.JointType.RightWrist:
type = JointType.RightWrist;
break;
case Astra.JointType.RightHand:
type = JointType.RightHand;
break;
case Astra.JointType.LeftKnee:
type = JointType.LeftKnee;
break;
case Astra.JointType.LeftFoot:
type = JointType.LeftFoot;
break;
case Astra.JointType.RightKnee:
type = JointType.RightKnee;
break;
case Astra.JointType.RightFoot:
type = JointType.RightFoot;
break;
}
var worldPosition = new Vector3(astra.WorldPosition.X, astra.WorldPosition.Y, astra.WorldPosition.Z);
var depthPosition = new Vector2(astra.DepthPosition.X, astra.DepthPosition.Y);
//ASTRA SDK CheatSheet
//skel.Joints[i].Orient.Matrix:
// 0, 1, 2,
// 3, 4, 5,
// 6, 7, 8
// -------
// right(X), up(Y), forward(Z)
//Vector3 jointRight = new Vector3(
// bodyJoint.Orientation.M00,
// bodyJoint.Orientation.M10,
// bodyJoint.Orientation.M20);
//Vector3 jointUp = new Vector3(
// bodyJoint.orientation.M01,
// bodyJoint.orientation.M11,
// bodyJoint.orientation.M21);
//Vector3 jointForward = new Vector3(
// bodyJoint.orientation.M02,
// bodyJoint.orientation.M12,
// bodyJoint.orientation.M22);
var upwards = new Vector3(astra.Orientation.M01, astra.Orientation.M11, astra.Orientation.M21);
var forward = new Vector3(-astra.Orientation.M02, -astra.Orientation.M12, -astra.Orientation.M22);
var joint = new Joint(type, status, worldPosition, depthPosition, forward, upwards);
return(joint);
}
public Voxar.Joint GetJoint(Kinect.Joint kinectJoint)
{
Voxar.Status status = Voxar.Status.NotTracking;
JointType type = JointType.BaseSpine;
switch (kinectJoint.TrackingState)
{
case Kinect.TrackingState.NotTracked:
status = Voxar.Status.NotTracking;
break;
case Kinect.TrackingState.Inferred:
status = Voxar.Status.Inferred;
break;
case Kinect.TrackingState.Tracked:
status = Voxar.Status.Tracking;
break;
}
switch (kinectJoint.JointType)
{
case Kinect.JointType.Head:
type = JointType.Head;
break;
case Kinect.JointType.Neck:
type = JointType.Neck;
break;
case Kinect.JointType.SpineShoulder:
type = JointType.ShoulderSpine;
break;
case Kinect.JointType.ShoulderLeft:
type = JointType.LeftShoulder;
break;
case Kinect.JointType.ShoulderRight:
type = JointType.RightShoulder;
break;
case Kinect.JointType.SpineMid:
type = JointType.MiddleSpine;
break;
case Kinect.JointType.SpineBase:
type = JointType.BaseSpine;
break;
case Kinect.JointType.HipLeft:
type = JointType.LeftHip;
break;
case Kinect.JointType.HipRight:
type = JointType.RightHip;
break;
case Kinect.JointType.ElbowLeft:
type = JointType.LeftElbow;
break;
case Kinect.JointType.WristLeft:
type = JointType.LeftWrist;
break;
case Kinect.JointType.HandLeft:
type = JointType.LeftHand;
break;
case Kinect.JointType.ElbowRight:
type = JointType.RightElbow;
break;
case Kinect.JointType.WristRight:
type = JointType.RightWrist;
break;
case Kinect.JointType.HandRight:
type = JointType.RightHand;
break;
case Kinect.JointType.KneeLeft:
type = JointType.LeftKnee;
break;
case Kinect.JointType.FootLeft:
type = JointType.LeftFoot;
break;
case Kinect.JointType.KneeRight:
type = JointType.RightKnee;
break;
case Kinect.JointType.FootRight:
type = JointType.RightFoot;
break;
case Kinect.JointType.AnkleLeft:
type = JointType.LeftAnkle;
break;
case Kinect.JointType.AnkleRight:
type = JointType.RightAnkle;
break;
default:
throw new System.Exception("invalid jointType");
break;
}
var worldPosition = new Vector3(kinectJoint.Position.X * 1000f, kinectJoint.Position.Y * 1000f, kinectJoint.Position.Z * 1000f);
var depthPosition = new Vector2(0, 0); // TODO: retrieve depth
// var upwards = new Vector3(astra.Orientation.M01, astra.Orientation.M11, astra.Orientation.M21);
// var forward = new Vector3(-astra.Orientation.M02, -astra.Orientation.M12, -astra.Orientation.M22);
var upwards = new Vector3(0, 1, 0); // TODO: retrieve sensor's coordinates;
var forward = new Vector3(0, 0, 1); // TODO: retrieve sensor's coordinates;
Voxar.Joint joint = new Voxar.Joint(type, status, worldPosition, depthPosition, forward, upwards);
return(joint);
}
