Drake
cubic_polynomial.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <ostream>
5 
7 #include "drake/common/unused.h"
8 
9 namespace drake {
10 namespace maliput {
11 namespace multilane {
12 
13 /// A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
15  public:
17 
18  /// Default constructor, all zero coefficients.
19 
20  CubicPolynomial() : CubicPolynomial(0., 0., 0., 0.) {}
21 
22  /// Constructs a cubic polynomial given all four coefficients.
23  CubicPolynomial(double a, double b, double c, double d)
24  : a_(a), b_(b), c_(c), d_(d) {
25  const double df = f_p(1.) - f_p(0.);
26  s_1_ = std::sqrt(1. + (df * df));
27  }
28 
29  // Returns the a coefficient.
30  double a() const { return a_; }
31 
32  // Returns the b coefficient.
33  double b() const { return b_; }
34 
35  // Returns the c coefficient.
36  double c() const { return c_; }
37 
38  // Returns the d coefficient.
39  double d() const { return d_; }
40 
41  /// Returns the order of the polynomial, based on
42  /// its nonzero coefficients.
43  int order() const {
44  if (d_ != 0.0) return 3;
45  if (c_ != 0.0) return 2;
46  if (b_ != 0.0) return 1;
47  return 0;
48  }
49 
50  /// Checks whether the polynomial is _exactly_ zero.
51  bool is_zero() const {
52  return (a_ == 0.0 && b_ == 0.0 && c_ == 0.0 && d_ == 0.0);
53  }
54 
55  /// Evaluates the polynomial f at @p p.
56  double f_p(double p) const {
57  return a_ + (b_ * p) + (c_ * p * p) + (d_ * p * p * p);
58  }
59 
60  /// Evaluates the derivative df/dp at @p p.
61  double f_dot_p(double p) const {
62  return b_ + (2. * c_ * p) + (3. * d_ * p * p);
63  }
64 
65  /// Evaluates the double-derivative d^2f/dp^2 at @p p.
66  double f_ddot_p(double p) const { return (2. * c_) + (6. * d_ * p); }
67 
68  // TODO(maddog@tri.global) s_p() and p_s() need to be replaced with a
69  // properly integrated path-length parameterization.
70  // For the moment, we are calculating the length by
71  // approximating the curve with a single linear
72  // segment from (0, f(0)) to (1, f(1)), which is
73  // not entirely awful for relatively flat curves.
74  /// Returns the path-length s along the curve (p, f(p)) from p = 0 to @p p.
75  double s_p(double p) const { return s_1_ * p; }
76 
77  /// Returns the inverse of the path-length parameterization s_p(p).
78  double p_s(double s) const { return s / s_1_; }
79 
80  // TODO(maddog@tri.global) Until s(p) is a properly integrated path-length
81  // parameterization, we have a need to calculate the
82  // derivative of the actual linear function
83  // involved in our bogus path-length approximation.
84  double fake_gprime(double p) const {
85  unused(p);
86  // return df; which is...
87  return f_p(1.) - f_p(0.);
88  }
89 
90  private:
91  double a_{};
92  double b_{};
93  double c_{};
94  double d_{};
95  double s_1_{};
96 };
97 
98 /// Streams a string representation of `cubic_polynomial` into `out`.
99 /// Returns `out`. This method is provided for the purposes of debugging or
100 /// text-logging. It is not intended for serialization.
101 std::ostream& operator<<(std::ostream& out,
102  const CubicPolynomial& cubic_polynomial);
103 
104 } // namespace multilane
105 } // namespace maliput
106 } // namespace drake
double f_ddot_p(double p) const
Evaluates the double-derivative d^2f/dp^2 at p.
Definition: cubic_polynomial.h:66
double p_s(double s) const
Returns the inverse of the path-length parameterization s_p(p).
Definition: cubic_polynomial.h:78
int order() const
Returns the order of the polynomial, based on its nonzero coefficients.
Definition: cubic_polynomial.h:43
Definition: bullet_model.cc:22
double fake_gprime(double p) const
Definition: cubic_polynomial.h:84
double c() const
Definition: cubic_polynomial.h:36
CubicPolynomial(double a, double b, double c, double d)
Constructs a cubic polynomial given all four coefficients.
Definition: cubic_polynomial.h:23
double a() const
Definition: cubic_polynomial.h:30
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:75
Definition: arc_road_curve.cc:11
double d() const
Definition: cubic_polynomial.h:39
bool is_zero() const
Checks whether the polynomial is exactly zero.
Definition: cubic_polynomial.h:51
#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:61
double b() const
Definition: cubic_polynomial.h:33
double f_p(double p) const
Evaluates the polynomial f at p.
Definition: cubic_polynomial.h:56
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: cubic_polynomial.h:14
std::ostream & operator<<(std::ostream &out, const StartReference::Spec &start_spec)
Streams a string representation of start_spec into out.
Definition: builder.cc:19
Expression sqrt(const Expression &e)
Definition: symbolic_expression.cc:566
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...