GCC Code Coverage Report

Directory: ./
File: include/hpp/core/time-parameterization/polynomial.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 40 46 87.0%
Branches: 28 52 53.8%
Line Branch Exec Source
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #ifndef HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
30 #define HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
31
32 #include <hpp/constraints/differentiable-function.hh>
33 #include <hpp/core/config.hh>
34 #include <hpp/core/fwd.hh>
35 #include <hpp/core/path/math.hh>
36 #include <hpp/core/time-parameterization.hh>
37
38 namespace hpp {
39 namespace core {
40 namespace timeParameterization {
41 class HPP_CORE_DLLAPI Polynomial : public TimeParameterization {
42 public:
43 6 Polynomial(const vector_t& param) : a(param) {
44
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 6 times.
 30 for (size_type i = 0; i < a.size(); ++i) {
45
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 24 times.
 24 assert(a[i] < std::numeric_limits<value_type>::infinity());
46
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 24 times.
 24 assert(a[i] > -std::numeric_limits<value_type>::infinity());
47 }
48 6 }
49
50 const vector_t& parameters() const { return a; }
51
52 2 TimeParameterizationPtr_t copy() const {
53
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return TimeParameterizationPtr_t(new Polynomial(*this));
54 }
55
56 /// Computes \f$ \sum_{i=0}^n a_i t^i \f$
57 19 value_type value(const value_type& t) const { return val(t); }
58
59 /// Computes \f$ \sum_{i=1}^n i a_i t^{i-1} \f$
60 20010 value_type derivative(const value_type& t, const size_type& order) const {
61 20010 return Jac(t, order);
62 }
63
64 /// Compute the bound of the derivative on \f$ [ low, up ] \f$.
65 /// Three cases are handled:
66 /// \li first order: \f$ B = |a_1| \f$
67 /// \li second order: \f$ B = \max{|J(low)|, |J(up)|} \f$
68 /// \li third order:
69 /// Let \f$ x_m = - \frac{a_2}{3 a_3} \f$ be the extremal point
70 /// of the derivative and \f$ M = \max{|J(low)|, |J(up)|}\f$.
71 /// Then:
72 /// - if \f$ low < x_m < up \f$,
73 /// \f$ B = \max{ |a_1 - \frac{a_2}{3 a_3}|, M } \f$
74 /// - else \f$ B = M \f$
75 9 value_type derivativeBound(const value_type& low,
76 const value_type& up) const {
77 using std::fabs;
78 using std::max;
79
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 9 switch (a.size()) {
80 3 case 2:
81 3 return fabs(a[1]);
82 break;
83 ✗ case 3:
84 ✗ return max(fabs(Jac(low)), fabs(Jac(up)));
85 break;
86 6 case 4: {
87
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 const value_type x_m = -a[2] / (3 * a[3]);
88
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 const value_type M = max(fabs(Jac(low)), fabs(Jac(up)));
89
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
 6 if (low < x_m && x_m < up)
90
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 return max(M, fabs(a[1] - a[2] / 3 * a[3]));
91 else
92 4 return M;
93 } break;
94 ✗ default:
95 ✗ throw std::logic_error("not implemented");
96 }
97 }
98
99 private:
100 19 value_type val(const value_type& t) const {
101 19 value_type tn = 1;
102 19 value_type res = a[0];
103
2/2
✓ Branch 1 taken 31 times.
✓ Branch 2 taken 19 times.
 50 for (size_type i = 1; i < a.size(); ++i) {
104 31 tn *= t;
105 31 res += a[i] * tn;
106 }
107
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 19 times.
 19 assert(res == res);
108 19 return res;
109 }
110
111
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 value_type Jac(const value_type& t) const { return Jac(t, 1); }
112
113 20022 value_type Jac(const value_type& t, const size_type& order) const {
114
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20022 times.
 20022 if (order >= a.size()) return 0;
115 20022 const size_type MaxOrder = 10;
116
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20022 times.
 20022 if (a.size() > MaxOrder)
117 ✗ throw std::invalid_argument(
118 ✗ "Cannot compute the derivative of order greater than 10.");
119 typedef path::binomials<MaxOrder> Binomials_t;
120 20022 const Binomials_t::Factorials_t& factors = Binomials_t::factorials();
121
122 20022 value_type res = 0;
123 20022 value_type tn = 1;
124
2/2
✓ Branch 1 taken 100060 times.
✓ Branch 2 taken 20022 times.
 120082 for (size_type i = order; i < a.size(); ++i) {
125 100060 res += value_type(factors[i] / factors[i - order]) * a[i] * tn;
126 100060 tn *= t;
127 }
128 20022 return res;
129 }
130
131 vector_t a;
132 }; // class Polynomial
133 } // namespace timeParameterization
134 } // namespace core
135 } // namespace hpp
136 #endif // HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
137