source: rrlib_geometry/functions.h @ 93:a5e38d1d01b4

//
// You received this file as part of RRLib
// Robotics Research Library
//
// Copyright (C) Finroc GbR (finroc.org)
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//----------------------------------------------------------------------
/*!\file    functions.h
 *
 * \author  Tobias Foehst
 *
 * \date    2010-12-27
 *
 * \brief
 *
 * \b
 *
 * A few words for functions.h
 *
 */
//----------------------------------------------------------------------
#ifndef __rrlib__geometry__functions_h__
#define __rrlib__geometry__functions_h__

//----------------------------------------------------------------------
// External includes with <>
//----------------------------------------------------------------------
#include <cmath>

//----------------------------------------------------------------------
// Internal includes with ""
//----------------------------------------------------------------------

//----------------------------------------------------------------------
// Debugging
//----------------------------------------------------------------------
#include <cassert>

//----------------------------------------------------------------------
// Namespace declaration
//----------------------------------------------------------------------
namespace rrlib
{
namespace geometry
{



template <typename TElement>
inline math::tAngleRad GetSlope(const tLineSegment<2, TElement> &line_segment)
{
  return std::atan2(line_segment.End().Y() - line_segment.Begin().Y(), line_segment.End().X() - line_segment.Begin().X());
}

template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
inline double GetCurvature(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement parameter)
{
  typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative first_derivative(curve.GetDerivative());
  typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative::tDerivative second_derivative(first_derivative.GetDerivative());

  math::tVector<3, TElement, math::vector::Cartesian> first(first_derivative(parameter));
  math::tVector<3, TElement, math::vector::Cartesian> second(second_derivative(parameter));
  double first_length = first.Length();

  return CrossProduct(first, second).Length() / (first_length * first_length * first_length);
}

template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
inline double GetCurvature(const tSplineCurve<Tdimension, TElement, Tdegree> &spline, TElement parameter)
{
  TElement local_t;
  auto bezier = spline.GetBezierCurveForParameter(parameter, local_t);
  return GetCurvature(*bezier, local_t);
}

namespace internal
{
  template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
  double GetMaxCurvatureHelper(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement &parameter, TElement lower, TElement upper)
{
  auto middle = (upper + lower) / 2;
  auto lower_half_parameter = (lower + middle) / 2;
  auto upper_half_parameter = (middle + upper) / 2;
  auto lower_half_curvature = GetCurvature(curve, lower_half_parameter);
  auto upper_half_curvature = GetCurvature(curve, upper_half_parameter);

  if (lower_half_curvature > upper_half_curvature)
  {
    if (math::IsEqual(lower, lower_half_parameter))
    {
      parameter = lower_half_parameter;
      return lower_half_curvature;
    }
    return GetMaxCurvatureHelper(curve, parameter, lower, middle);
  }
  if (math::IsEqual(upper, upper_half_parameter))
  {
    parameter = upper_half_parameter;
    return upper_half_curvature;
  }
  return GetMaxCurvatureHelper(curve, parameter, middle, upper);
}
}

template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
inline double GetMaxCurvature(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement &parameter)
{
  return internal::GetMaxCurvatureHelper(curve, parameter, 0.0, 1.0);
}

template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
inline double GetMaxCurvature(const tSplineCurve<Tdimension, TElement, Tdegree> &spline, TElement &parameter)
{
  assert(spline.NumberOfSegments() > 0);
  double max_curvature = GetMaxCurvature(*spline.GetBezierCurveForSegment(0), parameter);
  for (std::size_t i = 1; i < spline.NumberOfSegments(); ++i)
  {
    TElement parameter_candidate;
    double curvature_candidate = GetMaxCurvature(*spline.GetBezierCurveForSegment(i), parameter_candidate);
    if (curvature_candidate > max_curvature)
    {
      max_curvature = curvature_candidate;
      parameter = i + parameter_candidate;
    }
  }
  return max_curvature;
}



//----------------------------------------------------------------------
// End of namespace declaration
//----------------------------------------------------------------------
}
}

#endif