private IEnumerator CheckAllPins(IAlignmentManager alignMgr)
{
for (int i = 0; i < pinData.Length; ++i)
{
alignMgr.AddAlignmentAnchor(pinData[i].name, pinData[i].virtualPose, pinData[i].lockedPose);
}
alignMgr.SendAlignmentAnchors();
for (int i = 0; i < pinData.Length; ++i)
{
int nextIdx = (i + 1) % pinData.Length;
CheckAlignment(alignMgr, pinData[i].virtualPose.position, pinData[i].lockedPose.position);
CheckAlignment(alignMgr,
(pinData[i].virtualPose.position + pinData[nextIdx].virtualPose.position) * 0.5f,
(pinData[i].lockedPose.position + pinData[nextIdx].virtualPose.position) * 0.5f);
}
for (int i = 0; i < pinData.Length - 3; ++i)
{
int j = (i + 1) % pinData.Length;
int k = (j + 1) % pinData.Length;
CheckAlignment(alignMgr,
(pinData[i].virtualPose.position + pinData[j].virtualPose.position + pinData[k].virtualPose.position) / 3.0f,
(pinData[i].lockedPose.position + pinData[j].lockedPose.position + pinData[k].virtualPose.position) / 3.0f);
}
alignMgr.ClearAlignmentAnchors();
alignMgr.SendAlignmentAnchors();
yield return(null);
}
/// <summary>
/// Accept the rotation as computed by the IOrienter.
/// </summary>
/// <param name="mgr">The alignment manager which needs to receive the updated Pose.</param>
/// <param name="lockedRotation">The new world locked rotation to adopt.</param>
public void PushRotation(IAlignmentManager mgr, Quaternion lockedRotation)
{
/// Append the modeling pose rotation. This will cancel out when computing the
/// pinnedFromLocked transform, so that the computed rotation gets applied as is.
LockedPose = new Pose(LockedPose.position, lockedRotation * ModelingPose.rotation);
PushAlignmentData(mgr);
}
private WorldLockingManager()
{
CreateUpdaterNode();
/// It might look nicer to pull these off into internal setup functions,
/// but by leaving them in the constructor, these fields can be marked "readonly",
/// which they conceptually are.
if (Core.Plugin.HasEngine())
{
Plugin = new Plugin();
}
else
{
Plugin = new PluginNoop();
}
DiagnosticRecordings.Start(Plugin);
headPoseTracker = new HeadPoseTrackerCamera();
/// This should never fail. It's a null-manager.
anchorManager = AnchorManagerNull.TryCreate(Plugin, headPoseTracker);
Debug.Assert(anchorManager != null, "Null manager creation should never fail");
var fm = new FragmentManager(Plugin);
fragmentManager = fm;
attachmentPointManager = fm;
/// Note the alignmentManager accesses the FragmentManager in its constructor
/// to register for refit notifications. Either FragmentManager needs to be fully
/// setup before constructing AlignmentManager, or that registration needs to be deferred.
alignmentManager = new AlignmentManager(this);
}
/// <summary>
/// Adjust to refit operations.
/// </summary>
/// <param name="mainId">The new combined fragment.</param>
/// <param name="absorbedIds">Id's of other fragments being merged into mainId.</param>
/// <remarks>
/// This callback occurs *after* the refit operation. As part of the refit,
/// positions of the managed SpacePinOrientables may have changed, and therefore
/// their implied orientations must be re-calculated.
/// Note that there is an apparent race condition, as there is no order guarantee on
/// the order of refit notifications, and the AlignmentManager also relies on the
/// refit notification to adjust after refit operations.
/// However, both the Orienter and the AlignmentManager rely only on the positions
/// having been set, which was accomplished during the refit and before the refit
/// notification. So it really doesn't matter whether the Orienter.OnRefit or the
/// AlignmentManager.OnRefit is called first.
/// </remarks>
private void OnRefit(FragmentId mainId, FragmentId[] absorbedIds)
{
IAlignmentManager alignMgr = WorldLockingManager.GetInstance().AlignmentManager;
Reorient(mainId, alignMgr);
alignMgr.SendAlignmentAnchors();
}
private void CheckAlignment(IAlignmentManager alignMgr, Pose virtualPose, Pose lockedPose)
{
WorldLockingManager mgr = WorldLockingManager.GetInstance();
alignMgr.ComputePinnedPose(new Pose(lockedPose.position, Quaternion.identity));
Pose pinnedFromLocked = alignMgr.PinnedFromLocked;
Pose frozenFromLocked = mgr.FrozenFromPinned.Multiply(pinnedFromLocked);
Pose lockedFromFrozen = frozenFromLocked.Inverse();
Pose computedLocked = lockedFromFrozen.Multiply(virtualPose);
bool areEqualPositions = computedLocked.position == lockedPose.position;
Assert.IsTrue(areEqualPositions, $"clp={computedLocked.position.ToString("F3")}"
+ $" lpp={lockedPose.position.ToString("F3")}"
+ $" vpp={virtualPose.position.ToString("F3")}"
+ $" FfP={mgr.FrozenFromPinned.position.ToString("F3")}"
+ $" PfL={pinnedFromLocked.position.ToString("F3")}"
);
bool areEqualRotatons = computedLocked.rotation == lockedPose.rotation;
Assert.IsTrue(areEqualRotatons, $"clr={computedLocked.rotation.ToString("F3")}"
+ $"lpr={lockedPose.rotation.ToString("F3")}"
+ $" FfP={mgr.FrozenFromPinned.position.ToString("F3")}"
+ $" PfL={pinnedFromLocked.position.ToString("F3")}"
);
}
private void VerifyAlignment(IAlignmentManager alignMgr, PinData[] pinData)
{
for (int i = 0; i < pinData.Length; ++i)
{
CheckAlignment(alignMgr, pinData[i].virtualPose, pinData[i].lockedPose);
}
}
/// <summary>
/// Notify the manager that all necessary updates have been submitted and
/// are ready for processing.
/// </summary>
/// <param name="mgr"></param>
protected void SendAlignmentData(IAlignmentManager mgr)
{
mgr.SendAlignmentAnchors();
CheckAttachment();
transform.SetGlobalPose(InitialPose);
}
/// <summary>
/// Apply the computed rotations to the orientables.
/// </summary>
/// <param name="mgr">The alignment manager.</param>
/// <returns>True on success.</returns>
private bool SetRotations(IAlignmentManager mgr)
{
for (int i = 0; i < actives.Count; ++i)
{
actives[i].orientable.PushRotation(mgr, actives[i].rotation);
}
return(true);
}
/// <summary>
/// Communicate the data from this point to the alignment manager.
/// </summary>
/// <param name="mgr"></param>
protected void PushAlignmentData(IAlignmentManager mgr)
{
if (PinActive)
{
mgr.RemoveAlignmentAnchor(AnchorId);
}
AnchorId = mgr.AddAlignmentAnchor(AnchorName, ModelingPoseGlobal, lockedPose);
}
/// <summary>
/// Accept the rotation as computed by the IOrienter.
/// </summary>
/// <param name="mgr">The alignment manager which needs to receive the updated Pose.</param>
/// <param name="lockedRotation">The new world locked rotation to adopt.</param>
public void PushRotation(IAlignmentManager mgr, Quaternion lockedRotation)
{
//Debug.Log($"PushRotation {name}: mgr={(mgr == WorldLockingManager.GetInstance().AlignmentManager ? "global" : "local")}");
/// Append the modeling pose rotation. This will cancel out when computing the
/// pinnedFromLocked transform, so that the computed rotation gets applied as is.
LockedPose = new Pose(LockedPose.position, lockedRotation * ModelingPoseGlobal.rotation);
PushAlignmentData(mgr);
}
/// <summary>
/// Notify the manager that all necessary updates have been submitted and
/// are ready for processing.
/// </summary>
/// <param name="mgr"></param>
protected void SendAlignmentData(IAlignmentManager mgr)
{
mgr.SendAlignmentAnchors();
CheckAttachment();
transform.SetLocalPose(RestorePoseLocal);
}
/// <summary>
/// Communicate the data from this point to the alignment manager.
/// </summary>
/// <param name="mgr"></param>
protected void PushAlignmentData(IAlignmentManager mgr)
{
DebugLogExtra($"F{Time.frameCount} Push: {name}: MPG={ModelingPoseGlobal.ToString("F3")} GfP={GlobalFromParent.ToString("F3")} R={restorePoseLocal.ToString("F3")} MPP={WorldLockingManager.GetInstance().PinnedFromFrozen.Multiply(ModelingPoseGlobal)}");
if (PinActive)
{
mgr.RemoveAlignmentAnchor(AnchorId);
}
AnchorId = mgr.AddAlignmentAnchor(AnchorName, ModelingPoseGlobal, lockedPose);
}
/// <summary>
/// Record the locked pose and push data to the manager.
/// </summary>
/// <param name="lockedPose"></param>
public virtual void SetLockedPose(Pose lockedPose)
{
this.lockedPose = lockedPose;
IAlignmentManager mgr = manager.AlignmentManager;
PushAlignmentData(mgr);
SendAlignmentData(mgr);
}
/// <summary>
/// Adjust to refit operations.
/// </summary>
/// <param name="mainId">The new combined fragment.</param>
/// <param name="absorbedIds">Id's of other fragments being merged into mainId.</param>
/// <remarks>
/// This callback occurs *after* the refit operation. As part of the refit,
/// positions of the managed SpacePinOrientables may have changed, and therefore
/// their implied orientations must be re-calculated.
/// Note that there is an apparent race condition, as there is no order guarantee on
/// the order of refit notifications, and the AlignmentManager also relies on the
/// refit notification to adjust after refit operations.
/// However, both the Orienter and the AlignmentManager rely only on the positions
/// having been set, which was accomplished during the refit and before the refit
/// notification. So it really doesn't matter whether the Orienter.OnRefit or the
/// AlignmentManager.OnRefit is called first.
/// </remarks>
private void OnRefit(FragmentId mainId, FragmentId[] absorbedIds)
{
/// Use the last alignment manager used. If none, use the wlt manager's current alignment manager.
IAlignmentManager alignMgr = cachedAlignmentManager;
if (alignMgr == null)
{
alignMgr = WorldLockingManager.GetInstance().AlignmentManager;
}
Reorient(mainId, alignMgr);
alignMgr.SendAlignmentAnchors();
}
private void VerifyAlignment(IAlignmentManager alignMgr, PinData[] pinData)
{
for (int i = 0; i < pinData.Length; ++i)
{
Debug.Log($"i={i}"
+ $" vp={pinData[i].virtualPose.position.ToString("F3")}"
+ $" lp={pinData[i].lockedPose.position.ToString("F3")}"
);
CheckAlignment(alignMgr, pinData[i].virtualPose, pinData[i].lockedPose);
}
}
private void VerifyAlignmentIdentity(IAlignmentManager alignMgr, PinData[] pinData)
{
for (int i = 0; i < pinData.Length; ++i)
{
alignMgr.ComputePinnedPose(new Pose(pinData[i].lockedPose.position, Quaternion.identity));
Pose pinnedFromLocked = alignMgr.PinnedFromLocked;
bool isIdentityPosition = pinnedFromLocked.position == Vector3.zero;
Assert.IsTrue(isIdentityPosition);
bool isIdentityRotation = pinnedFromLocked.rotation == Quaternion.identity;
Assert.IsTrue(isIdentityRotation);
}
}
private void SetAlignmentManager(IAlignmentManager manager)
{
if (alignmentManager != null)
{
alignmentManager.OnTriangulationBuilt -= OnNewTriangulationWasBuilt;
}
alignmentManager = manager;
if (alignmentManager != null)
{
alignmentManager.OnTriangulationBuilt += OnNewTriangulationWasBuilt;
}
}
private void CheckAlignment(IAlignmentManager alignMgr, Vector3 virtualPos, Vector3 lockedPos)
{
WorldLockingManager mgr = WorldLockingManager.GetInstance();
alignMgr.ComputePinnedPose(new Pose(lockedPos, Quaternion.identity));
Pose pinnedFromLocked = alignMgr.PinnedFromLocked;
Pose frozenFromLocked = mgr.FrozenFromPinned.Multiply(pinnedFromLocked);
Pose lockedFromFrozen = frozenFromLocked.Inverse();
Vector3 computedLocked = lockedFromFrozen.Multiply(virtualPos);
bool areEqual = computedLocked == lockedPos;
Assert.IsTrue(areEqual, $"c={computedLocked.ToString("F3")} l={lockedPos.ToString("F3")}");
}
/// <inheritdocs />
public void Reorient(FragmentId fragmentId, IAlignmentManager mgr)
{
if (!InitRotations(fragmentId))
{
return;
}
if (!ComputeRotations())
{
return;
}
if (!SetRotations(mgr))
{
return;
}
}
private IEnumerator CheckSinglePin(IAlignmentManager alignMgr, int pinIdx)
{
alignMgr.ClearAlignmentAnchors();
var id0 = alignMgr.AddAlignmentAnchor(pinData[pinIdx].name, pinData[pinIdx].virtualPose, pinData[pinIdx].lockedPose);
alignMgr.SendAlignmentAnchors();
yield return(null);
CheckAlignment(alignMgr, pinData[pinIdx].virtualPose.position, pinData[pinIdx].lockedPose.position);
CheckAlignment(alignMgr, pinData[pinIdx].virtualPose.position + new Vector3(1.0f, 0, 0), pinData[pinIdx].lockedPose.position + new Vector3(1.0f, 0, 0));
alignMgr.ClearAlignmentAnchors();
alignMgr.SendAlignmentAnchors();
yield return(null);
}
/// <summary>
/// Override of base SetLockedPose to allow insertion of the computation of rotation.
/// </summary>
/// <param name="lockedPose">The new pose in world locked space.</param>
/// <remarks>
/// Note that base class implementation is not invoked here, but rather this override
/// performs the same steps but with additional computations (for the rotation)
/// interleaved.
/// </remarks>
public override void SetLockedPose(Pose lockedPose)
{
this.LockedPose = lockedPose;
/// World locked space pose is meaningless outside the context of the current fragment.
/// Record that fragment id now.
/// Note that fragment id may change in the course of refit operations, but that's
/// okay because the locked pose will be world locked then too.
if (EnsureRegistered())
{
IAlignmentManager mgr = Manager.AlignmentManager;
Debug.Assert(Orienter != null, "Registration with orienter should not succeed with null orienter.");
Orienter.Reorient(FragmentId, mgr);
SendAlignmentData(mgr);
}
}
public IEnumerator SaveLoadTestSpacePinOrientable()
{
GameObject rig = loadHelper.LoadBasicSceneRig();
GameObject[] gos = new GameObject[pinData.Length];
SpacePinOrientable[] spos = new SpacePinOrientable[pinData.Length];
Quaternion rotThirty = Quaternion.AngleAxis(30.0f, new Vector3(0.0f, 1.0f, 0.0f));
GameObject orienterGO = new GameObject("Orienter");
IOrienter orienter = orienterGO.AddComponent <Orienter>();
for (int i = 0; i < pinData.Length; ++i)
{
gos[i] = new GameObject("GOs_" + i.ToString());
spos[i] = gos[i].AddComponent <SpacePinOrientable>();
spos[i].Orienter = orienter;
}
/// Wait for their Start's to be called.
yield return(null);
for (int i = 0; i < spos.Length; ++i)
{
spos[i].transform.SetGlobalPose(pinData[i].virtualPose);
spos[i].ResetModelingPose();
Vector3 rotPosition = rotThirty * pinData[i].virtualPose.position;
spos[i].SetFrozenPosition(rotPosition);
}
yield return(null);
IAlignmentManager alignMgr = WorldLockingManager.GetInstance().AlignmentManager;
/// This is an arbitrary position, not actually one of the pinned positions currently.
alignMgr.ComputePinnedPose(new Pose(pinData[0].lockedPose.position, Quaternion.identity));
Quaternion rot = Quaternion.Inverse(alignMgr.PinnedFromLocked.rotation);
bool isThirty = rot == rotThirty;
Assert.IsTrue(isThirty);
GameObject.Destroy(rig);
yield return(null);
}
private WorldLockingManager()
{
CreateUpdaterNode();
/// It might look nicer to pull these off into internal setup functions,
/// but by leaving them in the constructor, these fields can be marked "readonly",
/// which they conceptually are.
Plugin = new Plugin();
DiagnosticRecordings.Start(Plugin);
var am = new AnchorManager(Plugin);
anchorManager = am;
var fm = new FragmentManager(Plugin);
fragmentManager = fm;
attachmentPointManager = fm;
/// Note the alignmentManager accesses the FragmentManager in its constructor
/// to register for refit notifications. Either FragmentManager needs to be fully
/// setup before constructing AlignmentManager, or that registration needs to be deferred.
alignmentManager = new AlignmentManager(this);
}
private IEnumerator CheckDualPins(IAlignmentManager alignMgr, int pinIdx0, int pinIdx1)
{
alignMgr.AddAlignmentAnchor(pinData[pinIdx0].name, pinData[pinIdx0].virtualPose, pinData[pinIdx0].lockedPose);
alignMgr.AddAlignmentAnchor(pinData[pinIdx1].name, pinData[pinIdx1].virtualPose, pinData[pinIdx1].lockedPose);
alignMgr.SendAlignmentAnchors();
yield return(null);
CheckAlignment(alignMgr, pinData[pinIdx0].virtualPose.position, pinData[pinIdx0].lockedPose.position);
CheckAlignment(alignMgr, pinData[pinIdx1].virtualPose.position, pinData[pinIdx1].lockedPose.position);
CheckAlignment(alignMgr,
(pinData[pinIdx0].virtualPose.position + pinData[pinIdx1].virtualPose.position) * 0.5f,
(pinData[pinIdx0].lockedPose.position + pinData[pinIdx1].lockedPose.position) * 0.5f);
alignMgr.ClearAlignmentAnchors();
alignMgr.SendAlignmentAnchors();
yield return(null);
}
public IEnumerator SaveLoadTestSaveThenLoad()
{
GameObject rig = loadHelper.LoadBasicSceneRig();
WorldLockingManager wltMgr = WorldLockingManager.GetInstance();
var settings = wltMgr.Settings;
settings.AutoLoad = false;
settings.AutoSave = false;
wltMgr.Settings = settings;
IAlignmentManager alignMgr = wltMgr.AlignmentManager;
alignMgr.ClearAlignmentAnchors();
alignMgr.SendAlignmentAnchors();
// Verify alignment is identity
VerifyAlignmentIdentity(alignMgr, pinData);
// Add pins
for (int i = 0; i < pinData.Length; ++i)
{
pinData[i].anchorId = alignMgr.AddAlignmentAnchor(pinData[i].name, pinData[i].virtualPose, pinData[i].lockedPose);
}
alignMgr.SendAlignmentAnchors();
yield return(null);
// Verify alignment at pins
VerifyAlignment(alignMgr, pinData);
// Save
wltMgr.Save();
yield return(null);
// Verify alignment at pins, saving should be non-destructive.
VerifyAlignment(alignMgr, pinData);
// Clear
alignMgr.ClearAlignmentAnchors();
alignMgr.SendAlignmentAnchors();
yield return(null);
// Verify alignment is identity
VerifyAlignmentIdentity(alignMgr, pinData);
// Load
wltMgr.Load();
yield return(null);
for (int i = 0; i < pinData.Length; ++i)
{
pinData[i].anchorId = alignMgr.RestoreAlignmentAnchor(pinData[i].name, pinData[i].virtualPose);
}
alignMgr.SendAlignmentAnchors();
yield return(null);
// Verify alignment at pins, load should have restored them.
VerifyAlignment(alignMgr, pinData);
GameObject.Destroy(rig);
yield return(null);
}
/// <summary>
/// Accept the rotation as computed by the IOrienter.
/// </summary>
/// <param name="mgr">The alignment manager which needs to receive the updated Pose.</param>
/// <param name="lockedRotation">The new world locked rotation to adopt.</param>
public void PushRotation(IAlignmentManager mgr, Quaternion lockedRotation)
{
LockedPose = new Pose(LockedPose.position, lockedRotation);
PushAlignmentData(mgr);
}
