GCC Code Coverage Report

Directory: ./
File: src/path-validation/discretized.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 41 47 87.2%
Branches: 31 58 53.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <hpp/core/collision-path-validation-report.hh>
31 #include <hpp/core/collision-validation.hh>
32 #include <hpp/core/config-validations.hh>
33 #include <hpp/core/path-validation/discretized.hh>
34 #include <hpp/core/path.hh>
35 #include <hpp/core/projection-error.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/util/debug.hh>
38
39 namespace hpp {
40 namespace core {
41 namespace pathValidation {
42
43 62 DiscretizedPtr_t Discretized::create(const value_type& stepSize) {
44 62 Discretized* ptr = new Discretized(stepSize);
45 62 return DiscretizedPtr_t(ptr);
46 }
47
48 ✗ DiscretizedPtr_t Discretized::create(
49 const value_type& stepSize,
50 std::initializer_list<ConfigValidationPtr_t> validations) {
51 ✗ Discretized* ptr = new Discretized(stepSize, validations);
52 ✗ return DiscretizedPtr_t(ptr);
53 }
54
55 280 bool Discretized::validate(const PathPtr_t& path, bool reverse,
56 PathPtr_t& validPart,
57 PathValidationReportPtr_t& validationReport) {
58 hppDout(notice, "path validation, reverse : " << reverse);
59 280 ValidationReportPtr_t configReport;
60
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 280 times.
 280 assert(path);
61 280 bool valid = true;
62 280 const value_type tmin = path->timeRange().first;
63 280 const value_type tmax = path->timeRange().second;
64 280 unsigned finished = 0;
65
1/2
✓ Branch 3 taken 280 times.
✗ Branch 4 not taken.
 280 Configuration_t q(path->outputSize());
66 value_type T1, step, lastValidTime;
67
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 184 times.
 280 if (reverse) {
68 96 lastValidTime = tmax;
69 96 T1 = tmin;
70 96 step = -stepSize_;
71 } else {
72 184 lastValidTime = tmin;
73 184 T1 = tmax;
74 184 step = stepSize_;
75 }
76
77 280 value_type t = lastValidTime;
78
4/4
✓ Branch 0 taken 49392 times.
✓ Branch 1 taken 207 times.
✓ Branch 2 taken 49319 times.
✓ Branch 3 taken 73 times.
 49599 while (finished < 2 && valid) {
79
2/4
✓ Branch 2 taken 49319 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49314 times.
✗ Branch 6 not taken.
 49319 bool success = path->eval(q, t);
80
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 49314 times.
 49314 if (!success) {
81 ✗ validationReport = PathValidationReportPtr_t(new PathValidationReport(
82 ✗ t, ValidationReportPtr_t(new ProjectionError())));
83 ✗ valid = false;
84
3/4
✓ Branch 1 taken 49319 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 73 times.
✓ Branch 4 taken 49246 times.
 49314 } else if (!ConfigValidations::validate(q, configReport)) {
85
1/2
✓ Branch 1 taken 73 times.
✗ Branch 2 not taken.
 146 validationReport = CollisionPathValidationReportPtr_t(
86
1/2
✓ Branch 1 taken 73 times.
✗ Branch 2 not taken.
 146 new CollisionPathValidationReport(t, configReport));
87 73 valid = false;
88 } else {
89 49246 lastValidTime = t;
90 49246 t += step;
91 }
92
8/8
✓ Branch 0 taken 5424 times.
✓ Branch 1 taken 43895 times.
✓ Branch 2 taken 5257 times.
✓ Branch 3 taken 167 times.
✓ Branch 4 taken 43895 times.
✓ Branch 5 taken 5257 times.
✓ Branch 6 taken 250 times.
✓ Branch 7 taken 43645 times.
 49319 if ((reverse && t < T1) || (!reverse && t > T1)) {
93 417 t = T1;
94 417 finished++;
95 }
96 }
97
2/2
✓ Branch 0 taken 207 times.
✓ Branch 1 taken 73 times.
 280 if (valid) {
98 207 validPart = path;
99 207 return true;
100 } else {
101
2/2
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 60 times.
 73 if (reverse)
102
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 validPart = path->extract(lastValidTime, tmax);
103 else
104
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 validPart = path->extract(tmin, lastValidTime);
105 73 return false;
106 }
107 280 }
108
109 100 bool Discretized::validate(ConfigurationIn_t q, ValidationReportPtr_t& report) {
110
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 return ConfigValidations::validate(q, report);
111 }
112
113 } // namespace pathValidation
114 } // namespace core
115 } // namespace hpp
116