/// <summary>
/// Constructor
/// </summary>
public TgcKinect()
{
debugSkeleton = new TgcKinectDebugSkeleton();
positionScale = 100;
positionTranslate = new Vector3(0, 9, -25);
data = new TgcKinectSkeletonData();
historyFramesCount = 50;
bodyProportion = 6;
hands2dSpeed = new Vector2(1, 2);
cursorSize = new Vector2(64, 64);
skeletonCenter = new Vector3();
// PIR
//X: -0.05 a -0.2
//Y: 0.3 a 0.5
right_pir.x_min = -0.2f;
right_pir.x_max = -0.05f;
right_pir.y_min = 0.3f;
right_pir.y_max = 0.5f;
//Left
//X: 0.05 a 0.2
left_pir.x_min = 0.05f;
left_pir.x_max = 0.2f;
left_pir.y_min = 0.3f;
left_pir.y_max = 0.5f;
}
/// <summary>
/// Hacer analisis estadistico de los datos de posicion de una mano del esqueleto, en un eje determinado.
/// Guarda los datos en el AxisAnalysisData de ese eje, para esa mano
/// </summary>
private void computeAxisAnalysis(TgcKinectSkeletonData data, int handIndex, int axisIndex)
{
//Lugar donde tenemos que almacenar el resultado
TgcKinectSkeletonData.AxisAnalysisData analysis = data.HandsAnalysisData[handIndex][axisIndex];
int framesCount = data.HandsFrames.Count;
analysis.Min = float.MaxValue;
analysis.Max = float.MinValue;
float sum = 0;
int i = 0;
float value = 0;
float lastValue = 0;
float sumDiff = 0;
foreach (TgcKinectSkeletonData.HandFrame frame in data.HandsFrames)
{
lastValue = value;
value = frame.get3DValue(handIndex, axisIndex);
sum += value;
//min
if (value < analysis.Min)
{
analysis.Min = value;
}
//max
if (value > analysis.Max)
{
analysis.Max = value;
}
//diff con el anterior
if (i > 0)
{
sumDiff += value - lastValue;
}
i++;
}
//avg
analysis.Avg = sum / framesCount;
//diff
analysis.DiffAvg = sumDiff / (framesCount - 1);
//variance
float sumVariance = 0;
foreach (TgcKinectSkeletonData.HandFrame frame in data.HandsFrames)
{
value = frame.get3DValue(handIndex, axisIndex);
sumVariance += FastMath.Abs(analysis.Avg - value);
}
analysis.Variance = sumVariance / framesCount;
}
/// <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);
}