text: /****************************************************************************
   *
   *   Copyright (C) 2012-2021 PX4 Development Team. All rights reserved.
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions
   * are met:
   *
   * 1. Redistributions of source code must retain the above copyright
   *    notice, this list of conditions and the following disclaimer.
   * 2. Redistributions in binary form must reproduce the above copyright
   *    notice, this list of conditions and the following disclaimer in
   *    the documentation and/or other materials provided with the
   *    distribution.
   * 3. Neither the name PX4 nor the names of its contributors may be
   *    used to endorse or promote products derived from this software
   *    without specific prior written permission.
   *
   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
   * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
   * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   * POSSIBILITY OF SUCH DAMAGE.
   *
   ****************************************************************************/
  
  /**
   * @file geo.h
   *
   * Definition of geo / math functions to perform geodesic calculations
   *
   * @author Thomas Gubler <thomasgubler@student.ethz.ch>
   * @author Julian Oes <joes@student.ethz.ch>
   * @author Lorenz Meier <lm@inf.ethz.ch>
   * @author Anton Babushkin <anton.babushkin@me.com>
   * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
   */
  
  #pragma once
  
  #include <stdbool.h>
  #include <stdint.h>
  
  #include <lib/mathlib/mathlib.h>
  #include <lib/matrix/matrix/math.hpp>
  
  #ifndef hrt_absolute_time
  # define hrt_absolute_time() (0)
  #endif
  
  static constexpr float CONSTANTS_ONE_G = 9.80665f;						// m/s^2
  
  static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f;					// pascals (Pa)
  static constexpr float CONSTANTS_STD_PRESSURE_KPA = CONSTANTS_STD_PRESSURE_PA / 1000.0f;	// kilopascals (kPa)
  static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA /
  		100.0f;	// Millibar (mbar) (1 mbar = 100 Pa)
  
  static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C = 1.225f;				// kg/m^3
  static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f;					// J/(kg * K)
  static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f;				// °C
  
  static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000;					// meters (m)
  static constexpr float  CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH;		// meters (m)
  
  static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f;				// radians/second (rad/s)
  
  
  // XXX remove
  struct crosstrack_error_s {
  	bool past_end;		// Flag indicating we are past the end of the line/arc segment
  	float distance;		// Distance in meters to closest point on line/arc
  	float bearing;		// Bearing in radians to closest point on line/arc
  } ;
  
  
  /**
   * Returns the distance to the next waypoint in meters.
   *
   * @param lat_now current position in degrees (47.1234567°, not 471234567°)
   * @param lon_now current position in degrees (8.1234567°, not 81234567°)
   * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
   * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
   */
  float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
  
  /**
   * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
   * from waypoint A
   *
   * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
   * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
   * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
   * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
   * @param dist distance of target waypoint from waypoint A in meters (can be negative)
   * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
   * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
   */
  void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
  					double *lat_target, double *lon_target);
  
  /**
   * Creates a waypoint from given waypoint, distance and bearing
   * see http://www.movable-type.co.uk/scripts/latlong.html
   *
   * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
   * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
   * @param bearing in rad
   * @param distance in meters
   * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
   * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
   */
  void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
  					double *lat_target, double *lon_target);
  
  /**
   * Returns the bearing to the next waypoint in radians.
   *
   * @param lat_now current position in degrees (47.1234567°, not 471234567°)
   * @param lon_now current position in degrees (8.1234567°, not 81234567°)
   * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
   * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
   */
  float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
  
  void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
  				 float *v_e);
  
  void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
  				      float *v_e);
  
  void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
  				   double *lon_res);
  
  int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
  			 double lat_start, double lon_start, double lat_end, double lon_end);
  
  int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
  			double lat_center, double lon_center,
  			float radius, float arc_start_bearing, float arc_sweep);
  
  /*
   * Calculate distance in global frame
   */
  float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
  		double lat_next, double lon_next, float alt_next,
  		float *dist_xy, float *dist_z);
  
  /*
   * Calculate distance in local frame (NED)
   */
  float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
  		float x_next, float y_next, float z_next,
  		float *dist_xy, float *dist_z);
  
  
  /**
   * @brief C++ class for mapping lat/lon coordinates to local coordinated using a reference position
   */
  class MapProjection final
  {
  private:
  	uint64_t _ref_timestamp{0};
  	double _ref_lat{0.0};
  	double _ref_lon{0.0};
  	double _ref_sin_lat{0.0};
  	double _ref_cos_lat{0.0};
  	bool _ref_init_done{false};
  
  public:
  	/**
  	 * @brief Construct a new Map Projection object
  	 * The generated object will be uninitialized.
  	 * To initialize, use the `initReference` function
  	 */
  	MapProjection() = default;
  
  	/**
  	 * @brief Construct and initialize a new Map Projection object
  	 */
  	MapProjection(double lat_0, double lon_0)
  	{
  		initReference(lat_0, lon_0);
  	}
  
  	/**
  	 * @brief Construct and initialize a new Map Projection object
  	 */
  	MapProjection(double lat_0, double lon_0, uint64_t timestamp)
  	{
  		initReference(lat_0, lon_0, timestamp);
  	}
  
  	/**
  	 * Initialize the map transformation
  	 *
  	 * Initializes the transformation between the geographic coordinate system and
  	 * the azimuthal equidistant plane
  	 * @param lat in degrees (47.1234567°, not 471234567°)
  	 * @param lon in degrees (8.1234567°, not 81234567°)
  	 */
  	void initReference(double lat_0, double lon_0, uint64_t timestamp);
  
  	/**
  	 * Initialize the map transformation
  	 *
  	 * with reference coordinates on the geographic coordinate system
  	 * where the azimuthal equidistant plane's origin is located
  	 * @param lat in degrees (47.1234567°, not 471234567°)
  	 * @param lon in degrees (8.1234567°, not 81234567°)
  	 */
  	inline void initReference(double lat_0, double lon_0)
  	{
  		initReference(lat_0, lon_0, hrt_absolute_time());
  	}
  
  	/**
  	 * @return true, if the map reference has been initialized before
  	 */
  	bool isInitialized() const { return _ref_init_done; };
  
  	/**
  	 * @return the timestamp of the reference which the map projection was initialized with
  	 */
  	uint64_t getProjectionReferenceTimestamp() const { return _ref_timestamp; };
  
  	/**
  	 * @return the projection reference latitude in degrees
  	 */
  	double getProjectionReferenceLat() const { return math::degrees(_ref_lat); };
  
  	/**
  	 * @return the projection reference longitude in degrees
  	 */
  	double getProjectionReferenceLon() const { return math::degrees(_ref_lon); };
  
  	/**
  	 * Transform a point in the geographic coordinate system to the local
  	 * azimuthal equidistant plane using the projection
  	 * @param lat in degrees (47.1234567°, not 471234567°)
  	 * @param lon in degrees (8.1234567°, not 81234567°)
  	 * @param x north
  	 * @param y east
  	 */
  	void project(double lat, double lon, float &x, float &y) const;
  
  	/**
  	 * Transform a point in the geographic coordinate system to the local
  	 * azimuthal equidistant plane using the projection
  	 * @param lat in degrees (47.1234567°, not 471234567°)
  	 * @param lon in degrees (8.1234567°, not 81234567°)
  	 * @return the point in local coordinates as north / east
  	 */
  	inline matrix::Vector2f project(double lat, double lon) const
  	{
  		matrix::Vector2f res;
  		project(lat, lon, res(0), res(1));
  		return res;
  	}
  
  	/**
  	 * Transform a point in the local azimuthal equidistant plane to the
  	 * geographic coordinate system using the projection
  	 *
  	 * @param x north
  	 * @param y east
  	 * @param lat in degrees (47.1234567°, not 471234567°)
  	 * @param lon in degrees (8.1234567°, not 81234567°)
  	 */
  	void reproject(float x, float y, double &lat, double &lon) const;
  };
  