265 lines
8.9 KiB
C++
265 lines
8.9 KiB
C++
|
// Copyright 2018-2019 The MathWorks, Inc.
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @file
|
||
|
|
* @brief Interface for collision geometry representation based on support mapping
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifndef COLLISIONCODEGEN_COLLISIONGEOMETRY_HPP
|
||
|
|
#define COLLISIONCODEGEN_COLLISIONGEOMETRY_HPP
|
||
|
|
|
||
|
|
#ifdef BUILDING_LIBMWCOLLISIONCODEGEN
|
||
|
|
#include <ccd/ccd_vec3.h> // need to use some data type definition
|
||
|
|
#include <ccd/ccd_quat.h> // for ccd quaternion functions
|
||
|
|
#else
|
||
|
|
#include <ccd_vec3.h> // need to use some data type definition
|
||
|
|
#include <ccd_quat.h> // for ccd quaternion functions
|
||
|
|
#endif
|
||
|
|
#include "collisioncodegen_helper.hpp"
|
||
|
|
#include <iostream>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
|
||
|
|
namespace shared_robotics
|
||
|
|
{
|
||
|
|
class CollisionGeometry;
|
||
|
|
struct CollisionGeometryDeleter
|
||
|
|
{
|
||
|
|
void operator ()(CollisionGeometry* ptr);
|
||
|
|
};
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief The @c CollisionGeometry class.
|
||
|
|
* @details A collision geometry can be specified as a primitive (box, cylinder or sphere)
|
||
|
|
* or a convex mesh (vertices only, or vertices and faces). ccd algorithm interacts
|
||
|
|
* with a collision geometry through its support function
|
||
|
|
*/
|
||
|
|
class COLLISIONCODEGEN_API CollisionGeometry
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
|
||
|
|
/// enum for collision geometry types
|
||
|
|
enum Type
|
||
|
|
{
|
||
|
|
Box,
|
||
|
|
Sphere,
|
||
|
|
Cylinder,
|
||
|
|
ConvexMesh,
|
||
|
|
ConvexMeshFull
|
||
|
|
};
|
||
|
|
|
||
|
|
/// destructor
|
||
|
|
virtual ~CollisionGeometry();
|
||
|
|
|
||
|
|
/// constructor for box primitive
|
||
|
|
CollisionGeometry(ccd_real_t x, ccd_real_t y, ccd_real_t z): m_x(x), m_y(y), m_z(z)
|
||
|
|
{
|
||
|
|
initializePositionAndOrientation();
|
||
|
|
m_type = Box;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// constructor for sphere primitive
|
||
|
|
CollisionGeometry(ccd_real_t radius): m_radius(radius)
|
||
|
|
{
|
||
|
|
initializePositionAndOrientation();
|
||
|
|
m_type = Sphere;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// constructor for cylinder primitive
|
||
|
|
CollisionGeometry(ccd_real_t radius, ccd_real_t height): m_radius(radius), m_height(height)
|
||
|
|
{
|
||
|
|
initializePositionAndOrientation();
|
||
|
|
m_type = Cylinder;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// constructor for convex mesh (vertices only)
|
||
|
|
CollisionGeometry(ccd_real_t * vertices, int numVertices, bool isColumnMajor = true)
|
||
|
|
{
|
||
|
|
initializePositionAndOrientation();
|
||
|
|
m_type = ConvexMesh;
|
||
|
|
m_numVertices = static_cast<std::size_t>(numVertices);
|
||
|
|
|
||
|
|
ccd_vec3_t vert = { {0, 0, 0} };
|
||
|
|
std::size_t numV = m_numVertices;
|
||
|
|
|
||
|
|
if (isColumnMajor)
|
||
|
|
{
|
||
|
|
for (std::size_t k = 0; k < numV; k++)
|
||
|
|
{
|
||
|
|
ccdVec3Set(&vert, vertices[k], vertices[numV + k], vertices[2 * numV + k]);
|
||
|
|
m_vertices.push_back(vert);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
for (std::size_t k = 0; k < numV; k++)
|
||
|
|
{
|
||
|
|
ccdVec3Set(&vert, vertices[3 * k], vertices[3 * k + 1], vertices[3 * k + 2]);
|
||
|
|
m_vertices.push_back(vert);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// constructor for convex mesh (vertices and faces)
|
||
|
|
CollisionGeometry(ccd_real_t * vertices, int * faces, int numVertices, int numFaces, bool isColumnMajor = true)
|
||
|
|
{
|
||
|
|
initializePositionAndOrientation();
|
||
|
|
m_type = ConvexMeshFull;
|
||
|
|
m_numVertices = static_cast<std::size_t>(numVertices);
|
||
|
|
m_numFaces = static_cast<std::size_t>(numFaces);
|
||
|
|
|
||
|
|
ccd_vec3_t vert = { {0, 0, 0} };
|
||
|
|
idx_vec3_t face = { {0, 0, 0} };
|
||
|
|
std::size_t numV = m_numVertices;
|
||
|
|
std::size_t numF = m_numFaces;
|
||
|
|
|
||
|
|
if (isColumnMajor)
|
||
|
|
{
|
||
|
|
for (std::size_t k = 0; k < numV; k++)
|
||
|
|
{
|
||
|
|
ccdVec3Set(&vert, vertices[k], vertices[numV + k], vertices[2 * numV + k]);
|
||
|
|
m_vertices.push_back(vert);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (std::size_t q = 0; q < numF; q++)
|
||
|
|
{
|
||
|
|
idxVec3Set(&face, static_cast<std::size_t>(faces[q]),
|
||
|
|
static_cast<std::size_t>(faces[numF + q]),
|
||
|
|
static_cast<std::size_t>(faces[2 * numF + q]));
|
||
|
|
m_faces.push_back(face);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
for (std::size_t k = 0; k < numV; k++)
|
||
|
|
{
|
||
|
|
ccdVec3Set(&vert, vertices[3 * k], vertices[3 * k + 1], vertices[3 * k + 2]);
|
||
|
|
m_vertices.push_back(vert);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (std::size_t q = 0; q < numF; q++)
|
||
|
|
{
|
||
|
|
idxVec3Set(&face, static_cast<std::size_t>(faces[3 * q]),
|
||
|
|
static_cast<std::size_t>(faces[3 * q + 1]),
|
||
|
|
static_cast<std::size_t>(faces[3 * q + 2]));
|
||
|
|
m_faces.push_back(face);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
constructConnectionList();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// query the type of the collision geometry
|
||
|
|
std::string getType() const;
|
||
|
|
|
||
|
|
|
||
|
|
/// support function for libccd interface
|
||
|
|
void support(const ccd_vec3_t* dir,
|
||
|
|
ccd_vec3_t* supportVertex) const;
|
||
|
|
|
||
|
|
protected:
|
||
|
|
/// sign function
|
||
|
|
int sign(ccd_real_t val) const;
|
||
|
|
|
||
|
|
/// must only be invoked after m_vertices and m_faces are initialized
|
||
|
|
void constructConnectionList();
|
||
|
|
|
||
|
|
|
||
|
|
/// type of the geometry
|
||
|
|
Type m_type;
|
||
|
|
|
||
|
|
|
||
|
|
/// dimension x of the box, if the geometry is a box primitive
|
||
|
|
ccd_real_t m_x;
|
||
|
|
|
||
|
|
/// dimension x of the box, if the geometry is a box primitive
|
||
|
|
ccd_real_t m_y;
|
||
|
|
|
||
|
|
/// dimension x of the box, if the geometry is a box primitive
|
||
|
|
ccd_real_t m_z;
|
||
|
|
|
||
|
|
|
||
|
|
/// radius of the sphere, if the geometry is a sphere primitive, or radius of the bottom of a cylinder, if the geometry is a cylinder primitive
|
||
|
|
ccd_real_t m_radius;
|
||
|
|
|
||
|
|
|
||
|
|
/// height of the cylinder, if the geometry is a cylinder primitive
|
||
|
|
ccd_real_t m_height;
|
||
|
|
|
||
|
|
|
||
|
|
/// number of vertices in the convex mesh, if the geometry is a convex primitive
|
||
|
|
std::size_t m_numVertices;
|
||
|
|
|
||
|
|
/// number of faces of the convex mesh, if the geometry is a convex primitive
|
||
|
|
std::size_t m_numFaces;
|
||
|
|
|
||
|
|
/// vertices of the mesh (if the geometry is a mesh primitive)
|
||
|
|
/// vertices is saved as a vector of ccd_vec3_t structs with m_numVertices elements
|
||
|
|
std::vector<ccd_vec3_t> m_vertices;
|
||
|
|
|
||
|
|
/// faces of the convex mesh (if the geometry is a mesh primitive)
|
||
|
|
/// faces is save as an (m_numFaces)-by-1 vector of idx_vec3_t structs consisting of node indices
|
||
|
|
std::vector<idx_vec3_t> m_faces;
|
||
|
|
|
||
|
|
/// connection list for each node int the mesh
|
||
|
|
std::vector< std::vector<std::size_t> > m_connectionList;
|
||
|
|
|
||
|
|
public:
|
||
|
|
|
||
|
|
/// position of the geometry
|
||
|
|
ccd_vec3_t m_pos;
|
||
|
|
|
||
|
|
/// orientation of the geometry (ccd assuming [x, y, z, w] sequence)
|
||
|
|
ccd_quat_t m_quat;
|
||
|
|
|
||
|
|
private:
|
||
|
|
void initializePositionAndOrientation()
|
||
|
|
{
|
||
|
|
/// position of the geometry which is an array of size 3
|
||
|
|
for(int i = 0; i < 3; i++) m_pos.v[i] = 0;
|
||
|
|
|
||
|
|
/// orientation of the geometry which is an array of size 4 (ccd assuming [x, y, z, w] sequence)
|
||
|
|
for(int i = 0; i < 3; i++) m_quat.q[i] = 0;
|
||
|
|
m_quat.q[3] = 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
/// sign function
|
||
|
|
inline int shared_robotics::CollisionGeometry::sign(ccd_real_t val) const
|
||
|
|
{
|
||
|
|
|
||
|
|
if (CCD_FABS(val) < CCD_EPS)
|
||
|
|
{
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
else if (val < 0.0)
|
||
|
|
{
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline std::string shared_robotics::CollisionGeometry::getType() const
|
||
|
|
{
|
||
|
|
switch(m_type)
|
||
|
|
{
|
||
|
|
case Box:
|
||
|
|
return "Box";
|
||
|
|
case Sphere:
|
||
|
|
return "Sphere";
|
||
|
|
case Cylinder:
|
||
|
|
return "Cylinder";
|
||
|
|
case ConvexMeshFull:
|
||
|
|
return "ConvexMeshFull";
|
||
|
|
default:
|
||
|
|
return "ConvexMesh";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace shared_robotics
|
||
|
|
#endif
|