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