Changeset 93:a5e38d1d01b4 in rrlib_geometry for functions.h


Ignore:
Timestamp:
08.08.2019 17:33:06 (12 months ago)
Author:
Tobias Föhst <tobias.foehst@…>
Branch:
14.08
Children:
94:968120829e55, 95:68f11e37e58c
Phase:
public
Message:

Adds functions to get the curvature from a spline curve and to find the maximum curvature on Bezier curves and splines

File:
1 edited

Legend:

Unmodified
Added
Removed
  • functions.h

    r80 r93  
    4949// Debugging 
    5050//---------------------------------------------------------------------- 
     51#include <cassert> 
    5152 
    5253//---------------------------------------------------------------------- 
     
    7980} 
    8081 
     82template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> 
     83inline double GetCurvature(const tSplineCurve<Tdimension, TElement, Tdegree> &spline, TElement parameter) 
     84{ 
     85  TElement local_t; 
     86  auto bezier = spline.GetBezierCurveForParameter(parameter, local_t); 
     87  return GetCurvature(*bezier, local_t); 
     88} 
     89 
     90namespace internal 
     91{ 
     92  template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> 
     93  double GetMaxCurvatureHelper(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement &parameter, TElement lower, TElement upper) 
     94{ 
     95  auto middle = (upper + lower) / 2; 
     96  auto lower_half_parameter = (lower + middle) / 2; 
     97  auto upper_half_parameter = (middle + upper) / 2; 
     98  auto lower_half_curvature = GetCurvature(curve, lower_half_parameter); 
     99  auto upper_half_curvature = GetCurvature(curve, upper_half_parameter); 
     100 
     101  if (lower_half_curvature > upper_half_curvature) 
     102  { 
     103    if (math::IsEqual(lower, lower_half_parameter)) 
     104    { 
     105      parameter = lower_half_parameter; 
     106      return lower_half_curvature; 
     107    } 
     108    return GetMaxCurvatureHelper(curve, parameter, lower, middle); 
     109  } 
     110  if (math::IsEqual(upper, upper_half_parameter)) 
     111  { 
     112    parameter = upper_half_parameter; 
     113    return upper_half_curvature; 
     114  } 
     115  return GetMaxCurvatureHelper(curve, parameter, middle, upper); 
     116} 
     117} 
     118 
     119template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> 
     120inline double GetMaxCurvature(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement &parameter) 
     121{ 
     122  return internal::GetMaxCurvatureHelper(curve, parameter, 0.0, 1.0); 
     123} 
     124 
     125template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> 
     126inline double GetMaxCurvature(const tSplineCurve<Tdimension, TElement, Tdegree> &spline, TElement &parameter) 
     127{ 
     128  assert(spline.NumberOfSegments() > 0); 
     129  double max_curvature = GetMaxCurvature(*spline.GetBezierCurveForSegment(0), parameter); 
     130  for (std::size_t i = 1; i < spline.NumberOfSegments(); ++i) 
     131  { 
     132    TElement parameter_candidate; 
     133    double curvature_candidate = GetMaxCurvature(*spline.GetBezierCurveForSegment(i), parameter_candidate); 
     134    if (curvature_candidate > max_curvature) 
     135    { 
     136      max_curvature = curvature_candidate; 
     137      parameter = i + parameter_candidate; 
     138    } 
     139  } 
     140  return max_curvature; 
     141} 
     142 
    81143 
    82144 
Note: See TracChangeset for help on using the changeset viewer.