public void ApplyTransformations(WagSkeleton skeleton)
{
skeleton.TransformedPosition = CalibrateTransform.Transform(new Point3D()
{
X = skeleton.Position.X,
Y = skeleton.Position.Y,
Z = skeleton.Position.Z
});
foreach (JointType type in Enum.GetValues(typeof(JointType)))
{
Point3D transformpoint = CalibrateTransform.Transform(new Point3D()
{
X = skeleton.Joints[type].Position.X,
Y = skeleton.Joints[type].Position.Y,
Z = skeleton.Joints[type].Position.Z
});
SkeletonPoint jointPosition = new SkeletonPoint()
{
X = (float)transformpoint.X,
Y = (float)transformpoint.Y,
Z = (float)transformpoint.Z
};
skeleton.TransformedJoints[type] = new Joint()
{
TrackingState = skeleton.TransformedJoints[type].TrackingState,
Position = jointPosition
};
}
}
private double CalculateBotherAngleBeta(WagSkeleton userSkeleton, WagSkeleton botherSkeleton)
{
Point userPoint = new Point(userSkeleton.TransformedPosition.X, userSkeleton.TransformedPosition.Z);
Point botherPoint = new Point(botherSkeleton.TransformedPosition.X, botherSkeleton.TransformedPosition.Z);
//Point userPoint = new Point(-5, 20);
//Point botherPoint = new Point(0, 1);
Vector userBotherVector = new Vector(botherPoint.X - userPoint.X, botherPoint.Y - userPoint.Y);
Vector userDisplayVector = -(Vector)userPoint;
double botherAngleBeta = Math.Abs(Vector.AngleBetween(userBotherVector, userDisplayVector));
return botherAngleBeta;
}
public Zone DetectZone(WagSkeleton skeleton)
{
Zone calculatedZone = Zone.Ambient;
UserDisplayDistance = DetectUserPosition(skeleton);
if (UserDisplayDistance < STimSettings.CloseZoneConstrain)
calculatedZone = Zone.Close;
else if (UserDisplayDistance >= STimSettings.CloseZoneConstrain && userDisplayDistance < STimSettings.InteractionZoneConstrain)
calculatedZone = Zone.Interaction;
else if (UserDisplayDistance >= STimSettings.InteractionZoneConstrain && userDisplayDistance < STimSettings.NotificationZoneConstrain)
calculatedZone = Zone.Notification;
else if (UserDisplayDistance >= STimSettings.NotificationZoneConstrain)
calculatedZone = Zone.Ambient;
return calculatedZone;
}
private double CalculateSocialEffect(WagSkeleton userSkeleton, WagSkeleton[] skeletons)
{
double minBotherEffect = Math.Tanh((180 - 45) * BOTHER_PARAMETER * Math.PI / 180 / 2) + 0.5;
List<double> botherList = new List<double>();
foreach (WagSkeleton skel in skeletons)
{
if (skel == userSkeleton)
continue;
double botherAngleBeta = CalculateBotherAngleBeta(userSkeleton, skel);
double botherEffect = Math.Tanh((botherAngleBeta - 45) * BOTHER_PARAMETER * Math.PI / 180 / 2) + 0.5;
botherList.Add(botherEffect);
}
if (botherList.Count > 0)
minBotherEffect = botherList.Min();
return minBotherEffect;
}
private double CalculateOrientationAngle(WagSkeleton userSkeleton)
{
Microsoft.Kinect.Joint shoulderLeft = userSkeleton.TransformedJoints.SingleOrDefault(temp => temp.JointType == Microsoft.Kinect.JointType.ShoulderLeft);
Microsoft.Kinect.Joint shoulderRight = userSkeleton.TransformedJoints.SingleOrDefault(temp => temp.JointType == Microsoft.Kinect.JointType.ShoulderRight);
Point shoulderRightP = new Point(shoulderRight.Position.X, shoulderRight.Position.Z);
Point shoulderLeftP = new Point(shoulderLeft.Position.X, shoulderLeft.Position.Z);
Vector shoulderVector = new Vector(shoulderRightP.X - shoulderLeftP.X, shoulderRightP.Y - shoulderLeftP.Y);
Vector xAxis = new Vector (1, 0);
double orientationAngle = Vector.AngleBetween(xAxis, shoulderVector);
if(orientationAngle < 0)
orientationAngle = - orientationAngle;
return orientationAngle;
}
public AttentionSimple CalculateAttention(WagSkeleton userSkeleton)
{
double orientationEffect = 0;
if (userSkeleton.BodyOrientationAngle <= VISUAL_FIELD / 2)
orientationEffect = (1 - userSkeleton.BodyOrientationAngle / (VISUAL_FIELD / 2)) * 100;
double distanceEffect = (1 - userSkeleton.TransformedPosition.Z / TRACKING_DISTANCE) * 100;
double attention = orientationEffect * ORIENTATION_PARAMETER + distanceEffect * DISTANCE_PARAMETER;
AttentionSimple attentionSimple = new AttentionSimple()
{
SimpleDistanceEffect = distanceEffect,
SimpleOrientationEffect = orientationEffect,
SimpleAttentionValue = attention
};
return attentionSimple;
}
public AttentionSocial CalculateAttention(WagSkeleton userSkeleton, WagSkeleton [] skeletons)
{
//Calculate Social Function: Bother effect
double socialEffect = CalculateSocialEffect(userSkeleton, skeletons);
double orientationAngleAlpha = CalculateOrientationAngle(userSkeleton);
double orientationEffect = 1 - Math.Pow(orientationAngleAlpha / 180, 0.5);
double distanceEffect = TRACKING_DISTANCE / userSkeleton.TransformedPosition.Z;
double attention = orientationEffect * distanceEffect * socialEffect;
AttentionSocial attentionSocial = new AttentionSocial()
{
DistanceEffect = distanceEffect,
SocialEffect = socialEffect,
OrientationEffect = orientationEffect,
AttentionValue = attention
};
return attentionSocial;
}
//drawing color image and skeleton
private DrawingImage DrawImage(ColorImageFrame colorFrame, WagSkeleton[] skeletons)
{
DrawingGroup dgColorImageAndSkeleton = new DrawingGroup();
DrawingImage drawingImage = new DrawingImage(dgColorImageAndSkeleton);
skeletonDrawer = new SkeletonDrawer(KinectSensor);
using (DrawingContext drawingContext = dgColorImageAndSkeleton.Open())
{
InitializeDrawingImage(colorFrame, drawingContext);
if (skeletons != null && skeletons.Count() > 0)
{
foreach (WagSkeleton skeleton in skeletons)
{
if (skeleton.FramesNotSeen > 0 && !(VisitorCtr.IsBlocked && skeleton.TrackingId == ClosestVisitor.TrackingId))
continue;
skeletonDrawer.DrawUpperSkeleton(skeleton, drawingContext);
Joint head = skeleton.Joints.SingleOrDefault(temp => temp.JointType == JointType.Head);
System.Windows.Point headP = skeletonDrawer.SkeletonPointToScreen(head.Position);
headP.X -= 30; //These two hardcoded numbers are the displacements in X,Y of the white box content holder
headP.Y -= 25;
drawingContext.DrawRectangle(Brushes.White, new Pen(Brushes.White, 1), new System.Windows.Rect(headP, new Size(100, 100)));
//FormattedText
drawingContext.DrawText(
new FormattedText(
String.Format("ID: {0}", skeleton.TrackingId),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight, new Typeface("Verdana"), 15, System.Windows.Media.Brushes.Red),
headP);
if (STimSettings.IncludeStatusRender)
{
System.Windows.Point socialDataPos = headP;
socialDataPos.Y = headP.Y + 25;
drawingContext.DrawText(
new FormattedText(
skeleton.AttentionSocial.ToString(),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight, new Typeface("Verdana"), 15, System.Windows.Media.Brushes.Green),
socialDataPos);
System.Windows.Point simpleDataPos = headP;
simpleDataPos.Y = socialDataPos.Y + 30;
drawingContext.DrawText(
new FormattedText(
skeleton.AttentionSimple.ToString(),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight, new Typeface("Verdana"), 15, System.Windows.Media.Brushes.Green),
simpleDataPos);
}
}
}
}
//Make sure the image remains within the defined width and height
dgColorImageAndSkeleton.ClipGeometry = new RectangleGeometry(new System.Windows.Rect(0.0, 0.0, Constants.RENDER_WIDTH, Constants.RENDER_HEIGHT));
return drawingImage;
}
private Vector3D CalculateHeadOrientation(WagSkeleton skeleton)
{
Vector3D headOrientation = new Vector3D(0, 0, -1);
FaceTrackFrame face = skeleton.FaceFrame;
var FacePoints = face.Get3DShape();
Vector3DF eyeLeft = FacePoints[Constants.LEFT_EYE];
Vector3DF eyeRight = FacePoints[Constants.RIGHT_EYE];
Vector3DF faceTop = FacePoints[Constants.FACE_TOP];
Vector3DF faceBottom = FacePoints[Constants.FACE_BOTTOM];
Vector3D faceVectorHorizontal = new Vector3D(eyeLeft.X - eyeRight.X, eyeLeft.Y - eyeRight.Y, eyeLeft.Z - eyeRight.Z);
Vector3D faceVectorVertical = new Vector3D(faceTop.X - faceBottom.X, faceTop.Y - faceBottom.Y, faceTop.Z - faceBottom.Z);
headOrientation = Vector3D.CrossProduct(faceVectorHorizontal, faceVectorVertical);
headOrientation = Calibration.CalibrateTransform.Transform(headOrientation);
headOrientation.Normalize();
Matrix3D headPointsPointUpMatrix = new Matrix3D();
headPointsPointUpMatrix.RotateAt(new System.Windows.Media.Media3D.Quaternion(new Vector3D(int.MaxValue, 0, 0), -20), skeleton.TransformedJoints[JointType.Head].Position.ToPoint3D());
Vector3D lowered = headPointsPointUpMatrix.Transform(headOrientation);
return lowered;
}
private Point3D CalculateHeadLocation(WagSkeleton skeleton)
{
Point3D headLocation = new Point3D(0, 0, 0);
Joint head = skeleton.TransformedJoints[JointType.Head];
if (head != null && head.TrackingState == JointTrackingState.Tracked)
headLocation = head.Position.ToPoint3D();
return headLocation;
}
private double CalculateBodyOrientationAngle(WagSkeleton userSkeleton)
{
Microsoft.Kinect.Joint shoulderLeft = userSkeleton.TransformedJoints[JointType.ShoulderLeft];
Microsoft.Kinect.Joint shoulderRight = userSkeleton.TransformedJoints[JointType.ShoulderRight];
Vector shoulderVector = new Vector(shoulderRight.Position.X - shoulderLeft.Position.X, shoulderRight.Position.Z - shoulderLeft.Position.Z);
System.Windows.Media.Matrix matrix = new System.Windows.Media.Matrix();
matrix.Rotate(-90);
Vector bodyFacingDirection = matrix.Transform(shoulderVector);
Vector displayLocation = -new Vector(userSkeleton.HeadLocation.X, userSkeleton.HeadLocation.Z);
double orientationAngle = Math.Abs(Vector.AngleBetween(displayLocation, bodyFacingDirection));
return Math.Abs(orientationAngle);
}
/// <summary>
/// Every new frame this gets called
/// </summary>
/// <param name="skeleton"></param>
/// <param name="deltaMilliseconds"></param>
public void LoadNewSkeletonData(List<WagSkeleton> skeletons, WagSkeleton controllerSkeleton, byte[] drawingImage)
{
waitHandle.WaitOne();
imageSource = drawingImage;
if (skeletons == null || skeletons.Count == 0 || controllerSkeleton == null)
{
currentSkeletons = null;
controllerId = -1;
}
else
{
currentSkeletons = skeletons;
controllerId = controllerSkeleton.TrackingId;
Object[] logObjects = null;
List<VisitStatus> currentVisits = CreateVisitStatus();
DateTime loggingTime = DateTime.Now;
foreach (VisitStatus status in currentVisits)
{
Point viewInGrid = CalculateVision(status);
//Visitors.Count
//ImageFile
logObjects = new Object[]
{
loggingTime.ToString(STimSettings.DateTimeLogFormat),
loggingTime.Millisecond,
currentVisits.Count,
status.VisitId,
status.SkeletonId,
status.FramesNotSeen,
status.VisitInit.ToString(STimSettings.DateTimeLogFormat),
status.Zone,
status.IsControlling,
status.WasControlling,
status.HeadLocation.X,
status.HeadLocation.Y,
status.HeadLocation.Z,
status.HeadDirection.X,
status.HeadDirection.Y,
status.HeadDirection.Z,
viewInGrid.X,
viewInGrid.Y,
status.MovementDirection.X,
status.MovementDirection.Y,
status.MovementDistance,
status.BodyAngle,
status.AttentionSimple.SimpleAttentionValue,
status.AttentionSocial.SocialAttentionValue,
status.TouchInteraction,
status.GestureInteraction
};
LogInformation(logObjects, visitLogger);
}
lastControllerId = controllerId;
lastVisits = currentVisits;
lastPositions.Clear();
if (currentSkeletons != null)
{
foreach (WagSkeleton wagSkel in currentSkeletons)
lastPositions.Add(wagSkel.TrackingId, wagSkel.HeadLocation);
}
}
waitHandle.Set();
}
private double DetectUserPosition(WagSkeleton skeleton)
{
if (ClosePercent > STimSettings.BlockDepthPercent)
{
return STimSettings.CloseZoneConstrain / 2;
}
if (skeleton != null)
{
return skeleton.TransformedJoints[JointType.Head].Position.Z;
}
return STimSettings.NotificationZoneConstrain;
}
public void Calibrate(WagSkeleton skeleton)
{
if (skeleton == null || skeleton.Joints[JointType.Head].TrackingState != JointTrackingState.Tracked)
throw new Exception("Can Not Detect Head Position!");
captureCalibrationPositions[CalibrationHeadIndex] = skeleton.Joints[JointType.Head].Position.ToPoint3D();
if (++CalibrationHeadIndex >= standardCalibrationPositions.Length)
{
CalibrationHeadIndex = 0;
Point3D estimatedOrigin = OriginF.BruteForceEstimateOrigin(standardCalibrationPositions, captureCalibrationPositions, Constants.KINECT_DETECT_RANGE);
CalCulateTransformationMatrix(estimatedOrigin);
IsCalibrated = true;
}
}
