GCC Code Coverage Report

Directory: ./
File: src/liegroup-element.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 52 52 100.0%
Branches: 33 58 56.9%
Line Branch Exec Source
1 // Copyright (c) 2017, CNRS
2 // Authors: Florent Lamiraux
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 #include <boost/serialization/export.hpp>
30 #include <boost/serialization/split_free.hpp>
31 #include <hpp/pinocchio/liegroup-element.hh>
32 #include <hpp/pinocchio/serialization.hh>
33 #include <hpp/util/serialization.hh>
34
35 #include "../src/addition-visitor.hh"
36 #include "../src/is-normalized-visitor.hh"
37 #include "../src/log-visitor.hh"
38 #include "../src/size-visitor.hh"
39 #include "../src/substraction-visitor.hh"
40
41 namespace hpp {
42 namespace pinocchio {
43 typedef std::vector<LiegroupType> LiegroupTypes;
44
45 template <typename vector_type>
46 1204 LiegroupElementBase<vector_type>& LiegroupElementBase<vector_type>::operator+=(
47 vectorIn_t v) {
48
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 602 times.
 1204 assert(this->space_->nv() == v.size());
49
50 1204 liegroupType::AdditionVisitor<vector_type> av(this->value_, v);
51 1204 for (LiegroupTypes::const_iterator it = this->space_->liegroupTypes().begin();
52
2/2
✓ Branch 5 taken 602 times.
✓ Branch 6 taken 602 times.
 2408 it != this->space_->liegroupTypes().end(); ++it) {
53
1/2
✓ Branch 2 taken 602 times.
✗ Branch 3 not taken.
 1204 boost::apply_visitor(av, *it);
54 }
55
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 602 times.
 1204 assert(av.iq_ == this->space_->nq());
56
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 602 times.
 1204 assert(av.iv_ == this->space_->nv());
57 1204 return *this;
58 }
59
60 template LiegroupElementBase<vector_t>&
61 LiegroupElementBase<vector_t>::operator+=(vectorIn_t);
62 template LiegroupElementBase<vectorOut_t>&
63 LiegroupElementBase<vectorOut_t>::operator+=(vectorIn_t);
64
65 template <typename vector_type>
66 1204 LiegroupElement operator+(const LiegroupElementConstBase<vector_type>& e,
67 vectorIn_t v) {
68 1204 LiegroupElement result(e);
69
2/4
✓ Branch 1 taken 602 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 602 times.
✗ Branch 5 not taken.
 1204 result += v;
70 1204 return result;
71 }
72
73 template LiegroupElement operator+(const LiegroupElementConstBase<vector_t>& e,
74 vectorIn_t v);
75 template LiegroupElement operator+(
76 const LiegroupElementConstBase<vectorIn_t>& e, vectorIn_t v);
77 template LiegroupElement operator+(
78 const LiegroupElementConstBase<vectorOut_t>& e, vectorIn_t v);
79
80 template <typename vector_type1, typename vector_type2>
81 800 vector_t operator-(const LiegroupElementConstBase<vector_type1>& e1,
82 const LiegroupElementConstBase<vector_type2>& e2) {
83
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 400 times.
 800 assert(e1.space()->nq() == e2.space()->nq());
84
1/2
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 800 vector_t result(e1.space()->nv());
85
86
2/4
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 400 times.
 800 assert(*e1.space() == *e2.space());
87
88 800 liegroupType::SubstractionVisitor<vector_type1, vector_type2> sv(
89 e1.vector(), e2.vector(), result);
90 800 for (LiegroupTypes::const_iterator it = e1.space()->liegroupTypes().begin();
91
2/2
✓ Branch 6 taken 400 times.
✓ Branch 7 taken 400 times.
 1600 it != e1.space()->liegroupTypes().end(); ++it) {
92
1/2
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
 800 boost::apply_visitor(sv, *it);
93 }
94 1600 return result;
95 }
96
97 template vector_t operator-(const LiegroupElementConstBase<vector_t>& e1,
98 const LiegroupElementConstBase<vector_t>& e2);
99 template vector_t operator-(const LiegroupElementConstBase<vector_t>& e1,
100 const LiegroupElementConstBase<vectorIn_t>& e2);
101 template vector_t operator-(const LiegroupElementConstBase<vector_t>& e1,
102 const LiegroupElementConstBase<vectorOut_t>& e2);
103 template vector_t operator-(const LiegroupElementConstBase<vectorIn_t>& e1,
104 const LiegroupElementConstBase<vector_t>& e2);
105 template vector_t operator-(const LiegroupElementConstBase<vectorIn_t>& e1,
106 const LiegroupElementConstBase<vectorIn_t>& e2);
107 template vector_t operator-(const LiegroupElementConstBase<vectorIn_t>& e1,
108 const LiegroupElementConstBase<vectorOut_t>& e2);
109 template vector_t operator-(const LiegroupElementConstBase<vectorOut_t>& e1,
110 const LiegroupElementConstBase<vector_t>& e2);
111 template vector_t operator-(const LiegroupElementConstBase<vectorOut_t>& e1,
112 const LiegroupElementConstBase<vectorIn_t>& e2);
113 template vector_t operator-(const LiegroupElementConstBase<vectorOut_t>& e1,
114 const LiegroupElementConstBase<vectorOut_t>& e2);
115
116 template <typename vector_type>
117 10 bool checkNormalized(const LiegroupElementConstBase<vector_type>& e1,
118 const value_type& eps) {
119 10 bool result = true;
120
121 10 liegroupType::IsNormalizedVisitor<vector_type> isNormalizedvisitor(
122 e1.vector(), eps, result);
123 10 for (LiegroupTypes::const_iterator it = e1.space()->liegroupTypes().begin();
124
2/2
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 5 times.
 30 it != e1.space()->liegroupTypes().end(); ++it) {
125
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 20 boost::apply_visitor(isNormalizedvisitor, *it);
126 }
127 10 return result;
128 }
129
130 template bool checkNormalized(const LiegroupElementConstBase<vector_t>& e1,
131 const value_type& eps);
132 template bool checkNormalized(const LiegroupElementConstBase<vectorIn_t>& e1,
133 const value_type& eps);
134 template bool checkNormalized(const LiegroupElementConstBase<vectorOut_t>& e1,
135 const value_type& eps);
136
137 template <typename vector_type>
138 202 vector_t log(const LiegroupElementConstBase<vector_type>& lge) {
139 using liegroupType::LogVisitor;
140
1/2
✓ Branch 4 taken 101 times.
✗ Branch 5 not taken.
 202 vector_t res(lge.space()->nv());
141 202 size_type iq = 0, iv = 0;
142 202 for (LiegroupTypes::const_iterator it = lge.space()->liegroupTypes().begin();
143
2/2
✓ Branch 6 taken 102 times.
✓ Branch 7 taken 101 times.
 406 it != lge.space()->liegroupTypes().end(); ++it) {
144 204 liegroupType::SizeVisitor sizeVisitor;
145
1/2
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
 204 boost::apply_visitor(sizeVisitor, *it);
146
4/8
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 102 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 102 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 102 times.
✗ Branch 12 not taken.
 204 LogVisitor logVisitor(lge.vector().segment(iq, sizeVisitor.nq),
147
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
 204 res.segment(iv, sizeVisitor.nv));
148
1/2
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
 204 boost::apply_visitor(logVisitor, *it);
149 204 iq += sizeVisitor.nq;
150 204 iv += sizeVisitor.nv;
151 }
152 202 return res;
153 }
154
155 template vector_t log(const LiegroupElementConstBase<vector_t>& lge);
156 template vector_t log(const LiegroupElementConstBase<vectorIn_t>& lge);
157 template vector_t log(const LiegroupElementConstBase<vectorOut_t>& lge);
158 } // namespace pinocchio
159 } // namespace hpp
160
161 namespace boost {
162 namespace serialization {
163 template <class Archive>
164 24 void load(Archive& ar, hpp::pinocchio::LiegroupElement& c,
165 const unsigned int version) {
166 (void)version;
167 24 hpp::pinocchio::LiegroupSpacePtr_t space;
168
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 hpp::pinocchio::vector_t vector;
169
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 ar& make_nvp("space", space);
170
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 hpp::serialization::remove_duplicate::serialize_vector(ar, "vector", vector,
171 version);
172
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 c = hpp::pinocchio::LiegroupElement(vector, space);
173 }
174 template <class Archive>
175 24 void save(Archive& ar, const hpp::pinocchio::LiegroupElement& c,
176 const unsigned int version) {
177 (void)version;
178
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 24 ar& make_nvp("space", c.space());
179 24 hpp::serialization::remove_duplicate::save_vector(ar, "vector", c.vector(),
180 version);
181 }
182 } // namespace serialization
183 } // namespace boost
184
185 48 HPP_SERIALIZATION_SPLIT_FREE_IMPLEMENT(hpp::pinocchio::LiegroupElement)
186