public RotateJointTransform(int degrees, BodyPlaneType plane, RotationDirection rotationDirection = RotationDirection.Clockwise) : base( joint => { double cosInput = TrigonometryHelper.GetCosine(degrees); double sinInput = TrigonometryHelper.GetSine(degrees); if (rotationDirection == RotationDirection.CounterClockwise) { sinInput = -sinInput; } 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); switch (plane) { case BodyPlaneType.Frontal: result.Y = Z3Math.Add(Z3Math.Mul(cos, joint.Y), Z3Math.Mul(sin, joint.Z)); result.Z = Z3Math.Add(Z3Math.Mul(sinNeg, joint.Y), Z3Math.Mul(cos, joint.Z)); break; case BodyPlaneType.Sagittal: result.X = Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Y)); result.Y = Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Y)); break; case BodyPlaneType.Horizontal: result.X = Z3Math.Add(Z3Math.Mul(cos, joint.X), Z3Math.Mul(sin, joint.Z)); result.Z = Z3Math.Add(Z3Math.Mul(sinNeg, joint.X), Z3Math.Mul(cos, joint.Z)); break; default: break; } return(result); }) { }
// 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); }) { }