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