point.h

Go to the documentation of this file.

/*
  Copyright (C) 2018-2024 by the authors of the World Builder code.

  This file is part of the World Builder.

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Lesser General Public License as published
   by the Free Software Foundation, either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public License
   along with this program.  If not, see <https://www.gnu.org/licenses/>.
*/

#ifndef WORLD_BUILDER_POINT_H
#define WORLD_BUILDER_POINT_H
#include "nan.h"
#define _USE_MATH_DEFINES
#include <array>
#include <cmath>
#include <limits>
#include <iostream>

#include "world_builder/assert.h"
#include "world_builder/consts.h"
#include "world_builder/coordinate_system.h"

namespace WorldBuilder
{

  namespace FT
  {
    inline double fmod(const double x, const double y)
    {
      if (y < 0.0 || y > 0.0)
        {
          const double x_div_y = x/y;
          return (x_div_y-static_cast<int>(x_div_y))*y;
        }
      return NaN::DQNAN;
    }

    inline double fast_sin_d(const double angle)
    {
      constexpr double A = 4.0/(Consts::PI * Consts::PI);
      constexpr double oneminPmin = 1.-0.1952403377008734-0.01915214119105392;

      const double y = A* angle * ( Consts::PI - angle );
      return y*( oneminPmin + y*( 0.1952403377008734 + y * 0.01915214119105392 ) ) ;
    }

    inline double sin(const double raw_angle)
    {
      const double angle = (raw_angle > -Consts::PI && raw_angle < Consts::PI)
                           ?
                           raw_angle
                           :
                           FT::fmod(raw_angle + std::copysign(Consts::PI,raw_angle), Consts::PI * 2.0) - std::copysign(Consts::PI,raw_angle);

      if (angle >= 0)
        return fast_sin_d(angle);
      return -fast_sin_d(-angle);
    }

    inline double cos(const double angle)
    {
      return FT::sin((Consts::PI*0.5)-angle);
    }
  } // namespace FT


  template<unsigned int dim>
  class Point
  {
    public:
      inline
      Point(CoordinateSystem coordinate_system_)
        :
        point(std::array<double,dim>()),
        coordinate_system(coordinate_system_)
      {}

      inline
      Point(const std::array<double,dim> &location, CoordinateSystem coordinate_system_)
        :
        point(location),
        coordinate_system(coordinate_system_)
      {}

      inline
      Point(const Point<dim> &location, CoordinateSystem coordinate_system_)
        :
        point(location.get_array()),
        coordinate_system(coordinate_system_)
      {}

      inline
      Point(const Point<dim> &location)
        :
        point(location.get_array()),
        coordinate_system(location.get_coordinate_system())
      {}

      Point(const double x, const double y, CoordinateSystem coordinate_system);

      Point(const double x, const double y, const double z, CoordinateSystem coordinate_system);

      inline
      ~Point() = default;

      inline
      Point<dim> &operator=(const Point<dim> &point_right)
      {
        point = point_right.point;
        coordinate_system = point_right.coordinate_system;
        return *this;
      }

      inline
      double operator*(const Point<dim> &point_right) const
      {
        WBAssert(coordinate_system == point_right.get_coordinate_system(),
                 "Cannot take the dot product of two points which represent different coordinate systems.");
        const std::array<double,dim> &array = point_right.get_array();
        double dot_product = 0;
        for (unsigned int i = 0; i < dim; ++i)
          dot_product += point[i] * array[i];
        return dot_product;
      }


      inline
      Point<dim> operator*(const double scalar) const
      {
        // initialize the array to zero.
        std::array<double,dim> array;
        for (unsigned int i = 0; i < dim; ++i)
          array[i] = point[i] * scalar;
        return Point<dim>(array,coordinate_system);
      }

      inline
      Point<dim> operator/(const double scalar) const
      {
        // initialize the array to zero.
        std::array<double,dim> array;
        const double one_over_scalar = 1/scalar;
        for (unsigned int i = 0; i < dim; ++i)
          array[i] = point[i] * one_over_scalar;
        return Point<dim>(array,coordinate_system);
      }

      inline
      Point<dim> operator+(const Point<dim> &point_right) const
      {
        WBAssert(coordinate_system == point_right.get_coordinate_system(),
                 "Cannot add two points which represent different coordinate systems.");
        Point<dim> point_tmp(point,coordinate_system);
        for (unsigned int i = 0; i < dim; ++i)
          point_tmp[i] += point_right[i];

        return point_tmp;
      }


      inline
      Point<dim> operator-(const Point<dim> &point_right) const
      {
        WBAssert(coordinate_system == point_right.get_coordinate_system(),
                 "Cannot subtract two points which represent different coordinate systems. Internal has type " << static_cast<int>(coordinate_system)
                 << ", other point has type " << static_cast<int>(point_right.get_coordinate_system()));
        Point<dim> point_tmp(point,coordinate_system);
        for (unsigned int i = 0; i < dim; ++i)
          point_tmp[i] -= point_right[i];

        return point_tmp;
      }


      inline
      Point<dim> &operator*=(const double scalar)
      {
        for (unsigned int i = 0; i < dim; ++i)
          point[i] *= scalar;
        return *this;
      }
      inline
      Point<dim> &operator/=(const double scalar)
      {
        for (unsigned int i = 0; i < dim; ++i)
          point[i] /= scalar;
        return *this;
      }

      inline
      Point<dim> &operator+=(const Point<dim> &point_right)
      {
        WBAssert(coordinate_system == point_right.get_coordinate_system(),
                 "Cannot add two points which represent different coordinate systems.");
        for (unsigned int i = 0; i < dim; ++i)
          point[i] += point_right[i];
        return *this;
      }

      inline
      bool operator==(const Point<dim> &point_) const
      {
        if (coordinate_system != point_.coordinate_system)
          return false;
        WorldBuilder::Point<dim> point_tmp(point,coordinate_system);
        point_tmp -= point_;
        if (std::fabs(point_tmp[0]) > std::numeric_limits<double>::epsilon())
          return false;
        if (std::fabs(point_tmp[1]) > std::numeric_limits<double>::epsilon())
          return false;
        if (dim == 3 && std::fabs(point_tmp[2]) > std::numeric_limits<double>::epsilon())
          return false;

        return true;
      }

      inline
      Point<dim> &operator-=(const Point<dim> &point_right)
      {
        WBAssert(coordinate_system == point_right.get_coordinate_system(),
                 "Cannot subtract two points which represent different coordinate systems.");
        for (unsigned int i = 0; i < dim; ++i)
          point[i] -= point_right[i];
        return *this;
      }

      inline
      const double &operator[](const size_t index) const
      {
        WBAssert(index <= dim, "Can't ask for element " << index << " in an point with dimension " << dim << '.');
        return point[index];
      }


      inline double &operator[](const size_t index)
      {
        WBAssert(index <= dim, "Can't ask for element " << index << " in an point with dimension " << dim << '.');
        return point[index];
      }

      double
      distance(const Point<2> &two) const;

      double
      cheap_relative_distance_spherical(const Point<2> &two) const
      {
        const double d_longitude = two[0] - this->point[0];
        const double d_latitude = two[1] - this->point[1];
        const double sin_d_lat = FT::sin(d_latitude * 0.5);
        const double sin_d_long = FT::sin(d_longitude * 0.5);
        return (sin_d_lat * sin_d_lat) + (sin_d_long*sin_d_long) * FT::cos(this->point[1]) * FT::cos(two[1]);
      }

      double
      cheap_relative_distance_cartesian(const Point<2> &two) const
      {
        const double x_distance_to_reference_point = point[0]-two[0];
        const double y_distance_to_reference_point = point[1]-two[1];
        return (x_distance_to_reference_point*x_distance_to_reference_point) + (y_distance_to_reference_point*y_distance_to_reference_point);
      }

      inline
      const std::array<double,dim> &get_array() const
      {
        return point;
      }


      inline
      CoordinateSystem get_coordinate_system() const
      {
        return coordinate_system;
      }


      inline
      double norm() const
      {
        return std::sqrt(this->norm_square());
      }


      inline
      double norm_square() const
      {
        WBAssertThrow(false,"This function is only available in 2d or 3d.");
        return 0;
      }

      friend std::ostream &operator<<( std::ostream &output, const Point<dim> &stream_point )
      {
        for (size_t i = 0; i < dim-1; i++)
          {
            output <<  stream_point[i] << ' ';
          }
        output << stream_point[dim-1];

        return output;
      }


    private:
      std::array<double,dim> point;
      CoordinateSystem coordinate_system;

  };

  template <>
  inline
  double Point<2>::norm_square() const
  {
    return (point[0] * point[0]) + (point[1] * point[1]);
  }

  template <>
  inline
  double Point<3>::norm_square() const
  {
    return (point[0] * point[0]) + (point[1] * point[1]) + (point[2] * point[2]);
  }

  template<unsigned int dim>
  inline
  Point<dim> operator*(const double scalar, const Point<dim> &point)
  {
    return point*scalar;
  }
} // namespace WorldBuilder
#endif