public static IBodyRestriction DontTwistHipRestriction(int degreesThreshold = 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.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold);
BoolExpr expr2 = leftToRightShoulderVec.IsDegreesBetweenLessThan(leftToRightAnkleVec, degreesThreshold);
BoolExpr expr3 = leftToRightAnkleVec.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return(expr);
},
body => { return(1.0); });
return(result);
}
public static Z3Point3D DirectionPoint(Direction direction)
{
Z3Point3D point3D = new Z3Point3D();
switch (direction)
{
case Direction.Up:
point3D = Z3Point3D.Up();
break;
case Direction.Down:
point3D = Z3Point3D.Down();
break;
case Direction.Left:
point3D = Z3Point3D.Left();
break;
case Direction.Right:
point3D = Z3Point3D.Right();
break;
case Direction.Front:
point3D = Z3Point3D.Front();
break;
case Direction.Back:
point3D = Z3Point3D.Back();
break;
}
return(point3D);
}
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 = Z3Point3D.DirectionPoint(Direction.Left);
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 = Z3Point3D.DirectionPoint(Direction.Left);
Z3Point3D kneeLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D ankleLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D footLeft = Z3Point3D.DirectionPoint(Direction.Down);
Z3Point3D shoulderRight = Z3Point3D.DirectionPoint(Direction.Right);
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 = Z3Point3D.DirectionPoint(Direction.Right);
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 Z3Point3D GrabDistancePoint3D(Z3Point3D that)
{
// Manhattan distance vector
Z3Point3D result = this - that;
return(result);
}
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false)
: base(body =>
{
JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
Z3Point3D joint1Position = body.GetJointPosition(jointType);
Z3Point3D joint2Position = body.GetJointPosition(sidedHandType);
BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold);
if (dont)
{
expr = Z3.Context.MkNot(expr);
}
return(expr);
},
jointType,
JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide))
{
this.JointType = jointType;
this.HandSide = handSide;
if (dont)
{
isNegated = true;
}
else
{
isNegated = false;
}
}
public BoolExpr IsNearerThan(Z3Point3D that, double distanceThreshold)
{
ArithExpr distance = this.CalcApproximateDistance(that);
BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold));
return(result);
}
internal BoolExpr IsEqualTo(Z3Point3D that)
{
var result = Z3.Context.MkAnd(
Z3.Context.MkEq(this.X, that.X),
Z3.Context.MkEq(this.Y, that.Y),
Z3.Context.MkEq(this.Z, that.Z));
return(result);
}
public static Z3Point3D MkZ3Const(string prefix)
{
Z3Point3D result = new Z3Point3D();
result.X = Z3.Context.MkRealConst(prefix + " X");
result.Y = Z3.Context.MkRealConst(prefix + " Y");
result.Z = Z3.Context.MkRealConst(prefix + " Z");
return(result);
}
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 KeepAngleRestriction(JointType jointType, Direction direction, int angleThreshold = 15)
: base(
body =>
{
return(body.Joints[jointType].IsAngleBetweenLessThan(Z3Point3D.DirectionPoint(direction), angleThreshold));
},
jointType)
{
this.JointType = jointType;
this.Direction = direction;
}
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 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);
}
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false)
: base(body =>
{
JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
Z3Point3D joint1Position = body.GetJointZ3Position(jointType);
Z3Point3D joint2Position = body.GetJointZ3Position(sidedHandType);
BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold);
if (dont)
{
expr = Z3.Context.MkNot(expr);
}
return(expr);
},
body =>
{
var stopwatch = new Stopwatch();
stopwatch.Start();
var sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
var p1 = body.Positions[jointType];
var p2 = body.Positions[sidedHandType];
var distance = Math.Max(0.00000001, p1.DistanceTo(p2));
var percentage = Math.Min(1.0, distanceThreshold / distance);
if (dont)
{
percentage = Math.Min(1.0, distance / distanceThreshold);
}
//Console.WriteLine("tch " + stopwatch.ElapsedTicks);
return(percentage);
},
jointType,
JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide))
{
this.JointType = jointType;
this.HandSide = handSide;
if (dont)
{
isNegated = true;
}
else
{
isNegated = false;
}
}
public static Z3Body MkZ3Const()
{
Z3Body result = new Z3Body();
var jointTypes = EnumUtil.GetValues <JointType>();
foreach (var jointType in jointTypes)
{
result.Joints.Add(jointType, Z3Point3D.MkZ3Const(jointType.ToString()));
result.Norms.Add(jointType, Z3.Context.MkRealConst(jointType.ToString() + " Norm"));
}
return(result);
}
//public Dictionary<JointType, double> GrabDistances(Z3Body that, List<JointType> joints)
//{
// var distanceExprs = this.GrabDistancePoint3Ds(that, joints);
// var result = new Dictionary<JointType, double>();
// foreach (var jointType in joints)
// result.Add(jointType, Z3Math.GetRealValue(distanceExprs[jointType]));
// return result;
//}
//public List<JointType> GetList
public Z3Point3D GetJointZ3Position(JointType jointType)
{
Z3Point3D result = new Z3Point3D(0, 0, 0);
JointType currentJointType = jointType;
// SpineBase is the root of the coordinate system
while (currentJointType != JointType.SpineBase)
{
result = result + (this.Joints[currentJointType] * this.Norms[currentJointType]);
currentJointType = JointTypeHelper.GetFather(currentJointType);
}
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);
}
public Z3Point3D GetManhattanNormalized()
{
Z3Point3D result = new Z3Point3D();
ArithExpr higherCoord =
Z3Math.Max(
Z3Math.Max(
Z3Math.Abs(this.X),
Z3Math.Abs(this.Y)),
Z3Math.Abs(this.Z));
result.X = Z3Math.Div(this.X, higherCoord);
result.Y = Z3Math.Div(this.Y, higherCoord);
result.Z = Z3Math.Div(this.Z, higherCoord);
return(result);
}
// 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 ArithExpr CalcApproximateDistance(Z3Point3D that)
{
// Manhattan distance vector
Z3Point3D distVec = this.GrabDistancePoint3D(that);
//ArithExpr result =
// Z3Math.Add(
// Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.X, distVec.Y, distVec.Z)),
// Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Y, distVec.X, distVec.Z)),
// Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Z, distVec.Y, distVec.X)));
ArithExpr result =
Z3Math.Max(
Z3Math.Abs(distVec.X),
Z3Math.Abs(distVec.Y),
Z3Math.Abs(distVec.Z));
return(result);
}
public RotateJointTransform(int degrees, BodyPlaneType plane, RotationDirection rotationDirection = RotationDirection.Clockwise)
: base(
joint =>
{
double cosInput = TrigonometryHelper.GetCosine(degrees);
double sinInput = TrigonometryHelper.GetSine(degrees);
if (rotationDirection == RotationDirection.CounterClockwise)
{
sinInput = -sinInput;
}
var cos = Z3Math.Real(cosInput);
var sin = Z3Math.Real(sinInput);
var sinNeg = Z3Math.Real(-sinInput);
Z3Point3D result = new Z3Point3D(joint.X, joint.Y, joint.Z);
switch (plane)
{
case BodyPlaneType.Frontal:
result.Y = Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z));
result.Z = Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z));
break;
case BodyPlaneType.Sagittal:
result.X = Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y));
result.Y = Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y));
break;
case BodyPlaneType.Horizontal:
result.X = Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z));
result.Z = Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z));
break;
default:
break;
}
return(result);
})
{ }
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 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);
}
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 Z3Point3D Transform(Z3Point3D joint)
{
Z3Point3D result = TransformFunc(joint);
return(result);
}
public SetJointDirectionTransform(Z3Point3D jointToSet)
: base(joint => jointToSet)
{
}
public BoolExpr IsFartherThan(Z3Point3D that, double distanceThreshold)
{
BoolExpr result = Z3.Context.MkNot(this.IsNearerThan(that, distanceThreshold));
return(result);
}
// 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 BoolExpr IsDegreesBetweenGreaterThan(Z3Point3D that, int degreesThreshold)
{
BoolExpr result = Z3.Context.MkNot(this.IsDegreesBetweenLessThan(that, degreesThreshold));
return(result);
}
public static CompositeBodyTransform PointToTransform(JointType jointType, Direction direction)
{
Z3Point3D point3D = Z3Point3D.DirectionPoint(direction);
return(new CompositeBodyTransform(jointType, new SetJointDirectionTransform(point3D)));
}
