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