hpp-fcl  1.4.4
HPP fork of FCL -- The Flexible Collision Library
BV.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-2015, 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 
38 #ifndef HPP_FCL_BV_H
39 #define HPP_FCL_BV_H
40 
41 
42 #include <hpp/fcl/BV/kDOP.h>
43 #include <hpp/fcl/BV/AABB.h>
44 #include <hpp/fcl/BV/OBB.h>
45 #include <hpp/fcl/BV/RSS.h>
46 #include <hpp/fcl/BV/OBBRSS.h>
47 #include <hpp/fcl/BV/kIOS.h>
48 #include <hpp/fcl/math/transform.h>
49 
51 namespace hpp
52 {
53 namespace fcl
54 {
55 
57 namespace details
58 {
59 
61 template<typename BV1, typename BV2>
62 class Converter
63 {
64 private:
65  static void convert(const BV1& /*bv1*/, const Transform3f& /*tf1*/, BV2& /*bv2*/)
66  {
67  // should only use the specialized version, so it is private.
68  }
69 };
70 
71 
73 template<>
74 class Converter<AABB, AABB>
75 {
76 public:
77  static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
78  {
79  const Vec3f& center = bv1.center();
80  FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
81  Vec3f center2 = tf1.transform(center);
82  Vec3f delta(r, r, r);
83  bv2.min_ = center2 - delta;
84  bv2.max_ = center2 + delta;
85  }
86 };
87 
88 template<>
89 class Converter<AABB, OBB>
90 {
91 public:
92  static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
93  {
94  bv2.To.noalias() = tf1.transform(bv1.center());
95  bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
96  bv2.axes.noalias() = tf1.getRotation();
97  }
98 };
99 
100 template<>
101 class Converter<OBB, OBB>
102 {
103 public:
104  static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
105  {
106  bv2.extent.noalias() = bv1.extent;
107  bv2.To.noalias() = tf1.transform(bv1.To);
108  bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
109  }
110 };
111 
112 template<>
113 class Converter<OBBRSS, OBB>
114 {
115 public:
116  static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2)
117  {
118  Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
119  }
120 };
121 
122 template<>
123 class Converter<RSS, OBB>
124 {
125 public:
126  static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
127  {
128  bv2.extent.noalias() = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
129  bv2.To.noalias() = tf1.transform(bv1.Tr);
130  bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
131  }
132 };
133 
134 
135 template<typename BV1>
136 class Converter<BV1, AABB>
137 {
138 public:
139  static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
140  {
141  const Vec3f& center = bv1.center();
142  FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
143  Vec3f delta(r, r, r);
144  Vec3f center2 = tf1.transform(center);
145  bv2.min_ = center2 - delta;
146  bv2.max_ = center2 + delta;
147  }
148 };
149 
150 template<typename BV1>
151 class Converter<BV1, OBB>
152 {
153 public:
154  static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
155  {
156  AABB bv;
157  Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
158  Converter<AABB, OBB>::convert(bv, tf1, bv2);
159  }
160 };
161 
162 template<>
163 class Converter<OBB, RSS>
164 {
165 public:
166  static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
167  {
168  bv2.Tr = tf1.transform(bv1.To);
169  bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
170 
171  bv2.radius = bv1.extent[2];
172  bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
173  bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
174  }
175 };
176 
177 template<>
178 class Converter<RSS, RSS>
179 {
180 public:
181  static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
182  {
183  bv2.Tr.noalias() = tf1.transform(bv1.Tr);
184  bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
185 
186  bv2.radius = bv1.radius;
187  bv2.length[0] = bv1.length[0];
188  bv2.length[1] = bv1.length[1];
189  }
190 };
191 
192 template<>
193 class Converter<OBBRSS, RSS>
194 {
195 public:
196  static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2)
197  {
198  Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
199  }
200 };
201 
202 template<>
203 class Converter<AABB, RSS>
204 {
205 public:
206  static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
207  {
208  bv2.Tr = tf1.transform(bv1.center());
209 
211  FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
212  std::size_t id[3] = {0, 1, 2};
213 
214  for(std::size_t i = 1; i < 3; ++i)
215  {
216  for(std::size_t j = i; j > 0; --j)
217  {
218  if(d[j] > d[j-1])
219  {
220  {
221  FCL_REAL tmp = d[j];
222  d[j] = d[j-1];
223  d[j-1] = tmp;
224  }
225  {
226  std::size_t tmp = id[j];
227  id[j] = id[j-1];
228  id[j-1] = tmp;
229  }
230  }
231  }
232  }
233 
234  Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
235  bv2.radius = extent[id[2]];
236  bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
237  bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
238 
239  const Matrix3f& R = tf1.getRotation();
240  bool left_hand = (id[0] == (id[1] + 1) % 3);
241  if (left_hand) bv2.axes.col(0).noalias() = -R.col(id[0]);
242  else bv2.axes.col(0).noalias() = R.col(id[0]);
243  bv2.axes.col(1).noalias() = R.col(id[1]);
244  bv2.axes.col(2).noalias() = R.col(id[2]);
245  }
246 };
247 
248 template<>
249 class Converter<AABB, OBBRSS>
250 {
251 public:
252  static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2)
253  {
254  Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
255  Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
256  }
257 };
258 
259 }
260 
262 
263 
265 template<typename BV1, typename BV2>
266 static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
267 {
268  details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
269 }
270 
271 }
272 
273 } // namespace hpp
274 
275 #endif
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
double FCL_REAL
Definition: data_types.h:68
Definition: traversal_node_setup.h:775
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73