public Z3Body(Z3Body baseBody) : this()
{
foreach (var jointType in baseBody.Joints.Keys)
{
this.Positions.Add(jointType,
new Point3D(
baseBody.Positions[jointType].X,
baseBody.Positions[jointType].Y,
baseBody.Positions[jointType].Z));
this.Vectors.Add(jointType,
new Point3D(
baseBody.Vectors[jointType].X,
baseBody.Vectors[jointType].Y,
baseBody.Vectors[jointType].Z));
this.Joints.Add(jointType,
new Z3Point3D(
Z3Math.GetRealValue(baseBody.Joints[jointType].X),
Z3Math.GetRealValue(baseBody.Joints[jointType].Y),
Z3Math.GetRealValue(baseBody.Joints[jointType].Z)
));
this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType])));
}
}
public static Z3Point3D operator *(Z3Point3D p3d, ArithExpr value)
{
return(new Z3Point3D(
Z3Math.Mul(p3d.X, value),
Z3Math.Mul(p3d.Y, value),
Z3Math.Mul(p3d.Z, value)));
}
public static Z3Point3D operator /(Z3Point3D p3d, double value)
{
return(new Z3Point3D(
Z3Math.Div(p3d.X, Z3Math.Real(value)),
Z3Math.Div(p3d.Y, Z3Math.Real(value)),
Z3Math.Div(p3d.Z, Z3Math.Real(value))));
}
public BoolExpr IsNearerThan(Z3Point3D that, double distanceThreshold)
{
ArithExpr distance = this.CalcApproximateDistance(that);
BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold));
return(result);
}
//internal static SimpleBodyRestriction TouchRestriction(JointType jointType, JointSide handSide)
//{
// double maxY = TrigonometryHelper.GetSine(5);
// return new TouchBodyRestriction(jointType, handSide);
//}
internal static BoolExpr EvaluateNorms(Z3Body body1, Z3Body body2)
{
double normsThreshold = 0.1;
BoolExpr result = Z3Math.True;
var jointTypes = EnumUtil.GetValues <JointType>();
foreach (var jointType in jointTypes)
{
// Calc the distance between the two norms
ArithExpr distance =
Z3Math.Abs(
Z3Math.Sub(
body1.Norms[jointType],
body2.Norms[jointType]));
// Create the boolean expression to evaluate the distance
result =
Z3.Context.MkAnd(
result,
Z3.Context.MkLt(
distance,
Z3Math.Real(normsThreshold)));
}
return(result);
}
public static Z3Point3D operator -(Z3Point3D p3d, Z3Point3D that)
{
return(new Z3Point3D(
Z3Math.Sub(p3d.X, that.X),
Z3Math.Sub(p3d.Y, that.Y),
Z3Math.Sub(p3d.Z, that.Z)));
}
public Z3Point3D GetInverted()
{
ArithExpr invertedX = Z3Math.Neg(this.X);
ArithExpr invertedY = Z3Math.Neg(this.Y);
ArithExpr invertedZ = Z3Math.Neg(this.Z);
return(new Z3Point3D(invertedX, invertedY, invertedZ));
}
private static Dictionary <JointType, ArithExpr> CreateSyntheticNorms()
{
var result = new Dictionary <JointType, ArithExpr>();
ArithExpr spineBase = Z3Math.Real(0.0);
ArithExpr spineMid = Z3Math.Real(0.3);
ArithExpr spineShoulder = Z3Math.Real(0.3);
ArithExpr neck = Z3Math.Real(0.15);
ArithExpr head = Z3Math.Real(0.15);
ArithExpr shoulderLeft = Z3Math.Real(0.25);
ArithExpr elbowLeft = Z3Math.Real(0.25);
ArithExpr wristLeft = Z3Math.Real(0.25);
ArithExpr handLeft = Z3Math.Real(0.05);
ArithExpr handTipLeft = Z3Math.Real(0.05);
ArithExpr thumbLeft = Z3Math.Real(0.05);
ArithExpr hipLeft = Z3Math.Real(0.25);
ArithExpr kneeLeft = Z3Math.Real(0.35);
ArithExpr ankleLeft = Z3Math.Real(0.35);
ArithExpr footLeft = Z3Math.Real(0.1);
ArithExpr shoulderRight = Z3Math.Real(0.25);
ArithExpr elbowRight = Z3Math.Real(0.25);
ArithExpr wristRight = Z3Math.Real(0.25);
ArithExpr handRight = Z3Math.Real(0.05);
ArithExpr handTipRight = Z3Math.Real(0.05);
ArithExpr thumbRight = Z3Math.Real(0.05);
ArithExpr hipRight = Z3Math.Real(0.25);
ArithExpr kneeRight = Z3Math.Real(0.35);
ArithExpr ankleRight = Z3Math.Real(0.35);
ArithExpr footRight = Z3Math.Real(0.1);
result.Add(JointType.SpineBase, spineBase);
result.Add(JointType.SpineMid, spineMid);
result.Add(JointType.SpineShoulder, spineShoulder);
result.Add(JointType.Neck, neck);
result.Add(JointType.Head, head);
result.Add(JointType.ShoulderLeft, shoulderLeft);
result.Add(JointType.ElbowLeft, elbowLeft);
result.Add(JointType.WristLeft, wristLeft);
result.Add(JointType.HandLeft, handLeft);
result.Add(JointType.HandTipLeft, handTipLeft);
result.Add(JointType.ThumbLeft, thumbLeft);
result.Add(JointType.HipLeft, hipLeft);
result.Add(JointType.KneeLeft, kneeLeft);
result.Add(JointType.AnkleLeft, ankleLeft);
result.Add(JointType.FootLeft, footLeft);
result.Add(JointType.ShoulderRight, shoulderRight);
result.Add(JointType.ElbowRight, elbowRight);
result.Add(JointType.WristRight, wristRight);
result.Add(JointType.HandRight, handRight);
result.Add(JointType.HandTipRight, handTipRight);
result.Add(JointType.ThumbRight, thumbRight);
result.Add(JointType.HipRight, hipRight);
result.Add(JointType.KneeRight, kneeRight);
result.Add(JointType.AnkleRight, ankleRight);
result.Add(JointType.FootRight, footRight);
return(result);
}
// Arithmetic helpers
public static ArithExpr Abs(ArithExpr expr)
{
Expr result = Z3.Context.MkITE(
Z3.Context.MkGe(expr, Z3Math.Zero),
expr,
Z3Math.Neg(expr));
return(result as ArithExpr);
}
//public Z3Point3D GetApproximateNormalized()
//{
// Z3Point3D result = this.GetManhattanNormalized();
// result.X = CalcApproximateCoordFromManhattanToEuclidianSystem(result.X, result.Y, result.Z);
// result.Y = CalcApproximateCoordFromManhattanToEuclidianSystem(result.Y, result.X, result.Z);
// result.Z = CalcApproximateCoordFromManhattanToEuclidianSystem(result.Z, result.Y, result.X);
// return result;
//}
public ArithExpr Norm()
{
ArithExpr result =
Z3Math.Max(
Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(this.X, this.Y, this.Z)),
Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(this.Y, this.X, this.Z)),
Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(this.Z, this.Y, this.X)));
return(result);
}
public LowerThanBodyRestriction(JointType jointType, int angleThreshold)
: base(
body =>
{
double maxY = TrigonometryHelper.GetSine(angleThreshold);
return(Z3.Context.MkLt(body.Joints[jointType].Y, Z3Math.Real(maxY)));
},
jointType)
{
this.JointType = jointType;
}
public static void Run()
{
Z3Point3D constPoint = Z3Point3D.MkZ3Const("const"); // ("const X", "const Y", "const Z")
Z3Point3D normalized = new Z3Point3D();
ArithExpr higherCoord =
Z3Math.Max(
Z3Math.Max(
Z3Math.Abs(constPoint.X),
Z3Math.Abs(constPoint.Y)),
Z3Math.Abs(constPoint.Z));
normalized.X = Z3.Context.MkDiv(constPoint.X, constPoint.Y);
normalized.Y = Z3Math.One;//Z3.Context.MkDiv(constPoint.Y, higherCoord);
normalized.Z = Z3.Context.MkDiv(constPoint.Z, constPoint.Y);
normalized.X = CalcApproximateCoordFromManhattanToEuclidianSystem(normalized.X, normalized.Y, normalized.Z);
normalized.Y = CalcApproximateCoordFromManhattanToEuclidianSystem(normalized.Y, normalized.X, normalized.Z);
normalized.Z = CalcApproximateCoordFromManhattanToEuclidianSystem(normalized.Z, normalized.Y, normalized.X);
Z3Point3D up = Z3Point3D.DirectionPoint(Direction.Up); // (0, 1, 0)
Z3Point3D distVec = normalized - up;
ArithExpr distance =
Max(
Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.X, distVec.Y, distVec.Z)),
Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Y, distVec.X, distVec.Z)),
Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Z, distVec.Y, distVec.X)));
BoolExpr expr = Z3.Context.MkLt(distance, Z3.Context.MkReal(1, 2));
Solver solver = Z3.Context.MkSolver();
solver.Assert(expr);
Status status = solver.Check();
Statistics stats = solver.Statistics;
switch (status)
{
case Status.UNKNOWN:
Console.WriteLine("Solver check for witness returned Status.UNKNOWN because: " + solver.ReasonUnknown);
throw new ArgumentException("Test Failed Expception");
case Status.UNSATISFIABLE:
Console.WriteLine("There is no valid witness for " + expr);
throw new ArgumentException("Test Failed Expception");
case Status.SATISFIABLE:
Console.WriteLine("OK, model: " + solver.Model);
break;
}
}
public RotateDirectionRestriction(JointType jointType, Z3Point3D startPoint, int degrees, Direction direction)
: base(body =>
{
ArithExpr currentValue;
var targetValue = 0.0;
var directionSign = 1.0;
var currentPoint = body.Joints[jointType];
CalcZ3CurrentAndTargetValue(currentPoint, startPoint, degrees, direction,
out currentValue, out targetValue, out directionSign);
BoolExpr expr = Z3.Context.MkTrue();
switch (direction)
{
case Direction.Right:
case Direction.Up:
case Direction.Front:
expr = Z3.Context.MkGt(currentValue, Z3Math.Real(targetValue));
break;
case Direction.Left:
case Direction.Down:
case Direction.Back:
expr = Z3.Context.MkLt(currentValue, Z3Math.Real(targetValue)); break;
}
return(expr);
},
body =>
{
double currentValue;
var targetValue = 0.0;
var directionSign = 1.0;
var currentPoint = body.Vectors[jointType];
var startPointConverted = new Point3D(
startPoint.GetXValue(),
startPoint.GetYValue(),
startPoint.GetZValue());
CalcCurrentAndTargetValue(currentPoint, startPointConverted, degrees, direction,
out currentValue, out targetValue, out directionSign);
var percentage = PercentageCalculator.calc(
-1 * directionSign,
targetValue,
currentValue);
return(percentage);
},
jointType)
{
this.JointType = jointType;
this.Direction = direction;
this.Degrees = degrees;
}
public Z3Body(Z3Body baseBody)
{
foreach (var jointType in baseBody.Joints.Keys)
{
this.Joints.Add(jointType,
new Z3Point3D(
Z3Math.GetRealValue(baseBody.Joints[jointType].X),
Z3Math.GetRealValue(baseBody.Joints[jointType].Y),
Z3Math.GetRealValue(baseBody.Joints[jointType].Z)
));
this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType])));
}
}
internal void UpdateTargetBody(Z3Body startBody)
{
// delayed statements are a special case
// they can be treated as restrictions or as transformations
// in order to the pose to retrieve the composite restrictions and transformations
// we must first provide the start body to update its delayed statements
// so they are treated first on the update target function
this.Gesture.Steps[this.CurrentStep].Pose.UpdateDelayedStatements(startBody);
var stopwatch = new Stopwatch();
StatisticsEntrySynthesize synTime = new StatisticsEntrySynthesize();
stopwatch.Start();
var time0 = stopwatch.ElapsedMilliseconds;
if (this.Target != null && this.CurrentStep > 0)
{
// If last target is not null than use the target transformed joints instead of the start transformed joints
// This way no error is cumulated along the transforms due to the matching precisions
foreach (var jointType in this.Gesture.Steps[this.CurrentStep - 1].Pose.GetTransformJointTypes())
{
// The Z3Point3D depends on a specific Z3 context deep underneath
// We need to thread the context through
startBody.Joints[jointType] =
new Z3Point3D(
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].X),
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Y),
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Z),
Z3.Context);
}
}
this.Target = this.Gesture.Steps[this.CurrentStep].Pose.CalcNearestTargetBody(startBody);
this.LastDistanceVectors = UpdateDistanceVectors(startBody, Z3.Context);
var time1 = stopwatch.ElapsedMilliseconds - time0;
stopwatch.Stop();
synTime.timeMs = time1;
synTime.gestureName = this.Gesture.Name;
synTime.poseName = this.Gesture.Steps[CurrentStep].Pose.Name;
synTime.uid = frameCount;
GestureStatistics.synthTimes.Add(synTime);
frameCount++;
}
private Dictionary <JointType, Point3D> UpdateDistanceVectors(Z3Body body, Context localContext)
{
var distancesZ3Point3Ds = body.GrabDistancePoint3Ds(this.Target.Body, this.Target.GetAllJointTypes());
var distancesVector3Ds = new Dictionary <JointType, Point3D>();
foreach (var pointZ3 in distancesZ3Point3Ds)
{
distancesVector3Ds.Add(
pointZ3.Key,
new Point3D(
Z3Math.GetRealValue(pointZ3.Value.X),
Z3Math.GetRealValue(pointZ3.Value.Y),
Z3Math.GetRealValue(pointZ3.Value.Z)));
}
return(distancesVector3Ds);
}
public Z3Point3D GetManhattanNormalized()
{
Z3Point3D result = new Z3Point3D();
ArithExpr higherCoord =
Z3Math.Max(
Z3Math.Max(
Z3Math.Abs(this.X),
Z3Math.Abs(this.Y)),
Z3Math.Abs(this.Z));
result.X = Z3Math.Div(this.X, higherCoord);
result.Y = Z3Math.Div(this.Y, higherCoord);
result.Z = Z3Math.Div(this.Z, higherCoord);
return(result);
}
public ArithExpr CalcApproximateDistance(Z3Point3D that)
{
// Manhattan distance vector
Z3Point3D distVec = this.GrabDistancePoint3D(that);
//ArithExpr result =
// Z3Math.Add(
// Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.X, distVec.Y, distVec.Z)),
// Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Y, distVec.X, distVec.Z)),
// Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Z, distVec.Y, distVec.X)));
ArithExpr result =
Z3Math.Max(
Z3Math.Abs(distVec.X),
Z3Math.Abs(distVec.Y),
Z3Math.Abs(distVec.Z));
return(result);
}
public RotateJointTransform(int degrees, BodyPlaneType plane, RotationDirection rotationDirection = RotationDirection.Clockwise)
: base(
joint =>
{
double cosInput = TrigonometryHelper.GetCosine(degrees);
double sinInput = TrigonometryHelper.GetSine(degrees);
if (rotationDirection == RotationDirection.CounterClockwise)
{
sinInput = -sinInput;
}
var cos = Z3Math.Real(cosInput);
var sin = Z3Math.Real(sinInput);
var sinNeg = Z3Math.Real(-sinInput);
Z3Point3D result = new Z3Point3D(joint.X, joint.Y, joint.Z);
switch (plane)
{
case BodyPlaneType.Frontal:
result.Y = Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z));
result.Z = Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z));
break;
case BodyPlaneType.Sagittal:
result.X = Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y));
result.Y = Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y));
break;
case BodyPlaneType.Horizontal:
result.X = Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z));
result.Z = Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z));
break;
default:
break;
}
return(result);
})
{ }
internal void UpdateTargetBody(Z3Body start, Context localZ3Context)
{
var stopwatch = new Stopwatch();
StatisticsEntrySynthesize synTime = new StatisticsEntrySynthesize();
stopwatch.Start();
var time0 = stopwatch.ElapsedMilliseconds;
if (this.Target != null && this.CurrentStep > 0)
{
// If last target is not null than use the target transformed joints instead of the start transformed joints
// This way no error is cumulated along the transforms due to the matching precisions
foreach (var jointType in this.Gesture.Steps[this.CurrentStep - 1].Pose.GetTransformJointTypes())
{
// The Z3Point3D depends on a specific Z3 context deep underneath
// We need to thread the context through
start.Joints[jointType] =
new Z3Point3D(
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].X),
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Y),
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Z),
localZ3Context);
}
}
this.Target = this.Gesture.Steps[this.CurrentStep].Pose.CalcNearestTargetBody(start);
this.LastDistanceVectors = UpdateDistanceVectors(start, Z3.Context);
var time1 = stopwatch.ElapsedMilliseconds - time0;
stopwatch.Stop();
synTime.timeMs = time1;
synTime.gestureName = this.Gesture.Name;
synTime.poseName = this.Gesture.Steps[CurrentStep].Pose.Name;
synTime.uid = frameCount;
//totalZ3Time += time1;
frameCount++;
GestureStatistics.synthTimes.Add(synTime);
//Console.Write("z3time: " + (totalZ3Time / frameCount) + " \r");
}
public static Z3Body MkZ3Const()
{
var result = new Z3Body();
var jointTypes = EnumUtil.GetValues <JointType>();
foreach (var jointType in jointTypes)
{
result.Joints.Add(jointType, Z3Point3D.MkZ3Const(EnumUtil.GetDescription <JointType>(jointType)));
}
result.Norms.Add(JointType.SpineMid, Z3Math.Real(0.3));
result.Norms.Add(JointType.SpineShoulder, Z3Math.Real(0.3));
result.Norms.Add(JointType.ShoulderLeft, Z3Math.Real(0.2));
result.Norms.Add(JointType.ElbowLeft, Z3Math.Real(0.2));
result.Norms.Add(JointType.WristLeft, Z3Math.Real(0.2));
result.Norms.Add(JointType.HandLeft, Z3Math.Real(0.1));
result.Norms.Add(JointType.HandTipLeft, Z3Math.Real(0.1));
result.Norms.Add(JointType.ThumbLeft, Z3Math.Real(0.1));
result.Norms.Add(JointType.Neck, Z3Math.Real(0.1));
result.Norms.Add(JointType.Head, Z3Math.Real(0.2));
result.Norms.Add(JointType.ShoulderRight, Z3Math.Real(0.2));
result.Norms.Add(JointType.ElbowRight, Z3Math.Real(0.2));
result.Norms.Add(JointType.WristRight, Z3Math.Real(0.2));
result.Norms.Add(JointType.HandRight, Z3Math.Real(0.1));
result.Norms.Add(JointType.HandTipRight, Z3Math.Real(0.1));
result.Norms.Add(JointType.ThumbRight, Z3Math.Real(0.1));
result.Norms.Add(JointType.SpineBase, Z3Math.Real(0.0));
result.Norms.Add(JointType.HipLeft, Z3Math.Real(0.2));
result.Norms.Add(JointType.KneeLeft, Z3Math.Real(0.3));
result.Norms.Add(JointType.AnkleLeft, Z3Math.Real(0.3));
result.Norms.Add(JointType.FootLeft, Z3Math.Real(0.1));
result.Norms.Add(JointType.HipRight, Z3Math.Real(0.2));
result.Norms.Add(JointType.KneeRight, Z3Math.Real(0.3));
result.Norms.Add(JointType.AnkleRight, Z3Math.Real(0.3));
result.Norms.Add(JointType.FootRight, Z3Math.Real(0.1));
return(result);
}
//public ArithExpr CalcApproximateDistanceBetweenNormalized(Z3Point3D that)
//{
// Z3Point3D thisNormalized = this.GetApproximateNormalized();
// Z3Point3D thatNormalized = that.GetApproximateNormalized();
// ArithExpr result = thisNormalized.CalcApproximateDistance(thatNormalized);
// return result;
//}
// Assumes vectors are normalized
public BoolExpr IsDegreesBetweenLessThan(Z3Point3D that, int degreesThreshold)
{
double distanceThreshold = TrigonometryHelper.GetDistance(degreesThreshold);
ArithExpr distance = this.CalcApproximateDistance(that);
BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold));
//// TODO remove this, test code
//SolverCheckResult checkResult = Z3AnalysisInterface.CheckStatus(result);
//if (checkResult.Status == Status.SATISFIABLE)
//{
// var joint = new Z3Point3D(
// checkResult.Model.Evaluate(this.X, true) as ArithExpr,
// checkResult.Model.Evaluate(this.Y, true) as ArithExpr,
// checkResult.Model.Evaluate(this.Z, true) as ArithExpr);
// var distanceSolvedExpr = checkResult.Model.Evaluate(distance, true) as ArithExpr;
// var distanceValue = Z3Math.GetRealValue(distanceSolvedExpr);
//}
//// end of test code
return(result);
}
/// <summary>
/// This function converts Kinect joints to a Z3 body.
/// It also changes the basis of body coordinates to make it
/// invariant to body position in relation to the sensor.
///
/// The origin of the new coordinate system is user hips.
/// </summary>
/// <param name="kinectJoints"></param>
/// <returns></returns>
public static Z3Body CreateZ3Body(
IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>
kinectJoints)
{
var jointVectors = new Dictionary <Microsoft.Kinect.JointType, Vector3D>();
jointVectors.Add(Microsoft.Kinect.JointType.SpineMid, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineMid], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointVectors.Add(Microsoft.Kinect.JointType.SpineShoulder, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineShoulder], kinectJoints[Microsoft.Kinect.JointType.SpineMid]));
jointVectors.Add(Microsoft.Kinect.JointType.ShoulderLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderLeft], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder]));
jointVectors.Add(Microsoft.Kinect.JointType.ElbowLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowLeft], kinectJoints[Microsoft.Kinect.JointType.ShoulderLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.WristLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristLeft], kinectJoints[Microsoft.Kinect.JointType.ElbowLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.HandLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandLeft], kinectJoints[Microsoft.Kinect.JointType.WristLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.HandTipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipLeft], kinectJoints[Microsoft.Kinect.JointType.HandLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.ThumbLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbLeft], kinectJoints[Microsoft.Kinect.JointType.HandLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.Neck, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Neck], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder]));
jointVectors.Add(Microsoft.Kinect.JointType.Head, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Head], kinectJoints[Microsoft.Kinect.JointType.Neck]));
jointVectors.Add(Microsoft.Kinect.JointType.ShoulderRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderRight], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder]));
jointVectors.Add(Microsoft.Kinect.JointType.ElbowRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowRight], kinectJoints[Microsoft.Kinect.JointType.ShoulderRight]));
jointVectors.Add(Microsoft.Kinect.JointType.WristRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristRight], kinectJoints[Microsoft.Kinect.JointType.ElbowRight]));
jointVectors.Add(Microsoft.Kinect.JointType.HandRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandRight], kinectJoints[Microsoft.Kinect.JointType.WristRight]));
jointVectors.Add(Microsoft.Kinect.JointType.HandTipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipRight], kinectJoints[Microsoft.Kinect.JointType.HandRight]));
jointVectors.Add(Microsoft.Kinect.JointType.ThumbRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbRight], kinectJoints[Microsoft.Kinect.JointType.HandRight]));
// Spine base is the root of the system, as it has no direction to store, it stores its own position
jointVectors.Add(Microsoft.Kinect.JointType.SpineBase, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineBase], new Joint()));
jointVectors.Add(Microsoft.Kinect.JointType.HipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointVectors.Add(Microsoft.Kinect.JointType.KneeLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeLeft], kinectJoints[Microsoft.Kinect.JointType.HipLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.AnkleLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleLeft], kinectJoints[Microsoft.Kinect.JointType.KneeLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.FootLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootLeft], kinectJoints[Microsoft.Kinect.JointType.AnkleLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.HipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointVectors.Add(Microsoft.Kinect.JointType.KneeRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeRight], kinectJoints[Microsoft.Kinect.JointType.HipRight]));
jointVectors.Add(Microsoft.Kinect.JointType.AnkleRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleRight], kinectJoints[Microsoft.Kinect.JointType.KneeRight]));
jointVectors.Add(Microsoft.Kinect.JointType.FootRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootRight], kinectJoints[Microsoft.Kinect.JointType.AnkleRight]));
var rotationMatrix = new Matrix3D();
InitMatrix(out rotationMatrix, kinectJoints);
rotationMatrix.Invert();
var joints = new Dictionary <PreposeGestures.JointType, Z3Point3D>();
var norms = new Dictionary <PreposeGestures.JointType, ArithExpr>();
norms.Add(PreposeGestures.JointType.SpineBase, Z3Math.Real(jointVectors[Microsoft.Kinect.JointType.SpineBase].Length));
joints.Add(PreposeGestures.JointType.SpineBase,
new Z3Point3D(
jointVectors[Microsoft.Kinect.JointType.SpineBase].X,
jointVectors[Microsoft.Kinect.JointType.SpineBase].Y,
jointVectors[Microsoft.Kinect.JointType.SpineBase].Z));
var jointTypes = EnumUtil.GetValues <Microsoft.Kinect.JointType>();
foreach (var jointType in jointTypes)
{
if (jointType != Microsoft.Kinect.JointType.SpineBase)
{
jointVectors[jointType] = jointVectors[jointType] * rotationMatrix;
var z3Joint = Convert(jointType);
norms.Add(z3Joint, Z3Math.Real(jointVectors[jointType].Length));
var temp = jointVectors[jointType];
temp.Normalize();
joints.Add(
z3Joint,
new Z3Point3D(
temp.X,
temp.Y,
-temp.Z));
}
}
return(new Z3Body(joints, norms));
}
public PutBodyRestriction(JointType jointType1, JointType jointType2, RelativeDirection direction, bool dont = false)
: base(body =>
{
var distanceThreshold = Z3Math.Real(0.01);
var joint1Position = body.GetJointZ3Position(jointType1);
var joint2Position = body.GetJointZ3Position(jointType2);
var expr = Z3.Context.MkTrue();
switch (direction)
{
case RelativeDirection.InFrontOfYour:
expr = Z3.Context.MkGt(joint1Position.Z, Z3Math.Add(joint2Position.Z, distanceThreshold));
break;
case RelativeDirection.BehindYour:
expr = Z3.Context.MkLt(joint1Position.Z, Z3Math.Sub(joint2Position.Z, distanceThreshold));
break;
case RelativeDirection.ToTheRightOfYour:
expr = Z3.Context.MkGt(joint1Position.X, Z3Math.Add(joint2Position.X, distanceThreshold));
break;
case RelativeDirection.ToTheLeftOfYour:
expr = Z3.Context.MkLt(joint1Position.X, Z3Math.Sub(joint2Position.X, distanceThreshold));
break;
case RelativeDirection.OnTopOfYour:
expr = Z3.Context.MkGt(joint1Position.Y, Z3Math.Add(joint2Position.Y, distanceThreshold));
break;
case RelativeDirection.BelowYour:
expr = Z3.Context.MkLt(joint1Position.Y, Z3Math.Sub(joint2Position.Y, distanceThreshold));
break;
}
if (dont)
{
expr = Z3.Context.MkNot(expr);
}
return(expr);
},
body =>
{
var stopwatch = new Stopwatch();
stopwatch.Start();
var distanceThreshold = 0.01;
var point1 = body.Positions[jointType1];
var point2 = body.Positions[jointType2];
var targetValue = 1.0;
var currentValue = 1.0;
var lowerBound = 0.0;
// inverting direction if expression is negated
if (dont)
{
switch (direction)
{
case RelativeDirection.ToTheRightOfYour: direction = RelativeDirection.ToTheLeftOfYour; break;
case RelativeDirection.ToTheLeftOfYour: direction = RelativeDirection.ToTheRightOfYour; break;
case RelativeDirection.OnTopOfYour: direction = RelativeDirection.BelowYour; break;
case RelativeDirection.BelowYour: direction = RelativeDirection.OnTopOfYour; break;
case RelativeDirection.InFrontOfYour: direction = RelativeDirection.BehindYour; break;
case RelativeDirection.BehindYour: direction = RelativeDirection.InFrontOfYour; break;
}
}
switch (direction)
{
case RelativeDirection.ToTheRightOfYour:
currentValue = point1.X;
targetValue = point2.X + distanceThreshold;
lowerBound = -1.0;
break;
case RelativeDirection.ToTheLeftOfYour:
currentValue = point1.X;
targetValue = point2.X - distanceThreshold;
lowerBound = 1.0;
break;
case RelativeDirection.OnTopOfYour:
currentValue = point1.Y;
targetValue = point2.Y + distanceThreshold;
lowerBound = -1.0;
break;
case RelativeDirection.BelowYour:
currentValue = point1.Y;
targetValue = point2.Y - distanceThreshold;
lowerBound = 1.0;
break;
case RelativeDirection.InFrontOfYour:
currentValue = point1.Z;
targetValue = point2.Z + distanceThreshold;
lowerBound = -1.0;
break;
case RelativeDirection.BehindYour:
currentValue = point1.Z;
targetValue = point2.Z - distanceThreshold;
lowerBound = 1.0;
break;
}
var percentage = PercentageCalculator.calc(lowerBound, targetValue, currentValue);
//Console.WriteLine("put " + stopwatch.ElapsedTicks);
return(percentage);
},
jointType1,
jointType2)
{
this.JointType1 = jointType1;
this.JointType2 = jointType2;
this.Direction = direction;
if (dont)
{
this.isNegated = true;
}
else
{
this.isNegated = false;
}
}
public Z3Point3D(double x, double y, double z)
{
this.X = Z3Math.Real(x);
this.Y = Z3Math.Real(y);
this.Z = Z3Math.Real(z);
}
public Z3Point3D(double x, double y, double z, Context localZ3Context)
{
this.X = Z3Math.Real(x, localZ3Context);
this.Y = Z3Math.Real(y, localZ3Context);
this.Z = Z3Math.Real(z, localZ3Context);
}
ArithExpr CalcApproximateCoordFromManhattanToEuclidianSystem(
ArithExpr firstCoord,
ArithExpr secondCoord,
ArithExpr thirdCoord)
{
// Work only with values length
// Values sign will be assigned again in the end
ArithExpr firstCoordLength = Z3Math.Abs(firstCoord);
ArithExpr secondCoordLength = Z3Math.Abs(secondCoord);
ArithExpr thirdCoordLength = Z3Math.Abs(thirdCoord);
// The all common length will be weighted by this
// This way for example a (1, 1, 1) vector will become
// A (0.57, 0.57, 0.57) with norm near to 1
//ArithExpr sqrt1div3 = Z3Math.Real(0.57735026918962576450914878050196);
ArithExpr sqrt1div3 = Z3Math.Real(0.577);
// The remaining common length will be weighted by this
// This way for example a (1, 1, 0) vector will become
// A (0.7, 0.7, 0.7) with norm near to 1
//ArithExpr sin45 = Z3Math.Real(0.70710678118654752440084436210485)
ArithExpr sin45 = Z3Math.Real(0.707);
// Calc common length between x, y, z
ArithExpr allCommonLength =
Z3Math.Min(
firstCoordLength,
secondCoordLength,
thirdCoordLength);
// Calc the common length between the target coord (firstCoord)
// and the higher coord between the second and third coords
ArithExpr lastTwoCommonLength =
Z3Math.Max(
Z3Math.Min(secondCoordLength, firstCoordLength),
Z3Math.Min(thirdCoordLength, firstCoordLength));
// Calc exclusevely common length with the remaining coordinate
ArithExpr lastTwoExclusiveCommonLength =
Z3Math.Sub(
lastTwoCommonLength,
allCommonLength);
// Calc remaining length
ArithExpr especificLength =
Z3Math.Sub(firstCoordLength,
Z3Math.Add(lastTwoExclusiveCommonLength, allCommonLength));
// Calc weighted lengths
ArithExpr weigthedLength1 = Z3Math.Mul(lastTwoExclusiveCommonLength, sin45);
ArithExpr weigthedLength2 = Z3Math.Mul(allCommonLength, sqrt1div3);
// Calc weighted result length
ArithExpr resultLength =
Z3Math.Add(
especificLength,
weigthedLength1,
weigthedLength2);
// The transform doesn't change the sign of the coordinate
// Recover it from original data
Expr result =
Z3.Context.MkITE(
Z3.Context.MkGe(firstCoord, Z3Math.Zero),
resultLength,
Z3Math.Neg(resultLength));
return(result as ArithExpr);
}
public double GetZValue()
{
return(Z3Math.GetRealValue(Z));
}
public double GetYValue()
{
return(Z3Math.GetRealValue(Y));
}
public double GetXValue()
{
return(Z3Math.GetRealValue(X));
}
