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