//returns the angles between the two bones in the 3 default planes (frontal, sagital, horizontal)
public static Dictionary <BasePlanes, float> CalculateAngleForBones(BodyJoints body, BoneType first, BoneType second)
{
var bones = ExtractBonesFromBodyJoints(body);
var angles = new Dictionary <BasePlanes, float>();
Dictionary <BasePlanes, Vector3> planes;
switch (first)
{
case BoneType.UpperNeck:
case BoneType.LowerNeck:
case BoneType.UpperBody:
case BoneType.LeftClavicule:
case BoneType.RightClavicule:
planes = GenerateUpperBodyPlanes(body);
break;
case BoneType.LowerBody:
case BoneType.LeftHipbone:
case BoneType.RightHipbone:
planes = GenerateLowerBodyPlanes(body);
break;
default:
Debug.Log("Devia ter implementado esse caso ein");
planes = new Dictionary <BasePlanes, Vector3>();
break;
}
angles.Add(BasePlanes.Frontal, Angle(bones[first], bones[second], planes[BasePlanes.Frontal]));
angles.Add(BasePlanes.Sagittal, Angle(bones[first], bones[second], planes[BasePlanes.Sagittal]));
angles.Add(BasePlanes.Horizontal, Angle(bones[first], bones[second], planes[BasePlanes.Horizontal]));
return(angles);
}
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 void OnNewBodyFrame <S, T>(S bodyStream, T bodyFrame)
{
Kinect.Body[] kinectBodies = bodyStream as Kinect.Body[];
// Voxar.Body[] bodies = new Voxar.Body[kinectBodies.Length];
Voxar.BodyJoints[] bodies = new Voxar.BodyJoints[1];
// int nBodies = 0;
foreach (Kinect.Body kinectBody in kinectBodies)
{
if (kinectBody.IsTracked)
{
// nBodies++;
bodies[0] = GetBody(kinectBody);
}
}
// Debug.Log(bodies[0].joints[Voxar.Joint.JointType.BaseSpine].worldPosition.x);
foreach (var receiver in BodyReceivers)
{
receiver.ReceiveData(bodies);
// Debug.Log("end onnewbodyframe");
}
}
public static Dictionary <BasePlanes, Vector3> GenerateCentralTrunkPlanes(BodyJoints body)
{
var planes = new Dictionary <BasePlanes, Vector3>();
var sagitalPlaneNormal = body.joints[JointType.RightHip].worldPosition - body.joints[JointType.LeftHip].worldPosition;
planes.Add(BasePlanes.Sagittal, sagitalPlaneNormal);
var horizontalPlaneNormal = Vector3.up;
planes.Add(BasePlanes.Horizontal, horizontalPlaneNormal);
var frontalPlaneNormal = Vector3.Cross(sagitalPlaneNormal, horizontalPlaneNormal);
planes.Add(BasePlanes.Frontal, frontalPlaneNormal);
return(planes);
}
public static Dictionary <BasePlanes, Vector3> GenerateUpperBodyPlanes(BodyJoints body)
{
var planes = new Dictionary <BasePlanes, Vector3>();
var sagitalPlaneNormal = body.joints[JointType.RightShoulder].worldPosition - body.joints[JointType.LeftShoulder].worldPosition;
planes.Add(BasePlanes.Sagittal, sagitalPlaneNormal);
var horizontalPlaneNormal = body.joints[JointType.MiddleSpine].worldPosition - body.joints[JointType.ShoulderSpine].worldPosition;
planes.Add(BasePlanes.Horizontal, horizontalPlaneNormal);
var frontalPlaneNormal = Vector3.Cross(sagitalPlaneNormal, horizontalPlaneNormal);
planes.Add(BasePlanes.Frontal, frontalPlaneNormal);
return(planes);
}
public static Dictionary <BoneType, Bone> ExtractBonesFromBodyJoints(BodyJoints body)
{
var bones = new Dictionary <BoneType, Bone>();
foreach (var entry in body.joints)
{
if (entry.Value.status == Status.Tracking)
{
Bone bone;
switch (entry.Key)
{
case Voxar.JointType.Head:
if (body.joints.ContainsKey(JointType.Neck))
{
var neck = body.joints[JointType.Neck];
bone = new Bone(neck, entry.Value);
bones[BoneType.UpperNeck] = bone;
}
break;
case Voxar.JointType.Neck:
if (body.joints.ContainsKey(JointType.ShoulderSpine))
{
var shoulderSpine = body.joints[JointType.ShoulderSpine];
bone = new Bone(shoulderSpine, entry.Value);
bones[BoneType.LowerNeck] = bone;
}
break;
case Voxar.JointType.ShoulderSpine:
if (body.joints.ContainsKey(JointType.LeftShoulder))
{
var leftShoulder = body.joints[JointType.LeftShoulder];
bone = new Bone(entry.Value, leftShoulder);
bones[BoneType.LeftClavicule] = bone;
}
if (body.joints.ContainsKey(JointType.RightShoulder))
{
var rightShoulder = body.joints[JointType.RightShoulder];
bone = new Bone(entry.Value, rightShoulder);
bones[BoneType.RightClavicule] = bone;
}
if (body.joints.ContainsKey(JointType.MiddleSpine))
{
var middleSpine = body.joints[JointType.MiddleSpine];
bone = new Bone(middleSpine, entry.Value);
bones[BoneType.UpperBody] = bone;
}
break;
case Voxar.JointType.LeftShoulder:
if (body.joints.ContainsKey(JointType.LeftElbow))
{
var leftElbow = body.joints[JointType.LeftElbow];
bone = new Bone(entry.Value, leftElbow);
bones[BoneType.LeftArm] = bone;
}
break;
case Voxar.JointType.RightShoulder:
if (body.joints.ContainsKey(JointType.RightElbow))
{
var rightElbow = body.joints[JointType.RightElbow];
bone = new Bone(entry.Value, rightElbow);
bones[BoneType.RightArm] = bone;
}
break;
case Voxar.JointType.LeftElbow:
if (body.joints.ContainsKey(JointType.LeftWrist))
{
var leftWrist = body.joints[Voxar.JointType.LeftWrist];
bone = new Bone(entry.Value, leftWrist);
bones[BoneType.LeftForearm] = bone;
}
break;
case Voxar.JointType.RightElbow:
if (body.joints.ContainsKey(JointType.RightWrist))
{
var rightWrist = body.joints[JointType.RightWrist];
bone = new Bone(entry.Value, rightWrist);
bones[BoneType.RightForearm] = bone;
}
break;
case Voxar.JointType.LeftWrist:
if (body.joints.ContainsKey(JointType.LeftHand))
{
var leftHand = body.joints[JointType.LeftHand];
bone = new Bone(entry.Value, leftHand);
bones[BoneType.LeftWrist] = bone;
}
break;
case Voxar.JointType.RightWrist:
if (body.joints.ContainsKey(JointType.RightHand))
{
var rightHand = body.joints[JointType.RightHand];
bone = new Bone(entry.Value, rightHand);
bones[BoneType.RightWrist] = bone;
}
break;
case Voxar.JointType.MiddleSpine:
if (body.joints.ContainsKey(JointType.BaseSpine))
{
var baseSpine = body.joints[JointType.BaseSpine];
bone = new Bone(baseSpine, entry.Value);
bones[BoneType.LowerBody] = bone;
}
break;
case JointType.BaseSpine:
if (body.joints.ContainsKey(JointType.LeftHip))
{
var leftHip = body.joints[JointType.LeftHip];
bone = new Bone(entry.Value, leftHip);
bones[BoneType.LeftHipbone] = bone;
}
if (body.joints.ContainsKey(JointType.RightHip))
{
var rightHip = body.joints[JointType.RightHip];
bone = new Bone(entry.Value, rightHip);
bones[BoneType.RightHipbone] = bone;
}
break;
case JointType.LeftHip:
if (body.joints.ContainsKey(JointType.LeftKnee))
{
var leftKnee = body.joints[JointType.LeftKnee];
bone = new Bone(entry.Value, leftKnee);
bones[BoneType.LeftThigh] = bone;
}
break;
case JointType.RightHip:
if (body.joints.ContainsKey(JointType.RightKnee))
{
var rightKnee = body.joints[JointType.RightKnee];
bone = new Bone(entry.Value, rightKnee);
bones[BoneType.RightThigh] = bone;
}
break;
case JointType.LeftKnee:
if (body.joints.ContainsKey(JointType.LeftAnkle))
{
var leftAnkle = body.joints[JointType.LeftAnkle];
bone = new Bone(entry.Value, leftAnkle);
bones[BoneType.LeftLeg] = bone;
}
break;
case JointType.RightKnee:
if (body.joints.ContainsKey(JointType.RightAnkle))
{
var rightAnkle = body.joints[JointType.RightAnkle];
bone = new Bone(entry.Value, rightAnkle);
bones[BoneType.RightLeg] = bone;
}
break;
case JointType.LeftAnkle:
if (body.joints.ContainsKey(JointType.LeftFoot))
{
var leftFoot = body.joints[JointType.LeftFoot];
bone = new Bone(entry.Value, leftFoot);
bones[BoneType.LeftAnkle] = bone;
}
break;
case JointType.RightAnkle:
if (body.joints.ContainsKey(JointType.RightFoot))
{
var rightFoot = body.joints[JointType.RightFoot];
bone = new Bone(entry.Value, rightFoot);
bones[BoneType.RightAnkle] = bone;
}
break;
case JointType.LeftHand:
case JointType.RightHand:
case JointType.LeftFoot:
case JointType.RightFoot:
default:
//This joints have no more bones associeted
break;
}
}
}
return(bones);
}
public PlaneBasedBodyAngles(BodyJoints body, BasePlanes targetPlane)
{
plane = targetPlane;
angles = new Dictionary <BoneType, Dictionary <BoneType, float> >();
for (int i = 0; i < Bone.boneTypeCount; i++)
{
angles.Add((BoneType)i, new Dictionary <BoneType, float>());
}
//init a PlaneBaseBodyAngles from the body
var bones = MovementAnalyzer.ExtractBonesFromBodyJoints(body);
var upperPlanes = MovementAnalyzer.GenerateUpperBodyPlanes(body);
var lowerPlanes = MovementAnalyzer.GenerateLowerBodyPlanes(body);
var centralPlanes = MovementAnalyzer.GenerateCentralTrunkPlanes(body);
foreach (var entry in bones)
{
var type = entry.Key;
var bone = entry.Value;
switch (type)
{
case BoneType.UpperNeck:
CheckAndAddAngle(type, BoneType.LowerNeck, bones, upperPlanes[plane]);
break;
case BoneType.LowerNeck:
CheckAndAddAngle(type, BoneType.UpperNeck, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.UpperBody, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftClavicule, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightClavicule, bones, upperPlanes[plane]);
break;
case BoneType.UpperBody:
CheckAndAddAngle(type, BoneType.LowerNeck, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftClavicule, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightClavicule, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.LowerBody, bones, upperPlanes[plane]);
break;
case BoneType.LeftClavicule:
CheckAndAddAngle(type, BoneType.LowerNeck, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightClavicule, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.UpperBody, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftArm, bones, upperPlanes[plane]);
break;
case BoneType.LeftArm:
CheckAndAddAngle(type, BoneType.LeftClavicule, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftForearm, bones, upperPlanes[plane]);
break;
case BoneType.LeftForearm:
CheckAndAddAngle(type, BoneType.LeftArm, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftWrist, bones, lowerPlanes[plane]);
break;
case BoneType.LeftWrist:
CheckAndAddAngle(type, BoneType.LeftForearm, bones, lowerPlanes[plane]);
break;
case BoneType.RightClavicule:
CheckAndAddAngle(type, BoneType.LowerNeck, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftClavicule, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.UpperBody, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightArm, bones, upperPlanes[plane]);
break;
case BoneType.RightArm:
CheckAndAddAngle(type, BoneType.RightClavicule, bones, upperPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightForearm, bones, upperPlanes[plane]);
break;
case BoneType.RightForearm:
CheckAndAddAngle(type, BoneType.RightArm, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightWrist, bones, lowerPlanes[plane]);
break;
case BoneType.RightWrist:
CheckAndAddAngle(type, BoneType.RightForearm, bones, lowerPlanes[plane]);
break;
case BoneType.LowerBody:
CheckAndAddAngle(type, BoneType.UpperBody, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftHipbone, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightHipbone, bones, lowerPlanes[plane]);
break;
case BoneType.LeftHipbone:
CheckAndAddAngle(type, BoneType.LowerBody, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightHipbone, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftThigh, bones, lowerPlanes[plane]);
break;
case BoneType.LeftThigh:
CheckAndAddAngle(type, BoneType.LeftHipbone, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftLeg, bones, lowerPlanes[plane]);
break;
case BoneType.LeftLeg:
CheckAndAddAngle(type, BoneType.LeftThigh, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftAnkle, bones, lowerPlanes[plane]);
break;
case BoneType.LeftAnkle:
CheckAndAddAngle(type, BoneType.LeftLeg, bones, lowerPlanes[plane]);
break;
case BoneType.RightHipbone:
CheckAndAddAngle(type, BoneType.LowerBody, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.LeftHipbone, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightThigh, bones, lowerPlanes[plane]);
break;
case BoneType.RightThigh:
CheckAndAddAngle(type, BoneType.RightHipbone, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightLeg, bones, lowerPlanes[plane]);
break;
case BoneType.RightLeg:
CheckAndAddAngle(type, BoneType.RightThigh, bones, lowerPlanes[plane]);
CheckAndAddAngle(type, BoneType.RightAnkle, bones, lowerPlanes[plane]);
break;
case BoneType.RightAnkle:
CheckAndAddAngle(type, BoneType.RightLeg, bones, lowerPlanes[plane]);
break;
}
}
}
