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);
}