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