private static IJoint BuildHand(Hand hand, IJoint wrist, int side)
{
var handJoint = new OrientedJoint();
if (hand.IsValid)
{
var palmNormal = hand.PalmNormal;
var handPosition = hand.PalmPosition;
handJoint.Orientation = new Vector4(10 * palmNormal.x, 10 * palmNormal.y, 10 * palmNormal.z, 0);
handJoint.Point = new Vector3(0, 0, -100);
var fingers = hand.Fingers;
foreach (var t in fingers)
{
var normal = t.Direction;
var position = t.TipPosition - handPosition;
handJoint.AddChild(CreateFinger(position, normal, FingerType2JointType(t.Type(), side)));
}
handJoint.Valid = true;
}
return handJoint;
}
private static IJoint CreateFinger(Vector position, Vector normal, JointType jt)
{
var finger = new OrientedJoint();
finger.JointType = jt;
finger.Point = new Vector3(position.x, position.y, position.z);
finger.Orientation = new Vector4(10 * normal.x, 10 * normal.y, 10 * normal.z, 0);
return finger;
}
/// <summary>
/// Creates the arm.
/// </summary>
/// <returns>The arm.</returns>
/// <param name="side">Side.</param>
public static IList<IJoint> CreateArm(Side side)
{
int handY = -50;
int wristX = 50;
int wristY = 0;
int elbowX = 75;
int elbowY = 218;
int shoulderX = 160;
var handOrientation = new Vector4(0, 0, 0, 0);
var arm = new List<IJoint>();
var shoulder = new OrientedJoint();
var elbow = new OrientedJoint();
var wrist = new OrientedJoint();
var hand = new HandJoint();
if (side == Side.LEFT)
{
shoulder.JointType = JointType.SHOULDER_LEFT;
elbow.JointType = JointType.ELBOW_LEFT;
wrist.JointType = JointType.WRIST_LEFT;
hand.JointType = JointType.HAND_LEFT;
}
else
{
shoulder.JointType = JointType.SHOULDER_RIGHT;
elbow.JointType = JointType.ELBOW_RIGHT;
wrist.JointType = JointType.WRIST_RIGHT;
hand.JointType = JointType.HAND_RIGHT;
}
int s = Convert.ToInt32(side);
// shoulders relative to neck
shoulder.Point = new Vector3(s * shoulderX, 400, 0);
shoulder.Valid = true;
arm.Add(shoulder);
// elbows relative to shoulders
elbow.Point = new Vector3(s * (elbowX + shoulderX), elbowY, 0);
elbow.Valid = true;
arm.Add(elbow);
wrist.Point = new Vector3(s * (wristX + shoulderX), wristY, 0);
wrist.Valid = true;
arm.Add(wrist);
hand.Orientation = handOrientation * -s;
hand.Point = new Vector3(s * (wristX + shoulderX), handY, 0);
hand.Valid = true;
arm.Add(hand);
return arm;
}
public void TestUpdate()
{
var s = Creator.GetNewDefaultSkeleton<InMapSkeleton>();
var head = new OrientedJoint {JointType = JointType.HEAD, Point = new Vector3(1, 2, 3)};
s.UpdateSkeleton(JointType.HEAD, head);
var head2 = s.GetJoint(JointType.HEAD);
Assert.AreEqual(head.Point, head2.Point);
Assert.AreEqual(head, head2);
Assert.AreEqual(3, s.Root.GetChildren().Count);
}
public static IJoint Add(IJoint j1, IJoint j2)
{
var newJoint = new OrientedJoint(j1.JointType, j1.Valid)
{
Point = j1.Point + j2.Point,
Orientation = j1.Orientation + j2.Orientation
};
foreach (var child in j1.GetChildren())
{
newJoint.AddChild(Add(child, j2.FindChild(child.JointType)));
}
return newJoint;
}
public static IJoint Div(IJoint j, int divisor)
{
var newJoint = new OrientedJoint(j.JointType, j.Valid)
{
Point = j.Point/divisor,
Orientation = j.Orientation/divisor
};
foreach (var child in j.GetChildren())
{
newJoint.AddChild(Div(child, divisor));
}
return newJoint;
}
/// <summary>
///
/// </summary>
/// <param name="j1"></param>
/// <param name="j2"></param>
/// <returns></returns>
public static IJoint Diff(IJoint j1, IJoint j2)
{
if (j1.JointType != j2.JointType)
{
throw new Exception("Joint types defer from each other.");
}
var newJoint = new OrientedJoint(j1.JointType, j1.Valid)
{
Point = j1.Point - j2.Point,
Orientation = j1.Orientation - j2.Orientation
};
foreach (var child in j1.GetChildren())
{
newJoint.AddChild(Diff(child, j2.FindChild(child.JointType)));
}
return newJoint;
}
/// <summary>
/// Builds the hand.
/// </summary>
/// <returns>The hand.</returns>
/// <param name="hand">Hand.</param>
/// <param name="side">Side.</param>
private static IList<IJoint> BuildHand(Hand hand, int side)
{
var handJoint = new OrientedJoint();
if (!hand.IsValid)
{
return new IJoint[]{handJoint};
}
var palmNormal = hand.PalmNormal;
var handPosition = hand.PalmPosition;
handJoint.Orientation = new Vector4(palmNormal.x, palmNormal.y, palmNormal.z, 0);
handJoint.Point = new Vector3(0, 0, -100);
var joints = new List<IJoint> { handJoint };
var fingers = hand.Fingers;
joints.AddRange(from t in fingers let normal = t.Direction let position = t.TipPosition - handPosition select CreateFinger(position, normal, FingerType2JointType(t.Type(), side)));
handJoint.Valid = true;
return joints;
}
/// <summary>
/// Creates the leg.
/// </summary>
/// <returns>The leg.</returns>
/// <param name="side">Side.</param>
public static IList<IJoint> CreateLeg(Side side)
{
int footLength = 255;
int ankleY = -830;
int kneeY = -427;
int hipX = 50;
int hipY = -100;
var leg = new List<IJoint>();
var footOrientation = new Vector4(0, 0, 0, 0);
var foot = new OrientedJoint();
var ankle = new OrientedJoint();
var knee = new OrientedJoint();
var hip = new OrientedJoint();
if (side == Side.LEFT)
{
foot.JointType = JointType.FOOT_LEFT;
ankle.JointType = JointType.ANKLE_LEFT;
knee.JointType = JointType.KNEE_LEFT;
hip.JointType = JointType.HIP_LEFT;
}
else
{
foot.JointType = JointType.FOOT_RIGHT;
ankle.JointType = JointType.ANKLE_RIGHT;
knee.JointType = JointType.KNEE_RIGHT;
hip.JointType = JointType.HIP_RIGHT;
}
int s = Convert.ToInt32(side);
hip.Point = new Vector3(s * hipX, hipY, 0);
hip.Valid = true;
leg.Add(hip);
knee.Point = new Vector3(s * hipX, kneeY, 0);
knee.Valid = true;
leg.Add(knee);
ankle.Point = new Vector3(s * hipX, ankleY, 0);
ankle.Valid = true;
leg.Add(ankle);
foot.Orientation = footOrientation;
foot.Point = new Vector3(s * hipX, ankleY, -footLength);
foot.Valid = true;
return leg;
}
/// <summary>
/// Creates the head.
/// </summary>
/// <returns>The head.</returns>
public static IJoint CreateHead()
{
int headY = 580;
var headOrientation = new Vector4(0, 0, 0, 0);
var head = new OrientedJoint
{
Orientation = headOrientation,
Point = new Vector3(0, headY, 0),
JointType = JointType.HEAD,
Valid = true
};
return head;
}
public IJoint Clone()
{
var j = new OrientedJoint(JointType, isValid) { Point = Point, Orientation = Orientation };
j.AddChildren(j.GetChildren());
return j;
}
private void UpdateSkeleton(KinectV2::Microsoft.Kinect.Body body, ISkeleton newSkeleton)
{
foreach (KeyValuePair<JointType, KinectV2::Microsoft.Kinect.JointType> jointMapping in mapping)
{
var joint = new OrientedJoint
{
JointType = jointMapping.Key,
Point = ToVec3(body.Joints[jointMapping.Value].Position),
Orientation = ToVec4(body.JointOrientations[jointMapping.Value].Orientation),
Valid = body.Joints[jointMapping.Value].TrackingState == KinectV2::Microsoft.Kinect.TrackingState.Tracked
};
newSkeleton.UpdateSkeleton(joint.JointType, joint);
}
}
/// <summary>
/// Creates the skeleton.
/// </summary>
/// <returns>The skeleton.</returns>
/// <param name="initSkeleton">Init skeleton.</param>
private ISkeleton CreateSkeleton(KinectV1::Microsoft.Kinect.Skeleton initSkeleton)
{
var s = new InMapSkeleton { ID = (uint)initSkeleton.TrackingId };
foreach (var jointMapping in mapping)
{
var joint = new OrientedJoint
{
JointType = jointMapping.Key,
Point = ToVec3(initSkeleton.Joints[jointMapping.Value].Position),
Orientation = ToVec4(initSkeleton.BoneOrientations[jointMapping.Value].AbsoluteRotation.Quaternion),
Valid = initSkeleton.Joints[jointMapping.Value].TrackingState == JointTrackingState.Tracked
};
s.UpdateSkeleton(joint.JointType, joint);
}
// TODO: iterate over all data in array
s.Valid = initSkeleton.TrackingState == SkeletonTrackingState.Tracked;
return s;
}
/// <summary>
/// Creates the finger.
/// </summary>
/// <returns>The finger.</returns>
/// <param name="position">Position.</param>
/// <param name="normal">Normal.</param>
/// <param name="jt">Joint type</param>
private static IJoint CreateFinger(Vector position, Vector normal, JointType jt)
{
var finger = new OrientedJoint
{
JointType = jt,
Point = new Vector3(position.x, position.y, position.z),
Orientation = new Vector4(normal.x, normal.y, normal.z, 0)
};
return finger;
}