source: rrlib_geometry/functions.h @ 80:b720f2f65019

14.08
Last change on this file since 80:b720f2f65019 was 80:b720f2f65019, checked in by Tobias Föhst <foehst@…>, 3 years ago

Replaces use of boost::enable_if_c by std::enable_if

File size: 3.1 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License along
18// with this program; if not, write to the Free Software Foundation, Inc.,
19// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21//----------------------------------------------------------------------
22/*!\file    functions.h
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2010-12-27
27 *
28 * \brief
29 *
30 * \b
31 *
32 * A few words for functions.h
33 *
34 */
35//----------------------------------------------------------------------
36#ifndef __rrlib__geometry__functions_h__
37#define __rrlib__geometry__functions_h__
38
39//----------------------------------------------------------------------
40// External includes with <>
41//----------------------------------------------------------------------
42#include <cmath>
43
44//----------------------------------------------------------------------
45// Internal includes with ""
46//----------------------------------------------------------------------
47
48//----------------------------------------------------------------------
49// Debugging
50//----------------------------------------------------------------------
51
52//----------------------------------------------------------------------
53// Namespace declaration
54//----------------------------------------------------------------------
55namespace rrlib
56{
57namespace geometry
58{
59
60
61
62template <typename TElement>
63inline math::tAngleRad GetSlope(const tLineSegment<2, TElement> &line_segment)
64{
65  return std::atan2(line_segment.End().Y() - line_segment.Begin().Y(), line_segment.End().X() - line_segment.Begin().X());
66}
67
68template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
69inline double GetCurvature(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement parameter)
70{
71  typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative first_derivative(curve.GetDerivative());
72  typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative::tDerivative second_derivative(first_derivative.GetDerivative());
73
74  math::tVector<3, TElement, math::vector::Cartesian> first(first_derivative(parameter));
75  math::tVector<3, TElement, math::vector::Cartesian> second(second_derivative(parameter));
76  double first_length = first.Length();
77
78  return CrossProduct(first, second).Length() / (first_length * first_length * first_length);
79}
80
81
82
83//----------------------------------------------------------------------
84// End of namespace declaration
85//----------------------------------------------------------------------
86}
87}
88
89#endif
Note: See TracBrowser for help on using the repository browser.