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