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