Drake
cubic_polynomial.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 
6 #include "drake/common/unused.h"
7 
8 namespace drake {
9 namespace maliput {
10 namespace multilane {
11 
12 /// A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
14  public:
16 
17  /// Default constructor, all zero coefficients.
18 
19  CubicPolynomial() : CubicPolynomial(0., 0., 0., 0.) {}
20 
21  /// Constructs a cubic polynomial given all four coefficients.
22  CubicPolynomial(double a, double b, double c, double d)
23  : a_(a), b_(b), c_(c), d_(d) {
24  const double df = f_p(1.) - f_p(0.);
25  s_1_ = std::sqrt(1. + (df * df));
26  }
27 
28  // Returns the a coefficient.
29  double a() const { return a_; }
30 
31  // Returns the b coefficient.
32  double b() const { return b_; }
33 
34  // Returns the c coefficient.
35  double c() const { return c_; }
36 
37  // Returns the d coefficient.
38  double d() const { return d_; }
39 
40  /// Returns the order of the polynomial, based on
41  /// its nonzero coefficients.
42  int order() const {
43  if (d_ != 0.0) return 3;
44  if (c_ != 0.0) return 2;
45  if (b_ != 0.0) return 1;
46  return 0;
47  }
48 
49  /// Checks whether the polynomial is _exactly_ zero.
50  bool is_zero() const {
51  return (a_ == 0.0 && b_ == 0.0 && c_ == 0.0 && d_ == 0.0);
52  }
53 
54  /// Evaluates the polynomial f at @p p.
55  double f_p(double p) const {
56  return a_ + (b_ * p) + (c_ * p * p) + (d_ * p * p * p);
57  }
58 
59  /// Evaluates the derivative df/dp at @p p.
60  double f_dot_p(double p) const {
61  return b_ + (2. * c_ * p) + (3. * d_ * p * p);
62  }
63 
64  /// Evaluates the double-derivative d^2f/dp^2 at @p p.
65  double f_ddot_p(double p) const { return (2. * c_) + (6. * d_ * p); }
66 
67  // TODO(maddog@tri.global) s_p() and p_s() need to be replaced with a
68  // properly integrated path-length parameterization.
69  // For the moment, we are calculating the length by
70  // approximating the curve with a single linear
71  // segment from (0, f(0)) to (1, f(1)), which is
72  // not entirely awful for relatively flat curves.
73  /// Returns the path-length s along the curve (p, f(p)) from p = 0 to @p p.
74  double s_p(double p) const { return s_1_ * p; }
75 
76  /// Returns the inverse of the path-length parameterization s_p(p).
77  double p_s(double s) const { return s / s_1_; }
78 
79  // TODO(maddog@tri.global) Until s(p) is a properly integrated path-length
80  // parameterization, we have a need to calculate the
81  // derivative of the actual linear function
82  // involved in our bogus path-length approximation.
83  double fake_gprime(double p) const {
84  unused(p);
85  // return df; which is...
86  return f_p(1.) - f_p(0.);
87  }
88 
89  private:
90  double a_{};
91  double b_{};
92  double c_{};
93  double d_{};
94  double s_1_{};
95 };
96 
97 } // namespace multilane
98 } // namespace maliput
99 } // namespace drake
double f_ddot_p(double p) const
Evaluates the double-derivative d^2f/dp^2 at p.
Definition: cubic_polynomial.h:65
double p_s(double s) const
Returns the inverse of the path-length parameterization s_p(p).
Definition: cubic_polynomial.h:77
int order() const
Returns the order of the polynomial, based on its nonzero coefficients.
Definition: cubic_polynomial.h:42
Definition: automotive_demo.cc:90
double fake_gprime(double p) const
Definition: cubic_polynomial.h:83
double c() const
Definition: cubic_polynomial.h:35
CubicPolynomial(double a, double b, double c, double d)
Constructs a cubic polynomial given all four coefficients.
Definition: cubic_polynomial.h:22
double a() const
Definition: cubic_polynomial.h:29
double s_p(double p) const
Returns the path-length s along the curve (p, f(p)) from p = 0 to p.
Definition: cubic_polynomial.h:74
Definition: arc_road_curve.cc:11
double d() const
Definition: cubic_polynomial.h:38
bool is_zero() const
Checks whether the polynomial is exactly zero.
Definition: cubic_polynomial.h:50
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:57
double f_dot_p(double p) const
Evaluates the derivative df/dp at p.
Definition: cubic_polynomial.h:60
double b() const
Definition: cubic_polynomial.h:32
double f_p(double p) const
Evaluates the polynomial f at p.
Definition: cubic_polynomial.h:55
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: cubic_polynomial.h:13
Expression sqrt(const Expression &e)
Definition: symbolic_expression.cc:567
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:51
Provides careful macros to selectively enable or disable the special member functions for copy-constr...