public static IBodyRestriction DontTwistHipRestriction(int angleThreshold = 15)
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
var result = new SimpleBodyRestriction(body =>
{
// Shoulders, hips and feet must be aligned
Z3Point3D leftToRightShoulderVec = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight];
Z3Point3D leftToRightHipVec = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight];
Z3Point3D leftAnklePosition =
body.Joints[JointType.HipLeft] +
body.Joints[JointType.KneeLeft] +
body.Joints[JointType.AnkleLeft];
Z3Point3D rightAnklePosition =
body.Joints[JointType.HipRight] +
body.Joints[JointType.KneeRight] +
body.Joints[JointType.AnkleRight];
Z3Point3D leftToRightAnkleVec = rightAnklePosition - leftAnklePosition;
BoolExpr expr1 = leftToRightShoulderVec.IsAngleBetweenLessThan(leftToRightHipVec, angleThreshold);
BoolExpr expr2 = leftToRightShoulderVec.IsAngleBetweenLessThan(leftToRightAnkleVec, angleThreshold);
BoolExpr expr3 = leftToRightAnkleVec.IsAngleBetweenLessThan(leftToRightHipVec, angleThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return expr;
});
return result;
}
public static IBodyRestriction DistributeWeightRestriction(int angleThreshold = 15)
{
var result = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
Z3Point3D legLeft = body.Joints[JointType.KneeLeft] + body.Joints[JointType.AnkleLeft];
Z3Point3D legRight = body.Joints[JointType.KneeRight] + body.Joints[JointType.AnkleRight];
Z3Point3D legsAverage = (legLeft + legRight) / 2;
BoolExpr expr = legsAverage.IsAngleBetweenLessThan(up, angleThreshold);
return expr;
});
return result;
}
// Therapy restrictions
public static IBodyRestriction StraightPostureRestriction(int angleThreshold = 15)
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
double distanceThreshold = TrigonometryHelper.GetSine(angleThreshold);
var result = new SimpleBodyRestriction(body =>
{
BoolExpr expr = body.Joints[JointType.SpineMid].IsNearerThan(up, distanceThreshold);
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.SpineShoulder].IsNearerThan(up, distanceThreshold));
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Neck].IsNearerThan(up, distanceThreshold));
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Head].IsNearerThan(up, distanceThreshold));
return expr;
});
return result;
}
public static Z3Body CreateBodyWitness(
Z3Body z3ConstCheckedBody,
Model model,
List<JointType> evaluatedJoints,
Z3Body defaultBody)
{
var witness = new Z3Body();
var jointTypes = EnumUtil.GetValues<JointType>();
foreach (var jointType in jointTypes)
{
if (evaluatedJoints.Contains(jointType))
{
var joint = new Z3Point3D(
model.Evaluate(z3ConstCheckedBody.Joints[jointType].X, true) as ArithExpr,
model.Evaluate(z3ConstCheckedBody.Joints[jointType].Y, true) as ArithExpr,
model.Evaluate(z3ConstCheckedBody.Joints[jointType].Z, true) as ArithExpr);
witness.Joints.Add(jointType, joint);
var norm = model.Evaluate(z3ConstCheckedBody.Norms[jointType]) as ArithExpr;
// Check if norm is still an app (meaning it can be anything), then set it to be the default norm
if (norm.ASTKind == Z3_ast_kind.Z3_APP_AST)
witness.Norms.Add(jointType, defaultBody.Norms[jointType]);
else
witness.Norms.Add(jointType, norm);
}
else
{
witness.Joints.Add(jointType, defaultBody.Joints[jointType]);
witness.Norms.Add(jointType, defaultBody.Norms[jointType]);
}
}
return witness;
}
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;
}
public SetJointDirectionTransform(Z3Point3D jointToSet)
: base(joint => jointToSet)
{
}
// 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;
})
{
}
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 Z3Point3D Transform(Z3Point3D joint)
{
Z3Point3D result = TransformFunc(joint);
return result;
}
public override bool Equals(object obj)
{
var that = obj as JointTransform;
Solver solver = Z3.Context.MkSolver();
solver.Push();
RealExpr x = Z3.Context.MkRealConst("x");
RealExpr y = Z3.Context.MkRealConst("y");
RealExpr z = Z3.Context.MkRealConst("z");
Z3Point3D joint = new Z3Point3D(x, y, z);
Z3Point3D thisJoint = this.TransformFunc(joint);
Z3Point3D thatJoint = that.TransformFunc(joint);
BoolExpr xEq = Z3.Context.MkEq(thisJoint.X, thatJoint.X);
BoolExpr yEq = Z3.Context.MkEq(thisJoint.Y, thatJoint.Y);
BoolExpr zEq = Z3.Context.MkEq(thisJoint.Z, thatJoint.Z);
BoolExpr equalsExpr = Z3.Context.MkAnd(xEq, yEq, zEq);
solver.Assert(Z3.Context.MkNot(equalsExpr));
Status status = solver.Check();
Statistics stats = solver.Statistics;
//Console.WriteLine("EqualsExpr: " + equalsExpr);
//Console.WriteLine("Proving: " + equalsExpr);
//switch (status)
//{
// case Status.UNKNOWN:
// Console.WriteLine("Unknown because:\n" + solver.ReasonUnknown);
// break;
// case Status.SATISFIABLE:
// throw new ArgumentException("Test Failed Expception");
// case Status.UNSATISFIABLE:
// Console.WriteLine("OK, proof:\n" + solver.Proof);
// break;
//}
bool result = false;
if (status == Status.UNSATISFIABLE)
result = true;
solver.Pop();
return result;
}
/// <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].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.Neck].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return expr;
},
body => { return 1.0; });
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].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr3 =
body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(
body.Joints[JointType.SpineShoulder], inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return expr;
},
body => { return 1.0; });
result.And(spine);
// Shoulders
// Make sure shoulders are not bent
var shoulders = new SimpleBodyRestriction(body =>
{
BoolExpr expr =
body.Joints[JointType.SpineMid].IsDegreesBetweenGreaterThan(
body.Joints[JointType.SpineShoulder], 120);
return expr;
},
body => { return 1.0; });
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;
},
body => { return 1.0; });
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.IsDegreesBetweenLessThan(hipsSum, 45);
return expr;
},
body => { return 1.0; });
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;
},
body => { return 1.0; });
result.And(legs);
// Ankles
// Make sure ankles are not inclinated up more than knees
// unless ankles are pointing back
// TODO
return result;
}
private static void CalcZ3CurrentAndTargetValue(
Z3Point3D currentPoint,
Z3Point3D startPoint,
int degrees,
Direction direction,
out ArithExpr currentValue,
out double targetValue,
out double directionSign)
{
// once it is rotating towards a direction there is a limit for the rotation
// in this case the limit is imposed to a single component (X, Y or Z) relative to the direction
var limitValue = Math.Sin(75 * Math.PI / 180.0);
var radians = degrees * Math.PI / 180.0; // assigning a double for angle in radians
// determining if the direction sign is negative
directionSign = 1.0;
switch (direction)
{
case Direction.Down:
case Direction.Back:
case Direction.Left:
directionSign = -1.0;
break;
}
// update limit based on the direction sign
limitValue *= directionSign;
// start value stores the component (X, Y or Z) from the startPoint
// determining the current and start value
currentValue = currentPoint.X;
var startValue = 0.0;
switch (direction)
{
case Direction.Right:
case Direction.Left:
startValue = startPoint.GetXValue();
currentValue = currentPoint.X;
break;
case Direction.Up:
case Direction.Down:
startValue = startPoint.GetYValue();
currentValue = currentPoint.Y;
break;
case Direction.Front:
case Direction.Back:
startValue = startPoint.GetZValue();
currentValue = currentPoint.Z;
break;
}
double startRadians = Math.Asin(startValue);
double targetRadians = startRadians + (directionSign * radians);
targetValue = Math.Sin(targetRadians);
// this first case tells that the rotation is bigger than the desired and
// is moving the vector (targetValue) on the opposite direction
// this rotation is not desired here because we are rotating towards a direction
// the rotation is not a pure transform
var targetIsLowerThanStart =
directionSign * targetValue <
directionSign * startValue;
// this second case tells that the rotation exceeded the limitValue
var targetExceedsLimit = Math.Abs(targetValue) > Math.Abs(limitValue);
// on both cases the targetValue should be the limitValue
if (targetIsLowerThanStart || targetExceedsLimit)
{
targetValue = limitValue;
}
}
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 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 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;
})
{
}
