private void UpdateVertices(NISelectedPlayer player, SkeletonJointPosition torsoPos)
{
//initial adjustment wait...
int countBuffer = 1; int countMax = 10;
if (countBoneLenghthUpdate >= countMax)
{
return;
}
if (countBoneLenghthUpdate <= countBuffer)
{
countBoneLenghthUpdate++;
return;
}
//adjustment
int count = countBoneLenghthUpdate - countBuffer;
if (count == 2)//for waist
{
SkinnedMeshRenderer mesh = GetComponentInChildren <SkinnedMeshRenderer>();
Vector3[] vertices = mesh.sharedMesh.vertices;
vertices[0].y += 100;
}
}
void TransformBone(uint userId, SkeletonJoint joint, Transform dest, bool move)
{
SkeletonJointPosition sjp = this.joints[userId][joint];
Point3D pos = sjp.position;
SkeletonJointOrientation ori = new SkeletonJointOrientation();
this.skeletonCapbility.GetSkeletonJointOrientation(userId, joint, ori);
float [] m = ori.Orientation.elements;
// only modify joint if confidence is high enough in this frame
if (ori.Confidence > 0.5)
{
// Z coordinate in OpenNI is opposite from Unity. We will create a quat
// to rotate from OpenNI to Unity (relative to initial rotation)
// Vector3 worldZVec = new Vector3(-ori.m02, -ori.m12, ori.m22);
Vector3 worldZVec = new Vector3(-m[2], -m[5], m[8]);
// Vector3 worldYVec = new Vector3(trans.ori.m01, trans.ori.m11, -trans.ori.m21);
Vector3 worldYVec = new Vector3(m[1], m[4], -m[7]);
Quaternion jointRotation = Quaternion.LookRotation(worldZVec, worldYVec);
Quaternion newRotation = jointRotation * initialRotations[(int)joint];
// Some smoothing
dest.rotation = Quaternion.Slerp(dest.rotation, newRotation, Time.deltaTime * 20);
}
if (move)
{
// dest.position = new Vector3(trans.pos.x/1000, trans.pos.y/1000 -1, -trans.pos.z/1000);
dest.position = new Vector3(pos.X / 1000, pos.Y / 1000 - 1, -pos.Z / 1000);
}
}
private Ray3D calibrateDeviceOnePosition(int user, byte device)
{
UserPrompt.Write("Turning on device " + device);
Device.turnOn(device);
Thread.Sleep(CALIBRATION_OFFSET_SEC * 1000);
Vector3D[] headPoints = new Vector3D[STEADY_SEC * SAMPLES_PER_SEC];
Vector3D[] rightHandPoints = new Vector3D[STEADY_SEC * SAMPLES_PER_SEC];
// Sample the user's hopefully steady hand.
SkeletonJointPosition head = new SkeletonJointPosition();
SkeletonJointPosition rightHand = new SkeletonJointPosition();
for (int i = 0; i < STEADY_SEC * SAMPLES_PER_SEC; i++)
{
head = userGenerator.SkeletonCapability.GetSkeletonJointPosition(user, SkeletonJoint.Head);
rightHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(user, SkeletonJoint.RightHand);
headPoints[i] = new Vector3D(head.Position.X, head.Position.Y, head.Position.Z);
rightHandPoints[i] = new Vector3D(rightHand.Position.X, rightHand.Position.Y, rightHand.Position.Z);
}
Thread.Sleep((CALIBRATION_SEC - STEADY_SEC - CALIBRATION_OFFSET_SEC) * 1000);
// Take the averages of each side.
Vector3D averageHeadPoint = new Vector3D(headPoints.Average(x => x.X),
headPoints.Average(x => x.Y), headPoints.Average(x => x.Z));
Vector3D averageRightHandPoint = new Vector3D(rightHandPoints.Average(x => x.X),
rightHandPoints.Average(x => x.Y), rightHandPoints.Average(x => x.Z));
UserPrompt.Write("Turning off device " + device);
Device.turnOff(device);
return(new Ray3D(averageHeadPoint, averageRightHandPoint));
}
public void UpdateBoneDistance(NISelectedPlayer player, SkeletonJointPosition torsoPos)
{
//initial adjustment wait...
int countBuffer = 6; int countMax = 10;
if (countBoneLenghthUpdate >= countMax)
{
return;
}
if (countBoneLenghthUpdate <= countBuffer)
{
countBoneLenghthUpdate++;
return;
}
//bone distance adjustment
int count = countBoneLenghthUpdate - countBuffer;
if (count == 1)//for waist
{
//AdjustBone(SkeletonJoint.Waist, SkeletonJoint.Torso, player, torsoPos);
//AdjustBone(SkeletonJoint.Neck, SkeletonJoint.Waist, player, torsoPos);
AdjustBone(SkeletonJoint.LeftShoulder, SkeletonJoint.RightShoulder, player, torsoPos);
AdjustBone(SkeletonJoint.LeftHip, SkeletonJoint.RightHip, player, torsoPos);
AdjustBone(SkeletonJoint.LeftElbow, SkeletonJoint.LeftShoulder, player, torsoPos);
AdjustBone(SkeletonJoint.LeftHand, SkeletonJoint.LeftElbow, player, torsoPos);
AdjustBone(SkeletonJoint.LeftKnee, SkeletonJoint.LeftHip, player, torsoPos);
//AdjustBone(SkeletonJoint.LeftFoot, SkeletonJoint.LeftKnee, player, torsoPos);
}
countBoneLenghthUpdate++;
}
public void JointFromJSON(SkeletonJoint j, Hashtable dict)
{
ArrayList positionList = (ArrayList)dict["Position"];
ArrayList orientationList = (ArrayList)dict["Orientation"];
SkeletonJointOrientation sjo = new SkeletonJointOrientation();
sjo.X1 = 1.0f;
SkeletonJointPosition sjp = new SkeletonJointPosition();
SkeletonJointTransformation xform = new SkeletonJointTransformation();
// object -> double ->float is okay, but object->float isn't
// (the object is a Double)
sjp.Position = new Point3D((float)(double)positionList[0],
(float)(double)positionList[1],
(float)(double)positionList[2]);
sjo.X1 = (float)(double)orientationList[0];
sjo.X2 = (float)(double)orientationList[1];
sjo.X3 = (float)(double)orientationList[2];
sjo.Y1 = (float)(double)orientationList[3];
sjo.Y2 = (float)(double)orientationList[4];
sjo.Y3 = (float)(double)orientationList[5];
sjo.Z1 = (float)(double)orientationList[6];
sjo.Z2 = (float)(double)orientationList[7];
sjo.Z3 = (float)(double)orientationList[8];
xform.Orientation = sjo;
xform.Position = sjp;
UpdateJoint(j, xform);
}
public void DrawSingleUser(ref WriteableBitmap image, int id, UserGenerator userGenerator, Point3D corner)
{
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.LeftHand, SkeletonJoint.LeftElbow, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.LeftElbow, SkeletonJoint.LeftShoulder, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.LeftShoulder, SkeletonJoint.Torso, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.LeftShoulder, SkeletonJoint.RightShoulder, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.Torso, SkeletonJoint.RightShoulder, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.RightShoulder, SkeletonJoint.RightElbow, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.RightElbow, SkeletonJoint.RightHand, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.Neck, SkeletonJoint.Head, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.Torso, SkeletonJoint.LeftHip, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.Torso, SkeletonJoint.RightHip, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.LeftHip, SkeletonJoint.RightHip, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.LeftHip, SkeletonJoint.LeftKnee, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.LeftKnee, SkeletonJoint.LeftFoot, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.RightHip, SkeletonJoint.RightKnee, corner);
DrawStickLine(ref image, id, userGenerator, SkeletonJoint.RightKnee, SkeletonJoint.RightFoot, corner);
DrawHeadAndHands(ref image, id, userGenerator, depthGenerator);
SkeletonJointPosition leftShoulder = new SkeletonJointPosition();
SkeletonJointPosition rightShoulder = new SkeletonJointPosition();
SkeletonJointPosition neck = new SkeletonJointPosition();
SkeletonJointPosition midShoulder = new SkeletonJointPosition();
leftShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftShoulder);
rightShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightShoulder);
neck = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.Neck);
midShoulder.Position = new Point3D((leftShoulder.Position.X + rightShoulder.Position.X) / 2,
(leftShoulder.Position.Y + rightShoulder.Position.Y) / 2,
(leftShoulder.Position.Z + rightShoulder.Position.Z) / 2);
midShoulder.Confidence = (leftShoulder.Confidence + rightShoulder.Confidence) / 2;
}
public SkeletonJointPosition GetSkeletonJointPosition(UserID user, SkeletonJoint joint)
{
SkeletonJointPosition position = new SkeletonJointPosition();
int status = SafeNativeMethods.xnGetSkeletonJointPosition(this.InternalObject, user, joint, ref position);
WrapperUtils.ThrowOnError(status);
return(position);
}
void MoveTransform(uint userId, SkeletonJoint joint, Transform dest)
{
SkeletonJointPosition pos = new SkeletonJointPosition();
this.skeletonCapbility.GetSkeletonJointPosition(userId, joint, ref pos);
Vector3 v3pos = new Vector3(pos.position.X, pos.position.Y, pos.position.Z);
dest.position = (v3pos / scale) + bias;
}
public static bool GetJointPosition(uint userID, int joint, ref SkeletonJointPosition pPosition)
{
if (GetJointPositionNative(userID, joint, ref pPosition))
{
//pPosition.x = -pPosition.x;
return(true);
}
return(false);
}
// Update is called once per frame
void Update()
{
//Debug.Log("Update");
this.context.WaitOneUpdateAll(this.depth);
int[] users = this.userGenerator.GetUsers();
foreach (int user in users)
{
if (this.skeletonCapability.IsTracking(user))
{
updatePuntoRef(skeletonCapability.GetSkeletonJointPosition(user, SkeletonJoint.Torso));
SkeletonJointPosition posHandIz = skeletonCapability.GetSkeletonJointPosition(user, SkeletonJoint.RightHand);
SkeletonJointPosition posHandDr = skeletonCapability.GetSkeletonJointPosition(user, SkeletonJoint.LeftHand);
if (isDentroCuadroSeguridad(posHandDr.Position) & isDentroCuadroSeguridad(posHandIz.Position))
{
float dist = distanciaEntreDosPuntos(posHandDr.Position, posHandIz.Position) / 100f;
float noNormalActual = (dist * 1f) / ESCALA_DISTANCIA;
int disNormal = (int)dist;
float esc = (disNormal * 1f) / ESCALA_DISTANCIA;
Debug.Log("Normal " + esc + " NO Normal " + noNormalActual);
Debug.Log(Mathf.Abs(noNormalActual - esc));
if (Mathf.Abs(noNormalActual - esc) < 0.4f && Mathf.Abs(noNormalActual - esc) > 0.1f)
{
transform.localScale = Vector3.Lerp(transform.localScale, new Vector3(esc, esc, esc), Time.time);
}
/*
* //Rotacion horizontal
* float x,y,z;
* x=y=z=0;
* if((posHandDr.Position.Z<posHandIz.Position.Z&isDentroMargenY(posHandDr.Position.Y,posHandIz.Position.Y))&(!isDentroMargenZ(posHandDr.Position.Z,posHandIz.Position.Z))){
* y=-valorRotation;
* }else if((posHandDr.Position.Z>posHandIz.Position.Z&isDentroMargenY(posHandDr.Position.Y,posHandIz.Position.Y))&(!isDentroMargenZ(posHandDr.Position.Z,posHandIz.Position.Z))){
* y=valorRotation;
* }
* //rotacion vertical
* if((posHandDr.Position.Y<posHandIz.Position.Y&isDentroMargenZ(posHandDr.Position.Z,posHandIz.Position.Z))&(!isDentroMargenY(posHandDr.Position.Y,posHandIz.Position.Y))){
* x=-valorRotation;
* }else if((posHandDr.Position.Y>posHandIz.Position.Y&isDentroMargenZ(posHandDr.Position.Z,posHandIz.Position.Z))&(!isDentroMargenY(posHandDr.Position.Y,posHandIz.Position.Y))){
* x=valorRotation;
* }
* transform.Rotate(new Vector3(x,y,z));
*/
}
//x
//positivo es hacia arriba
//negativo hacia abajo
}
}
}
public Vector3 CalcActualPosition(SkeletonJoint joint, NISelectedPlayer player, SkeletonJointPosition torsoPos)
{
Vector3 actualPos;
SkeletonJointTransformation trans;
if (player.GetSkeletonJoint(joint, out trans) == false || trans.Position.Confidence <= 0.5)
{
Debug.Log("No actual position for Joint: " + joint);
actualPos = ERROR_VECTOR;
}
actualPos = CalcJointPosition(joint, ref trans, ref torsoPos);
return(actualPos);
}
/// <summary>
/// Loads the 2D data for a specific skeleton joint.
/// </summary>
private void GetJoint(SkeletonCapability source,
int user, SkeletonJoint joint,
Dictionary <SkeletonJoint, SkeletonJointPosition> target)
{
SkeletonJointPosition pos;
if (joint == SkeletonJoint.Waist)
{
// Calculate the joint position as arithmetic mean of right
// and left hip joints, as it is not possible to poll it
// directly.
pos = new SkeletonJointPosition();
SkeletonJointPosition posLeft = source.GetSkeletonJointPosition(user, SkeletonJoint.LeftHip);
SkeletonJointPosition posRight = source.GetSkeletonJointPosition(user, SkeletonJoint.RightHip);
if (posLeft.Position.Z == 0 || posRight.Position.Z == 0)
{
pos.Confidence = 0;
pos.Position = new Point3D(
(posLeft.Position.X + posRight.Position.X) / 2,
(posLeft.Position.Y + posRight.Position.Y) / 2,
0);
}
else
{
pos.Confidence = Math.Min(posLeft.Confidence, posRight.Confidence);
pos.Position = depthGenerator.ConvertRealWorldToProjective(
new Point3D(
(posLeft.Position.X + posRight.Position.X) / 2,
(posLeft.Position.Y + posRight.Position.Y) / 2,
(posLeft.Position.Z + posRight.Position.Z) / 2));
}
}
else
{
pos = source.GetSkeletonJointPosition(user, joint);
if (pos.Position.Z == 0)
{
pos.Confidence = 0;
}
else
{
pos.Position = depthGenerator.ConvertRealWorldToProjective(pos.Position);
}
}
target[joint] = pos;
}
private void GetJoint(int user, SkeletonJoint joint)
{
SkeletonJointPosition pos = this.skeletonCapbility.GetSkeletonJointPosition(user, joint);
if (pos.Position.Z == 0)
{
pos.Confidence = 0;
}
else
{
pos.Position = this.depth.ConvertRealWorldToProjective(pos.Position);
}
this.joints[user][joint] = pos;
}
/// @brief updates the root position
///
/// This method updates the root position and if m_updateRootPosition is true, also move the entire transform
/// @note we do not update if we do not have a high enough confidence!
/// @param skelRoot the new central position
/// @param centerOffset the offset we should use on the center (when moving the root).
/// This is usually the starting position (so the skeleton will not "jump" when doing the first update
protected void UpdateRoot(SkeletonJointPosition skelRoot, Vector3 centerOffset)
{
if (skelRoot.Confidence < 0.5f)
{
return; // we are not confident enough!
}
m_rootPosition = NIConvertCoordinates.ConvertPos(skelRoot.Position);
m_rootPosition -= centerOffset;
m_rootPosition *= m_scale;
m_rootPosition += m_originalRootPosition;
if (m_updateRootPosition)
{
transform.position = transform.rotation * m_rootPosition;
}
}
SkeletonJointPosition GetJointPosition(uint user, SkeletonJoint joint)
{
SkeletonJointPosition pos = new SkeletonJointPosition();
this.skeletonCapbility.GetSkeletonJointPosition(user, joint, ref pos);
if (pos.position.Z == 0)
{
pos.fConfidence = 0;
}
else
{
pos.position = this.depth.ConvertRealWorldToProjective(pos.position);
}
return(pos);
}
public void DrawStickPoint(ref WriteableBitmap image, SkeletonJointPosition joint, Point3D corner)
{
byte[] point = { 0, 0, 255, 0,
0, 0, 255, 0,
0, 0, 255, 0,
0, 0, 255, 0,
0, 0, 255, 0,
0, 0, 255, 0,
0, 0, 255, 0,
0, 0, 255, 0,
0, 0, 255, 0, };
image.Lock();
image.WritePixels(new Int32Rect(Convert.ToInt32(joint.Position.X - 1),
Convert.ToInt32(joint.Position.Y - 1),
3, 3), point, 4, 0);
image.Unlock();
}
// 骨格の線を引く
void DrawLine(int player, SkeletonJoint eJoint1, SkeletonJoint eJoint2)
{
// 各箇所の座標を取得する
SkeletonJointPosition joint1 = GetJointPosition(player, eJoint1);
SkeletonJointPosition joint2 = GetJointPosition(player, eJoint2);
if (joint1.Confidence < 0.5 || joint2.Confidence < 0.5)
{
return;
}
// 現実の座標から画面の座標に変換する
Point3D[] pt = new Point3D[] { joint1.Position, joint2.Position };
pt = depth.ConvertRealWorldToProjective(pt);
Graphics g = Graphics.FromImage(bitmap);
g.DrawLine(pen, pt[0].X, pt[0].Y, pt[1].X, pt[1].Y);
}
/// <summary>
/// Converts a JointDictionary with 3D data to the corresponding one
/// with 2D data. Note that the Z-Coordinate is left unchanged, though
/// it has no meaning in the resulting 2D space and can be seen as 0.
/// </summary>
public JointDictionary Convert3Dto2D(JointDictionary source, DepthGenerator generator)
{
if (!source.Is3DData)
{
return(new JointDictionary(source));
}
else
{
JointDictionary ret = new JointDictionary(false);
foreach (SkeletonJoint joint in source.Keys)
{
SkeletonJointPosition pos = new SkeletonJointPosition();
pos.Confidence = source[joint].Confidence;
pos.Position = generator.ConvertRealWorldToProjective(source[joint].Position);
ret.Add(joint, pos);
}
return(ret);
}
}
/// This method tries to find the joint position of the relevant hand
///
/// @param confidence the confidence of the point found (if no point was found then a negative confidence is provided)
/// @return true if we found a position, false otherwise (in which case point is undefined).
protected Point3D GetSkelPoint(out float confidence)
{
confidence = -1.0f;
int userID;
SkeletonCapability skel = GetSkeletonCapability(out userID);
if (skel == null)
{
return(Point3D.ZeroPoint);
}
if (skel.IsJointAvailable(m_jointToUse) == false)
{
return(Point3D.ZeroPoint);
}
SkeletonJointPosition skelPos = skel.GetSkeletonJointPosition(userID, m_jointToUse);
confidence = skelPos.Confidence;
//Debug.Log("pos=(" + skelPos.Position.X + "," + skelPos.Position.Y + "," + skelPos.Position.Z + ") confidence=" + skelPos.Confidence+" time="+Time.frameCount);
return(skelPos.Position);
}
/// @brief Gets the current joint position for a specific joint
///
/// @param joint The joint we want the position to.
/// @param curPos [out] The current joint rotation
/// @return True on success and false on failure (e.g. the user is not tracking).
/// @note An exception might occur if there is an error (e.g. an illegal joint is used).
public bool GetSkeletonJointPosition(SkeletonJoint joint, out SkeletonJointPosition curPos)
{
curPos = NIPlayerCandidateObject.m_InitializedZero.Position;
if (!Tracking)
{
return(false);
}
if (m_user.Skeleton == null)
{
return(false);
}
try
{
curPos = m_user.Skeleton.GetSkeletonJointPosition(m_user.OpenNIUserID, joint);
}
catch
{
return(false);
}
return(true);
}
public void DrawOrientation(ref WriteableBitmap image, int id, UserGenerator userGenerator, SkeletonJoint joint, Point3D corner)
{
SkeletonJointOrientation orientation = new SkeletonJointOrientation();
SkeletonJointPosition position = new SkeletonJointPosition();
position = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, joint);
orientation = userGenerator.SkeletonCapability.GetSkeletonJointOrientation(id, joint);
if (position.Confidence != 1 && orientation.Confidence != 1)
{
return;
}
SkeletonJointPosition v1 = new SkeletonJointPosition();
SkeletonJointPosition v2 = new SkeletonJointPosition();
v1.Confidence = v2.Confidence = 1;
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X1,
v1.Position.Y + 100 * orientation.Y1,
v1.Position.Z + 100 * orientation.Z1);
DrawTheLine(ref image, ref v1, ref v2);
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X2,
v1.Position.Y + 100 * orientation.Y2,
v1.Position.Z + 100 * orientation.Z2);
DrawTheLine(ref image, ref v1, ref v2);
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X3,
v1.Position.Y + 100 * orientation.Y3,
v1.Position.Z + 100 * orientation.Z3);
DrawTheLine(ref image, ref v1, ref v2);
}
/// <summary>
/// Faz a leitura da próxima pose do usuário.
/// </summary>
public bool NextPose(User user, Skeleton skeleton)
{
if (user != null && reading)
{
if (reader.EndOfStream)
{
return(false);
}
var line = reader.ReadLine().Split(' ', '\r');
SkeletonJoint joint = SkeletonJoint.RightHand;
var jointPosition = new SkeletonJointPosition();
var point = new Point3D();
point.X = float.Parse(line[0]);
point.Y = float.Parse(line[1]);
point.Z = float.Parse(line[2]);
jointPosition.Position = point;
user.JointsPosition[joint] = jointPosition;
}
return(true);
}
public void DrawStickLine(ref WriteableBitmap image, int id, UserGenerator userGenerator, SkeletonJoint first, SkeletonJoint second, Point3D corner)
{
SkeletonJointPosition a = new SkeletonJointPosition();
SkeletonJointPosition b = new SkeletonJointPosition();
a = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, first);
b = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, second);
if (a.Confidence == 1 && b.Confidence == 1)
{
// choose color
}
else
{
if ((a.Position.X == 0 && a.Position.Y == 0 && a.Position.Z == 0) ||
(b.Position.X == 0 && b.Position.Y == 0 && b.Position.Z == 0))
{
return;
}
}
DrawTheLine(ref image, ref a, ref b);
}
public void DrawHeadAndHands(ref WriteableBitmap image, int id, UserGenerator userGenerator, DepthGenerator depthGenerator)
{
int headSize = 40; int handSize = 20;
SkeletonJointPosition head = new SkeletonJointPosition();
SkeletonJointPosition leftHand = new SkeletonJointPosition();
SkeletonJointPosition rightHand = new SkeletonJointPosition();
head = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.Head);
leftHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftHand);
rightHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightHand);
image.Lock();
var b = new Bitmap(image.PixelWidth, image.PixelHeight, image.BackBufferStride, System.Drawing.Imaging.PixelFormat.Format24bppRgb,
image.BackBuffer);
using (var bitmapGraphics = System.Drawing.Graphics.FromImage(b))
{
bitmapGraphics.SmoothingMode = SmoothingMode.HighSpeed;
bitmapGraphics.InterpolationMode = InterpolationMode.NearestNeighbor;
bitmapGraphics.CompositingMode = CompositingMode.SourceCopy;
bitmapGraphics.CompositingQuality = CompositingQuality.HighSpeed;
int[] headCoord = ConvertCoord(head, -headSize / 2);
int[] leftHandCoord = ConvertCoord(leftHand, -handSize / 2);
int[] rightHandCoord = ConvertCoord(rightHand, -handSize / 2);
bitmapGraphics.DrawEllipse(Pens.BlueViolet, headCoord[0], headCoord[1], headSize, headSize);
bitmapGraphics.DrawEllipse(Pens.BlueViolet, leftHandCoord[0], leftHandCoord[1], handSize, handSize);
bitmapGraphics.DrawEllipse(Pens.BlueViolet, rightHandCoord[0], rightHandCoord[1], handSize, handSize);
bitmapGraphics.Dispose();
}
image.AddDirtyRect(new Int32Rect(0, 0, image.PixelWidth, image.PixelHeight));
image.Unlock();
}
public void DrawTheLine(ref WriteableBitmap image, ref SkeletonJointPosition joint1, ref SkeletonJointPosition joint2)
{
image.Lock();
var b = new Bitmap(image.PixelWidth, image.PixelHeight, image.BackBufferStride, System.Drawing.Imaging.PixelFormat.Format24bppRgb,
image.BackBuffer);
using (var bitmapGraphics = System.Drawing.Graphics.FromImage(b))
{
bitmapGraphics.SmoothingMode = SmoothingMode.HighSpeed;
bitmapGraphics.InterpolationMode = InterpolationMode.NearestNeighbor;
bitmapGraphics.CompositingMode = CompositingMode.SourceCopy;
bitmapGraphics.CompositingQuality = CompositingQuality.HighSpeed;
int[] joint1Coord = ConvertCoord(joint1, 0);
int[] joint2Coord = ConvertCoord(joint2, 0);
bitmapGraphics.DrawLine(Pens.BlueViolet, joint1Coord[0], joint1Coord[1], joint2Coord[0], joint2Coord[1]);
bitmapGraphics.Dispose();
}
image.AddDirtyRect(new Int32Rect(0, 0, image.PixelWidth, image.PixelHeight));
image.Unlock();
}
/// @brief Utility method to calculate the @b LOCAL position of a joint
///
/// This method receives joint information and calculates the @b LOCAL position rotation of the joint
/// (compare to its parent transform) in Unity coordinate system.
/// @param centerOffset the new central position
/// @param joint the joint we want to calculate the position for
/// @param skelTrans the new transformation of the joint
/// @return the @b LOCAL position rotation of the joint (compare to its parent transform) in
/// Unity coordinate system
protected Vector3 CalcJointPosition(SkeletonJoint joint, ref SkeletonJointTransformation skelTrans, ref SkeletonJointPosition centerOffset)
{
Vector3 v3pos = NIConvertCoordinates.ConvertPos(skelTrans.Position.Position);
Vector3 v3Center = NIConvertCoordinates.ConvertPos(centerOffset.Position);
v3pos -= v3Center;
return(v3pos * m_scale);
}
/// @brief Utility method to calculate the rotation of a joint
///
/// This method receives joint information and calculates the rotation of the joint in Unity
/// coordinate system.
/// @param centerOffset the new central position
/// @param joint the joint we want to calculate the rotation for
/// @param skelTrans the new transformation of the joint
/// @return the rotation of the joint in Unity coordinate system
protected Quaternion CalcRotationForJoint(SkeletonJoint joint, ref SkeletonJointTransformation skelTrans, ref SkeletonJointPosition centerOffset)
{
// In order to convert the skeleton's orientation to Unity orientation we will
// use the Quaternion.LookRotation method to create the relevant rotation Quaternion.
// for Quaternion.LookRotation to work it needs a "forward" vector and an "upward" vector.
// These are generally the "Z" and "Y" axes respectively in the sensor's coordinate
// system. The orientation received from the skeleton holds these values in their
// appropriate members.
// Get the forward axis from "z".
Point3D sensorForward = Point3D.ZeroPoint;
sensorForward.X = skelTrans.Orientation.Z1;
sensorForward.Y = skelTrans.Orientation.Z2;
sensorForward.Z = skelTrans.Orientation.Z3;
// convert it to Unity
Vector3 worldForward = NIConvertCoordinates.ConvertPos(sensorForward);
worldForward *= -1.0f; // because the Unity "forward" axis is opposite to the world's "z" axis.
if (worldForward.magnitude == 0)
{
return(Quaternion.identity); // we don't have a good point to work with.
}
// Get the upward axis from "Y".
Point3D sensorUpward = Point3D.ZeroPoint;
sensorUpward.X = skelTrans.Orientation.Y1;
sensorUpward.Y = skelTrans.Orientation.Y2;
sensorUpward.Z = skelTrans.Orientation.Y3;
// convert it to Unity
Vector3 worldUpwards = NIConvertCoordinates.ConvertPos(sensorUpward);
if (worldUpwards.magnitude == 0)
{
return(Quaternion.identity); // we don't have a good point to work with.
}
Quaternion jointRotation = Quaternion.LookRotation(worldForward, worldUpwards);
Quaternion newRotation = transform.rotation * jointRotation * m_jointsInitialRotations[(int)joint];
// we try to limit the speed of the change.
return(Quaternion.Slerp(m_jointTransforms[(int)joint].rotation, newRotation, Time.deltaTime * m_rotationDampening));
}
/// @brief updates a single joint
///
/// This method updates a single joint. The decision of what to update (orientation, position)
/// depends on m_updateOrientation and m_updateJointPositions. Only joints with high confidence
/// are updated. @note it is possible to update only position or only orientation even though both
/// are expected if the confidence of one is low.
/// @param centerOffset the new central position
/// @param joint the joint we want to update
/// @param skelTrans the new transformation of the joint
protected void UpdateJoint(SkeletonJoint joint, SkeletonJointTransformation skelTrans, SkeletonJointPosition centerOffset)
{
// make sure something is hooked up to this joint
if ((int)joint >= m_jointTransforms.Length || !m_jointTransforms[(int)joint])
{
return;
}
// if we have debug lines to draw we need to collect the data.
if (m_linesDebugger != null)
{
Vector3 pos = CalcJointPosition(joint, ref skelTrans, ref centerOffset) + transform.position;
float posConf = skelTrans.Position.Confidence;
Quaternion rot = CalcRotationForJoint(joint, ref skelTrans, ref centerOffset);
float rotConf = skelTrans.Orientation.Confidence;
m_linesDebugger.UpdateJointInfoForJoint(joint, pos, posConf, rot, rotConf);
}
// modify orientation (if needed and confidence is high enough)
if (m_updateOrientation && skelTrans.Orientation.Confidence >= 0.5)
{
m_jointTransforms[(int)joint].rotation = CalcRotationForJoint(joint, ref skelTrans, ref centerOffset);
}
// modify position (if needed, and confidence is high enough)
if (m_updateJointPositions && skelTrans.Position.Confidence >= 0.5f)
{
m_jointTransforms[(int)joint].localPosition = CalcJointPosition(joint, ref skelTrans, ref centerOffset);
}
}
private void checkUserGestures(int id)
{
SkeletonJointPosition head = new SkeletonJointPosition();
SkeletonJointPosition leftHand = new SkeletonJointPosition();
SkeletonJointPosition rightHand = new SkeletonJointPosition();
SkeletonJointPosition leftShoulder = new SkeletonJointPosition();
SkeletonJointPosition rightShoulder = new SkeletonJointPosition();
head = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.Head);
leftHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftHand);
rightHand = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightHand);
leftShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.LeftShoulder);
rightShoulder = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, SkeletonJoint.RightShoulder);
OpenNI.Point3D headPoint = head.Position;
OpenNI.Point3D leftPoint = leftHand.Position;
OpenNI.Point3D rightPoint = rightHand.Position;
Ray3D leftPointer = new Ray3D(headPoint.X, headPoint.Y, headPoint.Z,
leftPoint.X, leftPoint.Y, leftPoint.Z);
Ray3D rightPointer = new Ray3D(headPoint.X, headPoint.Y, headPoint.Z,
rightPoint.X, rightPoint.Y, rightPoint.Z);
lock (animationLock)
{
raysToBeAnimated[0] = leftPointer;
raysToBeAnimated[1] = rightPointer;
}
Console.Write("Left vector: " + leftPointer);
Console.Write("Right vector: " + rightPointer);
if (VerticallyClose(leftPoint, rightPoint))
{
// Handle dimming.
if (FirstAboveSecond(leftPoint, headPoint) && FirstAboveSecond(rightPoint, headPoint))
{
Console.Write("Beginning dim down!");
dimmingDown = true;
dimmingStartY = leftPoint.Y;
}
else if (dimmingDown)
{
int dimPercent = (int)((dimmingStartY - leftPoint.Y) / TOTAL_DIMMING_DISTANCE);
if (dimPercent < 0)
{
dimPercent = 0;
}
else if (dimPercent > 100)
{
dimPercent = 100;
}
Device.dimAllToPercent(dimPercent);
}
else if (VerticallyClose(leftPoint, leftShoulder.Position) &&
VerticallyClose(leftShoulder.Position, rightShoulder.Position) &&
VerticallyClose(rightShoulder.Position, rightPoint))
{
Console.Write("Beginning dim up!");
dimmingUp = true;
dimmingStartY = leftPoint.Y;
}
else if (dimmingUp)
{
int dimPercent = (int)((leftPoint.Y - dimmingStartY) / TOTAL_DIMMING_DISTANCE);
if (dimPercent < 0)
{
dimPercent = 0;
}
else if (dimPercent > 100)
{
dimPercent = 100;
}
Device.dimAllToPercent(dimPercent);
}
}
else
{
// Allow pointing.
dimmingDown = false;
dimmingUp = false;
}
if (!dimmingUp && !dimmingDown)
{
foreach (Device d in devices)
{
if (leftPointer.closeTo(d.position) || rightPointer.closeTo(d.position))
{
d.isInFocus();
}
}
}
Console.Write("=============================");
}
private static extern bool GetJointPosition(uint userID, int joint, ref SkeletonJointPosition pTransformation);
