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_OBJECT_BASE_H
00039 #define FCL_COLLISION_OBJECT_BASE_H
00040
00041 #include "fcl/BV/AABB.h"
00042 #include "fcl/math/transform.h"
00043 #include <boost/shared_ptr.hpp>
00044
00045 namespace fcl
00046 {
00047
00049 enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
00050
00052 enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
00053 GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
00054
00056 class CollisionGeometry
00057 {
00058 public:
00059 CollisionGeometry() : cost_density(1),
00060 threshold_occupied(1),
00061 threshold_free(0)
00062 {
00063 }
00064
00065 virtual ~CollisionGeometry() {}
00066
00068 virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
00069
00071 virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
00072
00074 virtual void computeLocalAABB() = 0;
00075
00077 void* getUserData() const
00078 {
00079 return user_data;
00080 }
00081
00083 void setUserData(void *data)
00084 {
00085 user_data = data;
00086 }
00087
00089 inline bool isOccupied() const { return cost_density >= threshold_occupied; }
00090
00092 inline bool isFree() const { return cost_density <= threshold_free; }
00093
00095 inline bool isUncertain() const { return !isOccupied() && !isFree(); }
00096
00098 Vec3f aabb_center;
00099
00101 FCL_REAL aabb_radius;
00102
00104 AABB aabb_local;
00105
00107 void *user_data;
00108
00110 FCL_REAL cost_density;
00111
00113 FCL_REAL threshold_occupied;
00114
00116 FCL_REAL threshold_free;
00117 };
00118
00120 class CollisionObject
00121 {
00122 public:
00123 CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_) : cgeom(cgeom_)
00124 {
00125 cgeom->computeLocalAABB();
00126 computeAABB();
00127 }
00128
00129 CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Transform3f& tf) : cgeom(cgeom_), t(tf)
00130 {
00131 cgeom->computeLocalAABB();
00132 computeAABB();
00133 }
00134
00135 CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
00136 cgeom(cgeom_), t(Transform3f(R, T))
00137 {
00138 cgeom->computeLocalAABB();
00139 computeAABB();
00140 }
00141
00142 CollisionObject()
00143 {
00144 }
00145
00146 ~CollisionObject()
00147 {
00148 }
00149
00151 OBJECT_TYPE getObjectType() const
00152 {
00153 return cgeom->getObjectType();
00154 }
00155
00157 NODE_TYPE getNodeType() const
00158 {
00159 return cgeom->getNodeType();
00160 }
00161
00163 inline const AABB& getAABB() const
00164 {
00165 return aabb;
00166 }
00167
00169 inline void computeAABB()
00170 {
00171 if(t.getQuatRotation().isIdentity())
00172 {
00173 aabb = cgeom->aabb_local;
00174 }
00175 else
00176 {
00177 Vec3f center = t.transform(cgeom->aabb_center);
00178 Vec3f delta(cgeom->aabb_radius, cgeom->aabb_radius, cgeom->aabb_radius);
00179 aabb.min_ = center - delta;
00180 aabb.max_ = center + delta;
00181 }
00182 }
00183
00185 void* getUserData() const
00186 {
00187 return user_data;
00188 }
00189
00191 void setUserData(void *data)
00192 {
00193 user_data = data;
00194 }
00195
00197 inline const Vec3f& getTranslation() const
00198 {
00199 return t.getTranslation();
00200 }
00201
00203 inline const Matrix3f& getRotation() const
00204 {
00205 return t.getRotation();
00206 }
00207
00209 inline const Quaternion3f& getQuatRotation() const
00210 {
00211 return t.getQuatRotation();
00212 }
00213
00215 inline const Transform3f& getTransform() const
00216 {
00217 return t;
00218 }
00219
00221 void setRotation(const Matrix3f& R)
00222 {
00223 t.setRotation(R);
00224 }
00225
00227 void setTranslation(const Vec3f& T)
00228 {
00229 t.setTranslation(T);
00230 }
00231
00233 void setQuatRotation(const Quaternion3f& q)
00234 {
00235 t.setQuatRotation(q);
00236 }
00237
00239 void setTransform(const Matrix3f& R, const Vec3f& T)
00240 {
00241 t.setTransform(R, T);
00242 }
00243
00245 void setTransform(const Quaternion3f& q, const Vec3f& T)
00246 {
00247 t.setTransform(q, T);
00248 }
00249
00251 void setTransform(const Transform3f& tf)
00252 {
00253 t = tf;
00254 }
00255
00257 bool isIdentityTransform() const
00258 {
00259 return t.isIdentity();
00260 }
00261
00263 void setIdentityTransform()
00264 {
00265 t.setIdentity();
00266 }
00267
00269 const CollisionGeometry* getCollisionGeometry() const
00270 {
00271 return cgeom.get();
00272 }
00273
00275 FCL_REAL getCostDensity() const
00276 {
00277 return cgeom->cost_density;
00278 }
00279
00281 void setCostDensity(FCL_REAL c)
00282 {
00283 cgeom->cost_density = c;
00284 }
00285
00287 inline bool isOccupied() const
00288 {
00289 return cgeom->isOccupied();
00290 }
00291
00293 inline bool isFree() const
00294 {
00295 return cgeom->isFree();
00296 }
00297
00299 inline bool isUncertain() const
00300 {
00301 return cgeom->isUncertain();
00302 }
00303
00304 protected:
00305
00306 boost::shared_ptr<CollisionGeometry> cgeom;
00307
00308 Transform3f t;
00309
00311 mutable AABB aabb;
00312
00314 void *user_data;
00315 };
00316
00317 }
00318
00319 #endif