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
00038 #ifndef FCL_COLLISION_DATA_H
00039 #define FCL_COLLISION_DATA_H
00040
00041 #include "fcl/collision_object.h"
00042 #include "fcl/math/vec_3f.h"
00043 #include <vector>
00044 #include <set>
00045 #include <limits>
00046
00047
00048 namespace fcl
00049 {
00050
00052 struct Contact
00053 {
00055 const CollisionGeometry* o1;
00056
00058 const CollisionGeometry* o2;
00059
00064 int b1;
00065
00066
00071 int b2;
00072
00074 Vec3f normal;
00075
00077 Vec3f pos;
00078
00080 FCL_REAL penetration_depth;
00081
00082
00084 static const int NONE = -1;
00085
00086 Contact() : o1(NULL),
00087 o2(NULL),
00088 b1(NONE),
00089 b2(NONE)
00090 {}
00091
00092 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
00093 o2(o2_),
00094 b1(b1_),
00095 b2(b2_)
00096 {}
00097
00098 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
00099 const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
00100 o2(o2_),
00101 b1(b1_),
00102 b2(b2_),
00103 normal(normal_),
00104 pos(pos_),
00105 penetration_depth(depth_)
00106 {}
00107
00108 bool operator < (const Contact& other) const
00109 {
00110 if(b1 == other.b1)
00111 return b2 < other.b2;
00112 return b1 < other.b1;
00113 }
00114 };
00115
00117 struct CostSource
00118 {
00120 Vec3f aabb_min;
00121
00123 Vec3f aabb_max;
00124
00126 FCL_REAL cost_density;
00127
00128 CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_),
00129 aabb_max(aabb_max_),
00130 cost_density(cost_density_)
00131 {
00132 }
00133
00134 CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_),
00135 aabb_max(aabb.max_),
00136 cost_density(cost_density_)
00137 {
00138 }
00139
00140 CostSource() {}
00141
00142 bool operator < (const CostSource& other) const
00143 {
00144 return cost_density < other.cost_density;
00145 }
00146 };
00147
00148 struct CollisionResult;
00149
00151 struct CollisionRequest
00152 {
00154 size_t num_max_contacts;
00155
00157 bool enable_contact;
00158
00160 size_t num_max_cost_sources;
00161
00163 bool enable_cost;
00164
00165 CollisionRequest(size_t num_max_contacts_ = 1,
00166 bool enable_contact_ = false,
00167 size_t num_max_cost_sources_ = 1,
00168 bool enable_cost_ = false) : num_max_contacts(num_max_contacts_),
00169 enable_contact(enable_contact_),
00170 num_max_cost_sources(num_max_cost_sources_),
00171 enable_cost(enable_cost_)
00172 {
00173 }
00174
00175 bool isSatisfied(const CollisionResult& result) const;
00176 };
00177
00179 struct CollisionResult
00180 {
00181 private:
00183 std::vector<Contact> contacts;
00184
00186 std::set<CostSource> cost_sources;
00187
00188 public:
00189 CollisionResult()
00190 {
00191 }
00192
00193
00195 inline void addContact(const Contact& c)
00196 {
00197 contacts.push_back(c);
00198 }
00199
00201 inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources)
00202 {
00203 cost_sources.insert(c);
00204 if(cost_sources.size() > num_max_cost_sources)
00205 cost_sources.erase(cost_sources.begin());
00206 }
00207
00209 bool isCollision() const
00210 {
00211 return contacts.size() > 0;
00212 }
00213
00215 size_t numContacts() const
00216 {
00217 return contacts.size();
00218 }
00219
00221 size_t numCostSources() const
00222 {
00223 return cost_sources.size();
00224 }
00225
00227 const Contact& getContact(size_t i) const
00228 {
00229 if(i < contacts.size())
00230 return contacts[i];
00231 else
00232 return contacts.back();
00233 }
00234
00236 void getContacts(std::vector<Contact>& contacts_)
00237 {
00238 contacts_.resize(contacts.size());
00239 std::copy(contacts.begin(), contacts.end(), contacts_.begin());
00240 }
00241
00243 void getCostSources(std::vector<CostSource>& cost_sources_)
00244 {
00245 cost_sources_.resize(cost_sources.size());
00246 std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin());
00247 }
00248
00250 void clear()
00251 {
00252 contacts.clear();
00253 cost_sources.clear();
00254 }
00255 };
00256
00257 struct DistanceResult;
00258
00260 struct DistanceRequest
00261 {
00263 bool enable_nearest_points;
00264
00265 DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
00266 {
00267 }
00268
00269 bool isSatisfied(const DistanceResult& result) const;
00270 };
00271
00273 struct DistanceResult
00274 {
00275
00276 public:
00277
00279 FCL_REAL min_distance;
00280
00282 Vec3f nearest_points[2];
00283
00285 const CollisionGeometry* o1;
00286
00288 const CollisionGeometry* o2;
00289
00294 int b1;
00295
00300 int b2;
00301
00303 static const int NONE = -1;
00304
00305 DistanceResult(FCL_REAL min_distance_ = std::numeric_limits<FCL_REAL>::max()) : min_distance(min_distance_),
00306 o1(NULL),
00307 o2(NULL),
00308 b1(NONE),
00309 b2(NONE)
00310 {
00311 }
00312
00313
00315 void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
00316 {
00317 if(min_distance > distance)
00318 {
00319 min_distance = distance;
00320 o1 = o1_;
00321 o2 = o2_;
00322 b1 = b1_;
00323 b2 = b2_;
00324 }
00325 }
00326
00328 void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2)
00329 {
00330 if(min_distance > distance)
00331 {
00332 min_distance = distance;
00333 o1 = o1_;
00334 o2 = o2_;
00335 b1 = b1_;
00336 b2 = b2_;
00337 nearest_points[0] = p1;
00338 nearest_points[1] = p2;
00339 }
00340 }
00341
00343 void update(const DistanceResult& other_result)
00344 {
00345 if(min_distance > other_result.min_distance)
00346 {
00347 min_distance = other_result.min_distance;
00348 o1 = other_result.o1;
00349 o2 = other_result.o2;
00350 b1 = other_result.b1;
00351 b2 = other_result.b2;
00352 nearest_points[0] = other_result.nearest_points[0];
00353 nearest_points[1] = other_result.nearest_points[1];
00354 }
00355 }
00356
00358 void clear()
00359 {
00360 min_distance = std::numeric_limits<FCL_REAL>::max();
00361 o1 = NULL;
00362 o2 = NULL;
00363 b1 = NONE;
00364 b2 = NONE;
00365 }
00366 };
00367
00368
00369
00370
00371 }
00372
00373 #endif