FCL  0.6.0
Flexible Collision Library
taylor_matrix-inl.h
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_TAYLOR_MATRIX_INL_H
39 #define FCL_CCD_TAYLOR_MATRIX_INL_H
40 
41 #include "fcl/math/motion/taylor_model/taylor_matrix.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class TMatrix3<double>;
49 
50 //==============================================================================
51 extern template
52 TMatrix3<double> rotationConstrain(const TMatrix3<double>& m);
53 
54 //==============================================================================
55 extern template
56 TMatrix3<double> operator * (const Matrix3<double>& m, const TaylorModel<double>& a);
57 
58 //==============================================================================
59 extern template
60 TMatrix3<double> operator * (const TaylorModel<double>& a, const Matrix3<double>& m);
61 
62 //==============================================================================
63 extern template
64 TMatrix3<double> operator * (const TaylorModel<double>& a, const TMatrix3<double>& m);
65 
66 //==============================================================================
67 extern template
68 TMatrix3<double> operator * (double d, const TMatrix3<double>& m);
69 
70 //==============================================================================
71 extern template
72 TMatrix3<double> operator + (const Matrix3<double>& m1, const TMatrix3<double>& m2);
73 
74 //==============================================================================
75 extern template
76 TMatrix3<double> operator - (const Matrix3<double>& m1, const TMatrix3<double>& m2);
77 
78 //==============================================================================
79 template <typename S>
80 TMatrix3<S>::TMatrix3()
81 {
82  // Do nothing
83 }
84 
85 //==============================================================================
86 template <typename S>
87 TMatrix3<S>::TMatrix3(const std::shared_ptr<TimeInterval<S>>& time_interval)
88 {
89  setTimeInterval(time_interval);
90 }
91 
92 //==============================================================================
93 template <typename S>
94 TMatrix3<S>::TMatrix3(TaylorModel<S> m[3][3])
95 {
96  v_[0] = TVector3<S>(m[0]);
97  v_[1] = TVector3<S>(m[1]);
98  v_[2] = TVector3<S>(m[2]);
99 }
100 
101 //==============================================================================
102 template <typename S>
103 TMatrix3<S>::TMatrix3(const TVector3<S>& v1, const TVector3<S>& v2, const TVector3<S>& v3)
104 {
105  v_[0] = v1;
106  v_[1] = v2;
107  v_[2] = v3;
108 }
109 
110 //==============================================================================
111 template <typename S>
112 TMatrix3<S>::TMatrix3(const Matrix3<S>& m, const std::shared_ptr<TimeInterval<S>>& time_interval)
113 {
114  v_[0] = TVector3<S>(m.row(0), time_interval);
115  v_[1] = TVector3<S>(m.row(1), time_interval);
116  v_[2] = TVector3<S>(m.row(2), time_interval);
117 }
118 
119 //==============================================================================
120 template <typename S>
121 void TMatrix3<S>::setIdentity()
122 {
123  setZero();
124  v_[0][0].coeff(0) = 1;
125  v_[1][1].coeff(0) = 1;
126  v_[2][2].coeff(0) = 1;
127 
128 }
129 
130 //==============================================================================
131 template <typename S>
132 void TMatrix3<S>::setZero()
133 {
134  v_[0].setZero();
135  v_[1].setZero();
136  v_[2].setZero();
137 }
138 
139 //==============================================================================
140 template <typename S>
141 TVector3<S> TMatrix3<S>::getColumn(size_t i) const
142 {
143  return TVector3<S>(v_[0][i], v_[1][i], v_[2][i]);
144 }
145 
146 //==============================================================================
147 template <typename S>
148 const TVector3<S>& TMatrix3<S>::getRow(size_t i) const
149 {
150  return v_[i];
151 }
152 
153 //==============================================================================
154 template <typename S>
155 const TaylorModel<S>& TMatrix3<S>::operator () (size_t i, size_t j) const
156 {
157  return v_[i][j];
158 }
159 
160 //==============================================================================
161 template <typename S>
162 TaylorModel<S>& TMatrix3<S>::operator () (size_t i, size_t j)
163 {
164  return v_[i][j];
165 }
166 
167 //==============================================================================
168 template <typename S>
169 TMatrix3<S> TMatrix3<S>::operator * (const Matrix3<S>& m) const
170 {
171  const Vector3<S>& mc0 = m.col(0);
172  const Vector3<S>& mc1 = m.col(1);
173  const Vector3<S>& mc2 = m.col(2);
174 
175  return TMatrix3(TVector3<S>(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
176  TVector3<S>(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
177  TVector3<S>(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
178 }
179 
180 //==============================================================================
181 template <typename S>
182 TVector3<S> TMatrix3<S>::operator * (const Vector3<S>& v) const
183 {
184  return TVector3<S>(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
185 }
186 
187 //==============================================================================
188 template <typename S>
189 TVector3<S> TMatrix3<S>::operator * (const TVector3<S>& v) const
190 {
191  return TVector3<S>(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
192 }
193 
194 //==============================================================================
195 template <typename S>
196 TMatrix3<S> TMatrix3<S>::operator * (const TMatrix3<S>& m) const
197 {
198  const TVector3<S>& mc0 = m.getColumn(0);
199  const TVector3<S>& mc1 = m.getColumn(1);
200  const TVector3<S>& mc2 = m.getColumn(2);
201 
202  return TMatrix3(TVector3<S>(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
203  TVector3<S>(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
204  TVector3<S>(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
205 }
206 
207 //==============================================================================
208 template <typename S>
209 TMatrix3<S> TMatrix3<S>::operator * (const TaylorModel<S>& d) const
210 {
211  return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
212 }
213 
214 //==============================================================================
215 template <typename S>
216 TMatrix3<S> TMatrix3<S>::operator * (S d) const
217 {
218  return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d);
219 }
220 
221 //==============================================================================
222 template <typename S>
223 TMatrix3<S>& TMatrix3<S>::operator *= (const Matrix3<S>& m)
224 {
225  const Vector3<S>& mc0 = m.col(0);
226  const Vector3<S>& mc1 = m.col(1);
227  const Vector3<S>& mc2 = m.col(2);
228 
229  v_[0] = TVector3<S>(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
230  v_[1] = TVector3<S>(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
231  v_[2] = TVector3<S>(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
232  return *this;
233 }
234 
235 //==============================================================================
236 template <typename S>
237 TMatrix3<S>& TMatrix3<S>::operator *= (const TMatrix3<S>& m)
238 {
239  const TVector3<S>& mc0 = m.getColumn(0);
240  const TVector3<S>& mc1 = m.getColumn(1);
241  const TVector3<S>& mc2 = m.getColumn(2);
242 
243  v_[0] = TVector3<S>(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
244  v_[1] = TVector3<S>(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
245  v_[2] = TVector3<S>(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
246 
247  return *this;
248 }
249 
250 //==============================================================================
251 template <typename S>
252 TMatrix3<S>& TMatrix3<S>::operator *= (const TaylorModel<S>& d)
253 {
254  v_[0] *= d;
255  v_[1] *= d;
256  v_[2] *= d;
257  return *this;
258 }
259 
260 //==============================================================================
261 template <typename S>
262 TMatrix3<S>& TMatrix3<S>::operator *= (S d)
263 {
264  v_[0] *= d;
265  v_[1] *= d;
266  v_[2] *= d;
267  return *this;
268 }
269 
270 //==============================================================================
271 template <typename S>
272 TMatrix3<S> TMatrix3<S>::operator + (const TMatrix3<S>& m) const
273 {
274  return TMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
275 }
276 
277 //==============================================================================
278 template <typename S>
279 TMatrix3<S>& TMatrix3<S>::operator += (const TMatrix3<S>& m)
280 {
281  v_[0] += m.v_[0];
282  v_[1] += m.v_[1];
283  v_[2] += m.v_[2];
284  return *this;
285 }
286 
287 //==============================================================================
288 template <typename S>
289 TMatrix3<S> TMatrix3<S>::operator + (const Matrix3<S>& m) const
290 {
291  TMatrix3 res = *this;
292  res += m;
293  return res;
294 }
295 
296 //==============================================================================
297 template <typename S>
298 TMatrix3<S>& TMatrix3<S>::operator += (const Matrix3<S>& m)
299 {
300  for(std::size_t i = 0; i < 3; ++i)
301  {
302  for(std::size_t j = 0; j < 3; ++j)
303  v_[i][j] += m(i, j);
304  }
305 
306  return *this;
307 }
308 
309 //==============================================================================
310 template <typename S>
311 TMatrix3<S> TMatrix3<S>::operator - (const TMatrix3<S>& m) const
312 {
313  return TMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
314 }
315 
316 //==============================================================================
317 template <typename S>
318 TMatrix3<S>& TMatrix3<S>::operator -= (const TMatrix3<S>& m)
319 {
320  v_[0] -= m.v_[0];
321  v_[1] -= m.v_[1];
322  v_[2] -= m.v_[2];
323  return *this;
324 }
325 
326 //==============================================================================
327 template <typename S>
328 TMatrix3<S> TMatrix3<S>::operator - (const Matrix3<S>& m) const
329 {
330  TMatrix3 res = *this;
331  res -= m;
332  return res;
333 }
334 
335 //==============================================================================
336 template <typename S>
337 TMatrix3<S>& TMatrix3<S>::operator -= (const Matrix3<S>& m)
338 {
339  for(std::size_t i = 0; i < 3; ++i)
340  {
341  for(std::size_t j = 0; j < 3; ++j)
342  v_[i][j] -= m(i, j);
343  }
344 
345  return *this;
346 }
347 
348 //==============================================================================
349 template <typename S>
350 TMatrix3<S> TMatrix3<S>::operator - () const
351 {
352  return TMatrix3<S>(-v_[0], -v_[1], -v_[2]);
353 }
354 
355 //==============================================================================
356 template <typename S>
357 void TMatrix3<S>::print() const
358 {
359  getColumn(0).print();
360  getColumn(1).print();
361  getColumn(2).print();
362 }
363 
364 //==============================================================================
365 template <typename S>
366 IMatrix3<S> TMatrix3<S>::getBound() const
367 {
368  return IMatrix3<S>(v_[0].getBound(), v_[1].getBound(), v_[2].getBound());
369 }
370 
371 //==============================================================================
372 template <typename S>
373 IMatrix3<S> TMatrix3<S>::getBound(S l, S r) const
374 {
375  return IMatrix3<S>(v_[0].getBound(l, r), v_[1].getBound(l, r), v_[2].getBound(l, r));
376 }
377 
378 //==============================================================================
379 template <typename S>
380 IMatrix3<S> TMatrix3<S>::getBound(S t) const
381 {
382  return IMatrix3<S>(v_[0].getBound(t), v_[1].getBound(t), v_[2].getBound(t));
383 }
384 
385 //==============================================================================
386 template <typename S>
387 IMatrix3<S> TMatrix3<S>::getTightBound() const
388 {
389  return IMatrix3<S>(v_[0].getTightBound(), v_[1].getTightBound(), v_[2].getTightBound());
390 }
391 
392 //==============================================================================
393 template <typename S>
394 IMatrix3<S> TMatrix3<S>::getTightBound(S l, S r) const
395 {
396  return IMatrix3<S>(v_[0].getTightBound(l, r), v_[1].getTightBound(l, r), v_[2].getTightBound(l, r));
397 }
398 
399 //==============================================================================
400 template <typename S>
401 S TMatrix3<S>::diameter() const
402 {
403  S d = 0;
404 
405  S tmp = v_[0][0].remainder().diameter();
406  if(tmp > d) d = tmp;
407  tmp = v_[0][1].remainder().diameter();
408  if(tmp > d) d = tmp;
409  tmp = v_[0][2].remainder().diameter();
410  if(tmp > d) d = tmp;
411 
412  tmp = v_[1][0].remainder().diameter();
413  if(tmp > d) d = tmp;
414  tmp = v_[1][1].remainder().diameter();
415  if(tmp > d) d = tmp;
416  tmp = v_[1][2].remainder().diameter();
417  if(tmp > d) d = tmp;
418 
419  tmp = v_[2][0].remainder().diameter();
420  if(tmp > d) d = tmp;
421  tmp = v_[2][1].remainder().diameter();
422  if(tmp > d) d = tmp;
423  tmp = v_[2][2].remainder().diameter();
424  if(tmp > d) d = tmp;
425 
426  return d;
427 }
428 
429 //==============================================================================
430 template <typename S>
431 void TMatrix3<S>::setTimeInterval(const std::shared_ptr<TimeInterval<S>>& time_interval)
432 {
433  v_[0].setTimeInterval(time_interval);
434  v_[1].setTimeInterval(time_interval);
435  v_[2].setTimeInterval(time_interval);
436 }
437 
438 //==============================================================================
439 template <typename S>
440 void TMatrix3<S>::setTimeInterval(S l, S r)
441 {
442  v_[0].setTimeInterval(l, r);
443  v_[1].setTimeInterval(l, r);
444  v_[2].setTimeInterval(l, r);
445 }
446 
447 //==============================================================================
448 template <typename S>
449 const std::shared_ptr<TimeInterval<S>>& TMatrix3<S>::getTimeInterval() const
450 {
451  return v_[0].getTimeInterval();
452 }
453 
454 //==============================================================================
455 template <typename S>
456 TMatrix3<S>& TMatrix3<S>::rotationConstrain()
457 {
458  for(std::size_t i = 0; i < 3; ++i)
459  {
460  for(std::size_t j = 0; j < 3; ++j)
461  {
462  if(v_[i][j].remainder()[0] < -1) v_[i][j].remainder()[0] = -1;
463  else if(v_[i][j].remainder()[0] > 1) v_[i][j].remainder()[0] = 1;
464 
465  if(v_[i][j].remainder()[1] < -1) v_[i][j].remainder()[1] = -1;
466  else if(v_[i][j].remainder()[1] > 1) v_[i][j].remainder()[1] = 1;
467 
468  if((v_[i][j].remainder()[0] == -1) && (v_[i][j].remainder()[1] == 1))
469  {
470  v_[i][j].coeff(0) = 0;
471  v_[i][j].coeff(1) = 0;
472  v_[i][j].coeff(2) = 0;
473  v_[i][j].coeff(3) = 0;
474  }
475  }
476  }
477 
478  return *this;
479 }
480 
481 //==============================================================================
482 template <typename S>
483 TMatrix3<S> rotationConstrain(const TMatrix3<S>& m)
484 {
485  TMatrix3<S> res;
486 
487  for(std::size_t i = 0; i < 3; ++i)
488  {
489  for(std::size_t j = 0; j < 3; ++j)
490  {
491  if(m(i, j).remainder()[0] < -1) res(i, j).remainder()[0] = -1;
492  else if(m(i, j).remainder()[0] > 1) res(i, j).remainder()[0] = 1;
493 
494  if(m(i, j).remainder()[1] < -1) res(i, j).remainder()[1] = -1;
495  else if(m(i, j).remainder()[1] > 1) res(i, j).remainder()[1] = 1;
496 
497  if((m(i, j).remainder()[0] == -1) && (m(i, j).remainder()[1] == 1))
498  {
499  res(i, j).coeff(0) = 0;
500  res(i, j).coeff(1) = 0;
501  res(i, j).coeff(2) = 0;
502  res(i, j).coeff(3) = 0;
503  }
504  }
505  }
506 
507  return res;
508 }
509 
510 //==============================================================================
511 template <typename S>
512 TMatrix3<S> operator * (const Matrix3<S>& m, const TaylorModel<S>& a)
513 {
514  TMatrix3<S> res(a.getTimeInterval());
515  res(0, 0) = a * m(0, 0);
516  res(0, 1) = a * m(0, 1);
517  res(0, 1) = a * m(0, 2);
518 
519  res(1, 0) = a * m(1, 0);
520  res(1, 1) = a * m(1, 1);
521  res(1, 1) = a * m(1, 2);
522 
523  res(2, 0) = a * m(2, 0);
524  res(2, 1) = a * m(2, 1);
525  res(2, 1) = a * m(2, 2);
526 
527  return res;
528 }
529 
530 //==============================================================================
531 template <typename S>
532 TMatrix3<S> operator * (const TaylorModel<S>& a, const Matrix3<S>& m)
533 {
534  return m * a;
535 }
536 
537 //==============================================================================
538 template <typename S>
539 TMatrix3<S> operator * (const TaylorModel<S>& a, const TMatrix3<S>& m)
540 {
541  return m * a;
542 }
543 
544 //==============================================================================
545 template <typename S>
546 TMatrix3<S> operator * (S d, const TMatrix3<S>& m)
547 {
548  return m * d;
549 }
550 
551 //==============================================================================
552 template <typename S>
553 TMatrix3<S> operator + (const Matrix3<S>& m1, const TMatrix3<S>& m2)
554 {
555  return m2 + m1;
556 }
557 
558 //==============================================================================
559 template <typename S>
560 TMatrix3<S> operator - (const Matrix3<S>& m1, const TMatrix3<S>& m2)
561 {
562  return -m2 + m1;
563 }
564 
565 } // namespace fcl
566 
567 #endif
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45