/// <summary>
/// Convert the retrieved matrix to Unity lefthanded pose convention.
/// </summary>
/// <param name="newMatrix">Matrix to convert.</param>
/// <returns>Unity pose equivalent.</returns>
/// <remarks>
/// Note that any scale is discarded, returned pose is position+rotation only.
/// </remarks>
private Pose AdjustNewMatrix(System.Numerics.Matrix4x4 newMatrix)
{
// Convert from right to left coordinate system
newMatrix.M13 = -newMatrix.M13;
newMatrix.M23 = -newMatrix.M23;
newMatrix.M43 = -newMatrix.M43;
newMatrix.M31 = -newMatrix.M31;
newMatrix.M32 = -newMatrix.M32;
newMatrix.M34 = -newMatrix.M34;
/// Decompose into position + rotation (scale is discarded).
System.Numerics.Vector3 sysScale;
System.Numerics.Quaternion sysRotation;
System.Numerics.Vector3 sysPosition;
System.Numerics.Matrix4x4.Decompose(newMatrix, out sysScale, out sysRotation, out sysPosition);
Vector3 position = new Vector3(sysPosition.X, sysPosition.Y, sysPosition.Z);
Quaternion rotation = new Quaternion(sysRotation.X, sysRotation.Y, sysRotation.Z, sysRotation.W);
Pose pose = new Pose(position, rotation);
SimpleConsole.AddLine(trace, $"Adjusted {pose}");
return(pose);
}
/// <summary>
/// Cache the coordinate system for the QR code's spatial node, and the root.
/// </summary>
/// <returns></returns>
private bool CheckCoordinateSystem()
{
#if WLT_LEGACY_WSA
if (coordinateSystem == null)
{
SimpleConsole.AddLine(trace, $"Creating coord for {spatialNodeId}");
coordinateSystem = global::Windows.Perception.Spatial.Preview.SpatialGraphInteropPreview.CreateCoordinateSystemForNode(SpatialNodeId);
SimpleConsole.AddLine(trace, $"{spatialNodeId} create coord {(coordinateSystem == null ? "FAILED" : "success")}");
}
if (rootCoordinateSystem == null)
{
rootCoordinateSystem = System.Runtime.InteropServices.Marshal.GetObjectForIUnknown(
UnityEngine.XR.WSA.WorldManager.GetNativeISpatialCoordinateSystemPtr()
) as SpatialCoordinateSystem;
SimpleConsole.AddLine(trace, $"Getting Legacy root coordinate system {(rootCoordinateSystem == null ? "null" : "succeeded")}");
}
return(coordinateSystem != null);
#elif WLT_SPATIAL_GRAPH_NODE
if (spatialGraphNode == null)
{
spatialGraphNode = SpatialGraphNode.FromStaticNodeId(SpatialNodeId);
}
return(spatialGraphNode != null);
#else // WINDOWS_UWP
return(false);
#endif // WINDOWS_UWP
}
public async void DoClear()
{
working = true;
SetColors(Color.black);
SimpleConsole.AddLine(8, $"Clear cube, binder is {(binder == null ? "null" : binder.Name)}");
if (binder != null)
{
if (bindingOracle != null)
{
SimpleConsole.AddLine(8, $"Getting from {bindingOracle.Name}");
bindingOracle.Get(binder);
}
await binder.Clear();
if (bindingOracle != null)
{
SimpleConsole.AddLine(8, $"Putting empty binder to {bindingOracle.Name}");
bindingOracle.Put(binder);
}
}
SimpleConsole.AddLine(8, $"Finished.");
await ChangeColorForSeconds(finishSeconds, Color.green);
working = true;
}
public async void DoPurge()
{
working = true;
SetColors(Color.black);
SimpleConsole.AddLine(8, $"Purge cube, binder is {(binder == null ? "null" : binder.Name)}");
if (binder != null)
{
SimpleConsole.AddLine(8, $"Starting clear from {binder.Name}");
await binder.Clear();
SimpleConsole.AddLine(8, $"Starting purge from {binder.Name}");
await binder.Purge();
if (bindingOracle != null)
{
bindingOracle.Put(binder);
}
}
SimpleConsole.AddLine(8, $"Finished.");
await ChangeColorForSeconds(finishSeconds, Color.green);
working = false;
}
/// <summary>
/// Record whether the QRCodeWatcher reports itself as supported, and request access.
/// </summary>
private async void Start()
{
_isSupported = QRCodeWatcher.IsSupported();
_capabilityTask = QRCodeWatcher.RequestAccessAsync();
_accessStatus = await _capabilityTask;
_capabilityInitialized = true;
SimpleConsole.AddLine(log, $"Requested caps, access: {_accessStatus.ToString()}");
}
/// <summary>
/// Release all resources. Package is unusable after Release.
/// </summary>
public void Release()
{
SimpleConsole.AddLine(log, $"Release SpacePin {spacePin.name}");
Destroy(spacePin);
spacePin = null;
Destroy(highlightProxy);
highlightProxy = null;
}
/// <summary>
/// Capture a Removed event for later call on main thread.
/// </summary>
/// <param name="sender">Ignored.</param>
/// <param name="args">Args containing relevant QRCode.</param>
private void OnQRCodeRemovedEvent(object sender, QRCodeRemovedEventArgs args)
{
SimpleConsole.AddLine(trace, $"Removing {args.Code.Data}");
lock (pendingActions)
{
pendingActions.Enqueue(new PendingQRCode(PendingQRCode.QRAction.Remove, args.Code));
}
}
/// <summary>
/// Record whether the QRCodeWatcher reports itself as supported, and request access.
/// </summary>
private async void Start()
{
isSupported = QRCodeWatcher.IsSupported();
var capabilityTask = QRCodeWatcher.RequestAccessAsync();
accessStatus = await capabilityTask;
SimpleConsole.AddLine(log, $"Requested caps, access: {accessStatus.ToString()}");
}
/// <summary>
/// Capture the Enumeration Ended event for later call on main thread.
/// </summary>
/// <param name="sender">Ignored.</param>
/// <param name="e">Ignored.</param>
private void OnQREnumerationEnded(object sender, object e)
{
SimpleConsole.AddLine(log, "Enumerated");
lock (pendingActions)
{
pendingActions.Enqueue(new PendingQRCode(PendingQRCode.QRAction.Enumerated, null));
}
}
/// <summary>
/// Become active.
/// </summary>
private void OnEnable()
{
CheckComponents();
SetUpCallbacks();
SetUpSpacePins();
SimpleConsole.AddLine(trace, "QRSpacePin Enabled");
}
/// <summary>
/// Reset package to initial state. If space pin has been committed, it will be rescinded.
/// </summary>
public void Reset()
{
SimpleConsole.AddLine(log, $"Reset SpacePin {spacePin.name}");
spacePin.Reset();
Destroy(highlightProxy);
highlightProxy = null;
isSet = false;
}
/// <summary>
/// Unregister from callbacks.
/// </summary>
private void TearDownCallbacks()
{
miniManager.OnQRAdded -= OnQRCodeAdded;
miniManager.OnQRUpdated -= OnQRCodeUpdated;
miniManager.OnQRRemoved -= OnQRCodeRemoved;
miniManager.OnQREnumerated -= OnQRCodeEnumerated;
miniManager = null;
SimpleConsole.AddLine(trace, "Callbacks torn down");
}
/// <inheritdoc/>
public async Task <bool> Download()
{
if (!IsReady)
{
return(false);
}
bool allSuccessful = true;
List <SpacePinPegAndProps> readObjects = new List <SpacePinPegAndProps>();
List <CloudAnchorId> cloudAnchorList = new List <CloudAnchorId>();
Dictionary <CloudAnchorId, SpacePinASA> spacePinByCloudId = new Dictionary <CloudAnchorId, SpacePinASA>();
foreach (var spacePin in spacePins)
{
int bindingIdx = FindBindingBySpacePinId(spacePin.SpacePinId);
if (bindingIdx >= 0)
{
string cloudAnchorId = bindings[bindingIdx].cloudAnchorId;
cloudAnchorList.Add(cloudAnchorId);
spacePinByCloudId[cloudAnchorId] = spacePin;
}
}
if (cloudAnchorList.Count > 0)
{
var found = await publisher.Read(cloudAnchorList);
if (found != null)
{
foreach (var keyVal in found)
{
var cloudAnchorId = keyVal.Key;
var spacePin = spacePinByCloudId[cloudAnchorId];
var pegAndProps = keyVal.Value;
Debug.Assert(pegAndProps.localPeg != null);
readObjects.Add(new SpacePinPegAndProps()
{
spacePin = spacePin, pegAndProps = pegAndProps
});
}
}
else
{
SimpleConsole.AddLine(ConsoleHigh, $"publisher Read returned null looking for {cloudAnchorList.Count} ids");
}
}
var wltMgr = WorldLockingManager.GetInstance();
foreach (var readObj in readObjects)
{
Pose lockedPose = wltMgr.LockedFromFrozen.Multiply(readObj.pegAndProps.localPeg.GlobalPose);
SimpleConsole.AddLine(ConsoleLow, $"Dwn: {lockedPose.ToString("F3")}");
readObj.spacePin.SetLockedPose(lockedPose);
readObj.spacePin.SetLocalPeg(readObj.pegAndProps.localPeg);
}
return(allSuccessful);
}
/// <summary>
/// Register for callbacks on QR code events. These callbacks will happen on the main thread.
/// </summary>
private void SetUpCallbacks()
{
Debug.Assert(miniManager != null, "Expected required component QRCodeMiniManager");
miniManager.OnQRAdded += OnQRCodeAdded;
miniManager.OnQRUpdated += OnQRCodeUpdated;
miniManager.OnQRRemoved += OnQRCodeRemoved;
miniManager.OnQREnumerated += OnQRCodeEnumerated;
SimpleConsole.AddLine(trace, "Callbacks SetUp");
}
/// <summary>
/// Process a newly removed QR code.
/// </summary>
/// <param name="qrCode">The qr code to process.</param>
private void OnQRCodeRemoved(QRCode qrCode)
{
SimpleConsole.AddLine(trace, $"OnQRCodeRemoved {qrCode.Data}");
int idx = ExtractIndex(qrCode);
if (!QRCodeIndexValid(idx))
{
return;
}
spacePins[idx].Reset();
}
/// <summary>
/// Process a newly updated QR code.
/// </summary>
/// <param name="qrCode">The qr code to process.</param>
private void OnQRCodeUpdated(QRCode qrCode)
{
SimpleConsole.AddLine(trace, $"OnAdded {qrCode.Data}, enumerated {enumerationFinished}");
if (enumerationFinished)
{
int idx = ExtractIndex(qrCode);
if (!QRCodeIndexValid(idx))
{
return;
}
spacePins[idx].Update(qrCode);
}
}
/// <summary>
/// Record whether the QRCodeWatcher reports itself as supported, and request access.
/// </summary>
/// <remarks>
/// If the camera permission has not already been granted (GetPermissions has successfully completed),
/// then the call to QRCodeWather.RequestAccessAsync will never return, even after the user grants permissions.
/// See https://github.com/microsoft/MixedReality-WorldLockingTools-Samples/issues/20
/// </remarks>
private async void Start()
{
isSupported = QRCodeWatcher.IsSupported();
SimpleConsole.AddLine(log, $"QRCodeWatcher.IsSupported={isSupported}");
bool gotPermission = await GetPermissions();
if (gotPermission)
{
var capabilityTask = QRCodeWatcher.RequestAccessAsync();
accessStatus = await capabilityTask;
SimpleConsole.AddLine(log, $"Requested caps, access: {accessStatus.ToString()}");
}
}
/// <summary>
/// Commit the pose to the SpacePin system, deploying the highlight marker if one is specified.
/// </summary>
/// <param name="frozenPose">New pose in frozen space.</param>
/// <param name="lockedPose">New pose in locked space.</param>
/// <returns>True if pose successfully committed to SpacePin system.</returns>
private bool CommitPose(Pose frozenPose, Pose lockedPose)
{
SimpleConsole.AddLine(trace, $"Commit to {spacePin.name} F:{frozenPose} L:{lockedPose} S:{spacePin.transform.GetGlobalPose()}");
spacePin.SetFrozenPose(frozenPose);
DeployProxy();
isSet = true;
lastLockedPose = lockedPose;
SimpleConsole.AddLine(trace, "Deployed");
return(true);
}
/// <summary>
/// Create a new space pin package.
/// </summary>
/// <param name="owner">The owning space pin group.</param>
/// <param name="virtualObject">Corresponding virtual object (for pose) in the scene.</param>
/// <returns>The created package.</returns>
/// <remarks>
/// The created space pin package is ready to deploy, but currently idle.
/// </remarks>
public static SpacePinPackage Create(QRSpacePinGroup owner, Transform virtualObject)
{
SimpleConsole.AddLine(log, $"CreatePinPackage on {virtualObject.name}");
SpacePinPackage package = new SpacePinPackage();
package.spacePin = virtualObject.gameObject.AddComponent <SpacePinOrientable>();
package.spacePin.Orienter = owner.orienter;
package.highlightProxy = null;
package.highlightPrefab = owner.markerHighlightPrefab;
package.coordinateSystem = new QRSpatialCoord();
return(package);
}
/// <summary>
/// Determine whether a space pin has necessary setup to be published.
/// </summary>
/// <param name="spacePin">The space pin to check.</param>
/// <returns>True if the space pin can be published.</returns>
private bool IsReadyForPublish(SpacePinASA spacePin)
{
if (spacePin == null)
{
SimpleConsole.AddLine(ConsoleHigh, $"Getting null space pin to check ready for publish.");
return(false);
}
if (spacePin.Publisher != publisher)
{
SimpleConsole.AddLine(ConsoleHigh, $"SpacePin={spacePin.SpacePinId} has different publisher than binder={name}.");
return(false);
}
return(spacePin.IsReadyForPublish);
}
/// <summary>
/// Attempt to retrieve the current transform matrix.
/// </summary>
/// <returns>Non-null matrix on success.</returns>
private System.Numerics.Matrix4x4? GetNewMatrix()
{
Debug.Assert(rootCoordinateSystem != null);
// Get the relative transform from the unity origin
System.Numerics.Matrix4x4?newMatrix = coordinateSystem.TryGetTransformTo(rootCoordinateSystem);
SimpleConsole.AddLine(trace, $"Got new matrix {(newMatrix == null ? "null" : newMatrix.ToString())}");
if (newMatrix == null)
{
SimpleConsole.AddLine(log, "Coord: Got null newMatrix");
}
return(newMatrix);
}
/// <summary>
/// Accept the local peg assigned by the binder after it's been downloaded from the cloud.
/// </summary>
/// <param name="peg">The local peg to take.</param>
public void SetLocalPeg(ILocalPeg peg)
{
if (peg?.Name == localPeg?.Name)
{
SimpleConsole.AddLine(ConsoleHigh, $"Redundant SLP: {name} {peg.Name}");
return;
}
if (localPeg != null)
{
SimpleConsole.AddLine(ConsoleHigh, $"SLP release {localPeg?.Name} take {peg?.Name}");
Publisher.ReleaseLocalPeg(localPeg);
}
localPeg = peg;
SimpleConsole.AddLine(ConsoleLow, $"SLP: {name} - {localPeg.GlobalPose.position.ToString("F3")}");
}
/// <summary>
/// Attempt to set a space pin from the QR code.
/// </summary>
/// <param name="qrCode">The source QR code.</param>
/// <returns>True if a space pin was set from the current data.</returns>
/// <remarks>
/// Returning false does not necessarily mean an error occurred. For example, if the space pin
/// has already been set from the given QR code, and the location hasn't changed, no action
/// will be taken and the return value will be false. Or if the coordinate system is unable
/// to resolve the transform to global space, again the return will be false, indicating
/// trying again later.
/// </remarks>
public bool Update(QRCode qrCode)
{
SimpleConsole.AddLine(trace, $"Update SpacePin {(coordinateSystem == null ? "null" : coordinateSystem.SpatialNodeId.ToString())}");
coordinateSystem.SpatialNodeId = qrCode.SpatialGraphNodeId;
sizeMeters = qrCode.PhysicalSideLength;
Pose spongyPose;
if (!coordinateSystem.ComputePose(out spongyPose))
{
return(false);
}
Pose frozenPose = WorldLockingManager.GetInstance().FrozenFromSpongy.Multiply(spongyPose);
return(UpdatePose(frozenPose));
}
/// <summary>
/// Compute the head relative pose for the spatial node id.
/// </summary>
/// <param name="pose">If return value is true, the newly computed pose, else the last pose computed.</param>
/// <returns>True if a new pose was successfully computed.</returns>
/// <remarks>
/// This ultimately relies on SpatialCoordinateSystem.TryGetTransformTo.
/// TryGetTransformTo seems to fail for a while after the QR code is created.
/// Or maybe just spurious failure. Haven't found any documentation on behavior so far.
/// Main thing is to be prepared for failure, and just try back until success.
/// </remarks>
public bool ComputePose(out Pose pose)
{
SimpleConsole.AddLine(trace, "ComputePose");
if (CheckActive())
{
System.Numerics.Matrix4x4?newMatrix = GetNewMatrix();
if (newMatrix != null)
{
CurrentPose = AdjustNewMatrix(newMatrix.Value);
pose = CurrentPose;
return(true);
}
}
pose = CurrentPose;
return(false);
}
/// <summary>
/// Create a local peg based on current state (LockedPose).
/// </summary>
/// <remarks>
/// This typically happens when the SpacePinASA is locally manipulated into a new pose.
/// </remarks>
public async void ConfigureLocalPeg()
{
if (Publisher == null)
{
SimpleConsole.AddLine(ConsoleHigh, $"Publisher hasn't been set on SpacePin={name}");
return;
}
if (localPeg != null)
{
SimpleConsole.AddLine(ConsoleHigh, $"Releasing existing peg {name}");
Publisher.ReleaseLocalPeg(localPeg);
}
localPeg = await Publisher.CreateLocalPeg($"{SpacePinId}_peg", LockedPose);
SimpleConsole.AddLine(ConsoleLow, $"CLP: {name} - {localPeg.GlobalPose.position.ToString("F3")}");
}
/// <summary>
/// Create the QRCodeWatcher instance and register for the events to be transported to the main thread.
/// </summary>
private void SetUpQRWatcher()
{
try
{
qrWatcher = new QRCodeWatcher();
qrWatcher.Added += OnQRCodeAddedEvent;
qrWatcher.Updated += OnQRCodeUpdatedEvent;
qrWatcher.Removed += OnQRCodeRemovedEvent;
qrWatcher.EnumerationCompleted += OnQREnumerationEnded;
qrWatcher.Start();
}
catch (System.Exception e)
{
Debug.LogError($"Failed to start QRCodeWatcher, error: {e.Message}");
}
SimpleConsole.AddLine(log, $"SetUpQRWatcher {(qrWatcher != null ? "Success" : "Failed")}");
}
private bool CheckSetup()
{
binder = spacePinBinder;
bindingOracle = spacePinBinderFile;
bool good = true;
if (binder == null)
{
SimpleConsole.AddLine(11, $"Missing Space Pin Binder on {name}");
good = false;
}
if (bindingOracle == null)
{
SimpleConsole.AddLine(11, $"Missing binding oracle (Space Pin Binder File) on {name}");
good = false;
}
return(good);
}
/// <summary>
/// Determine if the new pose should be forwarded to the SpacePin system.
/// </summary>
/// <param name="lockedPose">The pose to test.</param>
/// <returns>True if sending the new pose is indicated.</returns>
/// <remarks>
/// If the pose hasn't been sent yet, it will always be indicated to send.
/// It is unusual that the position of the QR code as measured by the system changes
/// significantly enough to be worth resending. It usually only occurs when the first
/// reading was faulty (e.g. during a rapid head move).
/// </remarks>
private bool NeedCommit(Pose lockedPose)
{
if (!isSet)
{
SimpleConsole.AddLine(log, "Need commit because unset.");
return(true);
}
float RefreshThreshold = 0.01f; // one cm?
float distance = Vector3.Distance(lockedPose.position, lastLockedPose.position);
if (distance > RefreshThreshold)
{
SimpleConsole.AddLine(log, $"Need commit because new distance {distance}");
return(true);
}
SimpleConsole.AddLine(trace, $"No commit");
return(false);
}
/// <summary>
/// Publish the spacePin.
/// </summary>
/// <param name="spacePin">SpacePinASA to publish</param>
/// <returns>True on success.</returns>
/// <remarks>
/// It may be this should be a private member.
/// </remarks>
public async Task <bool> Publish(SpacePinASA spacePin)
{
if (!IsReady)
{
// mafinc - Should we wait until it is ready? Maybe as a binder option?
return(false);
}
int idx = FindSpacePin(spacePin);
if (idx < 0)
{
Debug.LogError($"Trying to publish unknown space pin. Must be added in inspector or AddSpacePin() first.");
return(false);
}
int cloudIdx = FindBindingBySpacePinId(spacePin.SpacePinId);
if (cloudIdx >= 0)
{
SimpleConsole.AddLine(ConsoleHigh, $"Publishing previously published space pin={spacePin.SpacePinId}, deleting from cloud first.");
await publisher.Delete(bindings[cloudIdx].cloudAnchorId);
RemoveBinding(spacePin.SpacePinId);
}
var obj = ExtractForPublisher(spacePin);
if (obj == null)
{
return(false);
}
CloudAnchorId cloudAnchorId = await publisher.Create(obj);
if (string.IsNullOrEmpty(cloudAnchorId))
{
Debug.LogError($"Failed to create cloud anchor for {spacePin.SpacePinId}");
return(false);
}
SetBinding(spacePin.SpacePinId, cloudAnchorId);
return(true);
}
public async void DoReset()
{
working = true;
SetColors(Color.black);
SimpleConsole.AddLine(8, $"Reset pins, binder is {(binder == null ? "null" : binder.Name)}");
var spacePinBinder = binder as SpacePinBinder;
if (spacePinBinder != null)
{
var spacePins = spacePinBinder.SpacePins;
foreach (var spacePin in spacePins)
{
spacePin.Reset();
SimpleConsole.AddLine(8, $"Reseting spacePin={spacePin.SpacePinId}");
}
}
SimpleConsole.AddLine(8, $"Finished.");
await ChangeColorForSeconds(finishSeconds, Color.green);
working = false;
}
