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