/// <summary>
/// If a potential new anchor was prepared (in a previous time step) and is now found to be
/// located, this routine finalizes it and prepares its edges to be added
/// </summary>
/// <param name="newEdges">List that will have new edges appended by this routine</param>
/// <returns>new anchor id (or Invalid if none was finalized)</returns>
private AnchorId FinalizeNewAnchor(out List <AnchorEdge> newEdges)
{
newEdges = new List <AnchorEdge>();
if ((newSpongyAnchor == null) || !newSpongyAnchor.IsLocated)
{
#if WLT_EXTRA_LOGGING
if (newSpongyAnchor != null)
{
DebugLogExtra($"Can't finalize {newSpongyAnchor.name} because it's still not located.");
}
#endif // WLT_EXTRA_LOGGING
return(AnchorId.Invalid);
}
AnchorId newId = ClaimAnchorId();
foreach (var id in newAnchorNeighbors)
{
newEdges.Add(new AnchorEdge()
{
anchorId1 = id, anchorId2 = newId
});
}
spongyAnchors.Add(new SpongyAnchorWithId()
{
anchorId = newId,
spongyAnchor = newSpongyAnchor
});
newSpongyAnchor = null;
return(newId);
}
/// <summary>
/// If a potential new anchor was prepared (in a previous time step) and is now found to be
/// located, this routine finalizes it and prepares its edges to be added
/// </summary>
/// <param name="newEdges">List that will have new edges appended by this routine</param>
/// <returns>new anchor id (or Invalid if none was finalized)</returns>
private AnchorId FinalizeNewAnchor(out List <AnchorEdge> newEdges)
{
newEdges = new List <AnchorEdge>();
if (!newSpongyAnchor || !newSpongyAnchor.isLocated)
{
return(AnchorId.Invalid);
}
AnchorId newId = ClaimAnchorId();
foreach (var id in newAnchorNeighbors)
{
newEdges.Add(new AnchorEdge()
{
anchorId1 = id, anchorId2 = newId
});
}
spongyAnchors.Add(new SpongyAnchorWithId()
{
anchorId = newId,
spongyAnchor = newSpongyAnchor
});
newSpongyAnchor = null;
return(newId);
}
protected override SpongyAnchor DestroyAnchor(AnchorId id, SpongyAnchor spongyAnchor)
{
if (spongyAnchor != null)
{
GameObject.Destroy(spongyAnchor.gameObject);
}
RemoveSpongyAnchorById(id);
return(null);
}
protected override SpongyAnchor DestroyAnchor(AnchorId id, SpongyAnchor spongyAnchor)
{
if (spongyAnchor is SpongyAnchorARF spongyARF)
{
spongyARF.Cleanup(arAnchorManager);
}
RemoveSpongyAnchorById(id);
return(null);
}
/// <summary>
/// prepare potential new anchor, which will only be finalized in a later time step
/// when isLocalized is actually found to be true (see code before)
/// </summary>
/// <param name="pose"></param>
/// <param name="neighbors"></param>
private void PrepareNewAnchor(Pose pose, List <AnchorId> neighbors)
{
if (newSpongyAnchor)
{
UnityEngine.Object.Destroy(newSpongyAnchor.gameObject);
}
newSpongyAnchor = CreateAnchor(NextAnchorId());
newSpongyAnchor.transform.SetGlobalPose(pose);
newAnchorNeighbors = neighbors;
}
/// <summary>
/// prepare potential new anchor, which will only be finalized in a later time step
/// when isLocated is actually found to be true (see code before)
/// </summary>
/// <param name="pose"></param>
/// <param name="neighbors"></param>
private void PrepareNewAnchor(Pose pose, List <AnchorId> neighbors)
{
if (newSpongyAnchor != null)
{
DebugLogExtra($"Discarding {newSpongyAnchor.name} (located={newSpongyAnchor.IsLocated}) because still not located");
newSpongyAnchor = DestroyAnchor(AnchorId.Invalid, newSpongyAnchor);
}
newSpongyAnchor = CreateAnchor(NextAnchorId(), worldAnchorParent, pose);
newAnchorNeighbors = neighbors;
}
/// <summary>
/// Delete all spongy anchor objects and reset internal state
/// </summary>
public void Reset()
{
foreach (var anchor in spongyAnchors)
{
UnityEngine.Object.Destroy(anchor.spongyAnchor.gameObject);
}
spongyAnchors.Clear();
ResetAnchorIds();
UnityEngine.Object.Destroy(newSpongyAnchor);
newSpongyAnchor = null;
}
private bool LostTrackingCleanup(string message)
{
DebugLogExtra($"{message} Frame {Time.frameCount}");
lastTrackingInactiveTime = Time.unscaledTime;
if (newSpongyAnchor)
{
newSpongyAnchor = DestroyAnchor(AnchorId.Invalid, newSpongyAnchor);
}
ErrorStatus = message;
return(false);
}
/// <summary>
/// If we have more local anchors than parameterized limit, destroy the furthest.
/// </summary>
/// <param name="maxDistAnchorId">Id of the furthest anchor.</param>
/// <param name="maxDistSpongyAnchor">Reference to the furthest anchor.</param>
private void CheckForCull(AnchorId maxDistAnchorId, SpongyAnchor maxDistSpongyAnchor)
{
/// Anchor limiting is only enabled with a positive limit value.
if (MaxLocalAnchors > 0)
{
if (SpongyAnchors.Count > MaxLocalAnchors)
{
if (maxDistSpongyAnchor != null)
{
DestroyAnchor(maxDistAnchorId, maxDistSpongyAnchor);
}
}
}
}
/// <summary>
/// Delete all spongy anchor objects and reset internal state
/// </summary>
public void Reset()
{
foreach (var anchor in spongyAnchors)
{
/// Pass in AnchorId.Invalid, not because the anchors aren't in a list, but
/// because it's faster to clear the entire lists rather than pulling out each element.
DestroyAnchor(AnchorId.Invalid, anchor.spongyAnchor);
}
// mafinc - should this clear frozen anchors as well?
spongyAnchors.Clear();
plugin.ClearFrozenAnchors();
newSpongyAnchor = DestroyAnchor(AnchorId.Invalid, newSpongyAnchor);
headTracker.Reset();
}
protected override SpongyAnchor DestroyAnchor(AnchorId id, SpongyAnchor spongyAnchor)
{
SpongyAnchorXR spongyAnchorXR = spongyAnchor as SpongyAnchorXR;
if (spongyAnchorXR != null)
{
Debug.Assert(anchorsByTrackableId[spongyAnchorXR.TrackableId] == spongyAnchorXR);
anchorsByTrackableId.Remove(spongyAnchorXR.TrackableId);
xrReferencePointManager.TryRemoveReferencePoint(spongyAnchorXR.TrackableId);
GameObject.Destroy(spongyAnchorXR.gameObject);
}
RemoveSpongyAnchorById(id);
return(null);
}
// Start is called before the first frame update
private void Start()
{
#if WLT_ENABLE_LEGACY_WSA
if (worldAnchor == null)
{
worldAnchor = gameObject.AddComponent <UnityEngine.XR.WSA.WorldAnchor>();
}
#else
dummy = this;
if (isSaved && lastNotLocatedTime > 0)
{
isSaved = false;
lastNotLocatedTime = float.NegativeInfinity;
}
#endif // WLT_ENABLE_LEGACY_WSA
}
/// <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> /// Platform dependent disposal of local anchors. /// </summary> /// <param name="id">The id of the anchor to destroy.</param> /// <param name="spongyAnchor">Reference to the anchor to destroy.</param> /// <returns>Null</returns> /// <remarks> /// The id is used to delete from any stored lists. If the SpongyAnchor hasn't been /// added to any lists (is still initializing), id can be AnchorId.Invalid. /// </remarks> protected abstract SpongyAnchor DestroyAnchor(AnchorId id, SpongyAnchor spongyAnchor);
/// <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);
}
