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 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 static CompositeBodyTransform PointToTransform(JointType jointType, Direction direction)
{
Z3Point3D point3D = Z3Point3D.DirectionPoint(direction);
return(new CompositeBodyTransform(jointType, new SetJointDirectionTransform(point3D)));
}
