Ejemplo n.º 1
0
        /// <summary>
        /// Create missing spongy anchors/edges and feed plugin with up-to-date input
        /// </summary>
        /// <returns>Boolean: Has the plugin received input to provide an adjustment?</returns>
        public bool Update()
        {
            ErrorStatus = "";

#if UNITY_WSA
            if (UnityEngine.XR.WSA.WorldManager.state != UnityEngine.XR.WSA.PositionalLocatorState.Active)
            {
                lastTrackingInactiveTime = Time.unscaledTime;

                if (newSpongyAnchor)
                {
                    UnityEngine.Object.Destroy(newSpongyAnchor.gameObject);
                    newSpongyAnchor = null;
                }

                ErrorStatus = "Lost Tracking";
                return(false);
            }
#endif // UNITY_WSA

            // To communicate spongyHead and spongyAnchor poses to the FrozenWorld engine, they must all be expressed
            // in the same coordinate system. Here, we do not care where this coordinate
            // system is defined and how it fluctuates over time, as long as it can be used to express the
            // relative poses of all the spongy objects within each time step.
            //
            // Note:
            // The low-level input obtained via InputTracking.GetLocal???(XRNode.Head) is automatically kept in sync with
            // Camera.main.transform.local??? (unless XRDevice.DisableAutoXRCameraTracking(Camera.main, true) is used to deactivate
            // this mechanism). In theory, both could be used interchangeably, potentially allowing to avoid the dependency
            // on low-level code at this point. It is not clear though, whether both values follow exactly the same timing or which
            // one is more correct to be used at this point. More research might be necessary.
            //
            // The decision between low-level access via InputTracking and high-level access via Camera.main.transform should
            // be coordinated with the decision between high-level access to WorldAnchor and low-level access to
            // Windows.Perception.Spatial.SpatialAnchor -- see comment at top of SpongyAnchor.cs
            Pose spongyHead = GetHeadPose();

            // place new anchors 1m below head
            Pose newSpongyAnchorPose = spongyHead;
            newSpongyAnchorPose.position.y -= 1;
            newSpongyAnchorPose.rotation    = Quaternion.identity;

            var activeAnchors        = new List <AnchorPose>();
            var innerSphereAnchorIds = new List <AnchorId>();
            var outerSphereAnchorIds = new List <AnchorId>();

            float    minDistSqr      = float.PositiveInfinity;
            AnchorId minDistAnchorId = 0;

            List <AnchorEdge> newEdges;
            AnchorId          newId = FinalizeNewAnchor(out newEdges);

            float innerSphereRadSqr = MinNewAnchorDistance * MinNewAnchorDistance;
            float outerSphereRadSqr = MaxAnchorEdgeLength * MaxAnchorEdgeLength;

            foreach (var keyval in spongyAnchors)
            {
                var id = keyval.anchorId;
                var a  = keyval.spongyAnchor;
                if (a.isLocated)
                {
                    float distSqr    = (a.transform.position - newSpongyAnchorPose.position).sqrMagnitude;
                    var   anchorPose = new AnchorPose()
                    {
                        anchorId = id, pose = a.transform.GetGlobalPose()
                    };
                    activeAnchors.Add(anchorPose);
                    if (distSqr < minDistSqr)
                    {
                        minDistSqr      = distSqr;
                        minDistAnchorId = id;
                    }
                    if (distSqr <= outerSphereRadSqr && id != newId)
                    {
                        outerSphereAnchorIds.Add(id);
                        if (distSqr <= innerSphereRadSqr)
                        {
                            innerSphereAnchorIds.Add(id);
                        }
                    }
                }
            }

            if (newId == 0 && innerSphereAnchorIds.Count == 0)
            {
                if (Time.unscaledTime <= lastTrackingInactiveTime + SpongyAnchor.TrackingStartDelayTime)
                {
                    // Tracking has become active only recently. We suppress creation of new anchors while
                    // new anchors may still be in transition due to SpatialAnchor easing.
                }
                else if (Time.unscaledTime < lastAnchorAddTime + AnchorAddOutTime)
                {
                    // short timeout after creating one anchor to prevent bursts of new, unlocatable anchors
                    // in case of problems in the anchor generation
                }
                else
                {
                    PrepareNewAnchor(newSpongyAnchorPose, outerSphereAnchorIds);
                    lastAnchorAddTime = Time.unscaledTime;
                }
            }

            if (activeAnchors.Count == 0)
            {
                ErrorStatus = "No active anchors";
                return(false);
            }

            // create edges between nearby existing anchors
            if (innerSphereAnchorIds.Count >= 2)
            {
                foreach (var i in innerSphereAnchorIds)
                {
                    if (i != minDistAnchorId)
                    {
                        newEdges.Add(new AnchorEdge()
                        {
                            anchorId1 = i, anchorId2 = minDistAnchorId
                        });
                    }
                }
            }

            plugin.ClearSpongyAnchors();
            plugin.Step_Init(spongyHead);
            plugin.AddSpongyAnchors(activeAnchors);
            plugin.SetMostSignificantSpongyAnchorId(minDistAnchorId);
            plugin.AddSpongyEdges(newEdges);
            plugin.Step_Finish();

            return(true);
        }
        /// <summary>
        /// Create missing spongy anchors/edges and feed plugin with up-to-date input
        /// </summary>
        /// <returns>Boolean: Has the plugin received input to provide an adjustment?</returns>
        public virtual bool Update()
        {
            ErrorStatus = "";

            if (!IsTracking())
            {
                return(LostTrackingCleanup("Lost Tracking"));
            }

            // To communicate spongyHead and spongyAnchor poses to the FrozenWorld engine, they must all be expressed
            // in the same coordinate system. Here, we do not care where this coordinate
            // system is defined and how it fluctuates over time, as long as it can be used to express the
            // relative poses of all the spongy objects within each time step.
            //
            Pose spongyHead = headTracker.GetHeadPose();

            // place new anchors 1m below head
            Pose newSpongyAnchorPose = spongyHead;

            newSpongyAnchorPose.position.y -= 1;
            newSpongyAnchorPose.rotation    = Quaternion.identity;

            var activeAnchors        = new List <AnchorPose>();
            var innerSphereAnchorIds = new List <AnchorId>();
            var outerSphereAnchorIds = new List <AnchorId>();

            float    minDistSqr      = float.PositiveInfinity;
            AnchorId minDistAnchorId = 0;

            float        maxDistSq           = 0;
            AnchorId     maxDistAnchorId     = AnchorId.Invalid;
            SpongyAnchor maxDistSpongyAnchor = null;

            List <AnchorEdge> newEdges;
            AnchorId          newId = FinalizeNewAnchor(out newEdges);

            float innerSphereRadSqr = MinNewAnchorDistance * MinNewAnchorDistance;
            float outerSphereRadSqr = MaxAnchorEdgeLength * MaxAnchorEdgeLength;

            foreach (var keyval in spongyAnchors)
            {
                var id = keyval.anchorId;
                var a  = keyval.spongyAnchor;
                if (a.IsLocated)
                {
                    Pose  aSpongyPose = a.SpongyPose;
                    float distSqr     = (aSpongyPose.position - newSpongyAnchorPose.position).sqrMagnitude;
                    var   anchorPose  = new AnchorPose()
                    {
                        anchorId = id, pose = aSpongyPose
                    };
                    activeAnchors.Add(anchorPose);
                    if (distSqr < minDistSqr)
                    {
                        minDistSqr      = distSqr;
                        minDistAnchorId = id;
                    }
                    if (distSqr <= outerSphereRadSqr && id != newId)
                    {
                        outerSphereAnchorIds.Add(id);
                        if (distSqr <= innerSphereRadSqr)
                        {
                            innerSphereAnchorIds.Add(id);
                        }
                    }
                    if (distSqr > maxDistSq)
                    {
                        maxDistSq           = distSqr;
                        maxDistAnchorId     = id;
                        maxDistSpongyAnchor = a;
                    }
                }
            }

            if (newId == 0 && innerSphereAnchorIds.Count == 0)
            {
                if (Time.unscaledTime <= lastTrackingInactiveTime + TrackingStartDelayTime)
                {
                    // Tracking has become active only recently. We suppress creation of new anchors while
                    // new anchors may still be in transition due to SpatialAnchor easing.
                    DebugLogExtra($"Skip new anchor creation because only recently gained tracking {Time.unscaledTime - lastTrackingInactiveTime}");
                }
                else if (Time.unscaledTime < lastAnchorAddTime + AnchorAddOutTime)
                {
                    // short timeout after creating one anchor to prevent bursts of new, unlocatable anchors
                    // in case of problems in the anchor generation
                    DebugLogExtra($"Skip new anchor creation because waiting on recently made anchor "
                                  + $"{Time.unscaledTime - lastAnchorAddTime} "
                                  + $"- {(newSpongyAnchor != null ? newSpongyAnchor.name : "null")}");
                }
                else
                {
                    PrepareNewAnchor(newSpongyAnchorPose, outerSphereAnchorIds);
                    lastAnchorAddTime = Time.unscaledTime;
                }
            }

            if (activeAnchors.Count == 0)
            {
                ErrorStatus = "No active anchors";
                return(false);
            }

            // create edges between nearby existing anchors
            if (innerSphereAnchorIds.Count >= 2)
            {
                foreach (var i in innerSphereAnchorIds)
                {
                    if (i != minDistAnchorId)
                    {
                        newEdges.Add(new AnchorEdge()
                        {
                            anchorId1 = i, anchorId2 = minDistAnchorId
                        });
                    }
                }
            }

            CheckForCull(maxDistAnchorId, maxDistSpongyAnchor);

#if WLT_DUMP_SPONGY
            DumpSpongy(spongyHead);
#endif // WLT_DUMP_SPONGY

            plugin.ClearSpongyAnchors();
            plugin.Step_Init(spongyHead);
            plugin.AddSpongyAnchors(activeAnchors);
            plugin.SetMostSignificantSpongyAnchorId(minDistAnchorId);
            plugin.AddSpongyEdges(newEdges);
            plugin.Step_Finish();

            return(true);
        }