private void RefreshBodyObject(Body body, GameObject bodyObject)
{
Windows.Kinect.Joint leftHandJoint = body.Joints[JointType.HandLeft];
Transform leftHandTransform = bodyObject.transform.Find(leftHandName);
leftHandTransform.localPosition = GetVector3FromJoint(leftHandJoint);
Windows.Kinect.Joint rightHandJoint = body.Joints[JointType.HandRight];
Transform rightHandTransform = bodyObject.transform.Find(rightHandName);
rightHandTransform.localPosition = GetVector3FromJoint(rightHandJoint);
JointOrientation lefthandOrientation = body.JointOrientations[JointType.WristLeft];
Quaternion leftRotationQuaternion = new Quaternion(lefthandOrientation.Orientation.X, lefthandOrientation.Orientation.Y, lefthandOrientation.Orientation.Z, lefthandOrientation.Orientation.W);
Vector3 leftEuler = leftRotationQuaternion.eulerAngles;
leftEuler = new Vector3(0, 0, leftEuler.z);
leftHandTransform.rotation = Quaternion.Euler(leftEuler);
JointOrientation righthandOrientation = body.JointOrientations[JointType.WristRight];
Quaternion rightRotationQuaternion = new Quaternion(righthandOrientation.Orientation.X, righthandOrientation.Orientation.Y, righthandOrientation.Orientation.Z, righthandOrientation.Orientation.W);
Vector3 rightEuler = rightRotationQuaternion.eulerAngles;
rightEuler = new Vector3(0, 0, rightEuler.z);
rightHandTransform.rotation = Quaternion.Euler(rightEuler);
}
void bodyReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
// Retrieve the current body frame and dispose it after the block
using (BodyFrame bFrame = e.FrameReference.AcquireFrame())
{
// Check whether the frame has not expired yet (i.e. it is available)
if (bFrame != null)
{
// Copy the body data into our buffer
bFrame.GetAndRefreshBodyData(bodies);
// TODO If you want to handle more than one body, you have to
// adapt the following lines
// Get the first tracked body
trackedBody = (from s in bodies
where s.IsTracked
select s).FirstOrDefault();
if (trackedBody != null)
{
RecognizeGestures(trackedBody);
JointOrientation headOrientation = trackedBody.JointOrientations[JointType.Head];
CameraSpacePoint headPosition = trackedBody.Joints[JointType.Head].Position;
// ... do stuff
}
// Draw the recognized bodies in the transparent overlay window
DrawStickMen(bodies);
}
}
}
protected Kinect2SJoint(SerializationInfo info, StreamingContext ctx)
{
this.TrackingState = (TrackingState)info.GetValue(Kinect2SJoint.KEY_TrackingState, typeof(TrackingState));
this.JointType = (JointType)info.GetValue(Kinect2SJoint.KEY_JointType, typeof(JointType));
this.CameraSpacePoint = (CameraSpacePoint)info.GetValue(Kinect2SJoint.KEY_CameraSpacePoint, typeof(CameraSpacePoint));
this.Orientation = ((JointOrientation)info.GetValue(Kinect2SJoint.KEY_Orientation, typeof(JointOrientation)));
}
/// <summary>
/// Update the Kinect Objects to the current Body location/rotation
/// </summary>
/// <param name="body">The Body</param>
/// <param name="kinectObjects">The Kinect 3D Objects to update</param>
public static void UpdateKinect3DObjects(Body body, List <Kinect3DObject> kinectObjects)
{
if (kinectObjects == null)
{
return;
}
foreach (Kinect3DObject kinectObject in kinectObjects)
{
Joint j1 = body.Joints[(JointType)kinectObject.FirstJoint];
JointOrientation jo1 = body.JointOrientations[(JointType)kinectObject.FirstJoint];
var grpTransfo = new Transform3DGroup();
if (kinectObject.IsMoveable)
{
kinectObject.OffsetX = (j1.Position.X * 10);
kinectObject.OffsetY = (j1.Position.Y * 10);
grpTransfo.Children.Add(new TranslateTransform3D(-kinectObject.OffsetX, kinectObject.OffsetY, 0));
}
if (kinectObject.IsRotateable)
{
grpTransfo.Children.Add(new RotateTransform3D(
new QuaternionRotation3D(
new Quaternion(jo1.Orientation.X, -jo1.Orientation.Y, -jo1.Orientation.Z, jo1.Orientation.W)),
j1.Position.X,
j1.Position.Y,
j1.Position.Z));
}
kinectObject.Model.Transform = grpTransfo;
}
}
OscElement ProcessHandStateInformation(int joint, Joint j, JointOrientation jo, HandState state, TrackingConfidence confidence, double time, int pointScale)
{
return(SendHandStateMessage(joint,
j.Position.X, j.Position.Y, j.Position.Z,
JointToConfidenceValue(j),
HandStateToValue(state),
TrackingConfidenceToValue(confidence), time,
pointScale));
}
void ProcessHandStateInformation(int joint, Joint j, JointOrientation jo, HandState state, TrackingConfidence confidence, double time, int pointScale, UdpWriter osc, StreamWriter fileWriter)
{
SendHandStateMessage(joint,
j.Position.X, j.Position.Y, j.Position.Z,
JointToConfidenceValue(j),
HandStateToValue(state),
TrackingConfidenceToValue(confidence), time,
pointScale, osc, fileWriter);
}
private static Quaternion GetQuaternionFromJoint(JointOrientation jointOrientation)
{
float ox = jointOrientation.Orientation.X;
float oy = jointOrientation.Orientation.Y;
float oz = jointOrientation.Orientation.Z;
float ow = jointOrientation.Orientation.W;
return(new Quaternion(ox, oy, oz, ow));
}
protected override void SerializeMiddle(BinaryWriter writer)
{
// Format:
// Load Size (4 bytes, signed) | Timestamp (8 bytes, signed) | Frame Type (4 bytes, bitset) | Tracked Body Count (1 byte, unsigned)
// Tracking ID (8 bytes, unsigned) | Hand Left Confidence (1 byte, unsigned) | Hand Left State (1 byte, unsigned)
// Hand Right Confidence (1 byte, unsigned) | Hand Right State (1 byte, unsigned)
// For each Joint, X, Y, Z, W, X, Y, Z (all floats)
//
// Description:
// Load Size: Number of bytes to be read further to read one DepthFrame
// Timestamp: For syncing frames at the client side
// Tracked Body Count: Number of bodies currently tracke by Kinect
// Tracking ID: The Tracking ID assigned by Kinect to the closest body
// Hand Left Confidence:
// Hand Left State:
// Hand Right Confidence:
// Hand Right State:
// All of the joints of the closest body in the format:
// Joint Type:
// Joint Tracking State:
// Joint Position X, Y, Z
// Joint Orientation W, X, Y, Z
// Note that BinaryWriter is documented to write data in little-endian form only
writer.Write(TrackedCount);
writer.Write(Engaged);
if (Engaged)
{
writer.Write(TrackingId);
writer.Write((byte)HandLeftConfidence);
writer.Write((byte)HandLeftState);
writer.Write((byte)HandRightConfidence);
writer.Write((byte)HandRightState);
// Assume we'll always have orientation for a joint that has a position
foreach (JointType j in Joints.Keys)
{
Joint joint = Joints[j];
JointOrientation jointOrient = JointOrientations[j];
writer.Write((byte)j);
writer.Write((byte)joint.TrackingState);
writer.Write(joint.Position.X);
writer.Write(joint.Position.Y);
writer.Write(joint.Position.Z);
writer.Write(jointOrient.Orientation.W);
writer.Write(jointOrient.Orientation.X);
writer.Write(jointOrient.Orientation.Y);
writer.Write(jointOrient.Orientation.Z);
}
}
}
public void AddIfTrackedOrInferred(Joint jointtoadd, JointOrientation jointtoaddoritent, Joint jointparent)
{
if (jointtoadd.TrackingState != TrackingState.NotTracked && jointparent.TrackingState != TrackingState.NotTracked)
{
this.JointQuaternions[jointtoadd.JointType] = this.JointQuaternions[jointparent.JointType] * jointtoaddoritent.Orientation.ToQuaternion();
}
this.JointQuaternions[jointtoadd.JointType] = jointtoaddoritent.Orientation.ToQuaternion();
//(new Quaternion(0,1,0,0)) *
}
public static Kinect2SJoint MakeInstance(Joint joint, JointOrientation orientation)
{
Kinect2SJoint serializableJoint = new Kinect2SJoint();
serializableJoint.TrackingState = joint.TrackingState;
serializableJoint.JointType = joint.JointType;
serializableJoint.CameraSpacePoint = joint.Position;
serializableJoint.Orientation = orientation;
return(serializableJoint);
}
public FubiTrackingData()
{
uint numJoints = (uint)FubiUtils.SkeletonJoint.NUM_JOINTS;
jointOrientations = new JointOrientation[numJoints];
for (uint i = 0; i < numJoints; ++i)
jointOrientations[i] = new JointOrientation();
jointPositions = new JointPosition[numJoints];
for (uint i = 0; i < numJoints; ++i)
jointPositions[i] = new JointPosition();
}
void Start()
{
rb = GetComponent <Rigidbody> ();
speedMilestoneCount = speedIncreaseMilestone;
moveSpeedStore = moveSpeed;
speedIncreaseMilestoneStore = speedIncreaseMilestone;
speedMilestoneCountStore = speedMilestoneCount;
myo = GetComponent <JointOrientation>();
}
private static object SerializeJoint(Body skeleton, JointType joint)
{
Joint bodyJoint = skeleton.Joints[joint];
JointOrientation orientation = skeleton.JointOrientations[joint];
return(new
{
Position = bodyJoint.Position.ToArray(),
State = bodyJoint.TrackingState.ToString(),
Rotation = orientation.Orientation.ToArray()
});
}
public void SetJointData(Joint sourceJoint, JointOrientation sourceOrientation, Quaternion kinectRot, float kinectHeight)
{
var pos = sourceJoint.Position;
var ori = sourceOrientation.Orientation;
position.x = pos.X;
position.y = pos.Y;
position.z = pos.Z;
position = kinectRot * position;
position.y += kinectHeight;
rotation.Set(ori.X, ori.Y, ori.Z, ori.W);
rotation *= kinectRot;
}
public FubiTrackingData()
{
uint numJoints = (uint)FubiUtils.SkeletonJoint.NUM_JOINTS;
jointOrientations = new JointOrientation[numJoints];
for (uint i = 0; i < numJoints; ++i)
{
jointOrientations[i] = new JointOrientation();
}
jointPositions = new JointPosition[numJoints];
for (uint i = 0; i < numJoints; ++i)
{
jointPositions[i] = new JointPosition();
}
}
void ProcessJointInformation(int joint, Joint j, JointOrientation jo, double time, int pointScale, UdpWriter osc, StreamWriter fileWriter)
{
if (j == null)
{
return;
}
if (jo == null)
{
return;
}
SendJointMessage(joint,
j.Position.X, j.Position.Y, j.Position.Z,
JointToConfidenceValue(j), time,
pointScale, osc, fileWriter);
}
OscElement ProcessJointInformation(int joint, Joint j, JointOrientation jo, double time, int pointScale)
{
if (j == null)
{
return(null);
}
if (jo == null)
{
return(null);
}
return(SendJointMessage(joint,
j.Position.X, j.Position.Y, j.Position.Z,
JointToConfidenceValue(j), time,
pointScale));
}
public virtual void Update(JointOrientation jointOrientation)
{
Update((KinectJointOrientation)jointOrientation);
}
private Quaternion JointOrientationToQuaternion(JointOrientation jointOrientation)
{
return(new Quaternion(jointOrientation.Orientation.X, jointOrientation.Orientation.Y, jointOrientation.Orientation.Z, jointOrientation.Orientation.W));
}
void Awake()
{
jointOrientation = hand.GetComponent <JointOrientation>();
}
/// <summary>
/// Store body(skeleton) information
/// </summary>
/// <param name="bodyFrame">body(skeleton) information to be stored</param>
/// <param name="frameNumber">frame number</param>
public static void Handle_BodyFrame(BodyFrame bodyFrame, String frameNumber)
{
String skeletonInfoPath = FramesAndPaths.GetSkeletonFilePath(FramesAndPaths.FileType.SkeletonInfo, "SkeletonInfo.txt");
try
{
using (StreamWriter skeletonWriter = new StreamWriter(skeletonInfoPath, true))
{
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
// A string to store all the skeletons' information in this frame
// 要写入 txt 文件的本帧所有骨骼信息
String peopleInfo = String.Empty;
foreach (Body body in bodies)
{
if (body.IsTracked)
{
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
IReadOnlyDictionary <JointType, JointOrientation> jointOrientations = body.JointOrientations;
// Acquire coordinates on camera space
// 获得 camera space 上关节点的三维坐标
int jointIndex = 0;
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an) inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
cameraSpacePositions[jointIndex] = joints[jointType].Position;
if (cameraSpacePositions[jointIndex].Z < 0)
{
cameraSpacePositions[jointIndex].Z = InferredZPositionClamp;
}
++jointIndex;
}
// Acquire coordinates on color space
// 获得 color space 上的 25 个关节点的二维坐标
MainWindow.coordinateMapper.MapCameraPointsToColorSpace(cameraSpacePositions, colorSpacePositions);
// Acquire coordinates on depth space
// 获得 depth space 上的 25 个关节点的二维坐标
MainWindow.coordinateMapper.MapCameraPointsToDepthSpace(cameraSpacePositions, depthSpacePositions);
// Acquire orientation information
// 获得关节点的旋转信息
jointIndex = 0;
foreach (JointType jointType in jointOrientations.Keys)
{
JointOrientation tmpOrientation = jointOrientations[jointType];
orientations[jointIndex++] = tmpOrientation.Orientation;
}
// frame number、tracking % 6、floor
ulong resizeId = body.TrackingId % 6;
Vector4 floor = bodyFrame.FloorClipPlane;
//String personInfo = String.Format("{0}, id = {1}, color = {2},\r\n",GlobalData.FrameNumberIncrement(GlobalData.FileType.SkeletonInfo),resizeId,bodyColors[resizeId]);
// A string to store current skeleton's information in this frame
// 要加入 peopleInfo 的本帧当前一具骨骼信息 personInfo
String personInfo = String.Format("{0}, id = {1}, floor = {2} {3} {4} {5}", frameNumber, resizeId,
floor.W, floor.X, floor.Y, floor.Z);
personInfo += "\r\n";
// Append coordinates on camera space
// 相机空间三维坐标
personInfo += "\tcamera_space_coordinates =";
personInfo += String.Format(" {0} {1} {2}", cameraSpacePositions[0].X, cameraSpacePositions[0].Y, cameraSpacePositions[0].Z);
for (int i = 1; i < cameraSpacePositions.Length; ++i)
{
personInfo += String.Format(", {0} {1} {2}", cameraSpacePositions[i].X, cameraSpacePositions[i].Y, cameraSpacePositions[i].Z);
}
personInfo += "\r\n";
// Append coordiantes on color space
// rgb骨架二维坐标
personInfo += "\tcolor_skeleton_coordinates =";
personInfo += String.Format(" {0} {1}", colorSpacePositions[0].X, colorSpacePositions[0].Y);
for (int i = 1; i < colorSpacePositions.Length; ++i)
{
personInfo += String.Format(", {0} {1}", colorSpacePositions[i].X, colorSpacePositions[i].Y);
}
personInfo += "\r\n";
// Append coordinates on depth space
// depth骨架二维坐标
personInfo += "\tdepth_skeleton_coordinates =";
personInfo += String.Format(" {0} {1}", depthSpacePositions[0].X, depthSpacePositions[0].Y);
for (int i = 1; i < depthSpacePositions.Length; ++i)
{
personInfo += String.Format(", {0} {1}", depthSpacePositions[i].X, depthSpacePositions[i].Y);
}
personInfo += "\r\n";
personInfo += "\tskeleton_orientations =";
personInfo += String.Format(" {0} {1} {2} {3}", orientations[0].W, orientations[0].X, orientations[0].Y, orientations[0].Z);
for (int i = 1; i < orientations.Length; ++i)
{
personInfo += String.Format(", {0} {1} {2} {3}", orientations[i].W, orientations[i].X, orientations[i].Y, orientations[i].Z);
}
peopleInfo += personInfo;
peopleInfo += "\r\n";
}
}
skeletonWriter.Write(peopleInfo);
}
}
catch (System.IO.IOException)
{ }
// Release bodyFrame
bodyFrame.Dispose();
}
/// <summary>
/// Update the joint orientation based on the referened <c>JointOrientation</c>.
/// </summary>
/// <param name="jointOrientation"></param>
public void Update(JointOrientation jointOrientation)
{
this._jointOrientation = jointOrientation;
}
/// <summary>
/// Update the joint position based on the referened <c>Joint</c>.
/// </summary>
public virtual void Update(JointOrientation jointOrientation)
{
Update((KinectJointOrientation)jointOrientation);
}
protected static CustomBody CreateBodyFromReader(BinaryReader reader, Version version)
{
CustomBody body = new CustomBody();
try
{
body.IsTracked = reader.ReadBoolean();
if (body.IsTracked)
{
#region Old Version
if (version < v02)
{
int count;
count = reader.ReadInt32(); // Activities.Count
for (int i = 0; i < count; i++)
{
reader.ReadInt32(); // key
reader.ReadInt32(); // value
}
count = reader.ReadInt32(); // Appearance.Count
for (int i = 0; i < count; i++)
{
reader.ReadInt32(); // key
reader.ReadInt32(); // value
}
}
#endregion
body.ClippedEdges = (FrameEdges)reader.ReadInt32();
#region Old Version
if (version < v02)
{
reader.ReadInt32(); // Engaged
int count;
count = reader.ReadInt32(); // Expressions.Count
for (int i = 0; i < count; i++)
{
reader.ReadInt32(); // key
reader.ReadInt32(); // value
}
}
#endregion
body.HandLeftConfidence = (TrackingConfidence)reader.ReadInt32();
body.HandLeftState = (HandState)reader.ReadInt32();
body.HandRightConfidence = (TrackingConfidence)reader.ReadInt32();
body.HandRightState = (HandState)reader.ReadInt32();
body.IsRestricted = reader.ReadBoolean();
int orientationCount = reader.ReadInt32();
for (var i = 0; i < orientationCount; i++)
{
var key = (JointType)reader.ReadInt32();
var value = new JointOrientation();
value.JointType = (JointType)reader.ReadInt32();
var vector = new Vector4();
vector.W = reader.ReadSingle();
vector.X = reader.ReadSingle();
vector.Y = reader.ReadSingle();
vector.Z = reader.ReadSingle();
value.Orientation = vector;
var dict = body.JointOrientations as IDictionary <JointType, JointOrientation>;
if (dict != null)
{
dict[key] = value;
}
}
int jointCount = reader.ReadInt32();
for (var i = 0; i < jointCount; i++)
{
var key = (JointType)reader.ReadInt32();
var value = new CustomJoint(key);
value.JointType = (JointType)reader.ReadInt32();
var position = new CameraSpacePoint();
position.X = reader.ReadSingle();
position.Y = reader.ReadSingle();
position.Z = reader.ReadSingle();
value.Position = position;
if (version > v03)
{
var depthPosition = new DepthSpacePoint();
depthPosition.X = reader.ReadSingle();
depthPosition.Y = reader.ReadSingle();
value.DepthPosition = depthPosition;
var colorPosition = new ColorSpacePoint();
colorPosition.X = reader.ReadSingle();
colorPosition.Y = reader.ReadSingle();
value.ColorPosition = colorPosition;
}
value.TrackingState = (TrackingState)reader.ReadInt32();
var dict = body.Joints as IDictionary <JointType, IJoint>;
if (dict != null)
{
dict[key] = value;
}
}
#if NETFX_CORE
var lean = new Point();
#else
var lean = new PointF();
#endif
lean.X = reader.ReadSingle();
lean.Y = reader.ReadSingle();
body.Lean = lean;
body.LeanTrackingState = (TrackingState)reader.ReadInt32();
body.TrackingId = reader.ReadUInt64();
if (version > v03)
{
body.HasMappedDepthPositions = reader.ReadBoolean();
body.HasMappedColorPositions = reader.ReadBoolean();
}
}
}
catch (Exception ex)
{
// TODO: Log This
System.Diagnostics.Debug.WriteLine(ex);
}
return(body);
}
private static Vector4 GetVector4FromJoint(JointOrientation joint)
{
return(new Vector4(joint.Orientation.X, joint.Orientation.Y, joint.Orientation.Z, joint.Orientation.W));
}
public static Quaternion JointOrientationToQuatertnion(JointOrientation a_jointOrient)
{
return(new Quaternion(a_jointOrient.Orientation.X, a_jointOrient.Orientation.Y,
a_jointOrient.Orientation.Z, a_jointOrient.Orientation.W));
}
void Start()
{
jointOrientation = JointOrientationObject.GetComponent <JointOrientation> ();
generator = generatorObject.GetComponent <Generator> ();
}
/// <summary>
/// Initializes a new instance of the <see cref="KinectJointOrientation"/> class.
/// </summary>
/// <param name="jointOrientation">The joint orientation.</param>
public KinectJointOrientation(JointOrientation jointOrientation)
{
this._jointOrientation = jointOrientation;
}
/// <summary>
/// Calculates the rotation of the specified joint orientation as a CameraSpacePoint.
/// </summary>
/// <param name="orientation">The orientation of a joint.</param>
/// <returns>The joint rotation in the X, Y and Z axis.</returns>
public static CameraSpacePoint Rotation(this JointOrientation orientation)
{
return(orientation.Orientation.QuaternionToEuler());
}
/// <summary>
/// Update the joint orientation based on the referened <c>JointOrientation</c>.
/// </summary>
/// <param name="jointOrientation"></param>
public void Update(JointOrientation jointOrientation)
{
this._jointOrientation = jointOrientation;
}
// Kinect -> Quaternion
private Quaternion kinectToQuaternion( JointOrientation j )
{
return new Quaternion (j.Orientation.X, j.Orientation.Y, j.Orientation.Z, j.Orientation.W);
}
public PseudoJoint(Joint joint, JointOrientation aOrientation)
{
this.joint = joint;
this.orientation = aOrientation;
}
public OscMessage BuildJointMessage(Body body, KeyValuePair <JointType, Joint> joint, JointOrientation orient)
{
var address = String.Format("/bodies/{0}/joints/{1}", body.TrackingId, joint.Key);
var position = joint.Value.Position;
//System.Diagnostics.Debug.WriteLine(address);
return(new OscMessage(address, position.X, position.Y, position.Z, orient.Orientation.X, orient.Orientation.Y, orient.Orientation.Z, orient.Orientation.W, joint.Value.TrackingState.ToString()));
}
private System.Numerics.Vector4 ToVector4(JointOrientation jointOrientation)
{
return(new System.Numerics.Vector4(jointOrientation.Orientation.X, jointOrientation.Orientation.Y,
jointOrientation.Orientation.Z, jointOrientation.Orientation.W));
}
public Quaternion ToQuaternion(JointOrientation j) => new Quaternion(j.Orientation.X, j.Orientation.Y, j.Orientation.Z, j.Orientation.W);
void Start()
{
jointOrientation = JointOrientationObject.GetComponent<JointOrientation> ();
generator = generatorObject.GetComponent<Generator> ();
}
