예제 #1
0
        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);
        }
예제 #2
0
        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));
        }