public static Z3Point3D operator -(Z3Point3D p3d, Z3Point3D that) { return(new Z3Point3D( Z3Math.Sub(p3d.X, that.X), Z3Math.Sub(p3d.Y, that.Y), Z3Math.Sub(p3d.Z, that.Z))); }
//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 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); }
// Negates a number: returns = -expr public static ArithExpr Neg(ArithExpr expr) { ArithExpr result = Z3Math.Sub(Zero, expr); 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); }
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; } }