public void OnSkeletonChanged(BoneMarkers marker, ISkeleton newSkeleton, IGame game)
{
if (_writer == null)
{
return;
}
for (int i = 0; i < (int)SkeletonMarkers.Count - 1; i++)
{
Vector3 v = newSkeleton.GetPositionOf((SkeletonMarkers)i);
_currentData[i] = new Vub.Etro.IO.Vector4(v.X, v.Y, v.Z, 0);
}
writeGameObjects(game, (int)labels.Length);
//if (newSkeleton is IRichSkeleton)
//{
// writeSkeletonAngles((IRichSkeleton)newSkeleton, labels.Length + game.GameObjects.Keys.Count);
// writeSkeletonQualities((IRichSkeleton)newSkeleton, labels.Length + game.GameObjects.Keys.Count + angleLabels.Length);
//}
_writer.WriteIntFrame(_currentData);
WriteAnalogData(game);
}
public void Check(ISkeleton skeleton, ISkeletonReport report)
{
SkeletonCorrectiveAngle correctiveAngle = new SkeletonCorrectiveAngle();
if (Wishness == 0)
{
return;
}
Vector3 firstBone;
Vector3 secondBone;
float currentAngle;
#region defineBonesToUse
//we globally make the vectors point in opposite direction
//ex : when we compare the chest and the vertical vector, the two vectors are pointing up
//ex : when we compare the chest and the head, the head vector points down
//ex : when we talk about chest, the arms and legs point out of the chest
//ex : when we compare lower parts with upper parts : the upper parts point to the chest and the lower parts to wirsts and ankles
//ex : when we compare lower/upper parts : they are pointing out of the chest
//ex : when we talk about legs, the chest point up and, when we talk about arms, it's pointing down
//the sternum define the angle between the shoulders vector and the hips vector -> the trunk torsion
//this is important to correctly compare the angles
switch (Angle)
{
case PlayerJoint.Spine:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));//chest
secondBone = Vector3.Up;
break;
case PlayerJoint.Sternum:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //shoulders (pointing right)
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft)); //hips (pointing right)
break;
case PlayerJoint.Head:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.Head)); //head (pointing down)
break;
case PlayerJoint.ShoulderLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //left upper arm
break;
case PlayerJoint.ElbowLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft),
skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft)); //left upper arm
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristLeft),
skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft)); //left lower arm
break;
case PlayerJoint.HipLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft)); //left upper leg
break;
case PlayerJoint.KneeLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.KneeLeft)); //left upper leg
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleLeft),
skeleton.GetPositionOf(SkeletonMarkers.KneeLeft)); //left lower leg
break;
case PlayerJoint.ShoulderRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight)); //right upper arm
break;
case PlayerJoint.ShoulderCenter:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight)); //right upper arm
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //left upper arm
break;
case PlayerJoint.ElbowRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
skeleton.GetPositionOf(SkeletonMarkers.ElbowRight)); //right upper arm
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristRight),
skeleton.GetPositionOf(SkeletonMarkers.ElbowRight)); //right lower arm
break;
case PlayerJoint.HipRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
skeleton.GetPositionOf(SkeletonMarkers.HipRight)); //right upper leg
break;
case PlayerJoint.HipCenter:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft)); //left upper leg
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
skeleton.GetPositionOf(SkeletonMarkers.HipRight)); //right upper leg
break;
case PlayerJoint.KneeRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.KneeRight)); //left upper leg
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleRight),
skeleton.GetPositionOf(SkeletonMarkers.KneeRight)); //left lower leg
break;
default:
firstBone = Vector3.Zero;
secondBone = Vector3.Zero;
break;
}
#endregion
#region check the angle
currentAngle = MathHelper.ToDegrees(GetAngleFromVectors(firstBone, secondBone));
if (Wishness > 0 && Math.Abs(currentAngle - WishedAngle) > Threshold)
{ //we want to reach this position and we currently don't -> wrong
correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
Wishness, restrictionPlane, true);
}
else if (Wishness < 0 && Math.Abs(currentAngle - WishedAngle) < Threshold)
{ //we don't want to reach this position but we currently do -> wrong
correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
Wishness, restrictionPlane, true);
}
#endregion
#region check if in the right plane
//the idea is, now, to check if the bones used are, more or less, in the choosen plane
//we will mesure the angle between the normal to the plane and the normal to the two vectors which forms the angle
//this angle will give us an information about the respect of this restriction plane
//create the frontal plane
Plane plane = new Plane(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter));
//define the plane to use
switch (restrictionPlane)
{
case physioPlanes.NONE:
return;
case physioPlanes.FRONTAL:
break;
case physioPlanes.SAGITTAL:
//that's the plane defined by the normal to the frontal plane and the trunk vector
//so we take the cross of these two vectors -> that define the normal to the plane
//the distance from the origine along these normal doesn't matter for us
plane = new Plane(Vector3.Cross(plane.Normal,
Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter))),
0);
break;
case physioPlanes.TRANSVERSE:
//the same principle is in application here
plane = new Plane(Vector3.Cross(plane.Normal,
Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipRight))),
0);
break;
default:
break;
}
//basically, we are checking the angle with any plane parallel to the restriction plane
float angleWithPlane = MathHelper.ToDegrees(GetAngleFromVectors(plane.Normal, Vector3.Cross(firstBone, secondBone)));
float thresholdPlane = 15;
if ((Math.Abs(angleWithPlane) > thresholdPlane) && (Math.Abs(angleWithPlane - 180) > thresholdPlane))
{ //we don't respect the restriction plane
correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
Wishness, restrictionPlane, false);
}
#endregion
if (correctiveAngle.Severity != 0) //so the correctiveAngle has been assigned
{
if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
{
report.GeneralSeverity = Math.Abs(Wishness);
}
report.CorrectiveCollection.CorrectiveItems.Add(correctiveAngle);
}
}
public void Check(ISkeleton skeleton, ISkeletonReport report)
{
if (Wishness == 0)
{
return;
}
//the positions we recieve from the Kinect are absolute positions ...
//we have to use relative position if we want to apply a constraint position efficiently
//-> use the hipcenter as (0,0,0) and convert the rest in relative positions
//warning : the values are in meter !
Vector3 currentRelativePosition;
#region getCurrentRelativePosition
switch (Joint)
{
case PlayerJoint.HipCenter:
currentRelativePosition = Vector3.Zero;
break;
case PlayerJoint.AnkleLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.AnkleRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ElbowLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ElbowRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.FootLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.FootLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.FootRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.FootRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HandLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HandLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HandRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HandRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.Head:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Head),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HipLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HipRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.KneeLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.KneeRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.Sternum:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Sternum),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ShoulderCenter:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ShoulderLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ShoulderRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.Spine:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Spine),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.WristLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.WristRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
default:
currentRelativePosition = Vector3.Zero;
break;
}
#endregion
//distance recieved in millimeters????
currentRelativePosition /= 1000;
//if the values are the same we do not test this direction
bool xProblemAnalyze = MinPos.X != MaxPos.X;
bool yProblemAnalyze = MinPos.Y != MaxPos.Y;
bool zProblemAnalyze = MinPos.Z != MaxPos.Z;
if (Wishness > 0)
{ //we want to reach this position
if ((xProblemAnalyze &&
(currentRelativePosition.X < MinPos.X || currentRelativePosition.X > MaxPos.X)) ||
(yProblemAnalyze &&
(currentRelativePosition.Y < MinPos.Y || currentRelativePosition.Y > MaxPos.Y)) ||
(zProblemAnalyze &&
(currentRelativePosition.Z < MinPos.Z || currentRelativePosition.Z > MaxPos.Z)))
{//well there's one problem but we don't really care which one here
if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
{
report.GeneralSeverity = Math.Abs(Wishness);
}
report.CorrectiveCollection.CorrectiveItems.Add(new SkeletonCorrecticePosition(
Joint, currentRelativePosition,
Vector3.Divide(Vector3.Add(MinPos, MaxPos), 2),
MinPos, MaxPos, Wishness));
}
}
else if (Wishness < 0)
{ //we don't want to reach this position
bool xProblemDetected = false;
bool yProblemDetected = false;
bool zProblemDetected = false;
if (xProblemAnalyze &&
currentRelativePosition.X > MinPos.X &&
currentRelativePosition.X < MaxPos.X)
{//problem and we want to check it
xProblemDetected = true;
}
if (yProblemAnalyze &&
currentRelativePosition.Y > MinPos.Y &&
currentRelativePosition.Y < MaxPos.Y)
{
yProblemDetected = true;
}
if (zProblemAnalyze &&
currentRelativePosition.Z > MinPos.Z &&
currentRelativePosition.Z < MaxPos.Z)
{
zProblemAnalyze = true;
}
if ((xProblemDetected || !xProblemAnalyze) &&
(yProblemDetected || !yProblemAnalyze) &&
(zProblemDetected || !zProblemAnalyze) &&
(xProblemAnalyze || yProblemAnalyze || zProblemAnalyze))
{//here we can be sure that, if we want to detect a problem on one or mutiple direction, it will be detected
//we are in a position which is not good !!
if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
{
report.GeneralSeverity = Math.Abs(Wishness);
}
report.CorrectiveCollection.CorrectiveItems.Add(new SkeletonCorrecticePosition(
Joint, currentRelativePosition,
Vector3.Divide(Vector3.Add(MinPos, MaxPos), 2),
MinPos, MaxPos, Wishness));
}
}
}
