public static IBodyRestriction DontTwistHipRestriction(int degreesThreshold = 15) { Z3Point3D up = new Z3Point3D(0, 1, 0); var result = new SimpleBodyRestriction(body => { // Shoulders, hips and feet must be aligned Z3Point3D leftToRightShoulderVec = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight]; Z3Point3D leftToRightHipVec = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight]; Z3Point3D leftAnklePosition = body.Joints[JointType.HipLeft] + body.Joints[JointType.KneeLeft] + body.Joints[JointType.AnkleLeft]; Z3Point3D rightAnklePosition = body.Joints[JointType.HipRight] + body.Joints[JointType.KneeRight] + body.Joints[JointType.AnkleRight]; Z3Point3D leftToRightAnkleVec = rightAnklePosition - leftAnklePosition; BoolExpr expr1 = leftToRightShoulderVec.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold); BoolExpr expr2 = leftToRightShoulderVec.IsDegreesBetweenLessThan(leftToRightAnkleVec, degreesThreshold); BoolExpr expr3 = leftToRightAnkleVec.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3); return(expr); }, body => { return(1.0); }); return(result); }
public static Z3Point3D DirectionPoint(Direction direction) { Z3Point3D point3D = new Z3Point3D(); switch (direction) { case Direction.Up: point3D = Z3Point3D.Up(); break; case Direction.Down: point3D = Z3Point3D.Down(); break; case Direction.Left: point3D = Z3Point3D.Left(); break; case Direction.Right: point3D = Z3Point3D.Right(); break; case Direction.Front: point3D = Z3Point3D.Front(); break; case Direction.Back: point3D = Z3Point3D.Back(); break; } return(point3D); }
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 = Z3Point3D.DirectionPoint(Direction.Left); 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 = Z3Point3D.DirectionPoint(Direction.Left); Z3Point3D kneeLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D ankleLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D footLeft = Z3Point3D.DirectionPoint(Direction.Down); Z3Point3D shoulderRight = Z3Point3D.DirectionPoint(Direction.Right); 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 = Z3Point3D.DirectionPoint(Direction.Right); 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); }
public Z3Point3D GrabDistancePoint3D(Z3Point3D that) { // Manhattan distance vector Z3Point3D result = this - that; return(result); }
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false) : base(body => { JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide); Z3Point3D joint1Position = body.GetJointPosition(jointType); Z3Point3D joint2Position = body.GetJointPosition(sidedHandType); BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold); if (dont) { expr = Z3.Context.MkNot(expr); } return(expr); }, jointType, JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide)) { this.JointType = jointType; this.HandSide = handSide; if (dont) { isNegated = true; } else { isNegated = false; } }
public BoolExpr IsNearerThan(Z3Point3D that, double distanceThreshold) { ArithExpr distance = this.CalcApproximateDistance(that); BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold)); return(result); }
internal BoolExpr IsEqualTo(Z3Point3D that) { var result = Z3.Context.MkAnd( Z3.Context.MkEq(this.X, that.X), Z3.Context.MkEq(this.Y, that.Y), Z3.Context.MkEq(this.Z, that.Z)); return(result); }
public static Z3Point3D MkZ3Const(string prefix) { Z3Point3D result = new Z3Point3D(); result.X = Z3.Context.MkRealConst(prefix + " X"); result.Y = Z3.Context.MkRealConst(prefix + " Y"); result.Z = Z3.Context.MkRealConst(prefix + " Z"); return(result); }
public static void Run() { Z3Point3D constPoint = Z3Point3D.MkZ3Const("const"); // ("const X", "const Y", "const Z") Z3Point3D normalized = new Z3Point3D(); ArithExpr higherCoord = Z3Math.Max( Z3Math.Max( Z3Math.Abs(constPoint.X), Z3Math.Abs(constPoint.Y)), Z3Math.Abs(constPoint.Z)); normalized.X = Z3.Context.MkDiv(constPoint.X, constPoint.Y); normalized.Y = Z3Math.One;//Z3.Context.MkDiv(constPoint.Y, higherCoord); normalized.Z = Z3.Context.MkDiv(constPoint.Z, constPoint.Y); normalized.X = CalcApproximateCoordFromManhattanToEuclidianSystem(normalized.X, normalized.Y, normalized.Z); normalized.Y = CalcApproximateCoordFromManhattanToEuclidianSystem(normalized.Y, normalized.X, normalized.Z); normalized.Z = CalcApproximateCoordFromManhattanToEuclidianSystem(normalized.Z, normalized.Y, normalized.X); Z3Point3D up = Z3Point3D.DirectionPoint(Direction.Up); // (0, 1, 0) Z3Point3D distVec = normalized - up; ArithExpr distance = Max( Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.X, distVec.Y, distVec.Z)), Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Y, distVec.X, distVec.Z)), Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Z, distVec.Y, distVec.X))); BoolExpr expr = Z3.Context.MkLt(distance, Z3.Context.MkReal(1, 2)); Solver solver = Z3.Context.MkSolver(); solver.Assert(expr); Status status = solver.Check(); Statistics stats = solver.Statistics; switch (status) { case Status.UNKNOWN: Console.WriteLine("Solver check for witness returned Status.UNKNOWN because: " + solver.ReasonUnknown); throw new ArgumentException("Test Failed Expception"); case Status.UNSATISFIABLE: Console.WriteLine("There is no valid witness for " + expr); throw new ArgumentException("Test Failed Expception"); case Status.SATISFIABLE: Console.WriteLine("OK, model: " + solver.Model); break; } }
public KeepAngleRestriction(JointType jointType, Direction direction, int angleThreshold = 15) : base( body => { return(body.Joints[jointType].IsAngleBetweenLessThan(Z3Point3D.DirectionPoint(direction), angleThreshold)); }, jointType) { this.JointType = jointType; this.Direction = direction; }
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 override bool Equals(object obj) { var that = obj as JointTransform; Solver solver = Z3.Context.MkSolver(); solver.Push(); RealExpr x = Z3.Context.MkRealConst("x"); RealExpr y = Z3.Context.MkRealConst("y"); RealExpr z = Z3.Context.MkRealConst("z"); Z3Point3D joint = new Z3Point3D(x, y, z); Z3Point3D thisJoint = this.TransformFunc(joint); Z3Point3D thatJoint = that.TransformFunc(joint); BoolExpr xEq = Z3.Context.MkEq(thisJoint.X, thatJoint.X); BoolExpr yEq = Z3.Context.MkEq(thisJoint.Y, thatJoint.Y); BoolExpr zEq = Z3.Context.MkEq(thisJoint.Z, thatJoint.Z); BoolExpr equalsExpr = Z3.Context.MkAnd(xEq, yEq, zEq); solver.Assert(Z3.Context.MkNot(equalsExpr)); Status status = solver.Check(); Statistics stats = solver.Statistics; //Console.WriteLine("EqualsExpr: " + equalsExpr); //Console.WriteLine("Proving: " + equalsExpr); //switch (status) //{ // case Status.UNKNOWN: // Console.WriteLine("Unknown because:\n" + solver.ReasonUnknown); // break; // case Status.SATISFIABLE: // throw new ArgumentException("Test Failed Expception"); // case Status.UNSATISFIABLE: // Console.WriteLine("OK, proof:\n" + solver.Proof); // break; //} bool result = false; if (status == Status.UNSATISFIABLE) { result = true; } solver.Pop(); return(result); }
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false) : base(body => { JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide); Z3Point3D joint1Position = body.GetJointZ3Position(jointType); Z3Point3D joint2Position = body.GetJointZ3Position(sidedHandType); BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold); if (dont) { expr = Z3.Context.MkNot(expr); } return(expr); }, body => { var stopwatch = new Stopwatch(); stopwatch.Start(); var sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide); var p1 = body.Positions[jointType]; var p2 = body.Positions[sidedHandType]; var distance = Math.Max(0.00000001, p1.DistanceTo(p2)); var percentage = Math.Min(1.0, distanceThreshold / distance); if (dont) { percentage = Math.Min(1.0, distance / distanceThreshold); } //Console.WriteLine("tch " + stopwatch.ElapsedTicks); return(percentage); }, jointType, JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide)) { this.JointType = jointType; this.HandSide = handSide; if (dont) { isNegated = true; } else { isNegated = false; } }
public static Z3Body MkZ3Const() { Z3Body result = new Z3Body(); var jointTypes = EnumUtil.GetValues <JointType>(); foreach (var jointType in jointTypes) { result.Joints.Add(jointType, Z3Point3D.MkZ3Const(jointType.ToString())); result.Norms.Add(jointType, Z3.Context.MkRealConst(jointType.ToString() + " Norm")); } return(result); }
//public Dictionary<JointType, double> GrabDistances(Z3Body that, List<JointType> joints) //{ // var distanceExprs = this.GrabDistancePoint3Ds(that, joints); // var result = new Dictionary<JointType, double>(); // foreach (var jointType in joints) // result.Add(jointType, Z3Math.GetRealValue(distanceExprs[jointType])); // return result; //} //public List<JointType> GetList public Z3Point3D GetJointZ3Position(JointType jointType) { Z3Point3D result = new Z3Point3D(0, 0, 0); JointType currentJointType = jointType; // SpineBase is the root of the coordinate system while (currentJointType != JointType.SpineBase) { result = result + (this.Joints[currentJointType] * this.Norms[currentJointType]); currentJointType = JointTypeHelper.GetFather(currentJointType); } return(result); }
public static IBodyRestriction DistributeWeightRestriction(int angleThreshold = 15) { var result = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); Z3Point3D legLeft = body.Joints[JointType.KneeLeft] + body.Joints[JointType.AnkleLeft]; Z3Point3D legRight = body.Joints[JointType.KneeRight] + body.Joints[JointType.AnkleRight]; Z3Point3D legsAverage = (legLeft + legRight) / 2; BoolExpr expr = legsAverage.IsAngleBetweenLessThan(up, angleThreshold); return(expr); }); return(result); }
public Z3Point3D GetManhattanNormalized() { Z3Point3D result = new Z3Point3D(); ArithExpr higherCoord = Z3Math.Max( Z3Math.Max( Z3Math.Abs(this.X), Z3Math.Abs(this.Y)), Z3Math.Abs(this.Z)); result.X = Z3Math.Div(this.X, higherCoord); result.Y = Z3Math.Div(this.Y, higherCoord); result.Z = Z3Math.Div(this.Z, higherCoord); return(result); }
// Therapy restrictions public static IBodyRestriction StraightPostureRestriction(int angleThreshold = 15) { Z3Point3D up = new Z3Point3D(0, 1, 0); double distanceThreshold = TrigonometryHelper.GetSine(angleThreshold); var result = new SimpleBodyRestriction(body => { BoolExpr expr = body.Joints[JointType.SpineMid].IsNearerThan(up, distanceThreshold); expr = Z3.Context.MkAnd(expr, body.Joints[JointType.SpineShoulder].IsNearerThan(up, distanceThreshold)); expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Neck].IsNearerThan(up, distanceThreshold)); expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Head].IsNearerThan(up, distanceThreshold)); return(expr); }); return(result); }
public ArithExpr CalcApproximateDistance(Z3Point3D that) { // Manhattan distance vector Z3Point3D distVec = this.GrabDistancePoint3D(that); //ArithExpr result = // Z3Math.Add( // Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.X, distVec.Y, distVec.Z)), // Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Y, distVec.X, distVec.Z)), // Z3Math.Abs(CalcApproximateCoordFromManhattanToEuclidianSystem(distVec.Z, distVec.Y, distVec.X))); ArithExpr result = Z3Math.Max( Z3Math.Abs(distVec.X), Z3Math.Abs(distVec.Y), Z3Math.Abs(distVec.Z)); return(result); }
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 CreateBodyWitness( Z3Body z3ConstCheckedBody, Model model, List <JointType> evaluatedJoints, Z3Body defaultBody) { var witness = new Z3Body(); var jointTypes = EnumUtil.GetValues <JointType>(); foreach (var jointType in jointTypes) { if (evaluatedJoints.Contains(jointType)) { var joint = new Z3Point3D( model.Evaluate(z3ConstCheckedBody.Joints[jointType].X, true) as ArithExpr, model.Evaluate(z3ConstCheckedBody.Joints[jointType].Y, true) as ArithExpr, model.Evaluate(z3ConstCheckedBody.Joints[jointType].Z, true) as ArithExpr); witness.Joints.Add(jointType, joint); var norm = model.Evaluate(z3ConstCheckedBody.Norms[jointType]) as ArithExpr; // Check if norm is still an app (meaning it can be anything), then set it to be the default norm if (norm.ASTKind == Z3_ast_kind.Z3_APP_AST) { witness.Norms.Add(jointType, defaultBody.Norms[jointType]); } else { witness.Norms.Add(jointType, norm); } } else { witness.Joints.Add(jointType, defaultBody.Joints[jointType]); witness.Norms.Add(jointType, defaultBody.Norms[jointType]); } } return(witness); }
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); }
private static void CalcZ3CurrentAndTargetValue( Z3Point3D currentPoint, Z3Point3D startPoint, int degrees, Direction direction, out ArithExpr currentValue, out double targetValue, out double directionSign) { // once it is rotating towards a direction there is a limit for the rotation // in this case the limit is imposed to a single component (X, Y or Z) relative to the direction var limitValue = Math.Sin(75 * Math.PI / 180.0); var radians = degrees * Math.PI / 180.0; // assigning a double for angle in radians // determining if the direction sign is negative directionSign = 1.0; switch (direction) { case Direction.Down: case Direction.Back: case Direction.Left: directionSign = -1.0; break; } // update limit based on the direction sign limitValue *= directionSign; // start value stores the component (X, Y or Z) from the startPoint // determining the current and start value currentValue = currentPoint.X; var startValue = 0.0; switch (direction) { case Direction.Right: case Direction.Left: startValue = startPoint.GetXValue(); currentValue = currentPoint.X; break; case Direction.Up: case Direction.Down: startValue = startPoint.GetYValue(); currentValue = currentPoint.Y; break; case Direction.Front: case Direction.Back: startValue = startPoint.GetZValue(); currentValue = currentPoint.Z; break; } double startRadians = Math.Asin(startValue); double targetRadians = startRadians + (directionSign * radians); targetValue = Math.Sin(targetRadians); // this first case tells that the rotation is bigger than the desired and // is moving the vector (targetValue) on the opposite direction // this rotation is not desired here because we are rotating towards a direction // the rotation is not a pure transform var targetIsLowerThanStart = directionSign * targetValue < directionSign * startValue; // this second case tells that the rotation exceeded the limitValue var targetExceedsLimit = Math.Abs(targetValue) > Math.Abs(limitValue); // on both cases the targetValue should be the limitValue if (targetIsLowerThanStart || targetExceedsLimit) { targetValue = limitValue; } }
public Z3Point3D Transform(Z3Point3D joint) { Z3Point3D result = TransformFunc(joint); return(result); }
public SetJointDirectionTransform(Z3Point3D jointToSet) : base(joint => jointToSet) { }
public BoolExpr IsFartherThan(Z3Point3D that, double distanceThreshold) { BoolExpr result = Z3.Context.MkNot(this.IsNearerThan(that, distanceThreshold)); 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 BoolExpr IsDegreesBetweenGreaterThan(Z3Point3D that, int degreesThreshold) { BoolExpr result = Z3.Context.MkNot(this.IsDegreesBetweenLessThan(that, degreesThreshold)); return(result); }
public static CompositeBodyTransform PointToTransform(JointType jointType, Direction direction) { Z3Point3D point3D = Z3Point3D.DirectionPoint(direction); return(new CompositeBodyTransform(jointType, new SetJointDirectionTransform(point3D))); }