Go to the documentation of this file.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_BODIES_
00038 #define GEOMETRIC_SHAPES_BODIES_
00039
00040 #include "geometric_shapes/shapes.h"
00041 #include <tf/LinearMath/Transform.h>
00042 #include <tf/LinearMath/Vector3.h>
00043
00044
00045 #include <vector>
00046
00054 namespace bodies
00055 {
00056
00058 struct BoundingSphere
00059 {
00060 tf::Vector3 center;
00061 double radius;
00062 };
00063
00065 struct BoundingCylinder
00066 {
00067 tf::Transform pose;
00068 double radius;
00069 double length;
00070 };
00071
00075 class Body
00076 {
00077 public:
00078
00079 Body(void)
00080 {
00081 m_scale = 1.0;
00082 m_padding = 0.0;
00083 m_pose.setIdentity();
00084 m_type = shapes::UNKNOWN_SHAPE;
00085 }
00086
00087 virtual ~Body(void)
00088 {
00089 }
00090
00092 shapes::ShapeType getType(void) const
00093 {
00094 return m_type;
00095 }
00096
00099 void setScale(double scale)
00100 {
00101 m_scale = scale;
00102 updateInternalData();
00103 }
00104
00106 double getScale(void) const
00107 {
00108 return m_scale;
00109 }
00110
00113 void setPadding(double padd)
00114 {
00115 m_padding = padd;
00116 updateInternalData();
00117 }
00118
00120 double getPadding(void) const
00121 {
00122 return m_padding;
00123 }
00124
00126 void setPose(const tf::Transform &pose)
00127 {
00128 m_pose = pose;
00129 updateInternalData();
00130 }
00131
00133 const tf::Transform& getPose(void) const
00134 {
00135 return m_pose;
00136 }
00137
00139 void setDimensions(const shapes::Shape *shape)
00140 {
00141 useDimensions(shape);
00142 updateInternalData();
00143 }
00144
00146 bool containsPoint(double x, double y, double z) const
00147 {
00148 return containsPoint(tf::Vector3(tfScalar(x), tfScalar(y), tfScalar(z)));
00149 }
00150
00155 virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const = 0;
00156
00158 virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const = 0;
00159
00162 virtual double computeVolume(void) const = 0;
00163
00166 virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
00167
00170 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const = 0;
00171
00172 protected:
00173
00174 virtual void updateInternalData(void) = 0;
00175 virtual void useDimensions(const shapes::Shape *shape) = 0;
00176
00177 shapes::ShapeType m_type;
00178 tf::Transform m_pose;
00179 double m_scale;
00180 double m_padding;
00181 };
00182
00184 class Sphere : public Body
00185 {
00186 public:
00187 Sphere(void) : Body()
00188 {
00189 m_type = shapes::SPHERE;
00190 }
00191
00192 Sphere(const shapes::Shape *shape) : Body()
00193 {
00194 m_type = shapes::SPHERE;
00195 setDimensions(shape);
00196 }
00197
00198 virtual ~Sphere(void)
00199 {
00200 }
00201
00202 virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00203 virtual double computeVolume(void) const;
00204 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00205 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00206 virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00207
00208 protected:
00209
00210 virtual void useDimensions(const shapes::Shape *shape);
00211 virtual void updateInternalData(void);
00212
00213 tf::Vector3 m_center;
00214 double m_radius;
00215 double m_radiusU;
00216 double m_radius2;
00217 };
00218
00220 class Cylinder : public Body
00221 {
00222 public:
00223 Cylinder(void) : Body()
00224 {
00225 m_type = shapes::CYLINDER;
00226 }
00227
00228 Cylinder(const shapes::Shape *shape) : Body()
00229 {
00230 m_type = shapes::CYLINDER;
00231 setDimensions(shape);
00232 }
00233
00234 virtual ~Cylinder(void)
00235 {
00236 }
00237
00238 virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00239 virtual double computeVolume(void) const;
00240 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00241 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00242 virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00243
00244 protected:
00245
00246 virtual void useDimensions(const shapes::Shape *shape);
00247 virtual void updateInternalData(void);
00248
00249 tf::Vector3 m_center;
00250 tf::Vector3 m_normalH;
00251 tf::Vector3 m_normalB1;
00252 tf::Vector3 m_normalB2;
00253
00254 double m_length;
00255 double m_length2;
00256 double m_radius;
00257 double m_radiusU;
00258 double m_radiusB;
00259 double m_radiusBSqr;
00260 double m_radius2;
00261 double m_d1;
00262 double m_d2;
00263 };
00264
00266 class Box : public Body
00267 {
00268 public:
00269 Box(void) : Body()
00270 {
00271 m_type = shapes::BOX;
00272 }
00273
00274 Box(const shapes::Shape *shape) : Body()
00275 {
00276 m_type = shapes::BOX;
00277 setDimensions(shape);
00278 }
00279
00280 virtual ~Box(void)
00281 {
00282 }
00283
00284 virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00285 virtual double computeVolume(void) const;
00286 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00287 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00288 virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00289
00290 protected:
00291
00292 virtual void useDimensions(const shapes::Shape *shape);
00293 virtual void updateInternalData(void);
00294
00295 tf::Vector3 m_center;
00296 tf::Vector3 m_normalL;
00297 tf::Vector3 m_normalW;
00298 tf::Vector3 m_normalH;
00299
00300 tf::Vector3 m_corner1;
00301 tf::Vector3 m_corner2;
00302
00303 double m_length;
00304 double m_width;
00305 double m_height;
00306 double m_length2;
00307 double m_width2;
00308 double m_height2;
00309 double m_radiusB;
00310 double m_radius2;
00311 };
00312
00313
00314
00315
00316
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
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00367 class ConvexMesh : public Body
00368 {
00369 public:
00370
00371 ConvexMesh(void) : Body()
00372 {
00373 m_type = shapes::MESH;
00374 }
00375
00376 ConvexMesh(const shapes::Shape *shape) : Body()
00377 {
00378 m_type = shapes::MESH;
00379 setDimensions(shape);
00380 }
00381
00382 virtual ~ConvexMesh(void)
00383 {
00384 }
00385
00386 virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00387 virtual double computeVolume(void) const;
00388
00389 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00390 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00391 virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00392
00393 const std::vector<unsigned int>& getTriangles() const {
00394 return m_triangles;
00395 }
00396
00397 const std::vector<tf::Vector3>& getVertices() const {
00398 return m_vertices;
00399 }
00400
00401 const std::vector<tf::Vector3>& getScaledVertices() const {
00402 return m_scaledVertices;
00403 }
00404
00405 protected:
00406
00407 virtual void useDimensions(const shapes::Shape *shape);
00408 virtual void updateInternalData(void);
00409
00410 unsigned int countVerticesBehindPlane(const tf::tfVector4& planeNormal) const;
00411 bool isPointInsidePlanes(const tf::Vector3& point) const;
00412
00413 std::vector<tf::tfVector4> m_planes;
00414 std::vector<tf::Vector3> m_vertices;
00415 std::vector<tf::Vector3> m_scaledVertices;
00416 std::vector<unsigned int> m_triangles;
00417 tf::Transform m_iPose;
00418
00419 tf::Vector3 m_center;
00420 tf::Vector3 m_meshCenter;
00421 double m_radiusB;
00422 double m_radiusBSqr;
00423 double m_meshRadiusB;
00424
00425 tf::Vector3 m_boxOffset;
00426 Box m_boundingBox;
00427 BoundingCylinder m_boundingCylinder;
00428 };
00429
00430
00432 Body* createBodyFromShape(const shapes::Shape *shape);
00433
00435 void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
00436
00437 class BodyVector {
00438 public:
00439
00440 BodyVector();
00441
00442 BodyVector(const std::vector<shapes::Shape*>& shapes,
00443 const std::vector<tf::Transform>& poses,
00444 double padding);
00445
00446 ~BodyVector();
00447
00448 void addBody(const shapes::Shape* shape, const tf::Transform& pose, double padding = 0.0);
00449
00450 void setPose(unsigned int i, const tf::Transform& pose);
00451
00452 unsigned int getSize() const {
00453 return bodies_.size();
00454 }
00455
00456 double getPadding() const {
00457 return padding_;
00458 }
00459
00460 const Body* getBody(unsigned int i) const;
00461 const Body* getPaddedBody(unsigned int i) const;
00462
00463 BoundingSphere getBoundingSphere(unsigned int i) const;
00464 BoundingSphere getPaddedBoundingSphere(unsigned int i) const;
00465
00466 double getBoundingSphereRadiusSquared(unsigned int i) const;
00467 double getPaddedBoundingSphereRadiusSquared(unsigned int i) const;
00468
00469 private:
00470
00471 std::vector<Body*> bodies_;
00472 double padding_;
00473 std::vector<Body*> padded_bodies_;
00474 std::vector<double> rsqrs_;
00475 std::vector<double> padded_rsqrs_;
00476
00477 };
00478
00479 }
00480
00481 #endif