//internal static SimpleBodyRestriction TouchRestriction(JointType jointType, JointSide handSide) //{ // double maxY = TrigonometryHelper.GetSine(5); // return new TouchBodyRestriction(jointType, handSide); //} internal static BoolExpr EvaluateNorms(Z3Body body1, Z3Body body2) { double normsThreshold = 0.1; BoolExpr result = Z3Math.True; var jointTypes = EnumUtil.GetValues <JointType>(); foreach (var jointType in jointTypes) { // Calc the distance between the two norms ArithExpr distance = Z3Math.Abs( Z3Math.Sub( body1.Norms[jointType], body2.Norms[jointType])); // Create the boolean expression to evaluate the distance result = Z3.Context.MkAnd( result, Z3.Context.MkLt( distance, Z3Math.Real(normsThreshold))); } return(result); }
//public Z3Point3D GetApproximateNormalized() //{ // Z3Point3D result = this.GetManhattanNormalized(); // result.X = CalcApproximateCoordFromManhattanToEuclidianSystem(result.X, result.Y, result.Z); // result.Y = CalcApproximateCoordFromManhattanToEuclidianSystem(result.Y, result.X, result.Z); // result.Z = CalcApproximateCoordFromManhattanToEuclidianSystem(result.Z, result.Y, result.X); // return result; //} public ArithExpr Norm() { ArithExpr result = Z3Math.Max( Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(this.X, this.Y, this.Z)), Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(this.Y, this.X, this.Z)), Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(this.Z, this.Y, this.X))); 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 Z3Point3D GetManhattanNormalized() { Z3Point3D result = new Z3Point3D(); ArithExpr higherCoord = Z3Math.Max( Z3Math.Max( Z3Math.Abs(this.X), Z3Math.Abs(this.Y)), Z3Math.Abs(this.Z)); result.X = Z3Math.Div(this.X, higherCoord); result.Y = Z3Math.Div(this.Y, higherCoord); result.Z = Z3Math.Div(this.Z, higherCoord); return(result); }
public ArithExpr CalcApproximateDistance(Z3Point3D that) { // Manhattan distance vector Z3Point3D distVec = this.GrabDistancePoint3D(that); //ArithExpr result = // Z3Math.Add( // Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.X, distVec.Y, distVec.Z)), // Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Y, distVec.X, distVec.Z)), // Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Z, distVec.Y, distVec.X))); ArithExpr result = Z3Math.Max( Z3Math.Abs(distVec.X), Z3Math.Abs(distVec.Y), Z3Math.Abs(distVec.Z)); return(result); }
ArithExpr CalcApproximateCoordFromManhattanToEuclidianSystem( ArithExpr firstCoord, ArithExpr secondCoord, ArithExpr thirdCoord) { // Work only with values length // Values sign will be assigned again in the end ArithExpr firstCoordLength = Z3Math.Abs(firstCoord); ArithExpr secondCoordLength = Z3Math.Abs(secondCoord); ArithExpr thirdCoordLength = Z3Math.Abs(thirdCoord); // The all common length will be weighted by this // This way for example a (1, 1, 1) vector will become // A (0.57, 0.57, 0.57) with norm near to 1 //ArithExpr sqrt1div3 = Z3Math.Real(0.57735026918962576450914878050196); ArithExpr sqrt1div3 = Z3Math.Real(0.577); // The remaining common length will be weighted by this // This way for example a (1, 1, 0) vector will become // A (0.7, 0.7, 0.7) with norm near to 1 //ArithExpr sin45 = Z3Math.Real(0.70710678118654752440084436210485) ArithExpr sin45 = Z3Math.Real(0.707); // Calc common length between x, y, z ArithExpr allCommonLength = Z3Math.Min( firstCoordLength, secondCoordLength, thirdCoordLength); // Calc the common length between the target coord (firstCoord) // and the higher coord between the second and third coords ArithExpr lastTwoCommonLength = Z3Math.Max( Z3Math.Min(secondCoordLength, firstCoordLength), Z3Math.Min(thirdCoordLength, firstCoordLength)); // Calc exclusevely common length with the remaining coordinate ArithExpr lastTwoExclusiveCommonLength = Z3Math.Sub( lastTwoCommonLength, allCommonLength); // Calc remaining length ArithExpr especificLength = Z3Math.Sub(firstCoordLength, Z3Math.Add(lastTwoExclusiveCommonLength, allCommonLength)); // Calc weighted lengths ArithExpr weigthedLength1 = Z3Math.Mul(lastTwoExclusiveCommonLength, sin45); ArithExpr weigthedLength2 = Z3Math.Mul(allCommonLength, sqrt1div3); // Calc weighted result length ArithExpr resultLength = Z3Math.Add( especificLength, weigthedLength1, weigthedLength2); // The transform doesn't change the sign of the coordinate // Recover it from original data Expr result = Z3.Context.MkITE( Z3.Context.MkGe(firstCoord, Z3Math.Zero), resultLength, Z3Math.Neg(resultLength)); return(result as ArithExpr); }
// the rotation towards a direction is limited to the directive vector of that direction public RotateJointTransform(int degrees, Direction direction) : base( joint => { // define current degrees var currentDegrees = 0.0; switch (direction) { case Direction.Right: case Direction.Left: currentDegrees = Math.Asin(joint.GetXValue()) * 180.0 / Math.PI; break; case Direction.Up: case Direction.Down: currentDegrees = Math.Asin(joint.GetYValue()) * 180.0 / Math.PI; break; case Direction.Front: case Direction.Back: currentDegrees = Math.Asin(joint.GetZValue()) * 180.0 / Math.PI; break; } // check if current degrees + input degrees is an overflow // which means the rotation is going towards the opposite direction if (currentDegrees + degrees > 90 || currentDegrees - degrees < -90) { // if so, set degrees as the complement to the limit value degrees = (int)(90.0 - currentDegrees); } double cosInput = TrigonometryHelper.GetCosine(degrees); double sinInput = TrigonometryHelper.GetSine(degrees); var cos = Z3Math.Real(cosInput); var sin = Z3Math.Real(sinInput); var sinNeg = Z3Math.Real(-sinInput); Z3Point3D result = new Z3Point3D(joint.X, joint.Y, joint.Z); // The performed rotation depends on current values of X, Y and Z // The rotation plane and direction changes depending on the relation between the coordinates switch (direction) { case Direction.Back: result.X = // if Abs(X) >= Abs(Y) // if X > 0 // rotate counter clockwise // else rotate clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Y) // then return Y // else rotate Y // if Y > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), joint.Y, Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Y) // if X > 0 // rotate counter clockwise // else rotate clockwise // else if Y > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr; break; case Direction.Front: result.X = // if Abs(X) >= Abs(Y) // if X > 0 // rotate clockwise // else rotate counter clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Y) // then return Y // else rotate Y // if Y > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), joint.Y, Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Y) // if X > 0 // rotate clockwise // else rotate counter clockwise // else if Y > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr; break; case Direction.Down: result.X = // if Abs(X) >= Abs(Z) // if X > 0 // rotate counter clockwise // else rotate clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Z) // if X > 0 // rotate counter clockwise // else rotate clockwise // else if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)))) as ArithExpr; break; case Direction.Up: result.X = // if Abs(X) >= Abs(Z) // if X > 0 // rotate clockwise // else rotate counter clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Z) // if X > 0 // rotate clockwise // else rotate counter clockwise // else if Z > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)))) as ArithExpr; break; case Direction.Right: result.X = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr; result.Y = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else return Y Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X))), joint.Y) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)))) as ArithExpr; break; case Direction.Left: result.X = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr; result.Y = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else return Y Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X))), joint.Y) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)))) as ArithExpr; break; default: break; } return(result); }) { }
public RotateJointTransform(int angle, Direction direction) : base( joint => { double cosInput = TrigonometryHelper.GetCosine(angle); double sinInput = TrigonometryHelper.GetSine(angle); var cos = Z3Math.Real(cosInput); var sin = Z3Math.Real(sinInput); var sinNeg = Z3Math.Real(-sinInput); Z3Point3D result = new Z3Point3D(joint.X, joint.Y, joint.Z); // The performed rotation depends on current values of X, Y and Z // The rotation plane and direction changes depending on the relation between the coordinates switch (direction) { case Direction.Back: result.X = // if Abs(X) >= Abs(Y) // if X > 0 // rotate counter clockwise // else rotate clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Y) // then return Y // else rotate Y // if Y > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), joint.Y, Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Y) // if X > 0 // rotate counter clockwise // else rotate clockwise // else if Y > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr; break; case Direction.Front: result.X = // if Abs(X) >= Abs(Y) // if X > 0 // rotate clockwise // else rotate counter clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Y)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Y) // then return Y // else rotate Y // if Y > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), joint.Y, Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.Z)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Y) // if X > 0 // rotate clockwise // else rotate counter clockwise // else if Y > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Y)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.Z)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)))) as ArithExpr; break; case Direction.Down: result.X = // if Abs(X) >= Abs(Z) // if X > 0 // rotate counter clockwise // else rotate clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Z) // if X > 0 // rotate counter clockwise // else rotate clockwise // else if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)))) as ArithExpr; break; case Direction.Up: result.X = // if Abs(X) >= Abs(Z) // if X > 0 // rotate clockwise // else rotate counter clockwise // else return X Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.X), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sinNeg, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y))), joint.X) as ArithExpr; result.Y = // if Abs(X) >= Abs(Z) // if X > 0 // rotate clockwise // else rotate counter clockwise // else if Z > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.X, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.X), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.Y)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.Y)))) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate clockwise // else rotate counter clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.X), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.Y)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.Y)))) as ArithExpr; break; case Direction.Right: result.X = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr; result.Y = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else return Y Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X))), joint.Y) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)))) as ArithExpr; break; case Direction.Left: result.X = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), Z3.Context.MkITE(Z3.Context.MkGe(joint.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sin, joint.Y), Z3Math.Mul(cos, joint.X))), Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(sinNeg, joint.Z), Z3Math.Mul(cos, joint.X)), Z3Math.Add(Z3Math.Mul(sin, joint.Z), Z3Math.Mul(cos, joint.X)))) as ArithExpr; result.Y = // if Abs(Y) >= Abs(Z) // if Y > 0 // rotate counter clockwise // else rotate clockwise // else return Y Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(result.Y), Z3Math.Abs(result.Z)), Z3.Context.MkITE(Z3.Context.MkGe(result.Y, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sinNeg, joint.X))), joint.Y) as ArithExpr; result.Z = // if Abs(X) >= Abs(Z) // then return Z // else rotate Z // if Z > 0 // rotate counter clockwise // else rotate clockwise Z3.Context.MkITE(Z3.Context.MkGe(Z3Math.Abs(joint.Y), Z3Math.Abs(joint.Z)), joint.Z, Z3.Context.MkITE(Z3.Context.MkGe(joint.Z, Z3Math.Zero), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sin, joint.X)), Z3Math.Add(Z3Math.Mul(cos, joint.Z), Z3Math.Mul(sinNeg, joint.X)))) as ArithExpr; break; default: break; } return(result); }) { }