Program Listing for File bodies.h
↰ Return to documentation for file (/tmp/ws/src/geometric_shapes/include/geometric_shapes/bodies.h)
// Copyright 2008 Willow Garage, Inc.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// * Neither the name of the Willow Garage, Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
/* Author: Ioan Sucan, E. Gil Jones */
#ifndef GEOMETRIC_SHAPES_BODIES_
#define GEOMETRIC_SHAPES_BODIES_
#define _USE_MATH_DEFINES
#include "geometric_shapes/aabb.h"
#include "geometric_shapes/shapes.h"
#include <eigen_stl_containers/eigen_stl_containers.h>
#include <random_numbers/random_numbers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <vector>
namespace bodies
{
struct BoundingSphere
{
Eigen::Vector3d center;
double radius;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct BoundingCylinder
{
Eigen::Isometry3d pose;
double radius;
double length;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class Body;
typedef std::shared_ptr<Body> BodyPtr;
typedef std::shared_ptr<const Body> BodyConstPtr;
class Body
{
public:
Body() : scale_(1.0), padding_(0.0), type_(shapes::UNKNOWN_SHAPE)
{
pose_.setIdentity();
}
virtual ~Body() = default;
inline shapes::ShapeType getType() const
{
return type_;
}
inline void setScaleDirty(double scale)
{
scale_ = scale;
}
inline void setScale(double scale)
{
setScaleDirty(scale);
updateInternalData();
}
inline double getScale() const
{
return scale_;
}
inline void setPaddingDirty(double padd)
{
padding_ = padd;
}
inline void setPadding(double padd)
{
setPaddingDirty(padd);
updateInternalData();
}
inline double getPadding() const
{
return padding_;
}
inline void setPoseDirty(const Eigen::Isometry3d& pose)
{
pose_ = pose;
}
inline void setPose(const Eigen::Isometry3d& pose)
{
setPoseDirty(pose);
updateInternalData();
}
inline const Eigen::Isometry3d& getPose() const
{
return pose_;
}
inline void setDimensionsDirty(const shapes::Shape* shape)
{
useDimensions(shape);
}
virtual std::vector<double> getDimensions() const = 0;
virtual std::vector<double> getScaledDimensions() const = 0;
inline void setDimensions(const shapes::Shape* shape)
{
setDimensionsDirty(shape);
updateInternalData();
}
inline bool containsPoint(double x, double y, double z, bool verbose = false) const
{
Eigen::Vector3d pt(x, y, z);
return containsPoint(pt, verbose);
}
virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const = 0;
virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const = 0;
virtual double computeVolume() const = 0;
virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
Eigen::Vector3d& result) const;
virtual void computeBoundingSphere(BoundingSphere& sphere) const = 0;
virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const = 0;
virtual void computeBoundingBox(AABB& bbox) const = 0;
inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const
{
return cloneAt(pose, padding_, scale_);
}
virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scaling) const = 0;
virtual void updateInternalData() = 0;
protected:
virtual void useDimensions(const shapes::Shape* shape) = 0;
double scale_;
double padding_;
shapes::ShapeType type_;
Eigen::Isometry3d pose_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class Sphere : public Body
{
public:
Sphere() : Body()
{
type_ = shapes::SPHERE;
}
Sphere(const shapes::Shape* shape) : Body()
{
type_ = shapes::SPHERE;
setDimensions(shape);
}
explicit Sphere(const BoundingSphere& sphere);
~Sphere() override = default;
std::vector<double> getDimensions() const override;
std::vector<double> getScaledDimensions() const override;
bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
double computeVolume() const override;
bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
Eigen::Vector3d& result) const override;
void computeBoundingSphere(BoundingSphere& sphere) const override;
void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
void computeBoundingBox(AABB& bbox) const override;
bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
void updateInternalData() override;
protected:
void useDimensions(const shapes::Shape* shape) override;
// shape-dependent data
double radius_;
// pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
Eigen::Vector3d center_;
double radiusU_;
double radius2_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class Cylinder : public Body
{
public:
Cylinder() : Body()
{
type_ = shapes::CYLINDER;
}
Cylinder(const shapes::Shape* shape) : Body()
{
type_ = shapes::CYLINDER;
setDimensions(shape);
}
explicit Cylinder(const BoundingCylinder& cylinder);
~Cylinder() override = default;
std::vector<double> getDimensions() const override;
std::vector<double> getScaledDimensions() const override;
bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
double computeVolume() const override;
bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
Eigen::Vector3d& result) const override;
void computeBoundingSphere(BoundingSphere& sphere) const override;
void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
void computeBoundingBox(AABB& bbox) const override;
bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
void updateInternalData() override;
protected:
void useDimensions(const shapes::Shape* shape) override;
// shape-dependent data
double length_;
double radius_;
// pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
Eigen::Vector3d center_;
Eigen::Vector3d normalH_;
Eigen::Vector3d normalB1_;
Eigen::Vector3d normalB2_;
double length2_;
double radiusU_;
double radiusB_;
double radiusBSqr_;
double radius2_;
double d1_;
double d2_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class Box : public Body
{
public:
Box() : Body()
{
type_ = shapes::BOX;
}
Box(const shapes::Shape* shape) : Body()
{
type_ = shapes::BOX;
setDimensions(shape);
}
explicit Box(const AABB& aabb);
~Box() override = default;
std::vector<double> getDimensions() const override;
std::vector<double> getScaledDimensions() const override;
bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
double computeVolume() const override;
bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
Eigen::Vector3d& result) const override;
void computeBoundingSphere(BoundingSphere& sphere) const override;
void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
void computeBoundingBox(AABB& bbox) const override;
bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
void updateInternalData() override;
protected:
void useDimensions(const shapes::Shape* shape) override; // (x, y, z) = (length, width, height)
// shape-dependent data
double length_;
double width_;
double height_;
// pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
Eigen::Vector3d center_;
Eigen::Matrix3d invRot_;
Eigen::Vector3d minCorner_;
Eigen::Vector3d maxCorner_;
double length2_;
double width2_;
double height2_;
double radiusB_;
double radius2_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class ConvexMesh : public Body
{
public:
ConvexMesh() : Body()
{
type_ = shapes::MESH;
scaled_vertices_ = nullptr;
}
ConvexMesh(const shapes::Shape* shape) : Body()
{
type_ = shapes::MESH;
scaled_vertices_ = nullptr;
setDimensions(shape);
}
~ConvexMesh() override = default;
std::vector<double> getDimensions() const override;
std::vector<double> getScaledDimensions() const override;
bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
double computeVolume() const override;
void computeBoundingSphere(BoundingSphere& sphere) const override;
void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
void computeBoundingBox(AABB& bbox) const override;
bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
const std::vector<unsigned int>& getTriangles() const;
const EigenSTL::vector_Vector3d& getVertices() const;
const EigenSTL::vector_Vector3d& getScaledVertices() const;
const EigenSTL::vector_Vector4d& getPlanes() const;
BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
void computeScaledVerticesFromPlaneProjections();
void correctVertexOrderFromPlanes();
void updateInternalData() override;
protected:
void useDimensions(const shapes::Shape* shape) override;
unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const;
bool isPointInsidePlanes(const Eigen::Vector3d& point) const;
// PIMPL structure
struct MeshData;
// shape-dependent data; keep this in one struct so that a cheap pointer copy can be done in cloneAt()
std::shared_ptr<MeshData> mesh_data_;
// pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
Eigen::Isometry3d i_pose_;
Eigen::Vector3d center_;
double radiusB_;
double radiusBSqr_;
Box bounding_box_;
// pointer to an array of scaled vertices
// If the padding is 0 & scaling is 1, then there is no need to have scaled vertices;
// we can just point to the vertices in mesh_data_.
// Otherwise, point to scaled_vertices_storage_
EigenSTL::vector_Vector3d* scaled_vertices_;
private:
std::unique_ptr<EigenSTL::vector_Vector3d> scaled_vertices_storage_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class BodyVector
{
public:
BodyVector();
BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Isometry3d& poses, double padding = 0.0);
~BodyVector();
void addBody(Body* body);
void addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding = 0.0);
void clear();
void setPose(unsigned int i, const Eigen::Isometry3d& pose);
std::size_t getCount() const;
bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
bool containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose = false) const;
bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const;
const Body* getBody(unsigned int i) const;
private:
std::vector<Body*> bodies_;
};
typedef std::shared_ptr<Body> BodyPtr;
typedef std::shared_ptr<const Body> BodyConstPtr;
} // namespace bodies
#endif