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); } } }