spatial_math.hpp

// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2013.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef SPATIAL_MATH_HPP
#define SPATIAL_MATH_HPP

#include <limits>
#include <cmath>
#include <sstream>
#include "spatial_import_type_traits.hpp"
#include "../exception.hpp"
#include "spatial_check_concept.hpp"

namespace spatial
{
  namespace except
  {
    template<typename Tp>
    inline typename enable_if<import::is_arithmetic<Tp> >::type
    check_positive_distance(Tp x)
    {
      if (x < Tp()) // Tp() == 0 by convention
        {
          std::stringstream out;
          out << x << " is negative";
          throw invalid_distance(out.str());
        }
    }

    template <typename Tp>
    inline typename enable_if_c
    <std::numeric_limits<Tp>::is_integer
    && std::numeric_limits<Tp>::is_signed, Tp>::type
    check_abs(Tp x)
    {
      if (x == (std::numeric_limits<Tp>::min)())
        {
          std::stringstream out;
          out << "absolute of " << x << " caused overflow";
          throw arithmetic_error(out.str());
        }
      return std::abs(x);
    }

    template <typename Tp>
    inline typename enable_if_c
    <!std::numeric_limits<Tp>::is_integer
    || !std::numeric_limits<Tp>::is_signed, Tp>::type
    check_abs(Tp x)
    { return std::abs(x); }

    template <typename Tp>
    inline typename enable_if<import::is_arithmetic<Tp>, Tp>::type
    check_positive_add(Tp x, Tp y)
    {
      // The additional bracket is to avoid conflict with MSVC min/max macros
      if (((std::numeric_limits<Tp>::max)() - x) < y)
        {
          std::stringstream out;
          out << x << " + " << y << " caused overflow";
          throw arithmetic_error(out.str());
        }
      return x + y;
    }

    template <typename Tp>
    inline typename enable_if<import::is_arithmetic<Tp>, Tp>::type
    check_square(Tp x)
    {
      Tp zero = Tp(); // get 0
      if (x != zero)
        {
          Tp abs = check_abs(x);
          if (((std::numeric_limits<Tp>::max)() / abs) < abs)
            {
              std::stringstream out;
              out << "square(" << x << ") caused overflow";
              throw arithmetic_error(out.str());
            }
          return x * x;
        }
      return zero;
    }

    template <typename Tp>
    inline typename enable_if<import::is_arithmetic<Tp>, Tp>::type
    check_positive_mul(Tp x, Tp y)
    {
      Tp zero = Tp(); // get 0
      if (x != zero)
        {
          if (((std::numeric_limits<Tp>::max)() / x) < y)
            {
              std::stringstream out;
              out << x << " * " << y << " caused overflow";
              throw arithmetic_error(out.str());
            }
          return x * y;
        }
      return zero;
    }
  } // spatial except

  namespace math
  {
    template <typename Key, typename Difference, typename Unit>
    inline typename enable_if<import::is_arithmetic<Unit>, Unit>::type
    square_euclid_distance_to_plane
    (dimension_type dim, const Key& origin, const Key& key, Difference diff)
    {
      Unit d = diff(dim, origin, key);
#ifdef SPATIAL_SAFER_ARITHMETICS
      return except::check_square(d);
#else
      return d * d;
#endif
    }

    template <typename Key, typename Difference, typename Unit>
    inline typename enable_if<import::is_arithmetic<Unit>, Unit>::type
    square_euclid_distance_to_key
    (dimension_type rank, const Key& origin, const Key& key, Difference diff)
    {
      Unit sum = square_euclid_distance_to_plane<Key, Difference, Unit>
        (0, origin, key, diff);
      for (dimension_type i = 1; i < rank; ++i)
        {
#ifdef SPATIAL_SAFER_ARITHMETICS
          sum = except::check_positive_add
            (square_euclid_distance_to_plane<Key, Difference, Unit>
             (i, origin, key, diff), sum);
#else
          sum += square_euclid_distance_to_plane
            <Key, Difference, Unit>(i, origin, key, diff);
#endif
        }
      return sum;
    }

    template <typename Key, typename Difference, typename Unit>
    inline typename enable_if<import::is_floating_point<Unit>, Unit>::type
    euclid_distance_to_plane
    (dimension_type dim, const Key& origin, const Key& key, Difference diff)
    {
      return std::abs(diff(dim, origin, key)); // floating types abs is always okay!
    }

    template <typename Key, typename Difference, typename Unit>
    inline typename enable_if<import::is_floating_point<Unit>, Unit>::type
    euclid_distance_to_key
    (dimension_type rank, const Key& origin, const Key& key, Difference diff)
    {
#ifdef SPATIAL_SAFER_ARITHMETICS
      // Find a non zero maximum or return 0
      Unit max = euclid_distance_to_plane<Key, Difference, Unit>
        (0, origin, key, diff);
      dimension_type max_dim = 0;
      for (dimension_type i = 1; i < rank; ++i)
        {
          Unit d = euclid_distance_to_plane<Key, Difference, Unit>
            (i, origin, key, diff);
          if (d > max) { max = d; max_dim = i; }
        }
      const Unit zero = Unit();
      if (max == zero) return zero; // they're all zero!
      // Compute the distance
      Unit sum = zero;
      for (dimension_type i = 0; i < max_dim; ++i)
        {
          Unit div = diff(i, origin, key) / max;
          sum += div * div;
        }
      for (dimension_type i = max_dim + 1; i < rank; ++i)
        {
          Unit div = diff(i, origin, key) / max;
          sum += div * div;
        }
      return except::check_positive_mul(max, std::sqrt(((Unit) 1.0) + sum));
#else
      return std::sqrt(square_euclid_distance_to_key<Key, Difference, Unit>
                       (rank, origin, key, diff));
#endif
    }

    template <typename Key, typename Difference, typename Unit>
    inline typename enable_if<import::is_arithmetic<Unit>, Unit>::type
    manhattan_distance_to_plane
    (dimension_type dim, const Key& origin, const Key& key, Difference diff)
    {
#ifdef SPATIAL_SAFER_ARITHMETICS
      return except::check_abs(diff(dim, origin, key));
#else
      return std::abs(diff(dim, origin, key));
#endif
    }

    template <typename Key, typename Difference, typename Unit>
    inline typename enable_if<import::is_arithmetic<Unit>, Unit>::type
    manhattan_distance_to_key
    (dimension_type rank, const Key& origin, const Key& key, Difference diff)
    {
      Unit sum = manhattan_distance_to_plane<Key, Difference, Unit>
        (0, origin, key, diff);
      for (dimension_type i = 1; i < rank; ++i)
        {
#ifdef SPATIAL_SAFER_ARITHMETICS
          sum = except::check_positive_add
            (manhattan_distance_to_plane<Key, Difference, Unit>
             (i, origin, key, diff), sum);
#else
          sum += manhattan_distance_to_plane
            <Key, Difference, Unit>(i, origin, key, diff);
#endif
        }
      return sum;
    }

    /*
      // For a future implementation where we take earth-like spheroid as an
      // example for non-euclidian spaces, or manifolds.
      math::great_circle_distance_to_key
      math::great_circle_distance_to_plane
      math::vincenty_distance_to_key
      math::vincenty_distance_to_plane
    */
  } // namespace math
}

#endif // SPATIAL_MATH_HPP