private void DrawLineBetweenJoints(Graphics g, Skeleton skel, SkeletonJoint.JointType j1, SkeletonJoint.JointType j2)
{
try
{
if (skel.State == Skeleton.SkeletonState.TRACKED)
{
SkeletonJoint joint1 = skel.getJoint(j1);
SkeletonJoint joint2 = skel.getJoint(j2);
if (joint1.Position.Z > 0 && joint2.Position.Z > 0)
{
Point j1PosEllipse = new Point();
Point j2PosEllipse = new Point();
NiTEWrapper.PointF j1PosLine = uTracker.ConvertJointCoordinatesToDepth(joint1.Position);
NiTEWrapper.PointF j2PosLine = uTracker.ConvertJointCoordinatesToDepth(joint2.Position);
j1PosEllipse.X = (int)j1PosLine.X - 5;
j1PosEllipse.Y = (int)j1PosLine.Y - 5;
j2PosEllipse.X = (int)j2PosLine.X - 5;
j2PosEllipse.Y = (int)j2PosLine.Y - 5;
j1PosLine.X -= 2;
j1PosLine.Y -= 2;
j2PosLine.X -= 2;
j2PosLine.Y -= 2;
g.DrawLine(new Pen(Brushes.White, 3), j1PosLine.X, j1PosLine.Y, j2PosLine.X, j2PosLine.Y);
g.DrawEllipse(new Pen(Brushes.White, 5), new Rectangle(j1PosEllipse, new System.Drawing.Size(5, 5)));
g.DrawEllipse(new Pen(Brushes.White, 5), new Rectangle(j2PosEllipse, new System.Drawing.Size(5, 5)));
}
}
}
catch (Exception) { }
}
/// <summary>
///
/// </summary>
/// <param name="pos1">回転対象</param>
/// <param name="pos2">目標ボーン</param>
/// <param name="skeleton"></param>
/// <param name="type1"></param>
/// <param name="type2"></param>
/// <returns></returns>
private void getRotation(PMXBone b1, PMXBone b2, Skeleton skeleton, SkeletonJoint skel1,
SkeletonJoint skel2)
{
SkeletonJoint targetBone = skel1;
SkeletonJoint baseBone = skel2;
if (targetBone.PositionConfidence < 0.5 || baseBone.PositionConfidence < 0.5)
{
return;
}
Vector3 t2b = Vector3.Normalize(ToVector3(targetBone.Position) - ToVector3(baseBone.Position));
Vector3 it2b = Vector3.Normalize(Vector3.TransformCoordinate(b2.Position, b2.GlobalPose) - Vector3.TransformCoordinate(b1.Position, b1.GlobalPose));
Vector3 axis = Vector3.Cross(t2b, it2b);
float angle = (float)-Math.Acos(Vector3.Dot(t2b, it2b));
b1.Rotation *= Quaternion.RotationAxis(axis, angle);
b1.UpdateGrobalPose();
}
public void DrawSkeleton(Skeleton skeleton, DrawingContext drawingContext)
{
if (skeleton == null)
return;
// Render Torso
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.Head, SkeletonJoint.JointType.Neck);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.LeftShoulder, SkeletonJoint.JointType.Neck);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.RightShoulder, SkeletonJoint.JointType.Neck);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.RightShoulder, SkeletonJoint.JointType.Torso);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.LeftShoulder, SkeletonJoint.JointType.Torso);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.Torso, SkeletonJoint.JointType.RightHip);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.Torso, SkeletonJoint.JointType.LeftHip);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.RightHip, SkeletonJoint.JointType.LeftHip);
// Left Arm
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.LeftShoulder, SkeletonJoint.JointType.LeftElbow);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.LeftElbow, SkeletonJoint.JointType.LeftHand);
// Right Arm
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.RightShoulder, SkeletonJoint.JointType.RightElbow);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.RightElbow, SkeletonJoint.JointType.RightHand);
// Left Leg
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.LeftHip, SkeletonJoint.JointType.LeftKnee);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.LeftKnee, SkeletonJoint.JointType.LeftFoot);
// Right Leg
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.RightHip, SkeletonJoint.JointType.RightKnee);
this.drawBone(skeleton, drawingContext, SkeletonJoint.JointType.RightKnee, SkeletonJoint.JointType.RightFoot);
// Render Joints
foreach (SkeletonJoint.JointType jointType in (SkeletonJoint.JointType[]) Enum.GetValues(typeof(SkeletonJoint.JointType)))
{
Brush drawBrush = null;
SkeletonJoint joint = skeleton.GetJoint(jointType);
if(!joint.IsValid)
continue;
drawBrush = this.trackedJointBrush;
drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), JOINT_THINCKNESS, JOINT_THINCKNESS);
}
}
private void DrawLineBetweenJoints(
Graphics g,
Skeleton skel,
SkeletonJoint.JointType j1,
SkeletonJoint.JointType j2)
{
try
{
if (skel.State == Skeleton.SkeletonState.Tracked)
{
SkeletonJoint joint1 = skel.GetJoint(j1);
SkeletonJoint joint2 = skel.GetJoint(j2);
if (joint1.Position.Z > 0 && joint2.Position.Z > 0)
{
Point joint1PosEllipse = new Point();
Point joint2PosEllipse = new Point();
PointF joint1PosLine = this.userTracker.ConvertJointCoordinatesToDepth(joint1.Position);
PointF joint2PosLine = this.userTracker.ConvertJointCoordinatesToDepth(joint2.Position);
joint1PosEllipse.X = (int)joint1PosLine.X - 5;
joint1PosEllipse.Y = (int)joint1PosLine.Y - 5;
joint2PosEllipse.X = (int)joint2PosLine.X - 5;
joint2PosEllipse.Y = (int)joint2PosLine.Y - 5;
joint1PosLine.X -= 2;
joint1PosLine.Y -= 2;
joint2PosLine.X -= 2;
joint2PosLine.Y -= 2;
g.DrawLine(new Pen(Brushes.White, 3), joint1PosLine, joint2PosLine);
g.DrawEllipse(new Pen(Brushes.White, 5), new Rectangle(joint1PosEllipse, new Size(5, 5)));
g.DrawEllipse(new Pen(Brushes.White, 5), new Rectangle(joint2PosEllipse, new Size(5, 5)));
}
}
}
catch (Exception)
{
}
}
internal void DrawCirlce(Skeleton skeleton, SkeletonJoint.JointType jointType, DrawingContext dc, double radius)
{
if (skeleton == null)
return;
Point3D jointPoint = skeleton.GetJoint(jointType).Position;
dc.DrawEllipse(TorqueBrush, TorquePen, SkeletonPointToScreen(jointPoint), radius, radius);
}
/// <summary>
/// Draws a bone line between two joints
/// </summary>
/// <param name="skeleton">skeleton to draw bones from</param>
/// <param name="drawingContext">drawing context to draw to</param>
/// <param name="jointType0">joint to start drawing from</param>
/// <param name="jointType1">joint to end drawing at</param>
private void drawBone(Skeleton skeleton, DrawingContext drawingContext, SkeletonJoint.JointType jointType0, SkeletonJoint.JointType jointType1)
{
if (skeleton.State != Skeleton.SkeletonState.Tracked)
return;
SkeletonJoint joint0 = skeleton.GetJoint(jointType0);
SkeletonJoint joint1 = skeleton.GetJoint(jointType1);
// If we can't find either of these joints, exit
if (joint0.Position.Z <=0 || joint1.Position.Z <=0)
{
return;
}
// We assume all drawn bones are inferred unless BOTH joints are tracked
Pen drawPen = this.trackedBonePen;
drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
}
/// <summary>
/// ITransformUpdaterのメンバーの実装
/// </summary>
public bool UpdateTransform()
{
UserTrackerFrameRef usrFrameRef = device.CurrentUserTrackerFrameRef;
if (CurrentTrackUserId >= 0)
{
UserData[] usrs =
(from sk in usrFrameRef.Users where sk.UserId == CurrentTrackUserId select sk).ToArray();
if (usrs.Length != 1)
{
return(true);
}
Skeleton skeleton = usrs[0].Skeleton;
if (skeleton.State == Skeleton.SkeletonState.Tracked)
{
trackTarget = skeleton;
if (TrackingUser != null)
{
TrackingUser(this, usrs[0]);
}
PMXBone head = getBone("頭");
PMXBone neck = getBone("首");
PMXBone torso = getBone("上半身");
PMXBone r_shoulder = getBone("右腕");
PMXBone r_elbow = getBone("右ひじ");
PMXBone r_hand = getBone("右手首");
PMXBone r_hip = getBone("右足");
PMXBone r_knee = getBone("右ひざ");
PMXBone r_foot = getBone("右足首");
PMXBone l_shoulder = getBone("左腕");
PMXBone l_elbow = getBone("左ひじ");
PMXBone l_hand = getBone("左手首");
PMXBone l_hip = getBone("左足");
PMXBone l_knee = getBone("左ひざ");
PMXBone l_foot = getBone("左足首");
SkeletonJoint sj_head = skeleton.GetJoint(SkeletonJoint.JointType.Head);
SkeletonJoint sj_neck = skeleton.GetJoint(SkeletonJoint.JointType.Neck);
SkeletonJoint sj_rShoulder = skeleton.GetJoint(SkeletonJoint.JointType.RightShoulder);
SkeletonJoint sj_lShoulder = skeleton.GetJoint(SkeletonJoint.JointType.LeftShoulder);
SkeletonJoint sj_rElbow = skeleton.GetJoint(SkeletonJoint.JointType.RightElbow);
SkeletonJoint sj_lElbow = skeleton.GetJoint(SkeletonJoint.JointType.LeftElbow);
SkeletonJoint sj_r_hand = skeleton.GetJoint(SkeletonJoint.JointType.RightHand);
SkeletonJoint sj_l_hand = skeleton.GetJoint(SkeletonJoint.JointType.LeftHand);
SkeletonJoint sj_torso = skeleton.GetJoint(SkeletonJoint.JointType.Torso);
SkeletonJoint sj_rHip = skeleton.GetJoint(SkeletonJoint.JointType.RightHip);
SkeletonJoint sj_lHip = skeleton.GetJoint(SkeletonJoint.JointType.LeftHip);
SkeletonJoint sj_rKnee = skeleton.GetJoint(SkeletonJoint.JointType.RightKnee);
SkeletonJoint sj_lKnee = skeleton.GetJoint(SkeletonJoint.JointType.LeftKnee);
SkeletonJoint sj_rFoot = skeleton.GetJoint(SkeletonJoint.JointType.RightFoot);
SkeletonJoint sj_lFoot = skeleton.GetJoint(SkeletonJoint.JointType.LeftFoot);
//腰のひねり
Vector3 shoulder_l2r =
Vector3.Normalize(
ToVector3(sj_rShoulder.Position) -
ToVector3(sj_lShoulder.Position));
Vector3 hip_l2r =
Vector3.Normalize(
ToVector3(sj_rHip.Position) -
ToVector3(sj_lHip.Position));
//shoulder_l2r.Normalize();hip_l2r.Normalize();
float angle = (float)Math.Acos(Math.Min(Vector3.Dot(shoulder_l2r, hip_l2r), 1f));
torso.Rotation *= Quaternion.RotationAxis(new Vector3(0, 1, 0), angle);
torso.UpdateGrobalPose();
getRotation(neck, head, skeleton, sj_neck,
sj_head);
getRotation(r_shoulder, r_elbow, skeleton,
sj_rShoulder, sj_rElbow);
getRotation(r_elbow, r_hand, skeleton,
sj_rElbow,
sj_r_hand);
getRotation(l_shoulder, l_elbow, skeleton,
sj_lShoulder, sj_lElbow);
getRotation(l_elbow, l_hand, skeleton,
sj_lElbow, sj_l_hand);
getRotation(torso, neck, skeleton, sj_torso, sj_neck);
getRotation(r_hip, r_knee, skeleton,
sj_rHip, sj_rKnee);
getRotation(r_knee, r_foot, skeleton,
sj_rKnee, sj_rFoot);
getRotation(l_hip, l_knee, skeleton,
sj_lHip, sj_lKnee);
getRotation(l_knee, l_foot, skeleton,
sj_lKnee, sj_lFoot);
}
}
return(true);
}
private void RunFatigueEngine(Skeleton skeleton, double deltaTimeInSeconds)
{
TotalTimeInSeconds += deltaTimeInSeconds;
SkeletonData measuredArms = new SkeletonData();
measuredArms.RightShoulderCms = Convert(skeleton.GetJoint(SkeletonJoint.JointType.RightShoulder).Position);
measuredArms.RightElbowCms = Convert(skeleton.GetJoint(SkeletonJoint.JointType.RightElbow).Position);
measuredArms.RightHandCms = Convert(skeleton.GetJoint(SkeletonJoint.JointType.RightHand).Position);
measuredArms.RightWristCms = engine.EstimateWristPosition(measuredArms.RightHandCms, measuredArms.RightElbowCms);
measuredArms.LeftShoulderCms = Convert(skeleton.GetJoint(SkeletonJoint.JointType.LeftShoulder).Position);
measuredArms.LeftElbowCms = Convert(skeleton.GetJoint(SkeletonJoint.JointType.LeftElbow).Position);
measuredArms.LeftHandCms = Convert(skeleton.GetJoint(SkeletonJoint.JointType.LeftHand).Position);
measuredArms.LeftWristCms = engine.EstimateWristPosition(measuredArms.LeftHandCms, measuredArms.LeftElbowCms);
armFatigueUpdate = engine.ProcessNewSkeletonData(measuredArms, deltaTimeInSeconds);
RightCE = armFatigueUpdate.RightArm.ConsumedEndurance;
LeftCE = armFatigueUpdate.LeftArm.ConsumedEndurance;
}
private DrawingImage DrawSkeleton(Skeleton skeleton, SolidColorBrush brush)
{
DrawingGroup dGroup = new DrawingGroup();
using (DrawingContext dc = dGroup.Open())
{
dc.DrawRectangle(brush, new Pen(Brushes.Black, 0.5), new Rect(0, 0, kinectSensor.FrameWidth, kinectSensor.FrameHeight));
skeletonDrawer.DrawSkeleton(skeleton, dc);
if(IsEngineStart)
skeletonDrawer.DrawCirlce(skeleton, NiTEWrapper.SkeletonJoint.JointType.RightShoulder, dc, armFatigueUpdate.RightArm.ArmStrength / TORQUE_MODIFIER);
}
DrawingImage dImageSource = new DrawingImage(dGroup);
dGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, kinectSensor.FrameWidth, kinectSensor.FrameHeight));
return dImageSource;
}
