コード例 #1
0
ファイル: DelayedStatements.cs プロジェクト: lsfcin/prepose
        internal void Update(Z3Body body)
        {
            this.Restriction = new CompositeBodyRestriction();
            this.Transform = new CompositeBodyTransform();

            // treating rotate direction
            // for each definition
                // if a joint appears once
                    // restrict
                // else
                    // transform

            // the rotate direction can be a transform or a restriction
            foreach (var definition in this.Statements)
            {
                var jointCount = 0;
                foreach (var definitionTemp in this.Statements)
                {
                    if (definition.JointType == definitionTemp.JointType)
                        jointCount++;
                }

                // if for a single joint there is only one rotate direction
                // then the restriction should be applied to give more
                // freedom for the user while performing the gesture
                if(jointCount == 1)
                {
                    this.Restriction.And(new RotateDirectionRestriction(
                            definition.JointType, body.Joints[definition.JointType],
                            definition.Degrees, definition.Direction));
                }

                // else if for that joint there is more than one rotate direction
                // then we create a transform to guarantee that all directions
                // are well represented
                else if(jointCount > 1)
                {
                    this.Transform = this.Transform.Compose(
                        new CompositeBodyTransform(definition.JointType,
                            new RotateJointTransform(definition.Degrees, definition.Direction)));
                }
            }
        }
コード例 #2
0
ファイル: BodyMatching.cs プロジェクト: mhusinsky/prepose
        /// <summary>
        /// Matches a Kinect body to a set of gestures within the app.
        /// </summary>
        /// <param name="kinectJoints">Body representation</param>
        /// <param name="jumpToNextPose">synthesizes a new body in correct position if true</param>
        /// <returns></returns>
        public static List<GestureStatus> TestBody(BodyMatcher matcher, 
            IReadOnlyDictionary<Microsoft.Kinect.JointType,
            Microsoft.Kinect.Joint> kinectJoints,
            bool jumpToNextPose = false)
        {
            // convert Kinect.Body to Z3Body
            var body = new Z3Body();
            if (!jumpToNextPose)
            {
                body = Z3KinectConverter.CreateZ3Body(kinectJoints);
            }
            else
            {
                //body = GetCopiedBodyValues(this.Gestures[this.GetMostAdvancedGesturesIDs()[0]].GetTarget().Body);
                var firstGestureBody = matcher.GetLastGestureTarget().Body;
                body = new Z3Body(firstGestureBody);
            }

            return matcher.TestBody(body);
        }
コード例 #3
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
        public List<GestureStatus> TestBody(Z3Body body)
        {
            var result = new List<GestureStatus>();

            // InitBody synthesizes the initial pose to go after. Synthesizing a pose is _not_ parallel-safe because the Z3 context isn't safe to share across threads
            foreach (var matcher in this.Matchers)
            {
                matcher.InitBody(body);
            }

            // Data parallel -- each matcher is independent of each other, so we can use Task Parallel Library to send each matcher to a different core if necessary
            // The task parallel library should take care of locking the result array for us...let's hope it doesn't serialize by locking result in the wrong way

            /*
            Parallel.ForEach(this.Matchers,
                matcher => {
                    Context Z3ThreadLocalContext = new Context(new Dictionary<string, string>() { { "MODEL", "true" }, { "PROOF_MODE", "2" } });
                    matcher.TestBody(body, this.Precision, Z3ThreadLocalContext);
                    result.Add(matcher.GetStatus());
                });
            */

            // Matching itself uses the Z3 context - and that can't be shared across threads!

            foreach (var matcher in this.Matchers)
            {
                matcher.TestBody(body, this.Precision, Z3.Context);
                result.Add(matcher.GetStatus());
            }

            // Check for each matcher to see if it succeeded. If it did, then synthesize a new target position.
            foreach (var matcher in this.Matchers)
            {
                if (matcher.GetStatus().succeededDetection)
                {
                    matcher.UpdateTargetBody(body);
                }
            }

            return result;
        }
コード例 #4
0
        /// <summary>
        /// Matches a Kinect body to a set of gestures within the app.
        /// </summary>
        /// <param name="kinectJoints">Body representation</param>
        /// <param name="jumpToNextPose">synthesizes a new body in correct position if true</param>
        /// <returns></returns>
        public static List <GestureStatus> TestBody(BodyMatcher matcher,
                                                    IReadOnlyDictionary <Microsoft.Kinect.JointType,
                                                                         Microsoft.Kinect.Joint> kinectJoints,
                                                    bool jumpToNextPose = false)
        {
            // convert Kinect.Body to Z3Body
            var body = new Z3Body();

            if (!jumpToNextPose)
            {
                body = Z3KinectConverter.CreateZ3Body(kinectJoints);
            }
            else
            {
                //body = GetCopiedBodyValues(this.Gestures[this.GetMostAdvancedGesturesIDs()[0]].GetTarget().Body);
                var firstGestureBody = matcher.GetLastGestureTarget().Body;
                body = new Z3Body(firstGestureBody);
            }

            return(matcher.TestBody(body));
        }
コード例 #5
0
        private static void AddZ3JointToVectors3D(
            Z3Body targetBody,
            IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints,
            Dictionary <Microsoft.Kinect.JointType, Vector3D> jointVectors,
            PreposeGestures.JointType jointType)
        {
            var vector3D = new Vector3D(
                targetBody.Joints[jointType].GetXValue(),
                targetBody.Joints[jointType].GetYValue(),
                -targetBody.Joints[jointType].GetZValue());

            //var norm = Z3Math.GetRealValue(targetBody.Norms[jointType]);
            var norm =
                new Vector3D(
                    baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.X,
                    baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Y,
                    baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Z).Length;

            vector3D = vector3D * norm;

            jointVectors.Add((Microsoft.Kinect.JointType)jointType, vector3D);
        }
コード例 #6
0
ファイル: Pose.cs プロジェクト: lsfcin/prepose
        public bool IsBodyAccepted(
			Z3Body input)
        {
            bool result = this.Restriction.IsBodyAccepted(input);
            return result;
        }
コード例 #7
0
ファイル: Z3KinectConverter.cs プロジェクト: caomw/prepose
        private static void AddZ3JointToVectors3D(
			Z3Body targetBody, 
            IReadOnlyDictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints,
			Dictionary<Microsoft.Kinect.JointType, Vector3D> jointVectors,
			PreposeGestures.JointType jointType)
        {
            var vector3D = new Vector3D(
                             targetBody.Joints[jointType].GetXValue(),
                             targetBody.Joints[jointType].GetYValue(),
                             -targetBody.Joints[jointType].GetZValue());

            //var norm = Z3Math.GetRealValue(targetBody.Norms[jointType]);
            var norm =
                new Vector3D(
                baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.X,
                baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Y,
                baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Z).Length;

            vector3D = vector3D * norm;

            jointVectors.Add((Microsoft.Kinect.JointType)jointType, vector3D);
        }
コード例 #8
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
        private Dictionary<JointType, Point3D> UpdateDistanceVectors(Z3Body body, Context localContext)
        {
            var distancesZ3Point3Ds = body.GrabDistancePoint3Ds(this.Target.Body, this.Target.GetAllJointTypes());
            var distancesVector3Ds = new Dictionary<JointType, Point3D>();

            foreach (var pointZ3 in distancesZ3Point3Ds)
            {
                distancesVector3Ds.Add(
                    pointZ3.Key,
                    new Point3D(
                        Z3Math.GetRealValue(pointZ3.Value.X),
                        Z3Math.GetRealValue(pointZ3.Value.Y),
                        Z3Math.GetRealValue(pointZ3.Value.Z)));
            }

            return distancesVector3Ds;
        }
コード例 #9
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
        internal void UpdateTargetBody(Z3Body start, Context localZ3Context)
        {
            var stopwatch = new Stopwatch();
            StatisticsEntrySynthesize synTime = new StatisticsEntrySynthesize();
            stopwatch.Start();
            var time0 = stopwatch.ElapsedMilliseconds;

            if (this.Target != null && this.CurrentStep > 0)
            {
                // If last target is not null than use the target transformed joints instead of the start transformed joints
                // This way no error is cumulated along the transforms due to the matching precisions
                foreach (var jointType in this.Gesture.Steps[this.CurrentStep - 1].Pose.GetTransformJointTypes())
                {
                    // The Z3Point3D depends on a specific Z3 context deep underneath
                    // We need to thread the context through
                    start.Joints[jointType] =
                        new Z3Point3D(
                            Z3Math.GetRealValue(this.Target.Body.Joints[jointType].X),
                            Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Y),
                            Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Z),
                            localZ3Context);
                }
            }

            this.Target = this.Gesture.Steps[this.CurrentStep].Pose.CalcNearestTargetBody(start);
            this.LastDistanceVectors = UpdateDistanceVectors(start, Z3.Context);

            var time1 = stopwatch.ElapsedMilliseconds - time0;
            stopwatch.Stop();

            synTime.timeMs = time1;
            synTime.gestureName = this.Gesture.Name;
            synTime.poseName = this.Gesture.Steps[CurrentStep].Pose.Name;
            synTime.uid = frameCount;
            //totalZ3Time += time1;
            frameCount++;
            GestureStatistics.synthTimes.Add(synTime);

            //Console.Write("z3time: " + (totalZ3Time / frameCount) + "                       \r");
        }
コード例 #10
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
        public GestureStatus TestBody(Z3Body body, int precision, Context localContext)
        {
            StatisticsEntryMatch matchStat = new StatisticsEntryMatch();

            var stopwatch = new Stopwatch();
            stopwatch.Start();
            var time0 = stopwatch.ElapsedMilliseconds;

            var time1 = stopwatch.ElapsedMilliseconds - time0;

            // NOT THREAD SAFE XXX
            this.LastDistanceVectors = this.UpdateDistanceVectors(body, localContext);

            var time2 = stopwatch.ElapsedMilliseconds - time1;

            // Check distance to target transformed joints and if body satisfies restrictions
            var distance = 2.0;

            if (this.Target.TransformedJoints.Count > 0)
                distance = CalcMaxDistance(this.LastDistanceVectors, this.Target.TransformedJoints);

            this.AccumulatedError += distance - this.LastDistance;
            this.LastDistance = distance;

            var time3 = stopwatch.ElapsedMilliseconds - time2;

            bool transformSucceeded = distance < TrigonometryHelper.GetDistance(precision);

            var time4 = stopwatch.ElapsedMilliseconds - time3;

            // NOT THREAD SAFE XXX
            bool restrictionSucceeded = this.Gesture.Steps[CurrentStep].Pose.IsBodyAccepted(body);

            var time5 = stopwatch.ElapsedMilliseconds - time4;

            matchStat.timeMs = time5;
            matchStat.gestureName = this.Gesture.Name;
            matchStat.uid = frameCount;
            matchStat.poseName = this.Gesture.Steps[CurrentStep].Pose.Name;

            bool succeeded =
                (transformSucceeded || this.Target.TransformedJoints.Count == 0) &&
                (restrictionSucceeded || this.Target.RestrictedJoints.Count == 0);

            // Overall succeeded represents whether the entire gesture was matched on this test, not just a single pose
            bool overallSucceeded = false;

            // If body is accepted move to matching the next pose in sequence
            if (succeeded)
            {
                this.CurrentStep += 1;
                this.AccumulatedError = 0;

                // Check if gesture is completed (all steps are finished)
                if (this.CurrentStep >= this.Gesture.Steps.Count)
                {
                    this.Reset(body);
                    this.CompletedCount += 1;
                    overallSucceeded = true;
                }

                this.UpdateTargetBody(body);
            }
            // If body was not accepted then check if error is higher than threshold
            // If accumulated error is too high the gesture is broken
            else if (this.AccumulatedError > 1)
            {
                //gesture.Reset(body);
            }

            var result = this.GetStatus();

            // The VisualGestureBuilder API for DiscreteGestureResult cares about whole gestures,
            // not about individual poses. We need to check for this case explicitly.
            // We then expose succeededDetection in a DiscreteGestureResult .
            result.succeededDetection = overallSucceeded;

            // TODO: check the semantics of succeededDetectionFirstFrame in the VisualGestureBuilder API
            // We here are assuming that firstFrame means this is the first frame where we successfully detected the gesture
            // We are assuming that firstFrame does NOT mean this is the first frame where we started tracking the gesture
            if (CompletedCount == 1 & overallSucceeded)
                result.succeededDetectionFirstFrame = true;
            else
                result.succeededDetectionFirstFrame = false;

            // TODO: check the semantics of confidence in the VisualGestureBuilder API.
            // Here confidence is the same as distance from the target body.
            // That is NOT the same as if confidence were a probability that we are correct
            // We want to have as close semantics as possible to the VisualGestureBuilder API, so we need to double check this
            result.confidence = (float)distance;

            var time6 = stopwatch.ElapsedMilliseconds - time5;
            frameCount++;

            // Record the statistics entry here
            GestureStatistics.matchTimes.Add(matchStat);
            return result;
        }
コード例 #11
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
 public void InitBody(Z3Body body)
 {
     InitBody(body, Z3.Context);
 }
コード例 #12
0
ファイル: Pose.cs プロジェクト: lsfcin/prepose
        // returns the min percentage of completion from all restrictions
        internal double CalcMinPercentage(Z3Body body, out string mainRestriction)
        {
            var percentage = this.Restriction.CalcPercentage(body, out mainRestriction);

            return percentage;
        }
コード例 #13
0
ファイル: Restrictions.cs プロジェクト: lsfcin/prepose
        public bool IsBodyAccepted(Z3Body body)
        {
            BoolExpr resultExpr = RestrictionFunc(body);

            Solver solver = Z3.Context.MkSolver();

            solver.Push();
            solver.Assert(Z3.Context.MkNot(resultExpr));
            Status status = solver.Check();
            Statistics stats = solver.Statistics;

            bool result = (status == Status.UNSATISFIABLE);

            solver.Pop();

            return result;
        }
コード例 #14
0
ファイル: PoseBuilder.cs プロジェクト: mhusinsky/prepose
        //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;
        }
コード例 #15
0
ファイル: Ambiguity.cs プロジェクト: caomw/prepose
 public PairwiseConflictException(string message, Gesture gesture1, Gesture gesture2, Z3Body body)
     : base(message)
 {
     this.Witness = body;
     this.Gesture1 = gesture1;
     this.Gesture2 = gesture2;
 }
コード例 #16
0
ファイル: Safety.cs プロジェクト: lsfcin/prepose
        /// <summary>
        /// Checks if the pose is within default safety 
        /// restrictions when the transform and restrictions 
        /// are applied.
        /// </summary>
        /// <returns>True if it's safe</returns>
        public static bool IsWithinSafetyRestrictions(Pose pose, out Z3Body witness)
        {
            Z3Body input = Z3Body.MkZ3Const();
            Z3Body transformed = pose.Transform.Transform(input);

            IBodyRestriction safe = Safety.DefaultSafetyRestriction();

            BoolExpr inputSafe = safe.Evaluate(input);
            BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);

            // Try to generate a unsafe witness using the transform
            BoolExpr outputUnsafe = Z3.Context.MkNot(safe.Evaluate(transformed));

            // Put together all expressions and search for unsat
            BoolExpr expr = Z3.Context.MkAnd(inputSafe, transformedRestricted, outputUnsafe);

            SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);

            if (solverResult.Status == Status.SATISFIABLE)
            {
                //Z3Body
                witness =
                    Z3AnalysisInterface.CreateBodyWitness(
                    transformed,
                    solverResult.Model,
                    pose.GetAllJointTypes(),
                    JointTypeHelper.CreateDefaultZ3Body());

                return false;
            }
            else if (solverResult.Status == Status.UNKNOWN)
            {
                //Z3Body
                witness = JointTypeHelper.CreateDefaultZ3Body();

                return false;
            }
            else
            {
                Contract.Assert(solverResult.Status == Status.UNSATISFIABLE);
                witness = null;
                return true;
            }
        }
コード例 #17
0
        void myBodyReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            if (this.IsPaused)
                return;

            PreposeGesturesFrame retFrame = null;
            PreposeGesturesFrameArrivedEventArgs upArgs = new PreposeGesturesFrameArrivedEventArgs();

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    // Perform the gesture matching on this frame
                    var z3body = new Z3Body();
                    bodyFrame.GetAndRefreshBodyData(this.bodies);

                    foreach (var body in this.bodies)
                    {
                        if (body.TrackingId == this.mySource.TrackingId)
                        {
                            // We are at the correct body - go ahead and feed it to the BodyMatcher
                            IReadOnlyDictionary<Microsoft.Kinect.JointType, Joint> joints = body.Joints;
                            z3body = Z3KinectConverter.CreateZ3Body(joints);
                            var result = this.PreposeGesturesFrameSource.myMatcher.TestBody(z3body);

                            // Fill in the gesture results for this frame
                            retFrame = new PreposeGesturesFrame(result, this.mySource, body.TrackingId, bodyFrame.RelativeTime);
                            break;
                        }
                    }
                }
            }

            // TODO: revisit the way the PreposeGesturesFrameReference is implemented to avoid keeping around massive amounts of frames
            PreposeGesturesFrameReference retFrameReference = new PreposeGesturesFrameReference(retFrame);
            upArgs.FrameReference = retFrameReference;

            // Signal that we have a new PreposeGesturesFrame arrived
            FrameArrived(this, upArgs);
        }
コード例 #18
0
ファイル: Pose.cs プロジェクト: lsfcin/prepose
        public bool IsTransformedBodyAccepted(Z3Body body)
        {
            bool result = false;

            Z3Body transformedBody = this.mTransform.Transform(body);
            result = this.IsBodyAccepted(transformedBody);

            return result;
        }
コード例 #19
0
ファイル: Pose.cs プロジェクト: lsfcin/prepose
 public void UpdateDelayedStatements(Z3Body startBody)
 {
     this.Delayed.Update(startBody);
 }
コード例 #20
0
ファイル: Transforms.cs プロジェクト: lsfcin/prepose
        /// <summary>
        /// Apply this transform to obtain an output body.
        /// </summary>
        /// <param name="inputBody"></param>
        /// <returns></returns>
        public Z3Body Transform(Z3Body inputBody)
        {
            var outputBody = new Z3Body();
            var jointTypes = EnumUtil.GetValues<JointType>();

            foreach (var jointType in jointTypes)
            {
                // Both this and inputBody have the jointType, so transform the joint
                if (this.JointTransforms.ContainsKey(jointType) &&
                    inputBody.Joints.ContainsKey(jointType))
                {
                    outputBody.Joints.Add(jointType,
                        this.JointTransforms[jointType].Transform(
                        inputBody.Joints[jointType]));

                    outputBody.Norms.Add(jointType,
                        inputBody.Norms[jointType]);
                }

                // The joint exists in body but there is no transform for it, so add it
                else if (inputBody.Joints.ContainsKey(jointType))
                {
                    outputBody.Joints.Add(jointType,
                        inputBody.Joints[jointType]);

                    outputBody.Norms.Add(jointType,
                        inputBody.Norms[jointType]);
                }

                // Do nothing otherwise
            }

            return outputBody;
        }
コード例 #21
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
 public Z3Target()
 {
     this.Body = new Z3Body();
     this.TransformedJoints = new List<JointType>();
     this.RestrictedJoints = new List<JointType>();
 }
コード例 #22
0
ファイル: Gestures.cs プロジェクト: lsfcin/prepose
        public void FinalResult(Z3Body input, out Z3Body transformed, out BoolExpr evaluation)
        {
            transformed = input;
            evaluation = Z3Math.True;

            foreach (var step in this.Steps)
            {
                var pose = step.Pose;
                transformed = pose.Transform.Transform(transformed);
                evaluation = Z3.Context.MkAnd(evaluation, pose.Restriction.Evaluate(transformed));
            }
        }
コード例 #23
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
 public void InitBody(Z3Body body, Context localContext)
 {
     GestureMatcherLocalContext = localContext;
     if (this.Target == null)
     {
         this.UpdateTargetBody(body);
     }
 }
コード例 #24
0
        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;
        }
コード例 #25
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
 internal void UpdateTargetBody(Z3Body start)
 {
     UpdateTargetBody(start, Z3.Context);
 }
コード例 #26
0
        // Generates a body witness which satisfies two conditions
        // 1. It is within a range (angle threshold) of a transform from a start body
        // 2. It is within the considered restrictions
        public static Z3Target GenerateTarget(
			BodyTransform transform, 
			CompositeBodyRestriction restriction,
			Z3Body startBody,
			int angleThreshold)
        {
            var z3ConstBody = Z3Body.MkZ3Const();
            z3ConstBody.Norms = startBody.Norms;
            var transformedBody = transform.Transform(startBody);

            var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList();
            var isNearExpr = z3ConstBody.IsAngleBetweenLessThan(transformedBody, joints, angleThreshold);
            var evaluateExpr = restriction.Evaluate(z3ConstBody);
            //var normsExpr = BodyRestrictionBuilder.EvaluateNorms(startBody, z3ConstBody);

            //var expr = Z3.Context.MkAnd(isNearExpr, evaluateExpr, normsExpr);
            //var expr = Z3.Context.MkAnd(evaluateExpr, normsExpr);
            var expr = Z3.Context.MkAnd(evaluateExpr, isNearExpr);

            var checkResult = CheckStatus(expr);
            if (checkResult.Status == Status.SATISFIABLE)
            {
                var witness = CreateBodyWitness(
                    z3ConstBody,
                    checkResult.Model,
                    restriction.GetJointTypes(),
                    startBody);

                var target = new Z3Target();
                target.Body = witness;
                target.RestrictedJoints = restriction.GetJointTypes();
                target.TransformedJoints = transform.GetJointTypes();

                foreach (var jointType in transform.GetJointTypes())
                    target.Body.Joints[jointType] = transformedBody.Joints[jointType];

                return target;
            }
            else
            {
                return null;
            }
        }
コード例 #27
0
ファイル: BodyMatching.cs プロジェクト: caomw/prepose
 private void Reset(Z3Body body)
 {
     this.CurrentStep = 0;
     this.LastDistance = 0;
     this.UpdateTargetBody(body);
     this.AccumulatedError = 0;
     this.Target = null;
 }
コード例 #28
0
ファイル: Restrictions.cs プロジェクト: lsfcin/prepose
 public BoolExpr Evaluate(Z3Body body)
 {
     return this.RestrictionFunc(body);
 }
コード例 #29
0
 public PoseSafetyException(string message, Pose pose, Z3Body body)
     : base(message)
 {
     this.Body = body;
     this.Pose = pose;
 }
コード例 #30
0
ファイル: Restrictions.cs プロジェクト: lsfcin/prepose
 public double Percentage(Z3Body body)
 {
     return this.PercentageFunc(body);
 }
コード例 #31
0
ファイル: Pose.cs プロジェクト: lsfcin/prepose
        public Z3Target CalcNearestTargetBody(Z3Body startBody)
        {
            //Z3Target target = null;

            //// Create binary search to look for nearest body
            //int numSteps = 1;
            //int degreesThreshold = 90;
            //int degreesIncrement = 90;

            //for (int i = 0; i < numSteps; ++i)
            //{
            //    // Ask for a witness which is within the range
            //    target = Z3AnalysisInterface.GenerateTarget(
            //        this.Transform,
            //        this.Restriction,
            //        startBody,
            //        degreesThreshold);

            //    // Update degrees threshold
            //    degreesIncrement /= 2;

            //    if (target != null)
            //    {
            //        degreesThreshold -= degreesIncrement;
            //    }
            //    else
            //    {
            //        degreesThreshold += degreesIncrement;
            //    }
            //}

            //// If target still null it probably means Z3 was unable to solve the restrictions
            //// This way we generate a target using onlye the transforms
            //if(target == null)
            //{
            //    target = new Z3Target();
            //    target.Body = this.Transform.Transform(startBody);
            //    target.TransformedJoints = this.Transform.GetJointTypes();
            //}

            //// If target still null assign a new body as an error proof policy
            //if(target == null)
            //{
            //    target = new Z3Target();
            //    target.Body = startBody;
            //}

            //return target;

            var target = new Z3Target();
            target.Body = this.Transform.Transform(startBody);
            target.TransformedJoints = this.Transform.GetJointTypes();

            // If target still null assign a new body as an error proof policy
            if(target == null)
            {
                target = new Z3Target();
                target.Body = startBody;
            }

            return target;
        }
コード例 #32
0
ファイル: Restrictions.cs プロジェクト: lsfcin/prepose
        public bool IsBodyAccepted(Z3Body body, Context localContext)
        {
            BoolExpr resultExpr = Evaluate(body);
            bool result = Z3.EvaluateBoolExpr(resultExpr, localContext);

            return result;
        }