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))); }