private void CheckAlignment(List <AnchorPose> anchorPoses, List <AnchorEdge> anchorEdges, Pose movement) { Pose spongyHead; IPlugin plugin = WorldLockingManager.GetInstance().Plugin; for (int k = 0; k < anchorPoses.Count; ++k) { spongyHead = anchorPoses[k].pose; plugin.ClearSpongyAnchors(); plugin.Step_Init(spongyHead); plugin.AddSpongyAnchors(anchorPoses); plugin.SetMostSignificantSpongyAnchorId(anchorPoses[k].anchorId); plugin.AddSpongyEdges(anchorEdges); plugin.Step_Finish(); var adjustment = plugin.GetAlignment(); Assert.IsTrue(adjustment == movement, $"k={k} adjustment={adjustment}, movement={movement}"); } }
private void UpdateAndCheck(IPlugin plugin, int prime, List <AnchorPose> anchorPoses, List <AnchorEdge> anchorEdges, List <AnchorId> frozenIds) { AnchorPose headPose = anchorPoses[prime]; plugin.ClearSpongyAnchors(); plugin.Step_Init(headPose.pose); plugin.AddSpongyAnchors(anchorPoses); plugin.SetMostSignificantSpongyAnchorId(headPose.anchorId); plugin.AddSpongyEdges(anchorEdges); plugin.Step_Finish(); FragmentId fragmentId = plugin.GetMostSignificantFragmentId(); Debug.Log($"fragmentId={fragmentId}"); var frozenAnchors = plugin.GetFrozenAnchors(); Assert.AreEqual(frozenAnchors.Length, frozenIds.Count, "Unexpected difference between plugin frozen anchors and client frozen anchors counts"); for (int i = 0; i < frozenAnchors.Length; ++i) { Assert.IsTrue(frozenIds.FindIndex(x => x == frozenAnchors[i].anchorId) >= 0, "Plugin has unexpected frozen id"); } }
/// <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); }