00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #ifndef FCL_BV_H
00038 #define FCL_BV_H
00039
00040
00041 #include "fcl/BV/kDOP.h"
00042 #include "fcl/BV/AABB.h"
00043 #include "fcl/BV/OBB.h"
00044 #include "fcl/BV/RSS.h"
00045 #include "fcl/BV/OBBRSS.h"
00046 #include "fcl/BV/kIOS.h"
00047 #include "fcl/math/transform.h"
00048
00050 namespace fcl
00051 {
00052
00054 namespace details
00055 {
00056
00058 template<typename BV1, typename BV2>
00059 class Converter
00060 {
00061 private:
00062 static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
00063 {
00064
00065 }
00066 };
00067
00068
00070 template<>
00071 class Converter<AABB, AABB>
00072 {
00073 public:
00074 static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
00075 {
00076 const Vec3f& center = bv1.center();
00077 FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5;
00078 Vec3f center2 = tf1.transform(center);
00079 Vec3f delta(r, r, r);
00080 bv2.min_ = center2 - delta;
00081 bv2.max_ = center2 + delta;
00082 }
00083 };
00084
00085 template<>
00086 class Converter<AABB, OBB>
00087 {
00088 public:
00089 static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
00090 {
00091 bv2.To = tf1.transform(bv1.center());
00092
00094 FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
00095 std::size_t id[3] = {0, 1, 2};
00096
00097 for(std::size_t i = 1; i < 3; ++i)
00098 {
00099 for(std::size_t j = i; j > 0; --j)
00100 {
00101 if(d[j] > d[j-1])
00102 {
00103 {
00104 FCL_REAL tmp = d[j];
00105 d[j] = d[j-1];
00106 d[j-1] = tmp;
00107 }
00108 {
00109 std::size_t tmp = id[j];
00110 id[j] = id[j-1];
00111 id[j-1] = tmp;
00112 }
00113 }
00114 }
00115 }
00116
00117 Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
00118 bv2.extent = Vec3f(extent[id[0]], extent[id[1]], extent[id[2]]);
00119 const Matrix3f& R = tf1.getRotation();
00120 bool left_hand = (id[0] == (id[1] + 1) % 3);
00121 bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
00122 bv2.axis[1] = R.getColumn(id[1]);
00123 bv2.axis[2] = R.getColumn(id[2]);
00124 }
00125 };
00126
00127 template<>
00128 class Converter<OBB, OBB>
00129 {
00130 public:
00131 static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
00132 {
00133 bv2.extent = bv1.extent;
00134 bv2.To = tf1.transform(bv1.To);
00135 bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
00136 bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
00137 bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
00138 }
00139 };
00140
00141 template<>
00142 class Converter<OBBRSS, OBB>
00143 {
00144 public:
00145 static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2)
00146 {
00147 Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
00148 }
00149 };
00150
00151 template<>
00152 class Converter<RSS, OBB>
00153 {
00154 public:
00155 static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
00156 {
00157 bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
00158 bv2.To = tf1.transform(bv1.Tr);
00159 bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
00160 bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
00161 bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
00162 }
00163 };
00164
00165
00166 template<typename BV1>
00167 class Converter<BV1, AABB>
00168 {
00169 public:
00170 static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
00171 {
00172 const Vec3f& center = bv1.center();
00173 FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
00174 Vec3f delta(r, r, r);
00175 Vec3f center2 = tf1.transform(center);
00176 bv2.min_ = center2 - delta;
00177 bv2.max_ = center2 + delta;
00178 }
00179 };
00180
00181 template<typename BV1>
00182 class Converter<BV1, OBB>
00183 {
00184 public:
00185 static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
00186 {
00187 AABB bv;
00188 Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
00189 Converter<AABB, OBB>::convert(bv, tf1, bv2);
00190 }
00191 };
00192
00193 template<>
00194 class Converter<OBB, RSS>
00195 {
00196 public:
00197 static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
00198 {
00199 bv2.Tr = tf1.transform(bv1.To);
00200 bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
00201 bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
00202 bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
00203
00204 bv2.r = bv1.extent[2];
00205 bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
00206 bv2.l[1] = 2 * (bv1.extent[1] - bv2.r);
00207 }
00208 };
00209
00210 template<>
00211 class Converter<RSS, RSS>
00212 {
00213 public:
00214 static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
00215 {
00216 bv2.Tr = tf1.transform(bv1.Tr);
00217 bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
00218 bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
00219 bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
00220
00221 bv2.r = bv1.r;
00222 bv2.l[0] = bv1.l[0];
00223 bv2.l[1] = bv1.l[1];
00224 }
00225 };
00226
00227 template<>
00228 class Converter<OBBRSS, RSS>
00229 {
00230 public:
00231 static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2)
00232 {
00233 Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
00234 }
00235 };
00236
00237 template<>
00238 class Converter<AABB, RSS>
00239 {
00240 public:
00241 static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
00242 {
00243 bv2.Tr = tf1.transform(bv1.center());
00244
00246 FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
00247 std::size_t id[3] = {0, 1, 2};
00248
00249 for(std::size_t i = 1; i < 3; ++i)
00250 {
00251 for(std::size_t j = i; j > 0; --j)
00252 {
00253 if(d[j] > d[j-1])
00254 {
00255 {
00256 FCL_REAL tmp = d[j];
00257 d[j] = d[j-1];
00258 d[j-1] = tmp;
00259 }
00260 {
00261 std::size_t tmp = id[j];
00262 id[j] = id[j-1];
00263 id[j-1] = tmp;
00264 }
00265 }
00266 }
00267 }
00268
00269 Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
00270 bv2.r = extent[id[2]];
00271 bv2.l[0] = (extent[id[0]] - bv2.r) * 2;
00272 bv2.l[1] = (extent[id[1]] - bv2.r) * 2;
00273
00274 const Matrix3f& R = tf1.getRotation();
00275 bool left_hand = (id[0] == (id[1] + 1) % 3);
00276 bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
00277 bv2.axis[1] = R.getColumn(id[1]);
00278 bv2.axis[2] = R.getColumn(id[2]);
00279 }
00280 };
00281
00282 }
00283
00285
00286
00288 template<typename BV1, typename BV2>
00289 static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
00290 {
00291 details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
00292 }
00293
00294 }
00295
00296 #endif