GCC Code Coverage Report

Directory: ./
File: src/continuous-validation/dichotomy.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 56 61 91.8%
Branches: 33 66 50.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 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/continuous-validation/dichotomy.hh>
32 #include <hpp/core/path-vector.hh>
33 #include <hpp/core/straight-path.hh>
34 #include <hpp/util/debug.hh>
35 #include <hpp/util/timer.hh>
36 #include <iterator>
37
38 #include "continuous-validation/helper.hh"
39 #include "continuous-validation/intervals.hh"
40
41 namespace hpp {
42 namespace core {
43 namespace continuousValidation {
44
45 HPP_DEFINE_TIMECOUNTER(CV_Dichotomy_validateStraightPath);
46
47 4 DichotomyPtr_t Dichotomy::create(const DevicePtr_t& robot,
48 const value_type& tolerance) {
49
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 Dichotomy* ptr = new Dichotomy(robot, tolerance);
50 4 DichotomyPtr_t shPtr(ptr);
51
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 ptr->init(shPtr);
52
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 ptr->initialize();
53 4 return shPtr;
54 }
55
56 8 Dichotomy::~Dichotomy() {}
57
58 318 bool Dichotomy::validateStraightPath(IntervalValidations_t& bpc,
59 const PathPtr_t& path, bool reverse,
60 PathPtr_t& validPart,
61 PathValidationReportPtr_t& report) {
62
2/2
✓ Branch 0 taken 86 times.
✓ Branch 1 taken 232 times.
 318 if (reverse)
63 86 return validateStraightPath<true>(bpc, path, validPart, report);
64 else
65 232 return validateStraightPath<false>(bpc, path, validPart, report);
66 }
67
68 template <bool reverse>
69 636 bool Dichotomy::validateStraightPath(IntervalValidations_t& bodyPairCollisions,
70 const PathPtr_t& path,
71 PathPtr_t& validPart,
72 PathValidationReportPtr_t& report) {
73 HPP_START_TIMECOUNTER(CV_Dichotomy_validateStraightPath);
74
75 // start by validating end of path
76 636 bool finished = false;
77 636 bool valid = true;
78
1/2
✓ Branch 1 taken 318 times.
✗ Branch 2 not taken.
 636 setPath(bodyPairCollisions, path, reverse);
79 636 Intervals validSubset;
80 636 const interval_t& tr(path->timeRange());
81 636 value_type t = first(tr, reverse);
82
2/4
✓ Branch 1 taken 318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 318 times.
✗ Branch 5 not taken.
 636 validSubset.unionInterval(std::make_pair(t, t));
83
1/2
✓ Branch 3 taken 318 times.
✗ Branch 4 not taken.
 636 Configuration_t q(path->outputSize());
84 value_type t0, t1, tmin, tmax;
85 636 int niters = 0;
86
2/2
✓ Branch 0 taken 8925 times.
✓ Branch 1 taken 318 times.
 18486 while (!finished) {
87 17850 ++niters;
88
2/4
✓ Branch 2 taken 8925 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8925 times.
✗ Branch 6 not taken.
 17850 bool success = path->eval(q, t);
89 17850 PathValidationReportPtr_t pathReport;
90 17850 interval_t interval;
91
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8925 times.
 17850 if (!success) {
92 ✗ report = PathValidationReportPtr_t(new PathValidationReport(
93 ✗ t, ValidationReportPtr_t(new ProjectionError())));
94 ✗ valid = false;
95
3/4
✓ Branch 1 taken 8925 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 51 times.
✓ Branch 4 taken 8874 times.
 17850 } else if (!validateConfiguration(bodyPairCollisions, q, t, interval,
96 pathReport)) {
97 102 report = pathReport;
98 102 valid = false;
99 } else {
100
1/2
✓ Branch 1 taken 8874 times.
✗ Branch 2 not taken.
 17748 validSubset.unionInterval(interval);
101 }
102
4/4
✓ Branch 0 taken 8874 times.
✓ Branch 1 taken 51 times.
✓ Branch 3 taken 267 times.
✓ Branch 4 taken 8607 times.
 17850 finished = (!valid || validSubset.contains(tr));
103 // Compute next parameter to check as middle of first non tested
104 // interval
105 17850 t0 = second(begin(validSubset.list(), reverse), reverse);
106 17850 t1 = second(tr, reverse);
107
2/2
✓ Branch 2 taken 7664 times.
✓ Branch 3 taken 1261 times.
 17850 if (validSubset.list().size() > 1)
108
1/2
✓ Branch 2 taken 7664 times.
✗ Branch 3 not taken.
 15328 t1 = first(Nth(validSubset.list(), 1, reverse), reverse);
109 17850 t = .5 * (t0 + t1);
110 }
111
2/2
✓ Branch 0 taken 51 times.
✓ Branch 1 taken 267 times.
 636 if (!valid) {
112
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 51 times.
 102 assert(begin(validSubset.list(), reverse).first <= first(tr, reverse));
113
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 51 times.
 102 assert(begin(validSubset.list(), reverse).second >= first(tr, reverse));
114 if (reverse) {
115
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 26 tmin = validSubset.list().rbegin()->first;
116 26 tmax = tr.second;
117 } else {
118 76 tmin = tr.first;
119 76 tmax = validSubset.list().begin()->second;
120 }
121
1/2
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
 102 validPart = path->extract(tmin, tmax);
122 } else {
123 534 validPart = path;
124 }
125 HPP_STOP_AND_DISPLAY_TIMECOUNTER(CV_Dichotomy_validateStraightPath);
126 if (niters > 1000) {
127 hppDout(notice,
128 "nb iterations, path length: " << niters << ", " << path->length());
129 hppDout(info, "path: " << *path);
130 }
131 636 return valid;
132 636 }
133
134 4 void Dichotomy::init(const DichotomyWkPtr_t weak) {
135
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 ContinuousValidation::init(weak);
136 4 weak_ = weak;
137 4 }
138
139 4 Dichotomy::Dichotomy(const DevicePtr_t& robot, const value_type& tolerance)
140 4 : ContinuousValidation(robot, tolerance), weak_() {
141 // Tolerance should be equal to 0, otherwise end of valid
142 // sub-path might be in collision.
143
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (tolerance != 0) {
144 ✗ throw std::runtime_error(
145 "Dichotomy path validation method does not"
146 ✗ "support penetration.");
147 }
148 4 }
149 } // namespace continuousValidation
150 } // namespace core
151 } // namespace hpp
152