public static SqlDouble[] DecodeGeohash(SqlString geohash)
{
ISpatialCoordinate coord = Geohash.DecodeGeoHash(geohash.ToString().ToCharArray());
SqlDouble[] coordinate = { new SqlDouble((double)coord.Latitude), new SqlDouble((double)coord.Longitude) };
return(coordinate);
}
bool ISpatialCoordinateService.TryGetKnownCoordinate(string id, out ISpatialCoordinate spatialCoordinate)
{
if (!TryParse(id, out TKey key))
{
throw new ArgumentException($"Id {id} is not recognized by this coordinate service.");
}
return(TryGetKnownCoordinate(key, out spatialCoordinate));
}
/// <summary>
/// Reads the GPS coordinate.
/// </summary>
private void ReadGPSCoordinate()
{
if (this.PhotoBitmap != null)
{
ISpatialCoordinate coord = null;
Bitmap photo = this.PhotoBitmap;
// Extract exif metadata
ExifFile file = ExifFile.Read(this.FilePath);
foreach (ExifProperty exifProperty in file.Properties)
{
Trace.WriteLine(exifProperty.Name.ToString() + "=" + exifProperty.Value.ToString());
}
if (file.Properties.ContainsKey(ExifTag.GPSImgDirection))
{
this.SetImageDirection(file.Properties[ExifTag.GPSImgDirection].Value.ToString());
}
if (file.Properties.ContainsKey(ExifTag.GPSLatitude) && file.Properties.ContainsKey(ExifTag.GPSLongitude))
{
float lon;
float lat;
GPSLatitudeLongitude latitude = (GPSLatitudeLongitude)file.Properties[ExifTag.GPSLatitude];
GPSLatitudeLongitude longitude = (GPSLatitudeLongitude)file.Properties[ExifTag.GPSLongitude];
lon = longitude.ToFloat();
lat = latitude.ToFloat();
if (file.Properties[ExifTag.GPSLongitudeRef].Value.ToString().StartsWith("W", StringComparison.CurrentCultureIgnoreCase))
{
lon = lon * -1;
}
if (file.Properties[ExifTag.GPSLatitudeRef].Value.ToString().StartsWith("S", StringComparison.CurrentCultureIgnoreCase))
{
lat = lat * -1;
}
if (lat != 0 & lon != 0)
{
coord = new Umbriel.GIS.Geohash.Coordinate(lon, lat);
this.Coordinate = coord;
}
}
}
else
{
throw new NullReferenceException();
}
}
/// <summary>
/// Adds a coordinate to be tracked by this service.
/// </summary>
protected void OnNewCoordinate(TKey id, ISpatialCoordinate spatialCoordinate)
{
ThrowIfDisposed();
if (knownCoordinates.TryAdd(id, spatialCoordinate))
{
CoordinatedDiscovered?.Invoke(spatialCoordinate);
}
else
{
UnityEngine.Debug.LogWarning($"Unexpected behavior, coordinate {id} was rediscovered.");
}
}
/// <inheritdoc/>
public override async Task <ISpatialCoordinate> LocalizeAsync(CancellationToken cancellationToken)
{
ISpatialCoordinate coordinateToReturn = null;
using (var cancellableCTS = CancellationTokenSource.CreateLinkedTokenSource(defaultCancellationToken, cancellationToken))
{
if (configuration.IsCoordinateCreator)
{
localizer.DebugLog("User getting initialized coordinate");
coordinateToReturn = await coordinateService.TryCreateCoordinateAsync(localizer.anchorPosition, Quaternion.Euler(localizer.anchorRotation), cancellableCTS.Token);
if (coordinateToReturn != null)
{
localizer.DebugLog($"Sending coordinate id: {coordinateToReturn.Id}");
peerConnection.SendData(writer => writer.Write(coordinateToReturn.Id));
localizer.DebugLog("Message sent.");
}
else
{
Debug.LogError("Coordinate discovery returned null coordinate");
return(null);
}
}
else
{
localizer.DebugLog("Non-host waiting for coord id to be sent over");
string coordinateIdentifier = await coordinateIdentifierTaskSource.Task.Unless(cancellableCTS.Token);
if (!cancellableCTS.Token.IsCancellationRequested)
{
localizer.DebugLog($"Coordinate id: {coordinateIdentifier}, starting discovery.");
if (await coordinateService.TryDiscoverCoordinatesAsync(cancellableCTS.Token, coordinateIdentifier))
{
localizer.DebugLog("Discovery complete, retrieving reference to ISpatialCoordinate");
if (!coordinateService.TryGetKnownCoordinate(coordinateIdentifier, out coordinateToReturn))
{
Debug.LogError("We discovered, but for some reason failed to get coordinate from service.");
}
}
else
{
Debug.LogError("Failed to discover spatial coordinate.");
}
}
}
}
return(coordinateToReturn);
}
/// <inheritdoc />
public override async Task <ISpatialCoordinate> LocalizeAsync(CancellationToken cancellationToken)
{
DebugLog($"Localizing, CanBeCanceled:{cancellationToken.CanBeCanceled}, IsCancellationRequested:{cancellationToken.IsCancellationRequested}");
ISpatialCoordinate coordinate = null;
using (var cancellableCTS = CancellationTokenSource.CreateLinkedTokenSource(defaultCancellationToken, cancellationToken))
{
if (!TrySendMarkerVisualDiscoveryMessage())
{
Debug.LogWarning("Failed to send marker visual discovery message, spatial localization failed.");
return(null);
}
// Receive marker to show
DebugLog("Waiting to have a coordinate id assigned");
await Task.WhenAny(coordinateAssigned.Task, Task.Delay(-1, cancellableCTS.Token));
if (string.IsNullOrEmpty(coordinateId))
{
DebugLog("Failed to assign coordinate id");
return(null);
}
using (var cts = CancellationTokenSource.CreateLinkedTokenSource(discoveryCTS.Token, cancellableCTS.Token))
{
DebugLog($"Attempting to discover coordinate: {coordinateId}, CanBeCanceled:{cts.Token.CanBeCanceled}, IsCancellationRequested:{cts.Token.IsCancellationRequested}");
if (await coordinateService.TryDiscoverCoordinatesAsync(cts.Token, new string[] { coordinateId.ToString() }))
{
DebugLog($"Coordinate discovery completed: {coordinateId}");
if (!coordinateService.TryGetKnownCoordinate(coordinateId, out coordinate))
{
DebugLog("Failed to find spatial coordinate although discovery completed.");
}
}
else
{
DebugLog("TryDiscoverCoordinatesAsync failed.");
}
}
DebugLog($"Waiting for coordinate to be found: {coordinateId}");
await Task.WhenAny(coordinateFound.Task, Task.Delay(-1, cancellableCTS.Token));
}
return(coordinate);
}
/// <inheritdoc />
public override async Task <ISpatialCoordinate> LocalizeAsync(CancellationToken cancellationToken)
{
if (!defaultCancellationToken.CanBeCanceled)
{
Debug.LogError("Session is invalid. No localization performed.");
return(null);
}
DebugLog($"Waiting for marker visual, CanBeCanceled:{cancellationToken.CanBeCanceled}, IsCancellationRequested:{cancellationToken.IsCancellationRequested}");
using (var cancellableCTS = CancellationTokenSource.CreateLinkedTokenSource(defaultCancellationToken, cancellationToken))
{
await Task.WhenAny(coordinateAssigned.Task, Task.Delay(-1, cancellableCTS.Token));
if (string.IsNullOrEmpty(coordinateId))
{
DebugLog("Failed to assign coordinate id");
return(null);
}
ISpatialCoordinate coordinate = null;
using (var cts = CancellationTokenSource.CreateLinkedTokenSource(discoveryCTS.Token, cancellableCTS.Token))
{
DebugLog($"Attempting to discover coordinate: {coordinateId}, CanBeCanceled:{cts.Token.CanBeCanceled}, IsCancellationRequested:{cts.Token.IsCancellationRequested}");
if (await coordinateService.TryDiscoverCoordinatesAsync(cts.Token, new string[] { coordinateId.ToString() }))
{
DebugLog($"Coordinate discovery completed: {coordinateId}");
if (!coordinateService.TryGetKnownCoordinate(coordinateId, out coordinate))
{
DebugLog("Failed to find spatial coordinate although discovery completed.");
}
else
{
SendCoordinateFound(coordinate.Id);
return(coordinate);
}
}
else
{
DebugLog("TryDiscoverCoordinatesAsync failed.");
}
}
}
return(null);
}
/// <inheritdoc />
public override async Task <ISpatialCoordinate> LocalizeAsync(CancellationToken cancellationToken)
{
localizer.DebugLog("Getting host coordinate");
ISpatialCoordinate spatialCoordinate = null;
using (var cancellableCTS = CancellationTokenSource.CreateLinkedTokenSource(defaultCancellationToken, cancellationToken))
{
await coordinateService.TryDiscoverCoordinatesAsync(cancellationToken, new int[] { settings.MarkerID });
if (!coordinateService.TryGetKnownCoordinate(settings.MarkerID, out spatialCoordinate))
{
Debug.LogError("Unexpected failure to discover a marker coordinate");
}
}
return(spatialCoordinate);
}
/// <inheritdoc />
public override async Task <ISpatialCoordinate> LocalizeAsync(CancellationToken cancellationToken)
{
if (!defaultCancellationToken.CanBeCanceled)
{
Debug.LogError("Session is invalid. No localization performed.");
return(null);
}
ISpatialCoordinate spatialCoordinate = null;
using (var cancellableCTS = CancellationTokenSource.CreateLinkedTokenSource(defaultCancellationToken, cancellationToken))
{
WorldAnchorCoordinateService coordinateService = await localizer.coordinateServiceTask.Unless(cancellationToken);
if (cancellationToken.IsCancellationRequested)
{
return(null);
}
if (settings.Mode == WorldAnchorLocalizationMode.LocateExistingAnchor)
{
if (coordinateService.TryGetKnownCoordinate(settings.AnchorId, out ISpatialCoordinate coordinate))
{
return(coordinate);
}
else
{
return(null);
}
}
else
{
spatialCoordinate = await coordinateService.CreateCoordinateAsync(settings.AnchorId, settings.AnchorPosition, settings.AnchorRotation, cancellationToken);
}
}
return(await Task.FromResult(spatialCoordinate));
}
/// <inheritdoc />
public override async Task <ISpatialCoordinate> LocalizeAsync(CancellationToken cancellationToken)
{
localizer.DebugLog("Getting host coordinate");
ISpatialCoordinate spatialCoordinate = null;
using (var cancellableCTS = CancellationTokenSource.CreateLinkedTokenSource(defaultCancellationToken, cancellationToken))
{
await coordinateService.TryDiscoverCoordinatesAsync(cancellationToken, new int[] { settings.TopMarkerID, settings.MiddleMarkerID, settings.BottomMarkerID });
if (!coordinateService.TryGetKnownCoordinate(settings.TopMarkerID, out ISpatialCoordinate topSpatialCoordinate) ||
!coordinateService.TryGetKnownCoordinate(settings.MiddleMarkerID, out ISpatialCoordinate middleSpatialCoordinate) ||
!coordinateService.TryGetKnownCoordinate(settings.BottomMarkerID, out ISpatialCoordinate bottomSpatialCoordinate))
{
Debug.LogError("Unexpected failure to discover a marker coordinate");
}
else
{
var topPosition = topSpatialCoordinate.CoordinateToWorldSpace(Vector3.zero);
var middlePosition = middleSpatialCoordinate.CoordinateToWorldSpace(Vector3.zero);
var bottomPosition = bottomSpatialCoordinate.CoordinateToWorldSpace(Vector3.zero);
// Find the point that is at the T intersection of the three markers by projecting the middle marker
// onto the top-bottom line segment
var markerIntersection = bottomPosition + Vector3.Project(middlePosition - bottomPosition, topPosition - bottomPosition);
// Create a stable rotation for the overall marker using the plane formed by the three
// individual markers. The order of points passed to the Plane constructor is important to
// ensure the normal direction is the correct direction to create the quaternion.
var markerPlane = new Plane(topPosition, middlePosition, bottomPosition);
var markerRotation = Quaternion.LookRotation(markerPlane.normal, bottomPosition - topPosition);
spatialCoordinate = new SpatialCoordinate(markerIntersection, markerRotation);
}
}
return(spatialCoordinate);
}
/// <inheritdoc />
public bool TryGetKnownCoordinate(TKey id, out ISpatialCoordinate spatialCoordinate)
{
return(knownCoordinates.TryGetValue(id, out spatialCoordinate));
}
/// <summary>
/// Converst coordinate space position to world space position.
/// </summary>
public static UQuaternion CoordinateToWorldSpace(this ISpatialCoordinate coordinate, UQuaternion quaternion)
{
return(coordinate.CoordinateToWorldSpace(quaternion.AsNumericsQuaternion()).AsUnityQuaternion());
}
/// <summary>
/// Converst coordinate space position to world space position.
/// </summary>
public static UVector3 CoordinateToWorldSpace(this ISpatialCoordinate coordinate, UVector3 vector)
{
return(coordinate.CoordinateToWorldSpace(vector.AsNumericsVector()).AsUnityVector());
}
public static SqlDouble GeohashLatitude(SqlString geohash)
{
ISpatialCoordinate coord = Geohash.DecodeGeoHash(geohash.ToString().ToCharArray());
return(new SqlDouble((double)coord.Latitude));
}
