private void OnMovementFromHeadingUpdated(object sender, pLab_HeadingFromMovementUpdatedEventArgs e)
{
if (arHeadingText != null)
{
arHeadingText.text = string.Format("AR Heading: {0} deg", e.arHeading);
}
if (gpsHeadingText != null)
{
gpsHeadingText.text = string.Format("GPS Heading: {0} deg", e.gpsHeading);
}
if (headingDifferenceText != null)
{
headingDifferenceText.text = string.Format("GPS-AR {0} / {1} / {2} deg", e.headingDifference, e.medianHeadingDifference, e.averageHeadingDifference);
}
}
/// <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);
}
}
}