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 
14  public:
16 
17 
18  CubicPolynomial() : CubicPolynomial(0., 0., 0., 0.) {}
19 
21  CubicPolynomial(double a, double b, double c, double d)
22  : a_(a), b_(b), c_(c), d_(d) {
23  const double df = f_p(1.) - f_p(0.);
24  s_1_ = std::sqrt(1. + (df * df));
25  }
26 
27  // Returns the a coefficient.
28  double a() const { return a_; }
29 
30  // Returns the b coefficient.
31  double b() const { return b_; }
32 
33  // Returns the c coefficient.
34  double c() const { return c_; }
35 
36  // Returns the d coefficient.
37  double d() const { return d_; }
38 
40  double f_p(double p) const {
41  return a_ + (b_ * p) + (c_ * p * p) + (d_ * p * p * p);
42  }
43 
45  double f_dot_p(double p) const {
46  return b_ + (2. * c_ * p) + (3. * d_ * p * p);
47  }
48 
50  double f_ddot_p(double p) const { return (2. * c_) + (6. * d_ * p); }
51 
52  // TODO(maddog@tri.global) s_p() and p_s() need to be replaced with a
53  // properly integrated path-length parameterization.
54  // For the moment, we are calculating the length by
55  // approximating the curve with a single linear
56  // segment from (0, f(0)) to (1, f(1)), which is
57  // not entirely awful for relatively flat curves.
59  double s_p(double p) const { return s_1_ * p; }
60 
62  double p_s(double s) const { return s / s_1_; }
63 
64  // TODO(maddog@tri.global) Until s(p) is a properly integrated path-length
65  // parameterization, we have a need to calculate the
66  // derivative of the actual linear function
67  // involved in our bogus path-length approximation.
68  double fake_gprime(double p) const {
69  unused(p);
70  // return df; which is...
71  return f_p(1.) - f_p(0.);
72  }
73 
74  private:
75  double a_{};
76  double b_{};
77  double c_{};
78  double d_{};
79  double s_1_{};
80 };
81 
82 } // namespace multilane
83 } // namespace maliput
84 } // namespace drake
double f_ddot_p(double p) const
Evaluates the double-derivative d^2f/dp^2 at p.
Definition: cubic_polynomial.h:50
double p_s(double s) const
Returns the inverse of the path-length parameterization s_p(p).
Definition: cubic_polynomial.h:62
Definition: automotive_demo.cc:88
double fake_gprime(double p) const
Definition: cubic_polynomial.h:68
double c() const
Definition: cubic_polynomial.h:34
CubicPolynomial(double a, double b, double c, double d)
Constructs a cubic polynomial given all four coefficients.
Definition: cubic_polynomial.h:21
double a() const
Definition: cubic_polynomial.h:28
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:59
Definition: arc_road_curve.cc:11
double d() const
Definition: cubic_polynomial.h:37
#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:59
double f_dot_p(double p) const
Evaluates the derivative df/dp at p.
Definition: cubic_polynomial.h:45
double b() const
Definition: cubic_polynomial.h:31
double f_p(double p) const
Evaluates the polynomial f at p.
Definition: cubic_polynomial.h:40
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:553
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:53
Provides careful macros to selectively enable or disable the special member functions for copy-constr...