public BoolExpr IsNearerThan(Z3Point3D that, double distanceThreshold)
{
ArithExpr distance = this.CalcApproximateDistance(that);
BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold));
return(result);
}
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 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])));
}
}
//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);
}
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);
}
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 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])));
}
}
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);
})
{ }
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);
}
public static IBodyRestriction TestBodyRestriction()
{
var result = new SimpleBodyRestriction(body =>
{
double distanceThreshold = 0.4;
ArithExpr distanceThresholdSquared = Z3Math.Real(distanceThreshold * distanceThreshold);
ArithExpr x1 = body.Joints[JointType.SpineMid].X;
ArithExpr y1 = body.Joints[JointType.SpineMid].Y;
ArithExpr z1 = body.Joints[JointType.SpineMid].Z;
ArithExpr x2 = Z3Math.Zero;
ArithExpr y2 = Z3Math.One;
ArithExpr z2 = Z3Math.Zero;
//ArithExpr x2 = body.Joints[JointType.SpineBase].X;
//ArithExpr y2 = body.Joints[JointType.SpineBase].Y;
//ArithExpr z2 = body.Joints[JointType.SpineBase].Z;
var xExpr = Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2));
var yExpr = Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2));
var zExpr = Z3Math.Mul(Z3Math.Sub(z1, z2), Z3Math.Sub(z1, z2));
var distanceSquared = Z3Math.Add(xExpr, yExpr, zExpr);
// This is runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), y2));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y2, y2)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y1)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Mul(y1, y2)));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2)));
// But this does not
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Add(y1, y2), Z3Math.Add(y1, y2)));
// This fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(0))));
// This does not
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1, 7))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 6)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 6))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 3)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 3))));
// This does not
// asserting: (x1 - 0)² + (y1 - 1/2)² <= 0.4²
// which is x1² + y1² - y/2 - y/2 + 1/4 <= 0.16
// which is x1² + y1² - y + 0.25 <= 0.16
// which is x1² + y1² - y <= -0.09
// if x1 = 0 and y1 = 0.1 we would have a solution
// 0 + 0.01 - 0.1 <= 0.09
// 0.09 <= 0.09
// but z3 is taking too long to solve this.
var addExpr = Z3Math.Add(
Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 2)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 2))));
// But this is not (Z3 just stops while solving it)
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2)));
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1)));
//BoolExpr expr = Z3.Context.MkLt(distanceSquared, distanceThresholdSquared);
BoolExpr expr = Z3.Context.MkLe(addExpr, distanceThresholdSquared);
// Why it complains about this Greater-Than case?
//BoolExpr expr = Z3.Context.MkGt(xExpr, distanceThresholdSquared);
return(expr);
},
body => { return(1.0); });
return(result);
}
public RotateJointTransform(int angle, Direction direction)
: base(
joint =>
{
double cosInput = TrigonometryHelper.GetCosine(angle);
double sinInput = TrigonometryHelper.GetSine(angle);
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);
// The performed rotation depends on current values of X, Y and Z
// The rotation plane and direction changes depending on the relation between the coordinates
switch (direction)
{
case Direction.Back:
result.X =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Y)
// then return Y
// else rotate Y
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
joint.Y,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else if Y > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr;
break;
case Direction.Front:
result.X =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Y)
// then return Y
// else rotate Y
// if Y > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
joint.Y,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else if Y > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr;
break;
case Direction.Down:
result.X =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)))) as ArithExpr;
break;
case Direction.Up:
result.X =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else if Z > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)))) as ArithExpr;
break;
case Direction.Right:
result.X =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr;
result.Y =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else return Y
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X))),
joint.Y) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)))) as ArithExpr;
break;
case Direction.Left:
result.X =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr;
result.Y =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else return Y
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X))),
joint.Y) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)))) as ArithExpr;
break;
default:
break;
}
return(result);
})
{ }
public PutBodyRestriction(JointType jointType1, JointType jointType2, RelativeDirection direction, bool dont = false)
: base(body =>
{
ArithExpr distanceThreshold = Z3Math.Real(0.1);
Z3Point3D joint1Position = body.GetJointPosition(jointType1);
Z3Point3D joint2Position = body.GetJointPosition(jointType2);
BoolExpr 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);
},
//JointTypeHelper.GetListFromLeafToRoot(jointType1).Union(JointTypeHelper.GetListFromLeafToRoot(jointType2)).ToList())
jointType1,
jointType2)
{
this.JointType1 = jointType1;
this.JointType2 = jointType2;
this.Direction = direction;
if (dont)
{
this.isNegated = true;
}
else
{
this.isNegated = false;
}
}
/// <summary>
/// This is a basic safety check to make sure we don't break any bones.
/// </summary>
/// <returns>True if safe</returns>
public static IBodyRestriction DefaultSafetyRestriction()
{
var result = new CompositeBodyRestriction();
int inclinationThreshold = 45;
// Head
// Make sure neck is not inclinated beyond the threshold
var head = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
BoolExpr expr1 = body.Joints[JointType.Head].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.Neck].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return(expr);
});
result.And(head);
// Spine
// Make sure spine is not inclinated beyond the threshold
var spine = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
BoolExpr expr1 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr3 =
body.Joints[JointType.SpineMid].IsAngleBetweenLessThan(
body.Joints[JointType.SpineShoulder], inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return(expr);
});
result.And(spine);
// Shoulders
// Make sure shoulders are not bent
var shoulders = new SimpleBodyRestriction(body =>
{
BoolExpr expr =
body.Joints[JointType.SpineMid].IsAngleBetweenGreaterThan(
body.Joints[JointType.SpineShoulder], 120);
return(expr);
});
result.And(shoulders);
// Elbows
// Make sure elbows are not behind the back
// And also that they are not on the top/back sub-space
var elbows = new SimpleBodyRestriction(body =>
{
BoolExpr exprRight1 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero),
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].X, Z3Math.Zero)));
BoolExpr exprRight2 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowRight].Y, Z3Math.Zero)));
BoolExpr exprLeft1 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].X, Z3Math.Zero)));
BoolExpr exprLeft2 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].Y, Z3Math.Zero)));
BoolExpr expr = Z3.Context.MkAnd(exprLeft1, exprLeft2, exprRight1, exprRight2);
return(expr);
});
result.And(elbows);
// Wrists
// Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows
// unless elbows are up or wrists are directed to torso
// TODO
// Hips
// Make sure hips are aligned with the shoulders or at lest within the range
var hips = new SimpleBodyRestriction(body =>
{
Z3Point3D shouldersSum =
body.Joints[JointType.ShoulderLeft].GetInverted() +
body.Joints[JointType.ShoulderRight];
Z3Point3D hipsSum =
body.Joints[JointType.HipLeft].GetInverted() +
body.Joints[JointType.HipRight];
BoolExpr expr = shouldersSum.IsAngleBetweenLessThan(hipsSum, 45);
return(expr);
});
result.And(hips);
// Legs
// Make sure legs are not higher than threshold
var legs = new SimpleBodyRestriction(body =>
{
BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.KneeLeft].Y, Z3Math.Real(0));
BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.KneeRight].Y, Z3Math.Real(0));
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return(expr);
});
result.And(legs);
// Ankles
// Make sure ankles are not inclinated up more than knees
// unless ankles are pointing back
// TODO
return(result);
}
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 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);
}
public Z3Point3D(double x, double y, double z)
{
this.X = Z3Math.Real(x);
this.Y = Z3Math.Real(y);
this.Z = Z3Math.Real(z);
}
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 static IBodyRestriction ShouldersRelaxedRestriction(int degreesThreshold = -5)
{
double maxY = TrigonometryHelper.GetSine(degreesThreshold);
var result = new SimpleBodyRestriction
(body =>
{
// Check if both shoulders have a lower Y than maxY
BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.ShoulderLeft].Y, Z3Math.Real(maxY));
BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.ShoulderRight].Y, Z3Math.Real(maxY));
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return(expr);
},
body => { return(1.0); });
return(result);
}
public SetJointDirectionTransform(double x, double y, double z)
: base(joint => new Z3Point3D(Z3Math.Real(x), Z3Math.Real(y), Z3Math.Real(z)))
{
}
// the rotation towards a direction is limited to the directive vector of that direction
public RotateJointTransform(int degrees, Direction direction)
: base(
joint =>
{
// define current degrees
var currentDegrees = 0.0;
switch (direction)
{
case Direction.Right:
case Direction.Left:
currentDegrees = Math.Asin(joint.GetXValue()) * 180.0 / Math.PI;
break;
case Direction.Up:
case Direction.Down:
currentDegrees = Math.Asin(joint.GetYValue()) * 180.0 / Math.PI;
break;
case Direction.Front:
case Direction.Back:
currentDegrees = Math.Asin(joint.GetZValue()) * 180.0 / Math.PI;
break;
}
// check if current degrees + input degrees is an overflow
// which means the rotation is going towards the opposite direction
if (currentDegrees + degrees > 90 ||
currentDegrees - degrees < -90)
{
// if so, set degrees as the complement to the limit value
degrees = (int)(90.0 - currentDegrees);
}
double cosInput = TrigonometryHelper.GetCosine(degrees);
double sinInput = TrigonometryHelper.GetSine(degrees);
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);
// The performed rotation depends on current values of X, Y and Z
// The rotation plane and direction changes depending on the relation between the coordinates
switch (direction)
{
case Direction.Back:
result.X =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Y)
// then return Y
// else rotate Y
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
joint.Y,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else if Y > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr;
break;
case Direction.Front:
result.X =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Y)
// then return Y
// else rotate Y
// if Y > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
joint.Y,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Y)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else if Y > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr;
break;
case Direction.Down:
result.X =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate counter clockwise
// else rotate clockwise
// else if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)))) as ArithExpr;
break;
case Direction.Up:
result.X =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else return X
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y))),
joint.X) as ArithExpr;
result.Y =
// if Abs(X) >= Abs(Z)
// if X > 0
// rotate clockwise
// else rotate counter clockwise
// else if Z > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate clockwise
// else rotate counter clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)))) as ArithExpr;
break;
case Direction.Right:
result.X =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr;
result.Y =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else return Y
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X))),
joint.Y) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)))) as ArithExpr;
break;
case Direction.Left:
result.X =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X))),
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)),
Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr;
result.Y =
// if Abs(Y) >= Abs(Z)
// if Y > 0
// rotate counter clockwise
// else rotate clockwise
// else return Y
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)),
Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X))),
joint.Y) as ArithExpr;
result.Z =
// if Abs(X) >= Abs(Z)
// then return Z
// else rotate Z
// if Z > 0
// rotate counter clockwise
// else rotate clockwise
Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)),
joint.Z,
Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)),
Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)))) as ArithExpr;
break;
default:
break;
}
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 static Dictionary <JointType, Z3Point3D> CreateSyntheticJoints()
{
var result = new Dictionary <JointType, Z3Point3D>();
Z3Point3D spineBase = Z3Point3D.DirectionPoint(Direction.Front);
Z3Point3D spineMid = Z3Point3D.DirectionPoint(Direction.Up);
Z3Point3D spineShoulder = Z3Point3D.DirectionPoint(Direction.Up);
Z3Point3D neck = Z3Point3D.DirectionPoint(Direction.Up);
Z3Point3D head = Z3Point3D.DirectionPoint(Direction.Up);
Z3Point3D shoulderLeft = new Z3Point3D();
shoulderLeft.X = Z3Math.Real(-0.7);
shoulderLeft.Y = Z3Math.Real(-0.7);
shoulderLeft.Z = Z3Math.Zero;
Z3Point3D elbowLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D wristLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D handLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D handTipLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D thumbLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D hipLeft = new Z3Point3D();
shoulderLeft.X = Z3Math.Real(-0.7);
shoulderLeft.Y = Z3Math.Real(-0.7);
shoulderLeft.Z = Z3Math.Zero;
Z3Point3D kneeLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D ankleLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D footLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D shoulderRight = new Z3Point3D();
shoulderRight.X = Z3Math.Real(0.7);
shoulderRight.Y = Z3Math.Real(-0.7);
shoulderRight.Z = Z3Math.Zero;
Z3Point3D elbowRight = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D wristRight = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D handRight = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D handTipRight = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D thumbRight = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D hipRight = new Z3Point3D();
shoulderRight.X = Z3Math.Real(0.7);
shoulderRight.Y = Z3Math.Real(-0.7);
shoulderRight.Z = Z3Math.Zero;
Z3Point3D kneeRight = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D ankleRight = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D footRight = Z3Point3D.DirectionPoint(Direction.Down);
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);
}
