interval_matrix-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/
38 #ifndef FCL_CCD_INTERVAL_MATRIX_INL_H
39 #define FCL_CCD_INTERVAL_MATRIX_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 struct IMatrix3<double>;
49 
50 //==============================================================================
51 extern template
52 IMatrix3<double> rotationConstrain(const IMatrix3<double>& m);
53 
54 //==============================================================================
55 template <typename S>
57 
58 //==============================================================================
59 template <typename S>
61 {
62  v_[0].setValue(v);
63  v_[1].setValue(v);
64  v_[2].setValue(v);
65 }
66 
67 //==============================================================================
68 template <typename S>
70 {
71  v_[0].setValue(m.row(0)[0], m.row(0)[1], m.row(0)[2]);
72  v_[1].setValue(m.row(1)[0], m.row(1)[1], m.row(1)[2]);
73  v_[2].setValue(m.row(2)[0], m.row(2)[1], m.row(2)[2]);
74 }
75 
76 //==============================================================================
77 template <typename S>
78 IMatrix3<S>::IMatrix3(S m[3][3][2])
79 {
80  v_[0].setValue(m[0]);
81  v_[1].setValue(m[1]);
82  v_[2].setValue(m[2]);
83 }
84 
85 //==============================================================================
86 template <typename S>
88 {
89  v_[0].setValue(m[0]);
90  v_[1].setValue(m[1]);
91  v_[2].setValue(m[2]);
92 }
93 
94 //==============================================================================
95 template <typename S>
97 {
98  v_[0].setValue(m[0]);
99  v_[1].setValue(m[1]);
100  v_[2].setValue(m[2]);
101 }
102 
103 //==============================================================================
104 template <typename S>
106 {
107  v_[0] = v1;
108  v_[1] = v2;
109  v_[2] = v3;
110 }
111 
112 //==============================================================================
113 template <typename S>
115 {
116  v_[0].setValue(1, 0, 0);
117  v_[1].setValue(0, 1, 0);
118  v_[2].setValue(0, 0, 1);
119 }
120 
121 //==============================================================================
122 template <typename S>
124 {
125  return IVector3<S>(v_[0][i], v_[1][i], v_[2][i]);
126 }
127 
128 //==============================================================================
129 template <typename S>
130 const IVector3<S>& IMatrix3<S>::getRow(size_t i) const
131 {
132  return v_[i];
133 }
134 
135 //==============================================================================
136 template <typename S>
138 {
139  return Vector3<S>(v_[0][i][0], v_[1][i][0], v_[2][i][0]);
140 }
141 
142 //==============================================================================
143 template <typename S>
145 {
146  return Vector3<S>(v_[i][0][0], v_[i][1][0], v_[i][2][0]);
147 }
148 
149 //==============================================================================
150 template <typename S>
152 {
153  return Vector3<S>(v_[0][i][1], v_[1][i][1], v_[2][i][1]);
154 }
155 
156 //==============================================================================
157 template <typename S>
159 {
160  return Vector3<S>(v_[i][0][1], v_[i][1][1], v_[i][2][1]);
161 }
162 
163 //==============================================================================
164 template <typename S>
166 {
167  Matrix3<S> m;
168  m << v_[0][0][1], v_[0][1][1], v_[0][2][1],
169  v_[1][0][1], v_[1][1][1], v_[1][2][1],
170  v_[2][0][1], v_[2][1][1], v_[2][2][1];
171  return m;
172 }
173 
174 //==============================================================================
175 template <typename S>
177 {
178  Matrix3<S> m;
179  m << v_[0][0][1], v_[0][1][1], v_[0][2][1],
180  v_[1][0][1], v_[1][1][1], v_[1][2][1],
181  v_[2][0][1], v_[2][1][1], v_[2][2][1];
182  return m;
183 }
184 
185 //==============================================================================
186 template <typename S>
187 const Interval<S>& IMatrix3<S>::operator ()(size_t i, size_t j) const
188 {
189  return v_[i][j];
190 }
191 
192 //==============================================================================
193 template <typename S>
195 {
196  return v_[i][j];
197 }
198 
199 //==============================================================================
200 template <typename S>
202 {
203  return IMatrix3(IVector3<S>(v_[0].dot(m.col(0)), v_[0].dot(m.col(1)), v_[0].dot(m.col(2))),
204  IVector3<S>(v_[1].dot(m.col(0)), v_[1].dot(m.col(1)), v_[1].dot(m.col(2))),
205  IVector3<S>(v_[2].dot(m.col(0)), v_[2].dot(m.col(1)), v_[2].dot(m.col(2))));
206 }
207 
208 //==============================================================================
209 template <typename S>
211 {
212  return IVector3<S>(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
213 }
214 
215 //==============================================================================
216 template <typename S>
218 {
219  return IVector3<S>(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
220 }
221 
222 //==============================================================================
223 template <typename S>
225 {
226  const IVector3<S>& mc0 = m.getColumn(0);
227  const IVector3<S>& mc1 = m.getColumn(1);
228  const IVector3<S>& mc2 = m.getColumn(2);
229 
230  return IMatrix3(IVector3<S>(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
231  IVector3<S>(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
232  IVector3<S>(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
233 }
234 
235 //==============================================================================
236 template <typename S>
238 {
239  v_[0].setValue(v_[0].dot(m.col(0)), v_[0].dot(m.col(1)), v_[0].dot(m.col(2)));
240  v_[1].setValue(v_[1].dot(m.col(0)), v_[1].dot(m.col(1)), v_[1].dot(m.col(2)));
241  v_[2].setValue(v_[2].dot(m.col(0)), v_[2].dot(m.col(1)), v_[2].dot(m.col(2)));
242  return *this;
243 }
244 
245 //==============================================================================
246 template <typename S>
248 {
249  const IVector3<S>& mc0 = m.getColumn(0);
250  const IVector3<S>& mc1 = m.getColumn(1);
251  const IVector3<S>& mc2 = m.getColumn(2);
252 
253  v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
254  v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
255  v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
256  return *this;
257 }
258 
259 //==============================================================================
260 template <typename S>
262 {
263  return IMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
264 }
265 
266 //==============================================================================
267 template <typename S>
269 {
270  v_[0] += m.v_[0];
271  v_[1] += m.v_[1];
272  v_[2] += m.v_[2];
273  return *this;
274 }
275 
276 //==============================================================================
277 template <typename S>
279 {
280  return IMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
281 }
282 
283 //==============================================================================
284 template <typename S>
286 {
287  v_[0] -= m.v_[0];
288  v_[1] -= m.v_[1];
289  v_[2] -= m.v_[2];
290  return *this;
291 }
292 
293 //==============================================================================
294 template <typename S>
296 {
297  for(std::size_t i = 0; i < 3; ++i)
298  {
299  for(std::size_t j = 0; j < 3; ++j)
300  {
301  if(v_[i][j][0] < -1) v_[i][j][0] = -1;
302  else if(v_[i][j][0] > 1) v_[i][j][0] = 1;
303 
304  if(v_[i][j][1] < -1) v_[i][j][1] = -1;
305  else if(v_[i][j][1] > 1) v_[i][j][1] = 1;
306  }
307  }
308 
309  return *this;
310 }
311 
312 //==============================================================================
313 template <typename S>
314 void IMatrix3<S>::print() const
315 {
316  std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl;
317  std::cout << "[" << v_[1][0][0] << "," << v_[1][0][1] << "]" << " [" << v_[1][1][0] << "," << v_[1][1][1] << "]" << " [" << v_[1][2][0] << "," << v_[1][2][1] << "]" << std::endl;
318  std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl;
319 }
320 
321 //==============================================================================
322 template <typename S>
324 {
325  IMatrix3<S> res;
326  for(std::size_t i = 0; i < 3; ++i)
327  {
328  for(std::size_t j = 0; j < 3; ++j)
329  {
330  if(m(i, j)[0] < -1) res(i, j)[0] = -1;
331  else if(m(i, j)[0] > 1) res(i, j)[0] = 1;
332 
333  if(m(i, j)[1] < -1) res(i, j)[1] = -1;
334  else if(m(i, j)[1] > 1) res(i, j)[1] = 1;
335  }
336  }
337 
338  return res;
339 }
340 
341 } // namespace fcl
342 
343 #endif
fcl::IMatrix3::operator*=
IMatrix3 & operator*=(const IMatrix3 &m)
Definition: interval_matrix-inl.h:247
fcl::IMatrix3::getColumnLow
Vector3< S > getColumnLow(size_t i) const
Definition: interval_matrix-inl.h:137
fcl::IMatrix3::IMatrix3
IMatrix3()
Definition: interval_matrix-inl.h:56
fcl::IMatrix3::v_
IVector3< S > v_[3]
Definition: interval_matrix.h:50
fcl::IMatrix3::print
void print() const
Definition: interval_matrix-inl.h:314
fcl::IMatrix3::getHigh
Matrix3< S > getHigh() const
Definition: interval_matrix-inl.h:176
fcl::IMatrix3::operator-=
IMatrix3 & operator-=(const IMatrix3 &m)
Definition: interval_matrix-inl.h:285
fcl::IMatrix3::getColumn
IVector3< S > getColumn(size_t i) const
Definition: interval_matrix-inl.h:123
fcl::IMatrix3::operator()
const Interval< S > & operator()(size_t i, size_t j) const
Definition: interval_matrix-inl.h:187
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::IMatrix3::operator+
IMatrix3 operator+(const IMatrix3 &m) const
Definition: interval_matrix-inl.h:261
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::IMatrix3::getColumnHigh
Vector3< S > getColumnHigh(size_t i) const
Definition: interval_matrix-inl.h:151
fcl::IMatrix3::operator*
IVector3< S > operator*(const Vector3< S > &v) const
Definition: interval_matrix-inl.h:210
fcl::rotationConstrain
template IMatrix3< double > rotationConstrain(const IMatrix3< double > &m)
fcl::IMatrix3::setIdentity
void setIdentity()
Definition: interval_matrix-inl.h:114
fcl::IMatrix3::getLow
Matrix3< S > getLow() const
Definition: interval_matrix-inl.h:165
fcl::Interval
Interval class for [a, b].
Definition: interval.h:50
fcl::IMatrix3::getRowHigh
Vector3< S > getRowHigh(size_t i) const
Definition: interval_matrix-inl.h:158
fcl::IMatrix3::rotationConstrain
IMatrix3 & rotationConstrain()
Definition: interval_matrix-inl.h:295
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::IMatrix3::operator+=
IMatrix3 & operator+=(const IMatrix3 &m)
Definition: interval_matrix-inl.h:268
fcl::IMatrix3::getRow
const IVector3< S > & getRow(size_t i) const
Definition: interval_matrix-inl.h:130
fcl::IMatrix3::operator-
IMatrix3 operator-(const IMatrix3 &m) const
Definition: interval_matrix-inl.h:278
fcl::IMatrix3
Definition: interval_matrix.h:48
interval_matrix.h
fcl::IMatrix3::getRowLow
Vector3< S > getRowLow(size_t i) const
Definition: interval_matrix-inl.h:144
fcl::IVector3
Definition: interval_vector.h:47


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48