taylor_vector-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_TAYLOR_VECTOR_INL_H
39 #define FCL_CCD_TAYLOR_VECTOR_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class FCL_EXPORT TVector3<double>;
49 
50 //==============================================================================
51 extern template
52 void generateTVector3ForLinearFunc(TVector3<double>& v, const Vector3<double>& position, const Vector3<double>& velocity);
53 
54 //==============================================================================
55 extern template
56 TVector3<double> operator * (const Vector3<double>& v, const TaylorModel<double>& a);
57 
58 //==============================================================================
59 extern template
60 TVector3<double> operator + (const Vector3<double>& v1, const TVector3<double>& v2);
61 
62 //==============================================================================
63 extern template
64 TVector3<double> operator - (const Vector3<double>& v1, const TVector3<double>& v2);
65 
66 //==============================================================================
67 template <typename S>
69 {
70  // Do nothing
71 }
72 
73 //==============================================================================
74 template <typename S>
75 TVector3<S>::TVector3(const std::shared_ptr<TimeInterval<S>>& time_interval)
76 {
77  setTimeInterval(time_interval);
78 }
79 
80 //==============================================================================
81 template <typename S>
83 {
84  i_[0] = v[0];
85  i_[1] = v[1];
86  i_[2] = v[2];
87 }
88 
89 //==============================================================================
90 template <typename S>
92 {
93  i_[0] = v1;
94  i_[1] = v2;
95  i_[2] = v3;
96 }
97 
98 //==============================================================================
99 template <typename S>
100 TVector3<S>::TVector3(const Vector3<S>& v, const std::shared_ptr<TimeInterval<S>>& time_interval)
101 {
102  i_[0] = TaylorModel<S>(v[0], time_interval);
103  i_[1] = TaylorModel<S>(v[1], time_interval);
104  i_[2] = TaylorModel<S>(v[2], time_interval);
105 }
106 
107 //==============================================================================
108 template <typename S>
110 {
111  i_[0].setZero();
112  i_[1].setZero();
113  i_[2].setZero();
114 }
115 
116 //==============================================================================
117 template <typename S>
119 {
120  return TVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]);
121 }
122 
123 //==============================================================================
124 template <typename S>
126 {
127  return TVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]);
128 }
129 
130 //==============================================================================
131 template <typename S>
133 {
134  return TVector3(-i_[0], -i_[1], -i_[2]);
135 }
136 
137 //==============================================================================
138 template <typename S>
140 {
141  i_[0] += other.i_[0];
142  i_[1] += other.i_[1];
143  i_[2] += other.i_[2];
144  return *this;
145 }
146 
147 //==============================================================================
148 template <typename S>
150 {
151  i_[0] -= other.i_[0];
152  i_[1] -= other.i_[1];
153  i_[2] -= other.i_[2];
154  return *this;
155 }
156 
157 //==============================================================================
158 template <typename S>
160 {
161  return TVector3(i_[0] + other[0], i_[1] + other[1], i_[2] + other[2]);
162 }
163 
164 //==============================================================================
165 template <typename S>
167 {
168  i_[0] += other[0];
169  i_[1] += other[1];
170  i_[2] += other[2];
171  return *this;
172 }
173 
174 //==============================================================================
175 template <typename S>
177 {
178  return TVector3(i_[0] - other[0], i_[1] - other[1], i_[2] - other[2]);
179 }
180 
181 //==============================================================================
182 template <typename S>
184 {
185  i_[0] -= other[0];
186  i_[1] -= other[1];
187  i_[2] -= other[2];
188  return *this;
189 }
190 
191 //==============================================================================
192 template <typename S>
194 {
195  return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
196 }
197 
198 //==============================================================================
199 template <typename S>
201 {
202  i_[0] *= d;
203  i_[1] *= d;
204  i_[2] *= d;
205  return *this;
206 }
207 
208 //==============================================================================
209 template <typename S>
211 {
212  return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
213 }
214 
215 //==============================================================================
216 template <typename S>
218 {
219  i_[0] *= d;
220  i_[1] *= d;
221  i_[2] *= d;
222  return *this;
223 }
224 
225 //==============================================================================
226 template <typename S>
228 {
229  return i_[i];
230 }
231 
232 //==============================================================================
233 template <typename S>
235 {
236  return i_[i];
237 }
238 
239 //==============================================================================
240 template <typename S>
242 {
243  return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2];
244 }
245 
246 //==============================================================================
247 template <typename S>
249 {
250  return TVector3<S>(i_[1] * other.i_[2] - i_[2] * other.i_[1],
251  i_[2] * other.i_[0] - i_[0] * other.i_[2],
252  i_[0] * other.i_[1] - i_[1] * other.i_[0]);
253 }
254 
255 //==============================================================================
256 template <typename S>
258 {
259  return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
260 }
261 
262 //==============================================================================
263 template <typename S>
265 {
266  return TVector3<S>(i_[1] * other[2] - i_[2] * other[1],
267  i_[2] * other[0] - i_[0] * other[2],
268  i_[0] * other[1] - i_[1] * other[0]);
269 }
270 
271 //==============================================================================
272 template <typename S>
274 {
275  return i_[0].getBound().diameter() * i_[1].getBound().diameter() * i_[2].getBound().diameter();
276 }
277 
278 //==============================================================================
279 template <typename S>
281 {
282  return IVector3<S>(i_[0].getBound(), i_[1].getBound(), i_[2].getBound());
283 }
284 
285 //==============================================================================
286 template <typename S>
288 {
289  return IVector3<S>(i_[0].getBound(l, r), i_[1].getBound(l, r), i_[2].getBound(l, r));
290 }
291 
292 //==============================================================================
293 template <typename S>
295 {
296  return IVector3<S>(i_[0].getBound(t), i_[1].getBound(t), i_[2].getBound(t));
297 }
298 
299 //==============================================================================
300 template <typename S>
302 {
303  return IVector3<S>(i_[0].getTightBound(), i_[1].getTightBound(), i_[2].getTightBound());
304 }
305 
306 //==============================================================================
307 template <typename S>
309 {
310  return IVector3<S>(i_[0].getTightBound(l, r), i_[1].getTightBound(l, r), i_[2].getTightBound(l, r));
311 }
312 
313 //==============================================================================
314 template <typename S>
315 void TVector3<S>::print() const
316 {
317  i_[0].print();
318  i_[1].print();
319  i_[2].print();
320 }
321 
322 //==============================================================================
323 template <typename S>
325 {
326  return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2];
327 }
328 
329 //==============================================================================
330 template <typename S>
331 void TVector3<S>::setTimeInterval(const std::shared_ptr<TimeInterval<S>>& time_interval)
332 {
333  i_[0].setTimeInterval(time_interval);
334  i_[1].setTimeInterval(time_interval);
335  i_[2].setTimeInterval(time_interval);
336 }
337 
338 //==============================================================================
339 template <typename S>
341 {
342  i_[0].setTimeInterval(l, r);
343  i_[1].setTimeInterval(l, r);
344  i_[2].setTimeInterval(l, r);
345 }
346 
347 //==============================================================================
348 template <typename S>
349 const std::shared_ptr<TimeInterval<S>>& TVector3<S>::getTimeInterval() const
350 {
351  return i_[0].getTimeInterval();
352 }
353 
354 //==============================================================================
355 template <typename S>
356 void generateTVector3ForLinearFunc(TVector3<S>& v, const Vector3<S>& position, const Vector3<S>& velocity)
357 {
358  generateTaylorModelForLinearFunc(v[0], position[0], velocity[0]);
359  generateTaylorModelForLinearFunc(v[1], position[1], velocity[1]);
360  generateTaylorModelForLinearFunc(v[2], position[2], velocity[2]);
361 }
362 
363 //==============================================================================
364 template <typename S>
366 {
367  TVector3<S> res(a.getTimeInterval());
368  res[0] = a * v[0];
369  res[1] = a * v[1];
370  res[2] = a * v[2];
371 
372  return res;
373 }
374 
375 //==============================================================================
376 template <typename S>
378 {
379  return v2 + v1;
380 }
381 
382 //==============================================================================
383 template <typename S>
385 {
386  return -v2 + v1;
387 }
388 
389 } // namespace fcl
390 
391 #endif
fcl::TVector3::operator[]
const TaylorModel< S > & operator[](size_t i) const
Definition: taylor_vector-inl.h:227
fcl::TVector3::operator+
TVector3 operator+(const TVector3 &other) const
Definition: taylor_vector-inl.h:118
fcl::TVector3::setZero
void setZero()
Definition: taylor_vector-inl.h:109
taylor_vector.h
fcl::TVector3< double >
template class FCL_EXPORT TVector3< double >
fcl::generateTaylorModelForLinearFunc
template void generateTaylorModelForLinearFunc(TaylorModel< double > &tm, double p, double v)
fcl::TVector3::operator*
TVector3 operator*(const TaylorModel< S > &d) const
Definition: taylor_vector-inl.h:193
fcl::operator-
template TMatrix3< double > operator-(const Matrix3< double > &m1, const TMatrix3< double > &m2)
fcl::TVector3::print
void print() const
Definition: taylor_vector-inl.h:315
fcl::TVector3::operator-=
TVector3 & operator-=(const TVector3 &other)
Definition: taylor_vector-inl.h:149
fcl::TVector3::operator+=
TVector3 & operator+=(const TVector3 &other)
Definition: taylor_vector-inl.h:139
fcl::generateTVector3ForLinearFunc
template void generateTVector3ForLinearFunc(TVector3< double > &v, const Vector3< double > &position, const Vector3< double > &velocity)
fcl::TVector3::setTimeInterval
void setTimeInterval(const std::shared_ptr< TimeInterval< S >> &time_interval)
Definition: taylor_vector-inl.h:331
fcl::TaylorModel< double >
template class FCL_EXPORT TaylorModel< double >
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::TVector3::volumn
S volumn() const
Definition: taylor_vector-inl.h:273
fcl::TVector3::cross
TVector3 cross(const TVector3 &other) const
Definition: taylor_vector-inl.h:248
fcl::TimeInterval
Definition: time_interval.h:50
fcl::TaylorModel::getTimeInterval
const std::shared_ptr< TimeInterval< S > > & getTimeInterval() const
Definition: taylor_model-inl.h:93
r
S r
Definition: test_sphere_box.cpp:171
fcl::TVector3::squareLength
TaylorModel< S > squareLength() const
Definition: taylor_vector-inl.h:324
fcl::operator*
template TMatrix3< double > operator*(const Matrix3< double > &m, const TaylorModel< double > &a)
fcl::TaylorModel
TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function over a t...
Definition: taylor_model.h:58
fcl::TVector3
Definition: taylor_vector.h:48
fcl::TVector3::getTightBound
IVector3< S > getTightBound() const
Definition: taylor_vector-inl.h:301
fcl::TVector3::i_
TaylorModel< S > i_[3]
Definition: taylor_vector.h:50
fcl::TVector3::operator*=
TVector3 & operator*=(const TaylorModel< S > &d)
Definition: taylor_vector-inl.h:200
fcl::TVector3::operator-
TVector3 operator-() const
Definition: taylor_vector-inl.h:132
fcl::TVector3::getBound
IVector3< S > getBound() const
Definition: taylor_vector-inl.h:280
fcl::TVector3::dot
TaylorModel< S > dot(const TVector3 &other) const
Definition: taylor_vector-inl.h:241
fcl::TVector3::getTimeInterval
const std::shared_ptr< TimeInterval< S > > & getTimeInterval() const
Definition: taylor_vector-inl.h:349
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::TVector3::TVector3
TVector3()
Definition: taylor_vector-inl.h:68
fcl::operator+
template TMatrix3< double > operator+(const Matrix3< double > &m1, const TMatrix3< double > &m2)
fcl::IVector3
Definition: interval_vector.h:47


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