private void OnLocationUpdated(object sender, pLab_LocationUpdatedEventArgs e)
{
if (locationText != null)
{
locationText.text = string.Format("Loc: {0},{1}", e.location.Lat, e.location.Lon);
locationText.color = Color.red;
StartCoroutine(LocationTextToDefault());
}
if (locationAccuracyText != null)
{
locationAccuracyText.text = string.Format("Loc. acc H:{0} V:{0}", e.horizontalAccuracy, e.verticalAccuracy);
}
if (locationDistanceChangeText != null)
{
float locationChange = 0f;
if (previousLocation != null)
{
locationChange = pLab_GeoTools.DistanceBetweenPoints(previousLocation, e.location);
}
else
{
previousLocation = new pLab_LatLon();
}
locationDistanceChangeText.text = string.Format("Loc. distance change: {0}m", locationChange);
previousLocation.Lat = e.location.Lat;
previousLocation.Lon = e.location.Lon;
}
}
/// <summary>
/// Get the "distance" between two points using UTM
/// </summary>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <returns></returns>
public static Vector2 UTMDifferenceBetweenPoints(pLab_LatLon startPoint, pLab_LatLon endPoint)
{
double UTMNorthingCurrent = 0;
double UTMEastingCurrent = 0;
double UTMNorthingPOI = 0;
double UTMEastingPOI = 0;
pLab_GeoTools.LatLongtoUTM(startPoint.Lat, startPoint.Lon, out UTMNorthingCurrent, out UTMEastingCurrent);
pLab_GeoTools.LatLongtoUTM(endPoint.Lat, endPoint.Lon, out UTMNorthingPOI, out UTMEastingPOI);
return(new Vector2((float)(UTMEastingPOI - UTMEastingCurrent), (float)(UTMNorthingPOI - UTMNorthingCurrent)));
}
/// <summary>
/// Reset GPS-AR -mode related variables
/// </summary>
private void ResetGPSARMode()
{
if (resetAfterTrackingLost)
{
hasGPSARHeading = false;
manualOffset = 0;
previousGPSCoordinates = null;
previousGPSAccuracy = 9999f;
initialGPSARDifference = 0;
lastGPSARHeadingDeltas.Clear();
}
}
/// <summary>
/// Distance in meters to another point/location, calculated using Haversine.
/// </summary>
/// <param name="otherPoint"></param>
/// <returns></returns>
public float DistanceToPoint(pLab_LatLon otherPoint)
{
return(pLab_GeoTools.DistanceBetweenPointsPythagoras(this, otherPoint));
}
/// <summary>
/// Move all tracked POI-objects
/// </summary>
private void RecheckPOITrackings()
{
pLab_LatLon currentLocation = locationProvider.Location;
if (currentLocation == null)
{
return;
}
Vector3 currentARCameraPosition = arCameraTransform.position;
float devicePosY = currentARCameraPosition.y;
currentARCameraPosition.y = 0;
float groundLevel = deviceElevationEstimater != null ? deviceElevationEstimater.GroundLevelEstimate : 0f;
if (PointOfInterests == null || PointOfInterests.Count == 0)
{
return;
}
for (int i = 0; i < PointOfInterests.Count; i++)
{
pLab_PointOfInterest poi = PointOfInterests[i];
//Calculate the distance between the points
float distanceBetween = currentLocation.DistanceToPointPythagoras(poi.Coordinates);
bool onlyUpdateHeight = false;
//Don't update the position if already so close
//Maybe should actually search for a plane close to this and "anchor" it to that, or try to find nearest plane to get the height
//If close tracking -> don't update the position to avoid jitter and unability to inspect object closely
if (!poi.CloseTracking && distanceBetween <= poi.CloseTrackingRadius)
{
if (!poi.FarTracking)
{
CreateObjectsForPOI(poi);
}
poi.TrackingState = POITrackingState.CloseTracking;
}
else if (!poi.Tracking && distanceBetween <= poi.TrackingRadius)
{
poi.TrackingState = POITrackingState.FarTracking;
CreateObjectsForPOI(poi);
}
else if (poi.Tracking && distanceBetween >= poi.TrackingExitRadius)
{
StopTrackingPOI(poi);
}
else if (poi.CloseTracking)
{
if (distanceBetween >= poi.CloseTrackingExitRadius)
{
poi.TrackingState = POITrackingState.FarTracking;
}
else
{
//Close Tracking is true AND poi is inside the closeTrackingRadius
onlyUpdateHeight = true;
}
}
POITrackerData trackerData = poiTrackerDatas.Find(x => x.POI == poi);
if (trackerData == null)
{
continue;
}
float trueNorthHeadingDifference = arTrueNorthFinder != null ? arTrueNorthFinder.Heading : 0;
pLab_PointOfInterestCanvasBase poiCanvas = trackerData.POICanvas;
pLab_PointOfInterestObjectBase poiObject = trackerData.POIObject;
//If we only update the height, set the heights and continue to the next POI
if (onlyUpdateHeight)
{
if (poiObject != null)
{
poiObject.UpdatePositionY(groundLevel, devicePosY);
}
//Update poi's canvas position
if (poiCanvas != null)
{
poiCanvas.UpdatePositionY(groundLevel, devicePosY);
}
continue;
}
Vector3 newPos = Vector3.zero;
float bearing = 0;
switch (positioningMode)
{
case PositioningMode.DistanceAndBearing:
//Get the bearing relative to north (this is why it's important for the z-axis to face north)
bearing = pLab_GeoTools.BearingFromPointAToB(currentLocation, poi.Coordinates);
/**
* Usually it should be X = cos(bearing), Y = sin(bearing) but for some reason it has to flipped other way around
* So X = sin(bearing) and Z = cos(bearing)
*/
newPos = new Vector3(distanceBetween * Mathf.Sin(bearing), 0, distanceBetween * Mathf.Cos(bearing));
break;
case PositioningMode.UTM:
Vector2 UTMDifference = pLab_GeoTools.UTMDifferenceBetweenPoints(currentLocation, poi.Coordinates);
newPos = new Vector3(UTMDifference.x, 0, UTMDifference.y);
//For debugging
bearing = Vector3.SignedAngle(Vector3.forward, newPos.normalized, Vector3.up);
bearing = pLab_MathConversions.DegAngle0To360(bearing);
bearing = pLab_MathConversions.DegToRad(bearing);
//END debug
distanceBetween = newPos.magnitude;
break;
}
//Offset new position with current camera position
trackerData.LastUpdateCameraPosition = currentARCameraPosition;
trackerData.LastUpdatePositionRelativeToCamera = newPos;
//Offset with true north difference so Z-axis == AR True-North-Axis
newPos = currentARCameraPosition + (Quaternion.AngleAxis(trueNorthHeadingDifference, Vector3.up) * newPos);
if (poiObject != null)
{
poiObject.UpdatePosition(newPos, groundLevel, devicePosY);
poiObject.UpdateRotation(trueNorthHeadingDifference);
}
//Update poi's canvas position
if (poiCanvas != null)
{
poiCanvas.UpdatePosition(newPos, groundLevel, devicePosY);
poiCanvas.UpdateDistance(distanceBetween);
}
if (updateDebug)
{
pointOfInterestDebug.UpdateItem(poi, distanceBetween, bearing, newPos);
}
}
updateDebug = false;
}
public static float BearingFromPointAToB(pLab_LatLon startPoint, pLab_LatLon endPoint)
{
return(BearingFromPointAToB(startPoint.Lat, startPoint.Lon, endPoint.Lat, endPoint.Lon));
}
public static float DistanceBetweenPointsPythagoras(pLab_LatLon startPoint, pLab_LatLon endPoint)
{
return(DistanceBetweenPointsPythagoras(startPoint.Lat, startPoint.Lon, endPoint.Lat, endPoint.Lon));
}
/// <summary>
/// Calculates GPS-AR Mode headings
/// </summary>
/// <param name="e"></param>
private void CalculateGPSARHeadings(pLab_LocationUpdatedEventArgs e)
{
if (previousGPSCoordinates == null)
{
previousGPSCoordinates = e.location;
previousARCameraPosition = arCameraTransform.position;
return;
}
bool isHeadingUsable = false;
bool isGPSHeadingUsable = false;
float newARHeading = 0;
float newGPSHeading = 0;
Vector3 newPosition = arCameraTransform.position;
Vector3 positionDeltaVector = newPosition - previousARCameraPosition;
distance = positionDeltaVector.magnitude;
if (distance > minDistanceBeforeUpdate)
{
newARHeading = Vector3.SignedAngle(Vector3.forward, positionDeltaVector, Vector3.up);
newARHeading = pLab_MathConversions.DegAngleToPositive(newARHeading);
isHeadingUsable = true;
}
pLab_LatLon newGpsCoordinates = e.location;
float deltaAccuracy = e.horizontalAccuracy - previousGPSAccuracy;
if (e.horizontalAccuracy < movementMinimumGPSAccuracy || deltaAccuracy <= 5f)
{
double distanceBetween = pLab_GeoTools.DistanceBetweenPointsPythagoras(previousGPSCoordinates, newGpsCoordinates);
if (distanceBetween > minDistanceBeforeUpdate)
{
newGPSHeading = pLab_GeoTools.BearingFromPointAToBInDegrees(previousGPSCoordinates, newGpsCoordinates);
//Convert to 0 - 360
newGPSHeading = pLab_MathConversions.DegAngle0To360(newGPSHeading);
isGPSHeadingUsable = true;
}
}
if (isHeadingUsable && isGPSHeadingUsable)
{
arHeading = newARHeading;
gpsHeading = newGPSHeading;
previousGPSCoordinates = newGpsCoordinates;
previousARCameraPosition = newPosition;
previousGPSAccuracy = e.horizontalAccuracy;
float diff = CalculateGPSARHeadingDifference(arHeading, gpsHeading);
if (lastGPSARHeadingDeltas.Count == 0)
{
initialGPSARDifference = diff;
lastGPSARHeadingDeltas.Add(0);
}
else
{
if (lastGPSARHeadingDeltas.Count >= maxLastMovementHeadings)
{
lastGPSARHeadingDeltas.RemoveAt(0);
}
float readingDelta = diff - initialGPSARDifference;
if (readingDelta > 180)
{
readingDelta -= 360;
}
else if (readingDelta < -180)
{
readingDelta += 360;
}
lastGPSARHeadingDeltas.Add(readingDelta);
}
medianGPSARHeadingDifference = pLab_MathTools.GetMedian(lastGPSARHeadingDeltas) + initialGPSARDifference;
averageGPSARHeadingDifference = lastGPSARHeadingDeltas.Average() + initialGPSARDifference;
hasGPSARHeading = true;
RecalculateHeading();
RotateGPSARHeadingObject();
TriggerNorthHeadingUpdatedEvent(false);
if (OnHeadingFromMovementUpdated != null)
{
pLab_HeadingFromMovementUpdatedEventArgs eventArgs = new pLab_HeadingFromMovementUpdatedEventArgs()
{
arHeading = arHeading,
gpsHeading = gpsHeading,
headingDifference = diff,
medianHeadingDifference = medianGPSARHeadingDifference,
averageHeadingDifference = averageGPSARHeadingDifference
};
OnHeadingFromMovementUpdated(this, eventArgs);
}
}
}
