private bool DeadRecoknedPositionOrRotationIsWrong()
{
GeocentricCoord deadReckonedPosition;
Vector3Float deadReckonedRotation;
DeadReckoningCalculator.Calculate(_lastSentState, Time.time - _lastSendTime, out deadReckonedPosition, out deadReckonedRotation);
double positionDeltaSquared = Math.Pow(deadReckonedPosition.X - State.Location.X, 2) + Math.Pow(deadReckonedPosition.Y - State.Location.Y, 2) + Math.Pow(deadReckonedPosition.Z - State.Location.Z, 2);
if (positionDeltaSquared > MovementThreshold * MovementThreshold)
{
return(true);
}
var deadRecoknedLatLon = (LatLonCoord)CoordConverter.GeocentricToGeodetic(deadReckonedPosition);
var currentLatLon = (LatLonCoord)CoordConverter.GeocentricToGeodetic((GeocentricCoord)State.Location);
Vector3 deadReckonedEuler = CoordConverter.OrientationToUnityEuler(deadReckonedRotation, deadRecoknedLatLon);
Vector3 currentEuler = CoordConverter.OrientationToUnityEuler(State.Orientation, currentLatLon);
float rotationDelta = Quaternion.Angle(Quaternion.Euler(deadReckonedEuler), Quaternion.Euler(currentEuler));
if (rotationDelta > RotationThreshold)
{
return(true);
}
return(false);
}
public void GetDeadRecoknedPositionAndRotation(out Vector3 position, out Quaternion rotation)
{
float elapsedTime = TimeSinceLastHeartbeat;
GeocentricCoord location;
Vector3Float orientation;
DeadReckoningCalculator.Calculate(State, elapsedTime, out location, out orientation);
var geod = CoordConverter.GeocentricToGeodetic(location);
var databaseLocation = CoordConverter.GeodeticToDatabase(geod);
position = (Vector3)databaseLocation;
rotation = Quaternion.Euler(CoordConverter.OrientationToUnityEuler(orientation, (LatLonCoord)geod));
}