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 GEOMETRIC_SHAPES_POINT_INCLUSION_
00038 #define GEOMETRIC_SHAPES_POINT_INCLUSION_
00039
00040 #include "pr2_navigation_self_filter/shapes.h"
00041 #include <LinearMath/btTransform.h>
00042
00043
00044 #include <vector>
00045
00055 namespace bodies
00056 {
00057
00059 struct BoundingSphere
00060 {
00061 btVector3 center;
00062 double radius;
00063 };
00064
00068 class Body
00069 {
00070 public:
00071
00072 Body(void)
00073 {
00074 m_scale = 1.0;
00075 m_padding = 0.0;
00076 m_pose.setIdentity();
00077 m_type = shapes::UNKNOWN_SHAPE;
00078 }
00079
00080 virtual ~Body(void)
00081 {
00082 }
00083
00085 shapes::ShapeType getType(void) const
00086 {
00087 return m_type;
00088 }
00089
00092 void setScale(double scale)
00093 {
00094 m_scale = scale;
00095 updateInternalData();
00096 }
00097
00099 double getScale(void) const
00100 {
00101 return m_scale;
00102 }
00103
00106 void setPadding(double padd)
00107 {
00108 m_padding = padd;
00109 updateInternalData();
00110 }
00111
00113 double getPadding(void) const
00114 {
00115 return m_padding;
00116 }
00117
00119 void setPose(const btTransform &pose)
00120 {
00121 m_pose = pose;
00122 updateInternalData();
00123 }
00124
00126 const btTransform& getPose(void) const
00127 {
00128 return m_pose;
00129 }
00130
00132 void setDimensions(const shapes::Shape *shape)
00133 {
00134 useDimensions(shape);
00135 updateInternalData();
00136 }
00137
00139 bool containsPoint(double x, double y, double z) const
00140 {
00141 return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
00142 }
00143
00148 virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const = 0;
00149
00151 virtual bool containsPoint(const btVector3 &p, bool verbose = false) const = 0;
00152
00155 virtual double computeVolume(void) const = 0;
00156
00159 virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
00160
00161 protected:
00162
00163 virtual void updateInternalData(void) = 0;
00164 virtual void useDimensions(const shapes::Shape *shape) = 0;
00165
00166 shapes::ShapeType m_type;
00167 btTransform m_pose;
00168 double m_scale;
00169 double m_padding;
00170 };
00171
00173 class Sphere : public Body
00174 {
00175 public:
00176 Sphere(void) : Body()
00177 {
00178 m_type = shapes::SPHERE;
00179 }
00180
00181 Sphere(const shapes::Shape *shape) : Body()
00182 {
00183 m_type = shapes::SPHERE;
00184 setDimensions(shape);
00185 }
00186
00187 virtual ~Sphere(void)
00188 {
00189 }
00190
00191 virtual bool containsPoint(const btVector3 &p, bool verbose=false) const;
00192 virtual double computeVolume(void) const;
00193 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00194 virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00195
00196 protected:
00197
00198 virtual void useDimensions(const shapes::Shape *shape);
00199 virtual void updateInternalData(void);
00200
00201 btVector3 m_center;
00202 double m_radius;
00203 double m_radiusU;
00204 double m_radius2;
00205 };
00206
00208 class Cylinder : public Body
00209 {
00210 public:
00211 Cylinder(void) : Body()
00212 {
00213 m_type = shapes::CYLINDER;
00214 }
00215
00216 Cylinder(const shapes::Shape *shape) : Body()
00217 {
00218 m_type = shapes::CYLINDER;
00219 setDimensions(shape);
00220 }
00221
00222 virtual ~Cylinder(void)
00223 {
00224 }
00225
00226 virtual bool containsPoint(const btVector3 &p, bool verbose=false) const;
00227 virtual double computeVolume(void) const;
00228 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00229 virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00230
00231 protected:
00232
00233 virtual void useDimensions(const shapes::Shape *shape);
00234 virtual void updateInternalData(void);
00235
00236 btVector3 m_center;
00237 btVector3 m_normalH;
00238 btVector3 m_normalB1;
00239 btVector3 m_normalB2;
00240
00241 double m_length;
00242 double m_length2;
00243 double m_radius;
00244 double m_radiusU;
00245 double m_radiusB;
00246 double m_radiusBSqr;
00247 double m_radius2;
00248 double m_d1;
00249 double m_d2;
00250 };
00251
00253 class Box : public Body
00254 {
00255 public:
00256 Box(void) : Body()
00257 {
00258 m_type = shapes::BOX;
00259 }
00260
00261 Box(const shapes::Shape *shape) : Body()
00262 {
00263 m_type = shapes::BOX;
00264 setDimensions(shape);
00265 }
00266
00267 virtual ~Box(void)
00268 {
00269 }
00270
00271 virtual bool containsPoint(const btVector3 &p, bool verbose = false) const;
00272 virtual double computeVolume(void) const;
00273 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00274 virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00275
00276 protected:
00277
00278 virtual void useDimensions(const shapes::Shape *shape);
00279 virtual void updateInternalData(void);
00280
00281 btVector3 m_center;
00282 btVector3 m_normalL;
00283 btVector3 m_normalW;
00284 btVector3 m_normalH;
00285
00286 btVector3 m_corner1;
00287 btVector3 m_corner2;
00288
00289 double m_length;
00290 double m_width;
00291 double m_height;
00292 double m_length2;
00293 double m_width2;
00294 double m_height2;
00295 double m_radiusB;
00296 double m_radius2;
00297 };
00298
00299
00300
00301
00302
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00353 class ConvexMesh : public Body
00354 {
00355 public:
00356
00357 ConvexMesh(void) : Body()
00358 {
00359 m_type = shapes::MESH;
00360 }
00361
00362 ConvexMesh(const shapes::Shape *shape) : Body()
00363 {
00364 m_type = shapes::MESH;
00365 setDimensions(shape);
00366 }
00367
00368 virtual ~ConvexMesh(void)
00369 {
00370 }
00371
00372 virtual bool containsPoint(const btVector3 &p, bool verbose = false) const;
00373 virtual double computeVolume(void) const;
00374
00375 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00376 virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00377
00378 protected:
00379
00380 virtual void useDimensions(const shapes::Shape *shape);
00381 virtual void updateInternalData(void);
00382
00383 unsigned int countVerticesBehindPlane(const btVector4& planeNormal) const;
00384 bool isPointInsidePlanes(const btVector3& point) const;
00385
00386 std::vector<btVector4> m_planes;
00387 std::vector<btVector3> m_vertices;
00388 std::vector<btVector3> m_scaledVertices;
00389 std::vector<unsigned int> m_triangles;
00390 btTransform m_iPose;
00391
00392 btVector3 m_center;
00393 btVector3 m_meshCenter;
00394 double m_radiusB;
00395 double m_radiusBSqr;
00396 double m_meshRadiusB;
00397
00398 btVector3 m_boxOffset;
00399 Box m_boundingBox;
00400 };
00401
00402
00404 Body* createBodyFromShape(const shapes::Shape *shape);
00405
00407 void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
00408
00409 }
00410
00411 #endif