public static Z3Point3D operator +(Z3Point3D p3d, Z3Point3D that) { return(new Z3Point3D( Z3Math.Add(p3d.X, that.X), Z3Math.Add(p3d.Y, that.Y), Z3Math.Add(p3d.Z, that.Z))); }
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); }) { }
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; } }
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); }
public static IBodyRestriction TestBodyRestriction() { var result = new SimpleBodyRestriction(body => { double distanceThreshold = 0.4; ArithExpr distanceThresholdSquared = Z3Math.Real(distanceThreshold * distanceThreshold); ArithExpr x1 = body.Joints[JointType.SpineMid].X; ArithExpr y1 = body.Joints[JointType.SpineMid].Y; ArithExpr z1 = body.Joints[JointType.SpineMid].Z; ArithExpr x2 = Z3Math.Zero; ArithExpr y2 = Z3Math.One; ArithExpr z2 = Z3Math.Zero; //ArithExpr x2 = body.Joints[JointType.SpineBase].X; //ArithExpr y2 = body.Joints[JointType.SpineBase].Y; //ArithExpr z2 = body.Joints[JointType.SpineBase].Z; var xExpr = Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)); var yExpr = Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2)); var zExpr = Z3Math.Mul(Z3Math.Sub(z1, z2), Z3Math.Sub(z1, z2)); var distanceSquared = Z3Math.Add(xExpr, yExpr, zExpr); // This is runs fine //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), y2)); // This runs fine //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1))); // This runs fine //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y2, y2))); // This runs fine //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y1))); // This runs fine //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2))); // This runs fine //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Mul(y1, y2))); // This runs fine //var addExpr = Z3Math.Add( // Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), // Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2))); // But this does not //var addExpr = Z3Math.Add( // Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), // Z3Math.Mul(Z3Math.Add(y1, y2), Z3Math.Add(y1, y2))); // This fine //var addExpr = Z3Math.Add( // Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), // Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(0)))); // This does not //var addExpr = Z3Math.Add( // Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), // Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1)))); // This runs fine //var addExpr = Z3Math.Add( // Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), // Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1, 7)))); // This runs fine //var addExpr = Z3Math.Add( // Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), // Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 6)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 6)))); // This runs fine //var addExpr = Z3Math.Add( // Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), // Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 3)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 3)))); // This does not // asserting: (x1 - 0)² + (y1 - 1/2)² <= 0.4² // which is x1² + y1² - y/2 - y/2 + 1/4 <= 0.16 // which is x1² + y1² - y + 0.25 <= 0.16 // which is x1² + y1² - y <= -0.09 // if x1 = 0 and y1 = 0.1 we would have a solution // 0 + 0.01 - 0.1 <= 0.09 // 0.09 <= 0.09 // but z3 is taking too long to solve this. var addExpr = Z3Math.Add( Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)), Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 2)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 2)))); // But this is not (Z3 just stops while solving it) //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2))); //var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1))); //BoolExpr expr = Z3.Context.MkLt(distanceSquared, distanceThresholdSquared); BoolExpr expr = Z3.Context.MkLe(addExpr, distanceThresholdSquared); // Why it complains about this Greater-Than case? //BoolExpr expr = Z3.Context.MkGt(xExpr, distanceThresholdSquared); return(expr); }, body => { return(1.0); }); 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 PutBodyRestriction(JointType jointType1, JointType jointType2, RelativeDirection direction, bool dont = false) : base(body => { ArithExpr distanceThreshold = Z3Math.Real(0.1); Z3Point3D joint1Position = body.GetJointPosition(jointType1); Z3Point3D joint2Position = body.GetJointPosition(jointType2); BoolExpr 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); }, //JointTypeHelper.GetListFromLeafToRoot(jointType1).Union(JointTypeHelper.GetListFromLeafToRoot(jointType2)).ToList()) jointType1, jointType2) { this.JointType1 = jointType1; this.JointType2 = jointType2; this.Direction = direction; if (dont) { this.isNegated = true; } else { this.isNegated = false; } }
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); }) { }