diff --git a/CHANGELOG.md b/CHANGELOG.md index 1e4074f97..2ba8ab1e7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,10 @@ * Switched to Eigen for math operations: [#96](https://github.com/flexible-collision-library/fcl/issues/96), [#150](https://github.com/flexible-collision-library/fcl/pull/150) * fcl::Transform defined to be an Isometry to facilitate inverses [#318](https://github.com/flexible-collision-library/fcl/pull/318) +* Geometry + + * Simplified Convex class, deprecating old constructor in favor of simpler, documented constructor: [#325](https://github.com/flexible-collision-library/fcl/pull/325) + * Broadphase * Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156) diff --git a/include/fcl/geometry/collision_geometry-inl.h b/include/fcl/geometry/collision_geometry-inl.h index 0fbfc01f6..b5695325e 100644 --- a/include/fcl/geometry/collision_geometry-inl.h +++ b/include/fcl/geometry/collision_geometry-inl.h @@ -141,6 +141,13 @@ S CollisionGeometry::computeVolume() const template Matrix3 CollisionGeometry::computeMomentofInertiaRelatedToCOM() const { + // TODO(SeanCurtis-TRI): This is *horribly* inefficient. In complex cases, + // this will require computing volume integrals three times. The + // CollisionGeometry class should have a single virtual function that will + // return all three quantities in one call so that particular sub-classes can + // override this to process this answer more efficiently. The default + // implementation can be exactly these three calls. + // See: https://github.com/flexible-collision-library/fcl/issues/327. Matrix3 C = computeMomentofInertia(); Vector3 com = computeCOM(); S V = computeVolume(); diff --git a/include/fcl/geometry/shape/convex-inl.h b/include/fcl/geometry/shape/convex-inl.h index eefe94a69..8c671067c 100644 --- a/include/fcl/geometry/shape/convex-inl.h +++ b/include/fcl/geometry/shape/convex-inl.h @@ -34,6 +34,7 @@ */ /** @author Jia Pan */ +/** @author Sean Curtis (2018) Modify API and correct implementation bugs. */ #ifndef FCL_SHAPE_CONVEX_INL_H #define FCL_SHAPE_CONVEX_INL_H @@ -49,59 +50,31 @@ class FCL_EXPORT Convex; //============================================================================== template -Convex::Convex( - Vector3* plane_normals, S* plane_dis, int num_planes_, - Vector3* points, int num_points_, int* polygons_) - : ShapeBase() -{ - plane_normals = plane_normals; - plane_dis = plane_dis; - num_planes = num_planes_; - points = points; - num_points = num_points_; - polygons = polygons_; - edges = nullptr; - +Convex::Convex(int num_vertices, Vector3* vertices, + int num_faces, int* faces) + : ShapeBase(), + num_vertices(num_vertices), + vertices(vertices), + num_faces(num_faces), + faces(faces) { + assert(vertices != nullptr); + assert(faces != nullptr); + // Compute an interior point. We're computing the mean point and *not* some + // alternative such as the centroid or bounding box center. Vector3 sum = Vector3::Zero(); - for(int i = 0; i < num_points; ++i) - { - sum += points[i]; + for(int i = 0; i < num_vertices; ++i) { + sum += vertices[i]; } - - center = sum * (S)(1.0 / num_points); - - fillEdges(); -} - -//============================================================================== -template -Convex::Convex(const Convex& other) - : ShapeBase(other) -{ - plane_normals = other.plane_normals; - plane_dis = other.plane_dis; - num_planes = other.num_planes; - points = other.points; - polygons = other.polygons; - edges = new Edge[other.num_edges]; - memcpy(edges, other.edges, sizeof(Edge) * num_edges); -} - -//============================================================================== -template -Convex::~Convex() -{ - delete [] edges; + interior_point = sum * (S)(1.0 / num_vertices); } //============================================================================== template -void Convex::computeLocalAABB() -{ - this->aabb_local.min_.setConstant(-std::numeric_limits::max()); - this->aabb_local.max_.setConstant(std::numeric_limits::max()); - for(int i = 0; i < num_points; ++i) - this->aabb_local += points[i]; +void Convex::computeLocalAABB() { + this->aabb_local.min_.setConstant(std::numeric_limits::max()); + this->aabb_local.max_.setConstant(-std::numeric_limits::max()); + for(int i = 0; i < num_vertices; ++i) + this->aabb_local += vertices[i]; this->aabb_center = this->aabb_local.center(); this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); @@ -109,15 +82,18 @@ void Convex::computeLocalAABB() //============================================================================== template -NODE_TYPE Convex::getNodeType() const -{ +NODE_TYPE Convex::getNodeType() const { return GEOM_CONVEX; } //============================================================================== +// TODO(SeanCurtis-TRI): When revisiting these, consider the following +// resources: +// https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf +// http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.56.127&rep=rep1&type=pdf +// http://number-none.com/blow/inertia/bb_inertia.doc template -Matrix3 Convex::computeMomentofInertia() const -{ +Matrix3 Convex::computeMomentofInertia() const { Matrix3 C = Matrix3::Zero(); Matrix3 C_canonical; @@ -125,35 +101,37 @@ Matrix3 Convex::computeMomentofInertia() const 1/120.0, 1/ 60.0, 1/120.0, 1/120.0, 1/120.0, 1/ 60.0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { + S vol_times_six = 0; + int* face_encoding = faces; + int* index = faces + 1; + for(int i = 0; i < num_faces; ++i) { + const int vertex_count = *face_encoding; + Vector3 face_center = Vector3::Zero(); + + // Compute the center of the face. + for(int j = 0; j < vertex_count; ++j) + face_center += vertices[index[j]]; + face_center = face_center * (1.0 / vertex_count); + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for(int j = 0; j < vertex_count; ++j) { int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int e_second = index[(j + 1) % vertex_count]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); Matrix3 A; // this is A' in the original document A.row(0) = v1; A.row(1) = v2; A.row(2) = v3; C += A.transpose() * C_canonical * A * d_six_vol; // change accordingly + vol_times_six += d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_encoding += vertex_count + 1; + index = face_encoding + 1; } S trace_C = C(0, 0) + C(1, 1) + C(2, 2); @@ -163,148 +141,101 @@ Matrix3 Convex::computeMomentofInertia() const -C(1, 0), trace_C - C(1, 1), -C(1, 2), -C(2, 0), -C(2, 1), trace_C - C(2, 2); - return m; + return m * (6 / vol_times_six); } //============================================================================== template -Vector3 Convex::computeCOM() const -{ +Vector3 Convex::computeCOM() const { Vector3 com = Vector3::Zero(); S vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { + int* face_encoding = faces; + int* index = faces + 1; + for(int i = 0; i < num_faces; ++i) { + const int vertex_count = *face_encoding; + Vector3 face_center = Vector3::Zero(); + + // TODO(SeanCurtis-TRI): See note in computeVolume() on the efficiency of + // this approach. + + // Compute the center of the polygon. + for(int j = 0; j < vertex_count; ++j) + face_center += vertices[index[j]]; + face_center = face_center * (1.0 / vertex_count); + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for(int j = 0; j < vertex_count; ++j) { int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int e_second = index[(j + 1) % vertex_count]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; - com += (points[e_first] + points[e_second] + plane_center) * d_six_vol; + com += (vertices[e_first] + vertices[e_second] + face_center) * d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_encoding += vertex_count + 1; + index = face_encoding + 1; } return com / (vol * 4); // here we choose zero as the reference } //============================================================================== -template -S Convex::computeVolume() const -{ +template S Convex::computeVolume() const { S vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { + int *face_encoding = faces; + int *index = faces + 1; + for(int i = 0; i < num_faces; ++i) { + const int vertex_count = *face_encoding; + Vector3 face_center = Vector3::Zero(); + + // TODO(SeanCurtis-TRI): While this is general, this is inefficient. If the + // face happens to be a triangle, this does 3X the requisite work. + // If the face is a 4-gon, then this does 2X the requisite work. + // As N increases in the N-gon this approach's inherent relative penalty + // shrinks. Ideally, this should at least key on 3-gon and 4-gon before + // falling through to this. + + // Compute the center of the polygon. + for(int j = 0; j < vertex_count; ++j) + face_center += vertices[index[j]]; + face_center = face_center * (1.0 / vertex_count); + + // TODO(SeanCurtis-TRI): Because volume serves as the weights for + // center-of-mass an inertia computations, it should be refactored into its + // own function that can be invoked by providing three vertices (the fourth + // being the origin). + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for(int j = 0; j < vertex_count; ++j) { int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int e_second = index[(j + 1) % vertex_count]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_encoding += vertex_count + 1; + index = face_encoding + 1; } return vol / 6; } -//============================================================================== -template -void Convex::fillEdges() -{ - int* points_in_poly = polygons; - if(edges) delete [] edges; - - int num_edges_alloc = 0; - for(int i = 0; i < num_planes; ++i) - { - num_edges_alloc += *points_in_poly; - points_in_poly += (*points_in_poly + 1); - } - - edges = new Edge[num_edges_alloc]; - - points_in_poly = polygons; - int* index = polygons + 1; - num_edges = 0; - Edge e; - bool isinset; - for(int i = 0; i < num_planes; ++i) - { - for(int j = 0; j < *points_in_poly; ++j) - { - e.first = std::min(index[j], index[(j+1)%*points_in_poly]); - e.second = std::max(index[j], index[(j+1)%*points_in_poly]); - isinset = false; - for(int k = 0; k < num_edges; ++k) - { - if((edges[k].first == e.first) && (edges[k].second == e.second)) - { - isinset = true; - break; - } - } - - if(!isinset) - { - edges[num_edges].first = e.first; - edges[num_edges].second = e.second; - ++num_edges; - } - } - - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; - } - - if(num_edges < num_edges_alloc) - { - Edge* tmp = new Edge[num_edges]; - memcpy(tmp, edges, num_edges * sizeof(Edge)); - delete [] edges; - edges = tmp; - } -} - //============================================================================== template std::vector> Convex::getBoundVertices( - const Transform3& tf) const -{ - std::vector> result(num_points); - for(int i = 0; i < num_points; ++i) + const Transform3& tf) const { + std::vector> result(num_vertices); + for(int i = 0; i < num_vertices; ++i) { - result[i] = tf * points[i]; + result[i] = tf * vertices[i]; } return result; diff --git a/include/fcl/geometry/shape/convex.h b/include/fcl/geometry/shape/convex.h index 11d663cc5..6d5fed874 100644 --- a/include/fcl/geometry/shape/convex.h +++ b/include/fcl/geometry/shape/convex.h @@ -34,6 +34,7 @@ */ /** @author Jia Pan */ +/** @author Sean Curtis (2018) Streamline API and document. */ #ifndef FCL_SHAPE_CONVEX_H #define FCL_SHAPE_CONVEX_H @@ -43,7 +44,37 @@ namespace fcl { -/// @brief Convex polytope +/// @brief A convex polytope +/// +/// The %Convex class represents a subset of general meshes: convex meshes. +/// The class represents its underlying mesh quite simply: an ordered list of +/// vertices, `V = [v₀, v₁, ..., vₙ₋₁]`, and an ordered list of faces, +/// `F = [f₀, f₁, ..., fₘ₋₁]`, built on those vertices (via vertex _indices_). +/// +/// A mesh is only compatible with %Convex if it satisfies the following +/// properties: +/// - Each face, fᵢ, must be planar and completely lies on a supporting +/// plane πᵢ. The ordered sets `F = [f₀, f₁, ..., fₙ₋₁]` and +/// `Π = [π₀, π₁, ..., πₙ₋₁]` are defined such that face fᵢ has supporting +/// plane πᵢ. +/// - A face fᵢ is defined by an ordered list of vertex indices (e.g., +/// `fᵢ = [iᵢ₀, iᵢ₁, ..., iᵢₖ₋₁]` (for a face with k vertices and k edges). +/// The _ordering_ of the vertex indices must visit the face's vertices in a +/// _counter-clockwise_ order when viewed from outside the mesh and the +/// vertices must all lie on the face's supporting plane. +/// - Define functions `πᵢ(x)` which report the signed distance from point `x` +/// to plane `πᵢ`. To be convex, it must be true that +/// `π(v) ≤ 0, ∀ π ∈ Π ∧ v ∈ V`. +/// +/// If those requirements are satisfied, for a given convex region, the mesh +/// with the smallest number of faces *may* have non-triangular faces. In fact, +/// they could be polygons with an arbitrary number of edges/vertices. The +/// %Convex class supports compact representation. But it *also* supports +/// representations that decompose a large n-gon into a set of smaller polygons +/// or triangles. In this case, each of the triangles supporting planes would +/// be the same and the requirements listed above would still be satisfied. +/// +/// @tparam S_ The scalar type; must be a valid Eigen scalar. template class FCL_EXPORT Convex : public ShapeBase { @@ -51,65 +82,85 @@ class FCL_EXPORT Convex : public ShapeBase using S = S_; - /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information - Convex(Vector3* plane_normals, - S* plane_dis, - int num_planes, - Vector3* points, - int num_points, - int* polygons); + // TODO(SeanCurtis-TRI): A huge shopping list of issues with this class are + // enumerated in https://github.com/flexible-collision-library/fcl/issues/326. + + /// @brief Constructor + /// + /// @note: The %Convex geometry does *not* take ownership of any of the data + /// provided. The data must remain valid for as long as the %Convex instance + /// and must be cleaned up explicitly. + /// + /// @warning: The %Convex class does *not* validate the input; it trusts that + /// the inputs truly represent a coherent convex polytope. + /// + /// @param num_vertices The number of vertices defined in `vertices`. + /// @param vertices The positions of the polytope vertices. + /// @param num_faces The number of faces defined in `faces`. + /// @param faces Encoding of the polytope faces. Must encode + /// `num_faces` number of faces. See member + /// documentation for details on encoding. + Convex(int num_vertices, Vector3* vertices, int num_faces, int* faces); /// @brief Copy constructor - Convex(const Convex& other); + Convex(const Convex& other) = default; - ~Convex(); + ~Convex() = default; - /// @brief Compute AABB + /// @brief Compute AABB in the geometry's canonical frame. void computeLocalAABB() override; - /// @brief Get node type: a conex polytope + /// @brief Get node type: a convex polytope. NODE_TYPE getNodeType() const override; - - Vector3* plane_normals; - S* plane_dis; - - /// @brief An array of indices to the points of each polygon, it should be the number of vertices - /// followed by that amount of indices to "points" in counter clockwise order - int* polygons; - - Vector3* points; - int num_points; - int num_edges; - int num_planes; - - struct Edge - { - int first, second; - }; - - Edge* edges; - - /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) - Vector3 center; - - /// based on http://number-none.com/blow/inertia/bb_inertia.doc + /// @brief The total number of vertices in the convex mesh. + int num_vertices; + + /// @brief The vertex positions in the geometry's frame G. + Vector3* vertices; + + /// @brief The total number of faces in the convex mesh. + int num_faces; + + /// @brief The representation of the *faces* of the convex hull. + /// + /// The array is the concatenation of an integer-based representations of each + /// face. A single face is encoded as a sub-array of ints where the first int + /// is the *number* n of vertices in the face, and the following n values + /// are ordered indices into `vertices` which visit the vertex values in a + /// *counter-clockwise* order (viewed from the outside). + /// + /// For a well-formed face `f` consisting of indices [i₀, i₁, ..., iₘ₋₁], it + /// should be the case that: + /// + /// `eⱼ × eⱼ₊₁ · n̂ₚ = |eⱼ × eⱼ₊₁|, ∀ 0 ≤ j < m, j ∈ ℤ`, where + /// `n̂ₚ` is the normal for plane `π` on which face `f` lies. + /// `eⱼ = vertices[iⱼ] - vertices[iⱼ₋₁]` is the edge vector of face `f` + /// defined by adjacent vertex indices at jᵗʰ vertex (wrapping around such + /// that `j - 1 = m - 1` for `j = 0`). + /// + /// Satisfying this condition implies the following: + /// 1. vertices are not coincident and + /// 3. the indices of the face correspond to a proper counter-clockwise + /// ordering. + int* faces; + + /// @brief A point guaranteed to be on the interior of the convex polytope, + /// used for collision. + Vector3 interior_point; + + // Documentation inherited. Matrix3 computeMomentofInertia() const override; - // Documentation inherited + // Documentation inherited. Vector3 computeCOM() const override; - // Documentation inherited + // Documentation inherited. S computeVolume() const override; /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; - -protected: - - /// @brief Get edge information - void fillEdges(); }; using Convexf = Convex; diff --git a/include/fcl/geometry/shape/utility-inl.h b/include/fcl/geometry/shape/utility-inl.h index 1fa77fc7a..518b64199 100644 --- a/include/fcl/geometry/shape/utility-inl.h +++ b/include/fcl/geometry/shape/utility-inl.h @@ -234,9 +234,9 @@ struct FCL_EXPORT ComputeBVImpl, Convex> const Vector3& T = tf.translation(); AABB bv_; - for(int i = 0; i < s.num_points; ++i) + for(int i = 0; i < s.num_vertices; ++i) { - Vector3 new_p = R * s.points[i] + T; + Vector3 new_p = R * s.vertices[i] + T; bv_ += new_p; } @@ -250,7 +250,7 @@ struct FCL_EXPORT ComputeBVImpl, Convex> { static void run(const Convex& s, const Transform3& tf, OBB& bv) { - fit(s.points, s.num_points, bv); + fit(s.vertices, s.num_vertices, bv); bv.axis = tf.linear(); bv.To = tf * bv.To; diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index c41f12d32..dbaa5495b 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -2105,15 +2105,15 @@ static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v ccd_real_t maxdot, dot; int i; Vector3* curp; - const auto& center = c->convex->center; + const auto& center = c->convex->interior_point; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &c->rot_inv); maxdot = -CCD_REAL_MAX; - curp = c->convex->points; + curp = c->convex->vertices; - for(i = 0; i < c->convex->num_points; ++i, curp += 1) + for(i = 0; i < c->convex->num_vertices; ++i, curp += 1) { ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]); dot = ccdVec3Dot(&dir, &p); @@ -2167,7 +2167,8 @@ template static void centerConvex(const void* obj, ccd_vec3_t* c) { const auto *o = static_cast*>(obj); - ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]); + const Vector3& p = o->convex->interior_point; + ccdVec3Set(c, p[0], p[1], p[2]); ccdQuatRotVec(c, &o->rot); ccdVec3Add(c, &o->pos); } diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h index f639f4bc3..fdd9c6628 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h @@ -183,9 +183,9 @@ Vector3 getSupport( { const Convex* convex = static_cast*>(shape); S maxdot = - std::numeric_limits::max(); - Vector3* curp = convex->points; + Vector3* curp = convex->vertices; Vector3 bestv = Vector3::Zero(); - for(int i = 0; i < convex->num_points; ++i, curp+=1) + for(int i = 0; i < convex->num_vertices; ++i, curp+=1) { S dot = dir.dot(*curp); if(dot > maxdot) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h index e89330df4..c11279ef6 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h @@ -500,9 +500,9 @@ bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, Vector3 v; S depth = std::numeric_limits::max(); - for(int i = 0; i < s1.num_points; ++i) + for(int i = 0; i < s1.num_vertices; ++i) { - Vector3 p = tf1 * s1.points[i]; + Vector3 p = tf1 * s1.vertices[i]; S d = new_s2.signedDistance(p); if(d < depth) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h index 858a8608f..1ecf16f65 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h @@ -648,9 +648,9 @@ bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, Vector3 v_min, v_max; S d_min = std::numeric_limits::max(), d_max = -std::numeric_limits::max(); - for(int i = 0; i < s1.num_points; ++i) + for(int i = 0; i < s1.num_vertices; ++i) { - Vector3 p = tf1 * s1.points[i]; + Vector3 p = tf1 * s1.vertices[i]; S d = new_s2.signedDistance(p); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 1ae1ea5ce..c2156d2f2 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -97,4 +97,5 @@ foreach(test ${tests}) add_fcl_test(${test}) endforeach(test) +add_subdirectory(geometry) add_subdirectory(narrowphase) diff --git a/test/geometry/CMakeLists.txt b/test/geometry/CMakeLists.txt new file mode 100644 index 000000000..a87b0abfb --- /dev/null +++ b/test/geometry/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(shape) diff --git a/test/geometry/shape/CMakeLists.txt b/test/geometry/shape/CMakeLists.txt new file mode 100644 index 000000000..b227271ab --- /dev/null +++ b/test/geometry/shape/CMakeLists.txt @@ -0,0 +1,8 @@ +set(tests + test_convex.cpp + ) + +# Build all the tests +foreach(test ${tests}) + add_fcl_test(${test}) +endforeach(test) diff --git a/test/geometry/shape/test_convex.cpp b/test/geometry/shape/test_convex.cpp new file mode 100644 index 000000000..07770966d --- /dev/null +++ b/test/geometry/shape/test_convex.cpp @@ -0,0 +1,522 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. Toyota Research Institute + * All rights reserved. + * + * 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 CNRS-LAAS and AIST 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 OWNER 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 Sean Curtis (sean@tri.global) (2018) */ + +// Tests the implementation of a convex polytope geometry. + +#include "fcl/geometry/shape/convex.h" + +#include + +#include +#include + +#include "eigen_matrix_compare.h" +#include "fcl/common/types.h" + +namespace fcl { +namespace { + +using std::max; + +// Necessary to satisfy Eigen's alignment requirements. See +// http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html#StlContainers_vector +template +using PoseVector = std::vector, + Eigen::aligned_allocator>>; + +// Utilities to print scalar type in error messages. +template +struct ScalarString { + static std::string value() { return "unknown"; } +}; + +template <> +struct ScalarString { + static std::string value() { return "double"; } +}; + +template <> +struct ScalarString { + static std::string value() { return "float"; } +}; + +// Base definition of a "unit" convex polytope. Specific instances should define +// faces, vertices, and quantities such as volume, center of mass, and moment of +// inertia in terms of a scale factor. +template +class Polytope { + public: + explicit Polytope(S scale) : scale_(scale) {} + + virtual int face_count() const = 0; + virtual int vertex_count() const = 0; + virtual S volume() const = 0; + virtual Vector3 com() const = 0; + virtual Matrix3 principal_inertia_tensor() const = 0; + virtual std::string description() const = 0; + + // The scale of the polytope to use with test tolerances. + S scale() const { return scale_; } + const Vector3* points() const { return &vertices_[0]; } + const int* polygons() const { return &polygons_[0]; } + Convex MakeConvex() const { + // The Polytope class makes the pointers to vertices and faces const access. + // The Convex class calls for non-const pointers. Temporarily const-casting + // them to make it compatible. + return Convex(vertex_count(), const_cast*>(points()), + face_count(), const_cast(polygons())); + } + Vector3 min_point() const { + Vector3 m; + m.setConstant(std::numeric_limits::max()); + for (const Vector3& v : vertices_) { + for (int i = 0; i < 3; ++i) { + if (v(i) < m(i)) m(i) = v(i); + } + } + return m; + } + Vector3 max_point() const { + Vector3 m; + m.setConstant(-std::numeric_limits::max()); + for (const Vector3& v : vertices_) { + for (int i = 0; i < 3; ++i) { + if (v(i) > m(i)) m(i) = v(i); + } + } + return m; + } + Vector3 aabb_center() const { + return (max_point() + min_point()) / 2; + } + S aabb_radius() const { return (min_point() - aabb_center()).norm(); } + void SetPose(const Transform3& X_WP) { + for (auto& v : vertices_) { + v = X_WP * v; + } + } + + protected: + void add_vertex(const Vector3& vertex) { vertices_.push_back(vertex); } + void add_face(std::initializer_list indices) { + polygons_.push_back(static_cast(indices.size())); + polygons_.insert(polygons_.end(), indices); + } + // Confirms the number of vertices and number of polygons matches the counts + // implied by vertex_count() and face_count(), respectively. + void confirm_data() { + // Confirm point count. + GTEST_ASSERT_EQ(vertex_count(), static_cast(vertices_.size())); + + // Confirm face count. + // Count the number of faces encoded in polygons_; + int count = 0; + int i = 0; + while (i < static_cast(polygons_.size())) { + ++count; + i += polygons_[i] + 1; + } + GTEST_ASSERT_EQ(i, static_cast(polygons_.size())) + << "Badly defined polygons"; + GTEST_ASSERT_EQ(face_count(), count); + } + + private: + std::vector> vertices_; + std::vector polygons_; + S scale_{1}; +}; + +// A simple regular tetrahedron with edges of length `scale` centered on the +// origin. +template +class EquilateralTetrahedron : public Polytope { + public: + // Constructs the tetrahedron (of edge length `s`). + explicit EquilateralTetrahedron(S scale) : Polytope(scale), scale_(scale) { + // Tetrahedron vertices in the tet's canonical frame T. The tet is + // "centered" on the origin so that it's center of mass is simple [0, 0, 0]. + const S z_base = -1 / S(2 * sqrt(6.)); + Vector3 points_T[] = {{S(0.5), S(-0.5 / sqrt(3.)), z_base}, + {S(-0.5), S(-0.5 / sqrt(3.)), z_base}, + {S(0), S(1. / sqrt(3.)), z_base}, + {S(0), S(0), S(sqrt(3. / 8))}}; + for (const auto& v : points_T) { + this->add_vertex(scale * v); + }; + + // Now add the polygons + this->add_face({0, 1, 2}); + this->add_face({1, 0, 3}); + this->add_face({0, 2, 3}); + this->add_face({2, 1, 3}); + + this->confirm_data(); + } + // Properties of the polytope. + int face_count() const final { return 4; } + int vertex_count() const final { return 4; } + virtual S volume() const final { + // This assumes unit mass. + S s = this->scale(); + return S(sqrt(2) / 12) * s * s * s; + } + virtual Vector3 com() const final { return Vector3::Zero(); } + virtual Matrix3 principal_inertia_tensor() const { + // TODO(SeanCurtis-TRI): Replace this with a legitimate tensor. + throw std::logic_error("Not implemented yet"); + }; + std::string description() const final { + return "Tetrahedron with scale: " + std::to_string(this->scale()); + } + + private: + S scale_{0}; +}; + +// A simple cube with sides of length `scale`. +template +class Cube : public Polytope { + public: + Cube(S scale) : Polytope(scale) { + // Cube vertices in the cube's canonical frame C. + Vector3 points_C[] = {{S(-0.5), S(-0.5), S(-0.5)}, // v0 + {S(0.5), S(-0.5), S(-0.5)}, // v1 + {S(-0.5), S(0.5), S(-0.5)}, // v2 + {S(0.5), S(0.5), S(-0.5)}, // v3 + {S(-0.5), S(-0.5), S(0.5)}, // v4 + {S(0.5), S(-0.5), S(0.5)}, // v5 + {S(-0.5), S(0.5), S(0.5)}, // v6 + {S(0.5), S(0.5), S(0.5)}}; // v7 + for (const auto& v : points_C) { + this->add_vertex(scale * v); + } + + // Now add the polygons + this->add_face({1, 3, 7, 5}); // +x + this->add_face({0, 4, 6, 2}); // -x + this->add_face({4, 5, 7, 6}); // +y + this->add_face({0, 2, 3, 1}); // -y + this->add_face({6, 7, 3, 2}); // +z + this->add_face({0, 1, 5, 4}); // -z + + this->confirm_data(); + } + + // Polytope properties + int face_count() const final { return 6; } + int vertex_count() const final { return 8; } + virtual S volume() const final { + S s = this->scale(); + return s * s * s; + } + virtual Vector3 com() const final { return Vector3::Zero(); } + virtual Matrix3 principal_inertia_tensor() const { + S scale_sqd = this->scale() * this->scale(); + // This assumes unit mass. + return Eigen::DiagonalMatrix(1. / 6., 1. / 6., 1. / 6.) * scale_sqd; + }; + std::string description() const final { + return "Cube with scale: " + std::to_string(this->scale()); + } +}; + +void testConvexConstruction() { + Cube cube{1}; + // Set the cube at some other location to make sure that the interior point + // test/ doesn't pass just because it initialized to zero. + Vector3 p_WB(1, 2, 3); + cube.SetPose(Transform3(Eigen::Translation3d(p_WB))); + Convex convex = cube.MakeConvex(); + + // This doesn't depend on the correct logic in the constructor. But this is + // as convenient a time as any to test that it reports the right node type. + EXPECT_EQ(convex.getNodeType(), GEOM_CONVEX); + + // The constructor computes the interior point. + EXPECT_TRUE(CompareMatrices(convex.interior_point, p_WB)); +} + +template