// 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