FCL  0.6.0
Flexible Collision Library
interval_vector-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_INTERVAL_VECTOR_INL_H
39 #define FCL_CCD_INTERVAL_VECTOR_INL_H
40 
41 #include "fcl/math/motion/taylor_model/interval_vector.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 struct IVector3<double>;
49 
50 //==============================================================================
51 extern template
52 IVector3<double> bound(const IVector3<double>& i, const Vector3<double>& v);
53 
54 //==============================================================================
55 extern template
56 IVector3<double> bound(const IVector3<double>& i, const IVector3<double>& v);
57 
58 //==============================================================================
59 template <typename S>
60 IVector3<S>::IVector3()
61 {
62  // Do nothing
63 }
64 
65 //==============================================================================
66 template <typename S>
67 IVector3<S>::IVector3(S v)
68 {
69  setValue(v);
70 }
71 
72 //==============================================================================
73 template <typename S>
74 IVector3<S>::IVector3(S x, S y, S z)
75 {
76  setValue(x, y, z);
77 }
78 
79 //==============================================================================
80 template <typename S>
81 IVector3<S>::IVector3(S xl, S xu, S yl, S yu, S zl, S zu)
82 {
83  setValue(xl, xu, yl, yu, zl, zu);
84 }
85 
86 //==============================================================================
87 template <typename S>
88 IVector3<S>::IVector3(S v[3][2])
89 {
90  setValue(v);
91 }
92 
93 //==============================================================================
94 template <typename S>
95 IVector3<S>::IVector3(Interval<S> v[3])
96 {
97  setValue(v);
98 }
99 
100 //==============================================================================
101 template <typename S>
102 IVector3<S>::IVector3(const Interval<S>& v1, const Interval<S>& v2, const Interval<S>& v3)
103 {
104  setValue(v1, v2, v3);
105 }
106 
107 //==============================================================================
108 template <typename S>
109 IVector3<S>::IVector3(const Vector3<S>& v)
110 {
111  setValue(v);
112 }
113 
114 //==============================================================================
115 template <typename S>
116 void IVector3<S>::setValue(S v)
117 {
118  i_[0].setValue(v);
119  i_[1].setValue(v);
120  i_[2].setValue(v);
121 }
122 
123 //==============================================================================
124 template <typename S>
125 void IVector3<S>::setValue(S x, S y, S z)
126 {
127  i_[0].setValue(x);
128  i_[1].setValue(y);
129  i_[2].setValue(z);
130 }
131 
132 //==============================================================================
133 template <typename S>
134 void IVector3<S>::setValue(S xl, S xu, S yl, S yu, S zl, S zu)
135 {
136  i_[0].setValue(xl, xu);
137  i_[1].setValue(yl, yu);
138  i_[2].setValue(zl, zu);
139 }
140 
141 //==============================================================================
142 template <typename S>
143 void IVector3<S>::setValue(S v[3][2])
144 {
145  i_[0].setValue(v[0][0], v[0][1]);
146  i_[1].setValue(v[1][0], v[1][1]);
147  i_[2].setValue(v[2][0], v[2][1]);
148 }
149 
150 //==============================================================================
151 template <typename S>
152 void IVector3<S>::setValue(Interval<S> v[3])
153 {
154  i_[0] = v[0];
155  i_[1] = v[1];
156  i_[2] = v[2];
157 }
158 
159 //==============================================================================
160 template <typename S>
161 void IVector3<S>::setValue(const Interval<S>& v1, const Interval<S>& v2, const Interval<S>& v3)
162 {
163  i_[0] = v1;
164  i_[1] = v2;
165  i_[2] = v3;
166 }
167 
168 //==============================================================================
169 template <typename S>
170 void IVector3<S>::setValue(const Vector3<S>& v)
171 {
172  i_[0].setValue(v[0]);
173  i_[1].setValue(v[1]);
174  i_[2].setValue(v[2]);
175 }
176 
177 //==============================================================================
178 template <typename S>
179 void IVector3<S>::setValue(S v[])
180 {
181  i_[0].setValue(v[0]);
182  i_[1].setValue(v[1]);
183  i_[2].setValue(v[2]);
184 }
185 
186 //==============================================================================
187 template <typename S>
188 void IVector3<S>::setZero()
189 {
190  setValue((S)0.0);
191 }
192 
193 //==============================================================================
194 template <typename S>
195 IVector3<S> IVector3<S>::operator + (const IVector3<S>& other) const
196 {
197  return IVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]);
198 }
199 
200 //==============================================================================
201 template <typename S>
202 IVector3<S>& IVector3<S>::operator += (const IVector3<S>& other)
203 {
204  i_[0] += other[0];
205  i_[1] += other[1];
206  i_[2] += other[2];
207  return *this;
208 }
209 
210 //==============================================================================
211 template <typename S>
212 IVector3<S> IVector3<S>::operator - (const IVector3<S>& other) const
213 {
214  return IVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]);
215 }
216 
217 //==============================================================================
218 template <typename S>
219 IVector3<S>& IVector3<S>::operator -= (const IVector3<S>& other)
220 {
221  i_[0] -= other[0];
222  i_[1] -= other[1];
223  i_[2] -= other[2];
224  return *this;
225 }
226 
227 //==============================================================================
228 template <typename S>
229 Interval<S> IVector3<S>::dot(const IVector3& other) const
230 {
231  return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2];
232 }
233 
234 //==============================================================================
235 template <typename S>
236 IVector3<S> IVector3<S>::cross(const IVector3<S>& other) const
237 {
238  return IVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1],
239  i_[2] * other.i_[0] - i_[0] * other.i_[2],
240  i_[0] * other.i_[1] - i_[1] * other.i_[0]);
241 }
242 
243 //==============================================================================
244 template <typename S>
245 Interval<S> IVector3<S>::dot(const Vector3<S>& other) const
246 {
247  return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
248 }
249 
250 //==============================================================================
251 template <typename S>
252 const Interval<S>&IVector3<S>::operator [](size_t i) const
253 {
254  return i_[i];
255 }
256 
257 //==============================================================================
258 template <typename S>
259 Interval<S>&IVector3<S>::operator [](size_t i)
260 {
261  return i_[i];
262 }
263 
264 //==============================================================================
265 template <typename S>
266 Vector3<S> IVector3<S>::getLow() const
267 {
268  return Vector3<S>(i_[0][0], i_[1][0], i_[2][0]);
269 }
270 
271 //==============================================================================
272 template <typename S>
273 Vector3<S> IVector3<S>::getHigh() const
274 {
275  return Vector3<S>(i_[0][1], i_[1][1], i_[2][1]);
276 }
277 
278 //==============================================================================
279 template <typename S>
280 IVector3<S> IVector3<S>::cross(const Vector3<S>& other) const
281 {
282  return IVector3(i_[1] * other[2] - i_[2] * other[1],
283  i_[2] * other[0] - i_[0] * other[2],
284  i_[0] * other[1] - i_[1] * other[0]);
285 }
286 
287 //==============================================================================
288 template <typename S>
289 S IVector3<S>::volumn() const
290 {
291  return i_[0].diameter() * i_[1].diameter() * i_[2].diameter();
292 }
293 
294 //==============================================================================
295 template <typename S>
296 void IVector3<S>::print() const
297 {
298  std::cout << "[" << i_[0][0] << "," << i_[0][1] << "]" << std::endl;
299  std::cout << "[" << i_[1][0] << "," << i_[1][1] << "]" << std::endl;
300  std::cout << "[" << i_[2][0] << "," << i_[2][1] << "]" << std::endl;
301 }
302 
303 //==============================================================================
304 template <typename S>
305 Vector3<S> IVector3<S>::center() const
306 {
307  return Vector3<S>(i_[0].center(), i_[1].center(), i_[2].center());
308 }
309 
310 //==============================================================================
311 template <typename S>
312 void IVector3<S>::bound(const IVector3& v)
313 {
314  if(v[0][0] < i_[0][0]) i_[0][0] = v[0][0];
315  if(v[1][0] < i_[1][0]) i_[1][0] = v[1][0];
316  if(v[2][0] < i_[2][0]) i_[2][0] = v[2][0];
317 
318  if(v[0][1] > i_[0][1]) i_[0][1] = v[0][1];
319  if(v[1][1] > i_[1][1]) i_[1][1] = v[1][1];
320  if(v[2][1] > i_[2][1]) i_[2][1] = v[2][1];
321 }
322 
323 //==============================================================================
324 template <typename S>
325 void IVector3<S>::bound(const Vector3<S>& v)
326 {
327  if(v[0] < i_[0][0]) i_[0][0] = v[0];
328  if(v[1] < i_[1][0]) i_[1][0] = v[1];
329  if(v[2] < i_[2][0]) i_[2][0] = v[2];
330 
331  if(v[0] > i_[0][1]) i_[0][1] = v[0];
332  if(v[1] > i_[1][1]) i_[1][1] = v[1];
333  if(v[2] > i_[2][1]) i_[2][1] = v[2];
334 }
335 
336 //==============================================================================
337 template <typename S>
338 IVector3<S> bound(const IVector3<S>& i, const IVector3<S>& v)
339 {
340  IVector3<S> res(i);
341  if(v[0][0] < res.i_[0][0]) res.i_[0][0] = v[0][0];
342  if(v[1][0] < res.i_[1][0]) res.i_[1][0] = v[1][0];
343  if(v[2][0] < res.i_[2][0]) res.i_[2][0] = v[2][0];
344 
345  if(v[0][1] > res.i_[0][1]) res.i_[0][1] = v[0][1];
346  if(v[1][1] > res.i_[1][1]) res.i_[1][1] = v[1][1];
347  if(v[2][1] > res.i_[2][1]) res.i_[2][1] = v[2][1];
348 
349  return res;
350 }
351 
352 //==============================================================================
353 template <typename S>
354 IVector3<S> bound(const IVector3<S>& i, const Vector3<S>& v)
355 {
356  IVector3<S> res(i);
357  if(v[0] < res.i_[0][0]) res.i_[0][0] = v[0];
358  if(v[1] < res.i_[1][0]) res.i_[1][0] = v[1];
359  if(v[2] < res.i_[2][0]) res.i_[2][0] = v[2];
360 
361  if(v[0] > res.i_[0][1]) res.i_[0][1] = v[0];
362  if(v[1] > res.i_[1][1]) res.i_[1][1] = v[1];
363  if(v[2] > res.i_[2][1]) res.i_[2][1] = v[2];
364 
365  return res;
366 }
367 
368 //==============================================================================
369 template <typename S>
370 bool IVector3<S>::overlap(const IVector3& v) const
371 {
372  if(v[0][1] < i_[0][0]) return false;
373  if(v[1][1] < i_[1][0]) return false;
374  if(v[2][1] < i_[2][0]) return false;
375 
376  if(v[0][0] > i_[0][1]) return false;
377  if(v[1][0] > i_[1][1]) return false;
378  if(v[2][0] > i_[2][1]) return false;
379 
380  return true;
381 }
382 
383 //==============================================================================
384 template <typename S>
385 bool IVector3<S>::contain(const IVector3& v) const
386 {
387  if(v[0][0] < i_[0][0]) return false;
388  if(v[1][0] < i_[1][0]) return false;
389  if(v[2][0] < i_[2][0]) return false;
390 
391  if(v[0][1] > i_[0][1]) return false;
392  if(v[1][1] > i_[1][1]) return false;
393  if(v[2][1] > i_[2][1]) return false;
394 
395  return true;
396 }
397 
398 } // namespace fcl
399 
400 #endif
Main namespace.
Definition: broadphase_bruteforce-inl.h:45