#include <ColdetModel.h>
Public Types | |
enum | PrimitiveType { SP_MESH, SP_BOX, SP_CYLINDER, SP_CONE, SP_SPHERE, SP_PLANE } |
Public Member Functions | |
void | addTriangle (int v1, int v2, int v3) |
void | addVertex (float x, float y, float z) |
void | build () |
build tree of bounding boxes to accelerate collision check | |
bool | checkCollisionWithPointCloud (const std::vector< Vector3 > &i_cloud, double i_radius) |
check collision between this triangle mesh and a point cloud | |
ColdetModel () | |
constructor | |
ColdetModel (const ColdetModel &org) | |
copy constructor | |
double | computeDistanceWithRay (const double *point, const double *dir) |
compute distance between a point and this mesh along ray | |
int | getAABBmaxNum () |
int | getAABBTreeDepth () |
void | getBoundingBoxData (const int depth, std::vector< Vector3 > &out_boxes) |
ColdetModelSharedDataSet * | getDataSet () |
int | getNumTriangles () const |
int | getNumVertices () const |
get the number of vertices | |
bool | getPrimitiveParam (unsigned int index, float &value) const |
get a parameter of primitive | |
PrimitiveType | getPrimitiveType () const |
get primitive type | |
void | getTriangle (int index, int &out_v1, int &out_v2, int &out_v3) const |
void | getVertex (int index, float &out_x, float &out_y, float &out_z) const |
get a vertex | |
bool | isValid () const |
check if build() is already called or not | |
const std::string & | name () const |
get name of this model | |
int | numofBBtoDepth (int minNumofBB) |
void | setName (const std::string &name) |
set name of this model | |
void | setNumPrimitiveParams (unsigned int nparam) |
set the number of parameters of primitive | |
void | setNumTriangles (int n) |
set the number of triangles | |
void | setNumVertices (int n) |
set the number of vertices | |
void | setPosition (const Matrix33 &R, const Vector3 &p) |
set position and orientation of this model | |
void | setPosition (const double *R, const double *p) |
set position and orientation of this model | |
bool | setPrimitiveParam (unsigned int index, float value) |
set a parameter of primitive | |
void | setPrimitivePosition (const double *R, const double *p) |
set position and orientation of primitive | |
void | setPrimitiveType (PrimitiveType ptype) |
set primitive type | |
void | setTriangle (int index, int v1, int v2, int v3) |
add a triangle | |
void | setVertex (int index, float x, float y, float z) |
add a vertex | |
virtual | ~ColdetModel () |
destructor | |
Private Member Functions | |
void | initialize () |
common part of constuctors | |
void | initNeighbor (int n) |
void | setNeighbor (int triangle0, int triangle1) |
void | setNeighborTriangle (int triangle, int vertex0, int vertex1, int vertex2) |
bool | setNeighborTriangleSub (int triangle, int vertex0, int vertex1) |
Private Attributes | |
ColdetModelSharedDataSet * | dataSet |
bool | isValid_ |
std::string | name_ |
IceMaths::Matrix4x4 * | pTransform |
transform of primitive | |
IceMaths::Matrix4x4 * | transform |
EdgeToTriangleMap | triangleMap |
Friends | |
class | ColdetModelPair |
Definition at line 61 of file ColdetModel.h.
Definition at line 64 of file ColdetModel.h.
constructor
Definition at line 24 of file ColdetModel.cpp.
ColdetModel::ColdetModel | ( | const ColdetModel & | org | ) |
copy constructor
Shape information stored in dataSet is shared with org
Definition at line 34 of file ColdetModel.cpp.
ColdetModel::~ColdetModel | ( | ) | [virtual] |
destructor
Definition at line 63 of file ColdetModel.cpp.
void ColdetModel::addTriangle | ( | int | v1, |
int | v2, | ||
int | v3 | ||
) |
add a triangle to the end of the vector
Definition at line 137 of file ColdetModel.cpp.
void ColdetModel::addVertex | ( | float | x, |
float | y, | ||
float | z | ||
) |
add a vertex to the end of the vector
Definition at line 104 of file ColdetModel.cpp.
build tree of bounding boxes to accelerate collision check
This method must be called before doing collision check
Definition at line 143 of file ColdetModel.cpp.
bool ColdetModel::checkCollisionWithPointCloud | ( | const std::vector< Vector3 > & | i_cloud, |
double | i_radius | ||
) |
check collision between this triangle mesh and a point cloud
i_cloud | points |
i_radius | radius of spheres assigned to the points |
Definition at line 331 of file ColdetModel.cpp.
double ColdetModel::computeDistanceWithRay | ( | const double * | point, |
const double * | dir | ||
) |
compute distance between a point and this mesh along ray
point | a point |
dir | direction of ray |
Definition at line 314 of file ColdetModel.cpp.
Definition at line 201 of file ColdetModel.cpp.
Definition at line 197 of file ColdetModel.cpp.
void ColdetModel::getBoundingBoxData | ( | const int | depth, |
std::vector< Vector3 > & | out_boxes | ||
) |
Definition at line 223 of file ColdetModel.cpp.
ColdetModelSharedDataSet* hrp::ColdetModel::getDataSet | ( | ) | [inline] |
Definition at line 245 of file ColdetModel.h.
int ColdetModel::getNumTriangles | ( | ) | const |
Definition at line 92 of file ColdetModel.cpp.
int ColdetModel::getNumVertices | ( | ) | const |
get the number of vertices
Definition at line 79 of file ColdetModel.cpp.
bool ColdetModel::getPrimitiveParam | ( | unsigned int | index, |
float & | value | ||
) | const |
get a parameter of primitive
index | index of the parameter |
value | value of the parameter |
Definition at line 298 of file ColdetModel.cpp.
Definition at line 128 of file ColdetModel.cpp.
void ColdetModel::getVertex | ( | int | index, |
float & | out_x, | ||
float & | out_y, | ||
float & | out_z | ||
) | const |
get a vertex
index | index of the vertex |
out_x | x position of the vertex |
out_y | y position of the vertex |
out_z | z position of the vertex |
Definition at line 110 of file ColdetModel.cpp.
void ColdetModel::initialize | ( | ) | [private] |
common part of constuctors
Definition at line 43 of file ColdetModel.cpp.
void ColdetModel::initNeighbor | ( | int | n | ) | [private] |
Definition at line 383 of file ColdetModel.cpp.
bool hrp::ColdetModel::isValid | ( | ) | const [inline] |
check if build() is already called or not
Definition at line 165 of file ColdetModel.h.
const std::string& hrp::ColdetModel::name | ( | ) | const [inline] |
int ColdetModel::numofBBtoDepth | ( | int | minNumofBB | ) |
Definition at line 190 of file ColdetModel.cpp.
void hrp::ColdetModel::setName | ( | const std::string & | name | ) | [inline] |
set name of this model
name | name of this model |
Definition at line 87 of file ColdetModel.h.
void ColdetModel::setNeighbor | ( | int | triangle0, |
int | triangle1 | ||
) | [private] |
Definition at line 387 of file ColdetModel.cpp.
void ColdetModel::setNeighborTriangle | ( | int | triangle, |
int | vertex0, | ||
int | vertex1, | ||
int | vertex2 | ||
) | [private] |
Definition at line 351 of file ColdetModel.cpp.
bool ColdetModel::setNeighborTriangleSub | ( | int | triangle, |
int | vertex0, | ||
int | vertex1 | ||
) | [private] |
Definition at line 357 of file ColdetModel.cpp.
void ColdetModel::setNumPrimitiveParams | ( | unsigned int | nparam | ) |
set the number of parameters of primitive
nparam | the number of parameters of primitive |
Definition at line 285 of file ColdetModel.cpp.
set the number of triangles
n | the number of triangles |
Definition at line 85 of file ColdetModel.cpp.
set the number of vertices
n | the number of vertices |
Definition at line 73 of file ColdetModel.cpp.
set position and orientation of this model
R | new orientation |
p | new position |
Definition at line 258 of file ColdetModel.cpp.
void ColdetModel::setPosition | ( | const double * | R, |
const double * | p | ||
) |
set position and orientation of this model
R | new orientation (length = 9) |
p | new position (length = 3) |
Definition at line 267 of file ColdetModel.cpp.
bool ColdetModel::setPrimitiveParam | ( | unsigned int | index, |
float | value | ||
) |
set a parameter of primitive
index | index of the parameter |
value | value of the parameter |
Definition at line 290 of file ColdetModel.cpp.
void ColdetModel::setPrimitivePosition | ( | const double * | R, |
const double * | p | ||
) |
set position and orientation of primitive
R | orientation relative to link (length = 9) |
p | position relative to link (length = 3) |
Definition at line 306 of file ColdetModel.cpp.
add a triangle
index | index of the triangle |
v1 | index of the first vertex |
v2 | index of the second vertex |
v3 | index of the third vertex |
Definition at line 119 of file ColdetModel.cpp.
void ColdetModel::setVertex | ( | int | index, |
float | x, | ||
float | y, | ||
float | z | ||
) |
add a vertex
index | index of the vertex |
x | x position of the vertex |
y | y position of the vertex |
z | z position of the vertex |
Definition at line 98 of file ColdetModel.cpp.
friend class ColdetModelPair [friend] |
Definition at line 265 of file ColdetModel.h.
Definition at line 255 of file ColdetModel.h.
bool hrp::ColdetModel::isValid_ [private] |
Definition at line 259 of file ColdetModel.h.
std::string hrp::ColdetModel::name_ [private] |
Definition at line 258 of file ColdetModel.h.
IceMaths::Matrix4x4* hrp::ColdetModel::pTransform [private] |
transform of primitive
Definition at line 257 of file ColdetModel.h.
IceMaths::Matrix4x4* hrp::ColdetModel::transform [private] |
Definition at line 256 of file ColdetModel.h.
Definition at line 260 of file ColdetModel.h.