public BoolExpr IsNearerThan(Z3Point3D that, double distanceThreshold) { ArithExpr distance = this.CalcApproximateDistance(that); BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold)); return(result); }
public static Z3Point3D operator /(Z3Point3D p3d, double value) { return(new Z3Point3D( Z3Math.Div(p3d.X, Z3Math.Real(value)), Z3Math.Div(p3d.Y, Z3Math.Real(value)), Z3Math.Div(p3d.Z, Z3Math.Real(value)))); }
public Z3Body(Z3Body baseBody) : this() { foreach (var jointType in baseBody.Joints.Keys) { this.Positions.Add(jointType, new Point3D( baseBody.Positions[jointType].X, baseBody.Positions[jointType].Y, baseBody.Positions[jointType].Z)); this.Vectors.Add(jointType, new Point3D( baseBody.Vectors[jointType].X, baseBody.Vectors[jointType].Y, baseBody.Vectors[jointType].Z)); this.Joints.Add(jointType, new Z3Point3D( Z3Math.GetRealValue(baseBody.Joints[jointType].X), Z3Math.GetRealValue(baseBody.Joints[jointType].Y), Z3Math.GetRealValue(baseBody.Joints[jointType].Z) )); this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType]))); } }
//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); }
private static Dictionary <JointType, ArithExpr> CreateSyntheticNorms() { var result = new Dictionary <JointType, ArithExpr>(); ArithExpr spineBase = Z3Math.Real(0.0); ArithExpr spineMid = Z3Math.Real(0.3); ArithExpr spineShoulder = Z3Math.Real(0.3); ArithExpr neck = Z3Math.Real(0.15); ArithExpr head = Z3Math.Real(0.15); ArithExpr shoulderLeft = Z3Math.Real(0.25); ArithExpr elbowLeft = Z3Math.Real(0.25); ArithExpr wristLeft = Z3Math.Real(0.25); ArithExpr handLeft = Z3Math.Real(0.05); ArithExpr handTipLeft = Z3Math.Real(0.05); ArithExpr thumbLeft = Z3Math.Real(0.05); ArithExpr hipLeft = Z3Math.Real(0.25); ArithExpr kneeLeft = Z3Math.Real(0.35); ArithExpr ankleLeft = Z3Math.Real(0.35); ArithExpr footLeft = Z3Math.Real(0.1); ArithExpr shoulderRight = Z3Math.Real(0.25); ArithExpr elbowRight = Z3Math.Real(0.25); ArithExpr wristRight = Z3Math.Real(0.25); ArithExpr handRight = Z3Math.Real(0.05); ArithExpr handTipRight = Z3Math.Real(0.05); ArithExpr thumbRight = Z3Math.Real(0.05); ArithExpr hipRight = Z3Math.Real(0.25); ArithExpr kneeRight = Z3Math.Real(0.35); ArithExpr ankleRight = Z3Math.Real(0.35); ArithExpr footRight = Z3Math.Real(0.1); result.Add(JointType.SpineBase, spineBase); result.Add(JointType.SpineMid, spineMid); result.Add(JointType.SpineShoulder, spineShoulder); result.Add(JointType.Neck, neck); result.Add(JointType.Head, head); result.Add(JointType.ShoulderLeft, shoulderLeft); result.Add(JointType.ElbowLeft, elbowLeft); result.Add(JointType.WristLeft, wristLeft); result.Add(JointType.HandLeft, handLeft); result.Add(JointType.HandTipLeft, handTipLeft); result.Add(JointType.ThumbLeft, thumbLeft); result.Add(JointType.HipLeft, hipLeft); result.Add(JointType.KneeLeft, kneeLeft); result.Add(JointType.AnkleLeft, ankleLeft); result.Add(JointType.FootLeft, footLeft); result.Add(JointType.ShoulderRight, shoulderRight); result.Add(JointType.ElbowRight, elbowRight); result.Add(JointType.WristRight, wristRight); result.Add(JointType.HandRight, handRight); result.Add(JointType.HandTipRight, handTipRight); result.Add(JointType.ThumbRight, thumbRight); result.Add(JointType.HipRight, hipRight); result.Add(JointType.KneeRight, kneeRight); result.Add(JointType.AnkleRight, ankleRight); result.Add(JointType.FootRight, footRight); return(result); }
public LowerThanBodyRestriction(JointType jointType, int angleThreshold) : base( body => { double maxY = TrigonometryHelper.GetSine(angleThreshold); return(Z3.Context.MkLt(body.Joints[jointType].Y, Z3Math.Real(maxY))); }, jointType) { this.JointType = jointType; }
public RotateDirectionRestriction(JointType jointType, Z3Point3D startPoint, int degrees, Direction direction) : base(body => { ArithExpr currentValue; var targetValue = 0.0; var directionSign = 1.0; var currentPoint = body.Joints[jointType]; CalcZ3CurrentAndTargetValue(currentPoint, startPoint, degrees, direction, out currentValue, out targetValue, out directionSign); BoolExpr expr = Z3.Context.MkTrue(); switch (direction) { case Direction.Right: case Direction.Up: case Direction.Front: expr = Z3.Context.MkGt(currentValue, Z3Math.Real(targetValue)); break; case Direction.Left: case Direction.Down: case Direction.Back: expr = Z3.Context.MkLt(currentValue, Z3Math.Real(targetValue)); break; } return(expr); }, body => { double currentValue; var targetValue = 0.0; var directionSign = 1.0; var currentPoint = body.Vectors[jointType]; var startPointConverted = new Point3D( startPoint.GetXValue(), startPoint.GetYValue(), startPoint.GetZValue()); CalcCurrentAndTargetValue(currentPoint, startPointConverted, degrees, direction, out currentValue, out targetValue, out directionSign); var percentage = PercentageCalculator.calc( -1 * directionSign, targetValue, currentValue); return(percentage); }, jointType) { this.JointType = jointType; this.Direction = direction; this.Degrees = degrees; }
public Z3Body(Z3Body baseBody) { foreach (var jointType in baseBody.Joints.Keys) { this.Joints.Add(jointType, new Z3Point3D( Z3Math.GetRealValue(baseBody.Joints[jointType].X), Z3Math.GetRealValue(baseBody.Joints[jointType].Y), Z3Math.GetRealValue(baseBody.Joints[jointType].Z) )); this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType]))); } }
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 static Z3Body MkZ3Const() { var result = new Z3Body(); var jointTypes = EnumUtil.GetValues <JointType>(); foreach (var jointType in jointTypes) { result.Joints.Add(jointType, Z3Point3D.MkZ3Const(EnumUtil.GetDescription <JointType>(jointType))); } result.Norms.Add(JointType.SpineMid, Z3Math.Real(0.3)); result.Norms.Add(JointType.SpineShoulder, Z3Math.Real(0.3)); result.Norms.Add(JointType.ShoulderLeft, Z3Math.Real(0.2)); result.Norms.Add(JointType.ElbowLeft, Z3Math.Real(0.2)); result.Norms.Add(JointType.WristLeft, Z3Math.Real(0.2)); result.Norms.Add(JointType.HandLeft, Z3Math.Real(0.1)); result.Norms.Add(JointType.HandTipLeft, Z3Math.Real(0.1)); result.Norms.Add(JointType.ThumbLeft, Z3Math.Real(0.1)); result.Norms.Add(JointType.Neck, Z3Math.Real(0.1)); result.Norms.Add(JointType.Head, Z3Math.Real(0.2)); result.Norms.Add(JointType.ShoulderRight, Z3Math.Real(0.2)); result.Norms.Add(JointType.ElbowRight, Z3Math.Real(0.2)); result.Norms.Add(JointType.WristRight, Z3Math.Real(0.2)); result.Norms.Add(JointType.HandRight, Z3Math.Real(0.1)); result.Norms.Add(JointType.HandTipRight, Z3Math.Real(0.1)); result.Norms.Add(JointType.ThumbRight, Z3Math.Real(0.1)); result.Norms.Add(JointType.SpineBase, Z3Math.Real(0.0)); result.Norms.Add(JointType.HipLeft, Z3Math.Real(0.2)); result.Norms.Add(JointType.KneeLeft, Z3Math.Real(0.3)); result.Norms.Add(JointType.AnkleLeft, Z3Math.Real(0.3)); result.Norms.Add(JointType.FootLeft, Z3Math.Real(0.1)); result.Norms.Add(JointType.HipRight, Z3Math.Real(0.2)); result.Norms.Add(JointType.KneeRight, Z3Math.Real(0.3)); result.Norms.Add(JointType.AnkleRight, Z3Math.Real(0.3)); result.Norms.Add(JointType.FootRight, Z3Math.Real(0.1)); return(result); }
//public ArithExpr CalcApproximateDistanceBetweenNormalized(Z3Point3D that) //{ // Z3Point3D thisNormalized = this.GetApproximateNormalized(); // Z3Point3D thatNormalized = that.GetApproximateNormalized(); // ArithExpr result = thisNormalized.CalcApproximateDistance(thatNormalized); // return result; //} // Assumes vectors are normalized public BoolExpr IsDegreesBetweenLessThan(Z3Point3D that, int degreesThreshold) { double distanceThreshold = TrigonometryHelper.GetDistance(degreesThreshold); ArithExpr distance = this.CalcApproximateDistance(that); BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold)); //// TODO remove this, test code //SolverCheckResult checkResult = Z3AnalysisInterface.CheckStatus(result); //if (checkResult.Status == Status.SATISFIABLE) //{ // var joint = new Z3Point3D( // checkResult.Model.Evaluate(this.X, true) as ArithExpr, // checkResult.Model.Evaluate(this.Y, true) as ArithExpr, // checkResult.Model.Evaluate(this.Z, true) as ArithExpr); // var distanceSolvedExpr = checkResult.Model.Evaluate(distance, true) as ArithExpr; // var distanceValue = Z3Math.GetRealValue(distanceSolvedExpr); //} //// end of test code return(result); }
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 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); }) { }
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; } }
/// <summary> /// This is a basic safety check to make sure we don't break any bones. /// </summary> /// <returns>True if safe</returns> public static IBodyRestriction DefaultSafetyRestriction() { var result = new CompositeBodyRestriction(); int inclinationThreshold = 45; // Head // Make sure neck is not inclinated beyond the threshold var head = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.Head].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.Neck].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return(expr); }); result.And(head); // Spine // Make sure spine is not inclinated beyond the threshold var spine = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr3 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan( body.Joints[JointType.SpineShoulder], inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3); return(expr); }); result.And(spine); // Shoulders // Make sure shoulders are not bent var shoulders = new SimpleBodyRestriction(body => { BoolExpr expr = body.Joints[JointType.SpineMid].IsAngleBetweenGreaterThan( body.Joints[JointType.SpineShoulder], 120); return(expr); }); result.And(shoulders); // Elbows // Make sure elbows are not behind the back // And also that they are not on the top/back sub-space var elbows = new SimpleBodyRestriction(body => { BoolExpr exprRight1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkLt(body.Joints[JointType.ElbowRight].X, Z3Math.Zero))); BoolExpr exprRight2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowRight].Y, Z3Math.Zero))); BoolExpr exprLeft1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].X, Z3Math.Zero))); BoolExpr exprLeft2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].Y, Z3Math.Zero))); BoolExpr expr = Z3.Context.MkAnd(exprLeft1, exprLeft2, exprRight1, exprRight2); return(expr); }); result.And(elbows); // Wrists // Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows // unless elbows are up or wrists are directed to torso // TODO // Hips // Make sure hips are aligned with the shoulders or at lest within the range var hips = new SimpleBodyRestriction(body => { Z3Point3D shouldersSum = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight]; Z3Point3D hipsSum = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight]; BoolExpr expr = shouldersSum.IsAngleBetweenLessThan(hipsSum, 45); return(expr); }); result.And(hips); // Legs // Make sure legs are not higher than threshold var legs = new SimpleBodyRestriction(body => { BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.KneeLeft].Y, Z3Math.Real(0)); BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.KneeRight].Y, Z3Math.Real(0)); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return(expr); }); result.And(legs); // Ankles // Make sure ankles are not inclinated up more than knees // unless ankles are pointing back // TODO return(result); }
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 Z3Point3D(double x, double y, double z, Context localZ3Context) { this.X = Z3Math.Real(x, localZ3Context); this.Y = Z3Math.Real(y, localZ3Context); this.Z = Z3Math.Real(z, localZ3Context); }
public Z3Point3D(double x, double y, double z) { this.X = Z3Math.Real(x); this.Y = Z3Math.Real(y); this.Z = Z3Math.Real(z); }
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; } }
public static IBodyRestriction ShouldersRelaxedRestriction(int degreesThreshold = -5) { double maxY = TrigonometryHelper.GetSine(degreesThreshold); var result = new SimpleBodyRestriction (body => { // Check if both shoulders have a lower Y than maxY BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.ShoulderLeft].Y, Z3Math.Real(maxY)); BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.ShoulderRight].Y, Z3Math.Real(maxY)); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return(expr); }, body => { return(1.0); }); return(result); }
public SetJointDirectionTransform(double x, double y, double z) : base(joint => new Z3Point3D(Z3Math.Real(x), Z3Math.Real(y), Z3Math.Real(z))) { }
// 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); }) { }
/// <summary> /// This function converts Kinect joints to a Z3 body. /// It also changes the basis of body coordinates to make it /// invariant to body position in relation to the sensor. /// /// The origin of the new coordinate system is user hips. /// </summary> /// <param name="kinectJoints"></param> /// <returns></returns> public static Z3Body CreateZ3Body( IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> kinectJoints) { var jointVectors = new Dictionary <Microsoft.Kinect.JointType, Vector3D>(); jointVectors.Add(Microsoft.Kinect.JointType.SpineMid, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineMid], kinectJoints[Microsoft.Kinect.JointType.SpineBase])); jointVectors.Add(Microsoft.Kinect.JointType.SpineShoulder, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineShoulder], kinectJoints[Microsoft.Kinect.JointType.SpineMid])); jointVectors.Add(Microsoft.Kinect.JointType.ShoulderLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderLeft], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder])); jointVectors.Add(Microsoft.Kinect.JointType.ElbowLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowLeft], kinectJoints[Microsoft.Kinect.JointType.ShoulderLeft])); jointVectors.Add(Microsoft.Kinect.JointType.WristLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristLeft], kinectJoints[Microsoft.Kinect.JointType.ElbowLeft])); jointVectors.Add(Microsoft.Kinect.JointType.HandLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandLeft], kinectJoints[Microsoft.Kinect.JointType.WristLeft])); jointVectors.Add(Microsoft.Kinect.JointType.HandTipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipLeft], kinectJoints[Microsoft.Kinect.JointType.HandLeft])); jointVectors.Add(Microsoft.Kinect.JointType.ThumbLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbLeft], kinectJoints[Microsoft.Kinect.JointType.HandLeft])); jointVectors.Add(Microsoft.Kinect.JointType.Neck, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Neck], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder])); jointVectors.Add(Microsoft.Kinect.JointType.Head, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Head], kinectJoints[Microsoft.Kinect.JointType.Neck])); jointVectors.Add(Microsoft.Kinect.JointType.ShoulderRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderRight], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder])); jointVectors.Add(Microsoft.Kinect.JointType.ElbowRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowRight], kinectJoints[Microsoft.Kinect.JointType.ShoulderRight])); jointVectors.Add(Microsoft.Kinect.JointType.WristRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristRight], kinectJoints[Microsoft.Kinect.JointType.ElbowRight])); jointVectors.Add(Microsoft.Kinect.JointType.HandRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandRight], kinectJoints[Microsoft.Kinect.JointType.WristRight])); jointVectors.Add(Microsoft.Kinect.JointType.HandTipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipRight], kinectJoints[Microsoft.Kinect.JointType.HandRight])); jointVectors.Add(Microsoft.Kinect.JointType.ThumbRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbRight], kinectJoints[Microsoft.Kinect.JointType.HandRight])); // Spine base is the root of the system, as it has no direction to store, it stores its own position jointVectors.Add(Microsoft.Kinect.JointType.SpineBase, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineBase], new Joint())); jointVectors.Add(Microsoft.Kinect.JointType.HipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase])); jointVectors.Add(Microsoft.Kinect.JointType.KneeLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeLeft], kinectJoints[Microsoft.Kinect.JointType.HipLeft])); jointVectors.Add(Microsoft.Kinect.JointType.AnkleLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleLeft], kinectJoints[Microsoft.Kinect.JointType.KneeLeft])); jointVectors.Add(Microsoft.Kinect.JointType.FootLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootLeft], kinectJoints[Microsoft.Kinect.JointType.AnkleLeft])); jointVectors.Add(Microsoft.Kinect.JointType.HipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase])); jointVectors.Add(Microsoft.Kinect.JointType.KneeRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeRight], kinectJoints[Microsoft.Kinect.JointType.HipRight])); jointVectors.Add(Microsoft.Kinect.JointType.AnkleRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleRight], kinectJoints[Microsoft.Kinect.JointType.KneeRight])); jointVectors.Add(Microsoft.Kinect.JointType.FootRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootRight], kinectJoints[Microsoft.Kinect.JointType.AnkleRight])); var rotationMatrix = new Matrix3D(); InitMatrix(out rotationMatrix, kinectJoints); rotationMatrix.Invert(); var joints = new Dictionary <PreposeGestures.JointType, Z3Point3D>(); var norms = new Dictionary <PreposeGestures.JointType, ArithExpr>(); norms.Add(PreposeGestures.JointType.SpineBase, Z3Math.Real(jointVectors[Microsoft.Kinect.JointType.SpineBase].Length)); joints.Add(PreposeGestures.JointType.SpineBase, new Z3Point3D( jointVectors[Microsoft.Kinect.JointType.SpineBase].X, jointVectors[Microsoft.Kinect.JointType.SpineBase].Y, jointVectors[Microsoft.Kinect.JointType.SpineBase].Z)); var jointTypes = EnumUtil.GetValues <Microsoft.Kinect.JointType>(); foreach (var jointType in jointTypes) { if (jointType != Microsoft.Kinect.JointType.SpineBase) { jointVectors[jointType] = jointVectors[jointType] * rotationMatrix; var z3Joint = Convert(jointType); norms.Add(z3Joint, Z3Math.Real(jointVectors[jointType].Length)); var temp = jointVectors[jointType]; temp.Normalize(); joints.Add( z3Joint, new Z3Point3D( temp.X, temp.Y, -temp.Z)); } } return(new Z3Body(joints, norms)); }
public static Dictionary <JointType, Z3Point3D> CreateSyntheticJoints() { var result = new Dictionary <JointType, Z3Point3D>(); Z3Point3D spineBase = Z3Point3D.DirectionPoint(Direction.Front); Z3Point3D spineMid = Z3Point3D.DirectionPoint(Direction.Up); Z3Point3D spineShoulder = Z3Point3D.DirectionPoint(Direction.Up); Z3Point3D neck = Z3Point3D.DirectionPoint(Direction.Up); Z3Point3D head = Z3Point3D.DirectionPoint(Direction.Up); Z3Point3D shoulderLeft = new Z3Point3D(); shoulderLeft.X = Z3Math.Real(-0.7); shoulderLeft.Y = Z3Math.Real(-0.7); shoulderLeft.Z = Z3Math.Zero; Z3Point3D elbowLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D wristLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D handLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D handTipLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D thumbLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D hipLeft = new Z3Point3D(); shoulderLeft.X = Z3Math.Real(-0.7); shoulderLeft.Y = Z3Math.Real(-0.7); shoulderLeft.Z = Z3Math.Zero; Z3Point3D kneeLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D ankleLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D footLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D shoulderRight = new Z3Point3D(); shoulderRight.X = Z3Math.Real(0.7); shoulderRight.Y = Z3Math.Real(-0.7); shoulderRight.Z = Z3Math.Zero; Z3Point3D elbowRight = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D wristRight = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D handRight = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D handTipRight = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D thumbRight = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D hipRight = new Z3Point3D(); shoulderRight.X = Z3Math.Real(0.7); shoulderRight.Y = Z3Math.Real(-0.7); shoulderRight.Z = Z3Math.Zero; Z3Point3D kneeRight = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D ankleRight = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D footRight = Z3Point3D.DirectionPoint(Direction.Down); result.Add(JointType.SpineBase, spineBase); result.Add(JointType.SpineMid, spineMid); result.Add(JointType.SpineShoulder, spineShoulder); result.Add(JointType.Neck, neck); result.Add(JointType.Head, head); result.Add(JointType.ShoulderLeft, shoulderLeft); result.Add(JointType.ElbowLeft, elbowLeft); result.Add(JointType.WristLeft, wristLeft); result.Add(JointType.HandLeft, handLeft); result.Add(JointType.HandTipLeft, handTipLeft); result.Add(JointType.ThumbLeft, thumbLeft); result.Add(JointType.HipLeft, hipLeft); result.Add(JointType.KneeLeft, kneeLeft); result.Add(JointType.AnkleLeft, ankleLeft); result.Add(JointType.FootLeft, footLeft); result.Add(JointType.ShoulderRight, shoulderRight); result.Add(JointType.ElbowRight, elbowRight); result.Add(JointType.WristRight, wristRight); result.Add(JointType.HandRight, handRight); result.Add(JointType.HandTipRight, handTipRight); result.Add(JointType.ThumbRight, thumbRight); result.Add(JointType.HipRight, hipRight); result.Add(JointType.KneeRight, kneeRight); result.Add(JointType.AnkleRight, ankleRight); result.Add(JointType.FootRight, footRight); return(result); }