/// <summary>
/// Tomar datos de esqueleto de kinect
/// </summary>
protected void updateKinectData()
{
for (int i = 0; i < kinectBonesMapping.Count; i++)
{
Tuple <JointType, int> mapping = kinectBonesMapping[i];
Vector3 kBonePos = TgcKinectUtils.toVector3(kinectSkeleton.Joints[mapping.Item1].Position);
kinectBonePos[mapping.Item2] = kBonePos;
}
}
/// <summary>
/// Dibujar esqueleto de debug
/// Llamar a init() la primera vez.
/// </summary>
public void render(Skeleton skeleton)
{
//Actualizar datos
if (skeleton != null)
{
//Obtener la posicion de todos los joints
int idx = -1;
foreach (Joint joint in skeleton.Joints)
{
idx++;
//Obtener posicion
jointBoxes[idx].Position = TgcKinectUtils.toVector3(joint.Position);
//Setear color segun estado del joint
Color jointColor = joint.TrackingState == JointTrackingState.Tracked ? Color.Red : Color.Yellow;
if (jointColor != jointBoxes[idx].Color)
{
jointBoxes[idx].Color = jointColor;
jointBoxes[idx].updateValues();
}
}
//Armar los huesos entre dos joints
idx = -1;
foreach (BoneOrientation bone in skeleton.BoneOrientations)
{
idx++;
//Indice de origen y de destino
int n1 = (int)bone.StartJoint;
int n2 = (int)bone.EndJoint;
//Actualizar posiciones
jointLines[idx].PStart = jointBoxes[n1].Position;
jointLines[idx].PEnd = jointBoxes[n2].Position;
jointLines[idx].updateValues();
}
}
//Render
for (int i = 0; i < jointLines.Length; i++)
{
jointLines[i].render();
}
for (int i = 0; i < jointBoxes.Length; i++)
{
jointBoxes[i].render();
}
}
public static Vector2 computeHand2DPos(SkeletonPoint handPosition, SkeletonPoint headPosition, KPIR pir, bool rightHand)
{
float minX = pir.x_min;
float maxX = pir.x_max;
float minY = pir.y_min;
float maxY = pir.y_max;
float width = GuiController.Instance.D3dDevice.Viewport.Width;
float height = GuiController.Instance.D3dDevice.Viewport.Height;
Vector2 handPos = TgcKinectUtils.toVector2(handPosition);
Vector2 headPos = TgcKinectUtils.toVector2(headPosition);
Vector2 diff = headPos - handPos;
Vector2 handPos2D = new Vector2();
if (rightHand)
{
handPos2D = new Vector2(
(1 - ((diff.X - minX) / (maxX - minX))) * width,
((diff.Y - minY) / (maxY - minY)) * height
);
}
else
{
handPos2D = new Vector2(
(1 - ((diff.X - minX)) / (maxX - minX)) * width,
((diff.Y - minY) / (maxY - minY)) * height
);
}
if (handPos2D.X < 0)
{
handPos2D.X = 0;
}
if (handPos2D.X >= width)
{
handPos2D.X = width;
}
if (handPos2D.Y < 0)
{
handPos2D.Y = 0;
}
if (handPos2D.Y >= height)
{
handPos2D.Y = height;
}
return(handPos2D);
}
/// <summary>
/// Actualizar posicion 2D de las manos
/// </summary>
private void updateHandsScreenPos(Skeleton rawSkeleton, TgcKinectSkeletonData data)
{
if (rawSkeleton.Joints[JointType.ShoulderRight].TrackingState == JointTrackingState.Tracked &&
rawSkeleton.Joints[JointType.Spine].TrackingState == JointTrackingState.Tracked &&
rawSkeleton.Joints[JointType.Head].TrackingState == JointTrackingState.Tracked)
{
halfBodyWidth = rawSkeleton.Joints[JointType.ShoulderRight].Position.X - rawSkeleton.Joints[JointType.Spine].Position.X;
halfBodyHeight = rawSkeleton.Joints[JointType.Head].Position.Y - rawSkeleton.Joints[JointType.Spine].Position.Y;
}
if (rawSkeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked &&
rawSkeleton.Joints[JointType.Head].TrackingState != JointTrackingState.NotTracked)
{
//Right
// Actualizo esta posicion tal como viene de la kinect, para debuggear
raw_pos_mano = new Vector3(rawSkeleton.Joints[JointType.HandRight].Position.X - rawSkeleton.Joints[JointType.Head].Position.X,
rawSkeleton.Joints[JointType.HandRight].Position.Y - rawSkeleton.Joints[JointType.Head].Position.Y,
rawSkeleton.Joints[JointType.HandRight].Position.Z - rawSkeleton.Joints[JointType.Head].Position.Z);
//X: -0.05 a -0.2
//Y: 0.1 a 0.5
data.Current.RightHandPos = TgcKinectUtils.computeHand2DPos(
rawSkeleton.Joints[JointType.HandRight].Position,
rawSkeleton.Joints[JointType.Head].Position, right_pir, true);
}
if (rawSkeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked &&
rawSkeleton.Joints[JointType.Head].TrackingState != JointTrackingState.NotTracked)
{
//Left
//X: 0.05 a 0.2
//Y: 0.1 a 0.5
data.Current.LefttHandPos = TgcKinectUtils.computeHand2DPos(
rawSkeleton.Joints[JointType.HandLeft].Position,
rawSkeleton.Joints[JointType.Head].Position, left_pir, false);
}
}
/// <summary>
/// Actualizar informacion de esqueleto
/// </summary>
/// <param name="rawSkeleton">Datos crudos trackeados</param>
/// <param name="data">Datos procesados que se actualizan</param>
private void buildSkeletonData(Skeleton rawSkeleton, TgcKinectSkeletonData data)
{
if (!skeletonTracked)
{
if (rawSkeleton.Joints[JointType.HipCenter].TrackingState == JointTrackingState.Tracked)
{
skeletonTracked = true;
skeletonCenter = TgcKinectUtils.toVector3(rawSkeleton.Joints[JointType.HipCenter].Position);
}
}
//Copiar esqueleto de frame actual a frame anterior, sin escalar las posiciones porque ya estaban escaladas de antes
this.copySkeleton(data.Current.KinectSkeleton, data.Previous.KinectSkeleton, false);
//Copiar posicion central
data.Previous.CenterPos = data.Current.CenterPos;
//Copiar BSphere de frame actual a frame anterior
data.Previous.RightHandSphere.setCenter(data.Current.RightHandSphere.Center);
data.Previous.LeftHandSphere.setCenter(data.Current.LeftHandSphere.Center);
//Copiar pos2D de las dos manos al frame anterior
data.Previous.RightHandPos = data.Current.RightHandPos;
data.Previous.LefttHandPos = data.Current.LefttHandPos;
//Copiar esqueleto recien trackeado al frame actual, adaptando proporciones
this.copySkeleton(rawSkeleton, data.Current.KinectSkeleton, true);
//Actualizar posicion central
data.Current.CenterPos = TgcKinectUtils.toVector3(data.Current.KinectSkeleton.Joints[JointType.HipCenter].Position);
//Actualizar BSphere de manos de frame actual
data.Current.RightHandSphere.setCenter(TgcKinectUtils.toVector3(data.Current.KinectSkeleton.Joints[JointType.HandRight].Position));
data.Current.LeftHandSphere.setCenter(TgcKinectUtils.toVector3(data.Current.KinectSkeleton.Joints[JointType.HandLeft].Position));
//Actualizar posicion 2D de manos
this.updateHandsScreenPos(rawSkeleton, data);
//Agregar nuevo cuadro a historial
TgcKinectSkeletonData.HandFrame newFrame = new TgcKinectSkeletonData.HandFrame();
newFrame.Pos3D = new Vector3[2];
newFrame.Pos2D = new Vector2[2];
newFrame.Pos3D[TgcKinectSkeletonData.RIGHT_HAND] = data.Current.RightHandSphere.Center;
newFrame.Pos3D[TgcKinectSkeletonData.LEFT_HAND] = data.Current.LeftHandSphere.Center;
newFrame.Pos2D[TgcKinectSkeletonData.RIGHT_HAND] = data.Current.RightHandPos;
newFrame.Pos2D[TgcKinectSkeletonData.LEFT_HAND] = data.Current.LefttHandPos;
data.HandsFrames.AddFirst(newFrame);
//Ver si hay que eliminar el ultimo cuadro viejo del historial
if (data.HandsFrames.Count > this.historyFramesCount)
{
data.HandsFrames.RemoveLast();
}
//Hacer analisis de datos en el historial de frames, para la mano derecha
this.computeAxisAnalysis(data, TgcKinectSkeletonData.RIGHT_HAND, 0);
this.computeAxisAnalysis(data, TgcKinectSkeletonData.RIGHT_HAND, 1);
this.computeAxisAnalysis(data, TgcKinectSkeletonData.RIGHT_HAND, 2);
//Hacer analisis de datos en el historial de frames, para la mano izquierda
this.computeAxisAnalysis(data, TgcKinectSkeletonData.LEFT_HAND, 0);
this.computeAxisAnalysis(data, TgcKinectSkeletonData.LEFT_HAND, 1);
this.computeAxisAnalysis(data, TgcKinectSkeletonData.LEFT_HAND, 2);
}