From 3fbc45de934c874d6369abc52e743707abba9e33 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 3 May 2021 09:20:18 -0700 Subject: [PATCH] Fix incorrect MultiSphereConvexHull rendering (#1579) --- CHANGELOG.md | 6 + dart/external/CMakeLists.txt | 1 + dart/external/convhull_3d/CMakeLists.txt | 9 + dart/external/convhull_3d/convhull_3d.h | 1167 ++++++++++++++++++ dart/gui/OpenGLRenderInterface.cpp | 47 +- dart/gui/OpenGLRenderInterface.hpp | 3 + dart/gui/RenderInterface.cpp | 7 + dart/gui/RenderInterface.hpp | 4 + dart/gui/glut/SimWindow.cpp | 2 +- dart/gui/osg/render/MeshShapeNode.cpp | 6 - dart/gui/osg/render/MultiSphereShapeNode.cpp | 81 +- dart/math/Geometry.hpp | 18 + dart/math/Icosphere.hpp | 106 ++ dart/math/Mesh.hpp | 108 ++ dart/math/TriMesh.cpp | 41 + dart/math/TriMesh.hpp | 121 ++ dart/math/detail/Geometry-impl.hpp | 131 ++ dart/math/detail/Icosphere-impl.hpp | 221 ++++ dart/math/detail/Mesh-impl.hpp | 156 +++ dart/math/detail/TriMesh-impl.hpp | 210 ++++ data/skel/shapes.skel | 8 + examples/CMakeLists.txt | 1 + examples/rigid_shapes/CMakeLists.txt | 19 + examples/rigid_shapes/README.md | 20 + examples/rigid_shapes/main.cpp | 79 ++ unittests/unit/CMakeLists.txt | 2 + unittests/unit/test_Icosphere.cpp | 74 ++ unittests/unit/test_TriMesh.cpp | 148 +++ 28 files changed, 2761 insertions(+), 35 deletions(-) create mode 100644 dart/external/convhull_3d/CMakeLists.txt create mode 100644 dart/external/convhull_3d/convhull_3d.h create mode 100644 dart/math/Icosphere.hpp create mode 100644 dart/math/Mesh.hpp create mode 100644 dart/math/TriMesh.cpp create mode 100644 dart/math/TriMesh.hpp create mode 100644 dart/math/detail/Geometry-impl.hpp create mode 100644 dart/math/detail/Icosphere-impl.hpp create mode 100644 dart/math/detail/Mesh-impl.hpp create mode 100644 dart/math/detail/TriMesh-impl.hpp create mode 100644 examples/rigid_shapes/CMakeLists.txt create mode 100644 examples/rigid_shapes/README.md create mode 100644 examples/rigid_shapes/main.cpp create mode 100644 unittests/unit/test_Icosphere.cpp create mode 100644 unittests/unit/test_TriMesh.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 5bb79c9a8af69..f0a0f78416a8a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,7 +2,13 @@ ### [DART 6.11.0 (TBD)](https://github.com/dartsim/dart/milestone/64?closed=1) +* Math + + * Added `Mesh`, `TriMesh`, and `Icosphere` classes: [#1579](https://github.com/dartsim/dart/pull/1579) + +* GUI + * Fixed incorrect MultiSphereConvexHull rendering: [#1579](https://github.com/dartsim/dart/pull/1579) ### [DART 6.10.1 (2021-04-19)](https://github.com/dartsim/dart/milestone/65?closed=1) diff --git a/dart/external/CMakeLists.txt b/dart/external/CMakeLists.txt index 08377bd4a98e6..aa2fa0a2121a1 100644 --- a/dart/external/CMakeLists.txt +++ b/dart/external/CMakeLists.txt @@ -1,3 +1,4 @@ +add_subdirectory(convhull_3d) add_subdirectory(imgui) add_subdirectory(ikfast) add_subdirectory(lodepng) diff --git a/dart/external/convhull_3d/CMakeLists.txt b/dart/external/convhull_3d/CMakeLists.txt new file mode 100644 index 0000000000000..13d6a913a6b6f --- /dev/null +++ b/dart/external/convhull_3d/CMakeLists.txt @@ -0,0 +1,9 @@ +# Search all header and source files +file(GLOB hdrs "*.h") + +# Install +install( + FILES ${hdrs} + DESTINATION include/dart/external/convhull_3d + COMPONENT headers +) diff --git a/dart/external/convhull_3d/convhull_3d.h b/dart/external/convhull_3d/convhull_3d.h new file mode 100644 index 0000000000000..72dc3977f6446 --- /dev/null +++ b/dart/external/convhull_3d/convhull_3d.h @@ -0,0 +1,1167 @@ +/* + Copyright (c) 2017-2018 Leo McCormack + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ +/* + * Filename: + * convhull_3d.h + * Description: + * A header only C implementation of the 3-D quickhull algorithm. + * The code is largely derived from the "computational-geometry-toolbox" + * by George Papazafeiropoulos (c) 2014, originally distributed under + * the BSD (2-clause) license. + * To include this implementation in a project, simply add this: + * #define CONVHULL_3D_ENABLE + * #include "convhull_3d.h" + * By default, the algorithm uses double floating point precision. To + * use single precision (less accurate but quicker), also add this: + * #define CONVHULL_3D_USE_FLOAT_PRECISION + * If your project has CBLAS linked, then you can also speed things up + * a tad by adding this: + * #define CONVHULL_3D_USE_CBLAS + * The code is C++ compiler safe. + * Reference: "The Quickhull Algorithm for Convex Hull, C. Bradford + * Barber, David P. Dobkin and Hannu Huhdanpaa, Geometry + * Center Technical Report GCG53, July 30, 1993" + * Dependencies: + * cblas (optional for speed ups, especially for very large meshes) + * Author, date created: + * Leo McCormack, 02.10.2017 + */ + +/********** + * PUBLIC: + *********/ + +#ifndef CONVHULL_3D_INCLUDED +#define CONVHULL_3D_INCLUDED + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef CONVHULL_3D_USE_FLOAT_PRECISION +typedef float CH_FLOAT; +# define ch_pow powf +# define ch_sqrt sqrtf +#else +typedef double CH_FLOAT; +# define ch_pow pow +# define ch_sqrt sqrt +#endif +typedef struct _ch_vertex +{ + union + { + CH_FLOAT v[3]; + struct + { + CH_FLOAT x, y, z; + }; + }; +} ch_vertex; +typedef ch_vertex ch_vec3; + +#ifndef ch_malloc +# define ch_malloc malloc +#endif +#ifndef ch_calloc +# define ch_calloc calloc +#endif +#ifndef ch_realloc +# define ch_realloc realloc +#endif +#ifndef ch_free +# define ch_free free +#endif + +/* builds the convexhull, returning the face indices corresponding to + * "in_vertices" */ +void convhull_3d_build( /* input arguments */ + ch_vertex* const in_vertices, /* vector of input + vertices; nVert x 1 */ + const int nVert, /* number of vertices */ + /* output arguments */ + int** out_faces, /* & of empty int*, output face indices; + flat: nOut_faces x 3 */ + int* nOut_faces); /* & of int, number of output face + indices */ + +/* exports the vertices, face indices, and face normals, as an 'obj' file, ready + * for GPU */ +void convhull_3d_export_obj( /* input arguments */ + ch_vertex* const vertices, /* vector of input + vertices; nVert x 1 */ + const int nVert, /* number of vertices */ + int* const + faces, /* face indices; flat: nFaces x 3 */ + const int nFaces, /* number of faces in hull */ + const int + keepOnlyUsedVerticesFLAG, /* 0: exports + in_vertices, 1: + exports only used + vertices */ + char* const obj_filename); /* obj filename, WITHOUT + extension */ + +/* exports the vertices, face indices, and face normals, as an 'm' file, for + * MatLab verification */ +void convhull_3d_export_m( /* input arguments */ + ch_vertex* const vertices, /* vector of input + vertices; nVert x 1 */ + const int nVert, /* number of vertices */ + int* const faces, /* face indices; flat: nFaces x 3 */ + const int nFaces, /* number of faces in hull */ + char* const + m_filename); /* m filename, WITHOUT extension */ + +/* reads an 'obj' file and extracts only the vertices */ +void extractVerticesFromObjFile(/* input arguments */ + char* const + obj_filename, /* obj filename, WITHOUT + extension */ + /* output arguments */ + ch_vertex** + out_vertices, /* & of empty ch_vertex*, + output vertices; out_nVert + x 1 */ + int* out_nVert); /* & of int, number of vertices + */ + +#ifdef __cplusplus +} /*extern "C"*/ +#endif + +#endif /* CONVHULL_3D_INCLUDED */ + +/************ + * INTERNAL: + ***********/ + +#ifndef CONVHULL_3D_ENABLE +#define CONVHULL_3D_ENABLE + +#include +#include +#include +#include +#include +#include +#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) +# define CV_STRNCPY(a, b, c) strncpy_s(a, c + 1, b, c); +# define CV_STRCAT(a, b) strcat_s(a, sizeof(b), b); +#else +# define CV_STRNCPY(a, b, c) strncpy(a, b, c); +# define CV_STRCAT(a, b) strcat(a, b); +#endif +#ifdef CONVHULL_3D_USE_FLOAT_PRECISION +# define CH_FLT_MIN FLT_MIN +# define CH_FLT_MAX FLT_MAX +# define CH_NOISE_VAL 0.00001f +#else +# define CH_FLT_MIN DBL_MIN +# define CH_FLT_MAX DBL_MAX +# define CH_NOISE_VAL 0.0000001 +#endif +#ifndef MIN +# define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#endif +#ifndef MAX +# define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif +#define CH_MAX_NUM_FACES 50000 + +/* structs for qsort */ +typedef struct float_w_idx +{ + CH_FLOAT val; + int idx; +} float_w_idx; + +typedef struct int_w_idx +{ + int val; + int idx; +} int_w_idx; + +/* internal functions prototypes: */ +static int cmp_asc_float(const void*, const void*); +static int cmp_desc_float(const void*, const void*); +static int cmp_asc_int(const void*, const void*); +static int cmp_desc_int(const void*, const void*); +static void sort_float(CH_FLOAT*, CH_FLOAT*, int*, int, int); +static void sort_int(int*, int*, int*, int, int); +static ch_vec3 cross(ch_vec3*, ch_vec3*); +static CH_FLOAT det_4x4(CH_FLOAT*); +static void plane_3d(CH_FLOAT*, CH_FLOAT*, CH_FLOAT*); +static void ismember(int*, int*, int*, int, int); + +/* internal functions definitions: */ +static int cmp_asc_float(const void* a, const void* b) +{ + struct float_w_idx* a1 = (struct float_w_idx*)a; + struct float_w_idx* a2 = (struct float_w_idx*)b; + if ((*a1).val < (*a2).val) + return -1; + else if ((*a1).val > (*a2).val) + return 1; + else + return 0; +} + +static int cmp_desc_float(const void* a, const void* b) +{ + struct float_w_idx* a1 = (struct float_w_idx*)a; + struct float_w_idx* a2 = (struct float_w_idx*)b; + if ((*a1).val > (*a2).val) + return -1; + else if ((*a1).val < (*a2).val) + return 1; + else + return 0; +} + +static int cmp_asc_int(const void* a, const void* b) +{ + struct int_w_idx* a1 = (struct int_w_idx*)a; + struct int_w_idx* a2 = (struct int_w_idx*)b; + if ((*a1).val < (*a2).val) + return -1; + else if ((*a1).val > (*a2).val) + return 1; + else + return 0; +} + +static int cmp_desc_int(const void* a, const void* b) +{ + struct int_w_idx* a1 = (struct int_w_idx*)a; + struct int_w_idx* a2 = (struct int_w_idx*)b; + if ((*a1).val > (*a2).val) + return -1; + else if ((*a1).val < (*a2).val) + return 1; + else + return 0; +} + +static void sort_float( + CH_FLOAT* in_vec, /* vector[len] to be sorted */ + CH_FLOAT* out_vec, /* if NULL, then in_vec is sorted "in-place" */ + int* new_idices, /* set to NULL if you don't need them */ + int len, /* number of elements in vectors, must be consistent with the input + data */ + int descendFLAG /* !1:ascending, 1:descending */ +) +{ + int i; + struct float_w_idx* data; + + data = (float_w_idx*)ch_malloc(len * sizeof(float_w_idx)); + for (i = 0; i < len; i++) + { + data[i].val = in_vec[i]; + data[i].idx = i; + } + if (descendFLAG) + qsort(data, len, sizeof(data[0]), cmp_desc_float); + else + qsort(data, len, sizeof(data[0]), cmp_asc_float); + for (i = 0; i < len; i++) + { + if (out_vec != NULL) + out_vec[i] = data[i].val; + else + in_vec[i] = data[i].val; /* overwrite input vector */ + if (new_idices != NULL) + new_idices[i] = data[i].idx; + } + ch_free(data); +} + +static void sort_int( + int* in_vec, /* vector[len] to be sorted */ + int* out_vec, /* if NULL, then in_vec is sorted "in-place" */ + int* new_idices, /* set to NULL if you don't need them */ + int len, /* number of elements in vectors, must be consistent with the input + data */ + int descendFLAG /* !1:ascending, 1:descending */ +) +{ + int i; + struct int_w_idx* data; + + data = (int_w_idx*)ch_malloc(len * sizeof(int_w_idx)); + for (i = 0; i < len; i++) + { + data[i].val = in_vec[i]; + data[i].idx = i; + } + if (descendFLAG) + qsort(data, len, sizeof(data[0]), cmp_desc_int); + else + qsort(data, len, sizeof(data[0]), cmp_asc_int); + for (i = 0; i < len; i++) + { + if (out_vec != NULL) + out_vec[i] = data[i].val; + else + in_vec[i] = data[i].val; /* overwrite input vector */ + if (new_idices != NULL) + new_idices[i] = data[i].idx; + } + ch_free(data); +} + +static ch_vec3 cross(ch_vec3* v1, ch_vec3* v2) +{ + ch_vec3 cross; + cross.x = v1->y * v2->z - v1->z * v2->y; + cross.y = v1->z * v2->x - v1->x * v2->z; + cross.z = v1->x * v2->y - v1->y * v2->x; + return cross; +} + +/* calculates the determinent of a 4x4 matrix */ +static CH_FLOAT det_4x4(CH_FLOAT* m) +{ + return m[3] * m[6] * m[9] * m[12] - m[2] * m[7] * m[9] * m[12] + - m[3] * m[5] * m[10] * m[12] + m[1] * m[7] * m[10] * m[12] + + m[2] * m[5] * m[11] * m[12] - m[1] * m[6] * m[11] * m[12] + - m[3] * m[6] * m[8] * m[13] + m[2] * m[7] * m[8] * m[13] + + m[3] * m[4] * m[10] * m[13] - m[0] * m[7] * m[10] * m[13] + - m[2] * m[4] * m[11] * m[13] + m[0] * m[6] * m[11] * m[13] + + m[3] * m[5] * m[8] * m[14] - m[1] * m[7] * m[8] * m[14] + - m[3] * m[4] * m[9] * m[14] + m[0] * m[7] * m[9] * m[14] + + m[1] * m[4] * m[11] * m[14] - m[0] * m[5] * m[11] * m[14] + - m[2] * m[5] * m[8] * m[15] + m[1] * m[6] * m[8] * m[15] + + m[2] * m[4] * m[9] * m[15] - m[0] * m[6] * m[9] * m[15] + - m[1] * m[4] * m[10] * m[15] + m[0] * m[5] * m[10] * m[15]; +} + +/* Calculates the coefficients of the equation of a PLANE in 3D. + * Original Copyright (c) 2014, George Papazafeiropoulos + * Distributed under the BSD (2-clause) license + */ +static void plane_3d(CH_FLOAT* p, CH_FLOAT* c, CH_FLOAT* d) +{ + int i, j, k, l; + int r[3]; + CH_FLOAT sign, det, norm_c; + CH_FLOAT pdiff[2][3], pdiff_s[2][2]; + + for (i = 0; i < 2; i++) + for (j = 0; j < 3; j++) + pdiff[i][j] = p[(i + 1) * 3 + j] - p[i * 3 + j]; + memset(c, 0, 3 * sizeof(CH_FLOAT)); + sign = 1.0; + for (i = 0; i < 3; i++) + r[i] = i; + for (i = 0; i < 3; i++) + { + for (j = 0; j < 2; j++) + { + for (k = 0, l = 0; k < 3; k++) + { + if (r[k] != i) + { + pdiff_s[j][l] = pdiff[j][k]; + l++; + } + } + } + det = pdiff_s[0][0] * pdiff_s[1][1] - pdiff_s[1][0] * pdiff_s[0][1]; + c[i] = sign * det; + sign *= -1.0; + } + norm_c = (CH_FLOAT)0.0; + for (i = 0; i < 3; i++) + norm_c += (ch_pow(c[i], 2.0)); + norm_c = ch_sqrt(norm_c); + for (i = 0; i < 3; i++) + c[i] /= norm_c; + (*d) = (CH_FLOAT)0.0; + for (i = 0; i < 3; i++) + (*d) += -p[i] * c[i]; +} + +static void ismember( + int* pLeft, /* left vector; nLeftElements x 1 */ + int* pRight, /* right vector; nRightElements x 1 */ + int* pOut, /* 0, unless pRight elements are present in pLeft then 1; + nLeftElements x 1 */ + int nLeftElements, /* number of elements in pLeft */ + int nRightElements /* number of elements in pRight */ +) +{ + int i, j; + memset(pOut, 0, nLeftElements * sizeof(int)); + for (i = 0; i < nLeftElements; i++) + for (j = 0; j < nRightElements; j++) + if (pLeft[i] == pRight[j]) + pOut[i] = 1; +} + +/* A C version of the 3D quickhull matlab implementation from here: + * https://www.mathworks.com/matlabcentral/fileexchange/48509-computational-geometry-toolbox?focused=3851550&tab=example + * (*out_faces) is returned as NULL, if triangulation fails * + * Original Copyright (c) 2014, George Papazafeiropoulos + * Distributed under the BSD (2-clause) license + * Reference: "The Quickhull Algorithm for Convex Hull, C. Bradford Barber, + * David P. Dobkin and Hannu Huhdanpaa, Geometry Center Technical Report GCG53, + * July 30, 1993" + */ +inline void convhull_3d_build( + ch_vertex* const in_vertices, + const int nVert, + int** out_faces, + int* nOut_faces) +{ + int i, j, k, l, h; + int nFaces, p, d; + int *aVec, *faces; + CH_FLOAT dfi, v, max_p, min_p; + CH_FLOAT *points, *cf, *cfi, *df, *p_s, *span; + + if (nVert < 3 || in_vertices == NULL) + { + (*out_faces) = NULL; + (*nOut_faces) = 0; + return; + } + + /* 3 dimensions. The code should theoretically work for >=2 dimensions, but + * "plane_3d" and "det_4x4" are hardcoded for 3, + * so would need to be rewritten */ + d = 3; + span = (CH_FLOAT*)ch_malloc(d * sizeof(CH_FLOAT)); + for (j = 0; j < d; j++) + { + max_p = (CH_FLOAT)2.23e-13; + min_p = (CH_FLOAT)2.23e+13; + for (i = 0; i < nVert; i++) + { + max_p = MAX(max_p, in_vertices[i].v[j]); + min_p = MIN(min_p, in_vertices[i].v[j]); + } + span[j] = max_p - min_p; + } + points = (CH_FLOAT*)ch_malloc(nVert * (d + 1) * sizeof(CH_FLOAT)); + for (i = 0; i < nVert; i++) + { + for (j = 0; j < d; j++) + points[i * (d + 1) + j] + = in_vertices[i].v[j] + + CH_NOISE_VAL * rand() + / (float)RAND_MAX; /* noise mitigates duplicates */ + points[i * (d + 1) + d] = 1.0f; /* add a last column of ones. Used only for + determinant calculation */ + } + + /* The initial convex hull is a simplex with (d+1) facets, where d is the + * number of dimensions */ + nFaces = (d + 1); + faces = (int*)ch_calloc(nFaces * d, sizeof(int)); + aVec = (int*)ch_malloc(nFaces * sizeof(int)); + for (i = 0; i < nFaces; i++) + aVec[i] = i; + + /* Each column of cf contains the coefficients of a plane */ + cf = (CH_FLOAT*)ch_malloc(nFaces * d * sizeof(CH_FLOAT)); + cfi = (CH_FLOAT*)ch_malloc(d * sizeof(CH_FLOAT)); + df = (CH_FLOAT*)ch_malloc(nFaces * sizeof(CH_FLOAT)); + p_s = (CH_FLOAT*)ch_malloc(d * d * sizeof(CH_FLOAT)); + for (i = 0; i < nFaces; i++) + { + /* Set the indices of the points defining the face */ + for (j = 0, k = 0; j < (d + 1); j++) + { + if (aVec[j] != i) + { + faces[i * d + k] = aVec[j]; + k++; + } + } + + /* Calculate and store the plane coefficients of the face */ + for (j = 0; j < d; j++) + for (k = 0; k < d; k++) + p_s[j * d + k] = points[(faces[i * d + j]) * (d + 1) + k]; + + /* Calculate and store the plane coefficients of the face */ + plane_3d(p_s, cfi, &dfi); + for (j = 0; j < d; j++) + cf[i * d + j] = cfi[j]; + df[i] = dfi; + } + CH_FLOAT* A; + int *bVec, *fVec, *asfVec, *face_tmp; + + /* Check to make sure that faces are correctly oriented */ + bVec = (int*)ch_malloc(4 * sizeof(int)); + for (i = 0; i < d + 1; i++) + bVec[i] = i; + + /* A contains the coordinates of the points forming a simplex */ + A = (CH_FLOAT*)ch_calloc((d + 1) * (d + 1), sizeof(CH_FLOAT)); + face_tmp = (int*)ch_malloc((d + 1) * sizeof(int)); + fVec = (int*)ch_malloc((d + 1) * sizeof(int)); + asfVec = (int*)ch_malloc((d + 1) * sizeof(int)); + for (k = 0; k < (d + 1); k++) + { + /* Get the point that is not on the current face (point p) */ + for (i = 0; i < d; i++) + fVec[i] = faces[k * d + i]; + sort_int(fVec, NULL, NULL, d, 0); /* sort accending */ + p = k; + for (i = 0; i < d; i++) + for (j = 0; j < (d + 1); j++) + A[i * (d + 1) + j] = points[(faces[k * d + i]) * (d + 1) + j]; + for (; i < (d + 1); i++) + for (j = 0; j < (d + 1); j++) + A[i * (d + 1) + j] = points[p * (d + 1) + j]; + + /* det(A) determines the orientation of the face */ + v = det_4x4(A); + + /* Orient so that each point on the original simplex can't see the opposite + * face */ + if (v < 0) + { + /* Reverse the order of the last two vertices to change the volume */ + for (j = 0; j < d; j++) + face_tmp[j] = faces[k * d + j]; + for (j = 0, l = d - 2; j < d - 1; j++, l++) + faces[k * d + l] = face_tmp[d - j - 1]; + + /* Modify the plane coefficients of the properly oriented faces */ + for (j = 0; j < d; j++) + cf[k * d + j] = -cf[k * d + j]; + df[k] = -df[k]; + for (i = 0; i < d; i++) + for (j = 0; j < (d + 1); j++) + A[i * (d + 1) + j] = points[(faces[k * d + i]) * (d + 1) + j]; + for (; i < (d + 1); i++) + for (j = 0; j < (d + 1); j++) + A[i * (d + 1) + j] = points[p * (d + 1) + j]; + } + } + + /* Coordinates of the center of the point set */ + CH_FLOAT *meanp, *absdist, *reldist, *desReldist; + meanp = (CH_FLOAT*)ch_calloc(d, sizeof(CH_FLOAT)); + for (i = d + 1; i < nVert; i++) + for (j = 0; j < d; j++) + meanp[j] += points[i * (d + 1) + j]; + for (j = 0; j < d; j++) + meanp[j] = meanp[j] / (CH_FLOAT)(nVert - d - 1); + + /* Absolute distance of points from the center */ + absdist = (CH_FLOAT*)ch_malloc((nVert - d - 1) * d * sizeof(CH_FLOAT)); + for (i = d + 1, k = 0; i < nVert; i++, k++) + for (j = 0; j < d; j++) + absdist[k * d + j] = (points[i * (d + 1) + j] - meanp[j]) / span[j]; + + /* Relative distance of points from the center */ + reldist = (CH_FLOAT*)ch_calloc((nVert - d - 1), sizeof(CH_FLOAT)); + desReldist = (CH_FLOAT*)ch_malloc((nVert - d - 1) * sizeof(CH_FLOAT)); + for (i = 0; i < (nVert - d - 1); i++) + for (j = 0; j < d; j++) + reldist[i] += ch_pow(absdist[i * d + j], 2.0); + + /* Sort from maximum to minimum relative distance */ + int num_pleft, cnt; + int *ind, *pleft; + ind = (int*)ch_malloc((nVert - d - 1) * sizeof(int)); + pleft = (int*)ch_malloc((nVert - d - 1) * sizeof(int)); + sort_float(reldist, desReldist, ind, (nVert - d - 1), 1); + + /* Initialize the vector of points left. The points with the larger relative + distance from the center are scanned first. */ + num_pleft = (nVert - d - 1); + for (i = 0; i < num_pleft; i++) + pleft[i] = ind[i] + d + 1; + + /* Loop over all remaining points that are not deleted. Deletion of points + occurs every #iter2del# iterations of this while loop */ + memset(A, 0, (d + 1) * (d + 1) * sizeof(CH_FLOAT)); + + /* cnt is equal to the points having been selected without deletion of + nonvisible points (i.e. points inside the current convex hull) */ + cnt = 0; + + /* The main loop for the quickhull algorithm */ + CH_FLOAT detA; + CH_FLOAT *points_cf, *points_s; + int *visible_ind, *visible, *nonvisible_faces, *f0, *face_s, *u, *gVec, + *horizon, *hVec, *pp, *hVec_mem_face; + int num_visible_ind, num_nonvisible_faces, n_newfaces, count, vis; + int f0_sum, u_len, start, num_p, index, horizon_size1; + int FUCKED; + FUCKED = 0; + u = horizon = NULL; + nFaces = d + 1; + visible_ind = (int*)ch_malloc(nFaces * sizeof(int)); + points_cf = (CH_FLOAT*)ch_malloc(nFaces * sizeof(CH_FLOAT)); + points_s = (CH_FLOAT*)ch_malloc(d * sizeof(CH_FLOAT)); + face_s = (int*)ch_malloc(d * sizeof(int)); + gVec = (int*)ch_malloc(d * sizeof(int)); + while ((num_pleft > 0)) + { + /* i is the first point of the points left */ + i = pleft[0]; + + /* Delete the point selected */ + for (j = 0; j < num_pleft - 1; j++) + pleft[j] = pleft[j + 1]; + num_pleft--; + if (num_pleft == 0) + ch_free(pleft); + else + pleft = (int*)ch_realloc(pleft, num_pleft * sizeof(int)); + + /* Update point selection counter */ + cnt++; + + /* find visible faces */ + for (j = 0; j < d; j++) + points_s[j] = points[i * (d + 1) + j]; + points_cf = (CH_FLOAT*)ch_realloc(points_cf, nFaces * sizeof(CH_FLOAT)); + visible_ind = (int*)ch_realloc(visible_ind, nFaces * sizeof(int)); +#ifdef CONVHULL_3D_USE_CBLAS +# ifdef CONVHULL_3D_USE_FLOAT_PRECISION + cblas_sgemm( + CblasRowMajor, + CblasNoTrans, + CblasTrans, + 1, + nFaces, + d, + 1.0f, + points_s, + d, + cf, + d, + 0.0f, + points_cf, + nFaces); +# else + cblas_dgemm( + CblasRowMajor, + CblasNoTrans, + CblasTrans, + 1, + nFaces, + d, + 1.0, + points_s, + d, + cf, + d, + 0.0, + points_cf, + nFaces); +# endif +#else + for (j = 0; j < nFaces; j++) + { + points_cf[j] = 0; + for (k = 0; k < d; k++) + points_cf[j] += points_s[k] * cf[j * d + k]; + } +#endif + num_visible_ind = 0; + for (j = 0; j < nFaces; j++) + { + if (points_cf[j] + df[j] > 0.0) + { + num_visible_ind++; /* will sum to 0 if none are visible */ + visible_ind[j] = 1; + } + else + visible_ind[j] = 0; + } + num_nonvisible_faces = nFaces - num_visible_ind; + + /* proceed if there are any visible faces */ + if (num_visible_ind != 0) + { + /* Find visible face indices */ + visible = (int*)ch_malloc(num_visible_ind * sizeof(int)); + for (j = 0, k = 0; j < nFaces; j++) + { + if (visible_ind[j] == 1) + { + visible[k] = j; + k++; + } + } + + /* Find nonvisible faces */ + nonvisible_faces + = (int*)ch_malloc(num_nonvisible_faces * d * sizeof(int)); + f0 = (int*)ch_malloc(num_nonvisible_faces * d * sizeof(int)); + for (j = 0, k = 0; j < nFaces; j++) + { + if (visible_ind[j] == 0) + { + for (l = 0; l < d; l++) + nonvisible_faces[k * d + l] = faces[j * d + l]; + k++; + } + } + + /* Create horizon (count is the number of the edges of the horizon) */ + count = 0; + for (j = 0; j < num_visible_ind; j++) + { + /* visible face */ + vis = visible[j]; + for (k = 0; k < d; k++) + face_s[k] = faces[vis * d + k]; + sort_int(face_s, NULL, NULL, d, 0); + ismember(nonvisible_faces, face_s, f0, num_nonvisible_faces * d, d); + u_len = 0; + + /* u are the nonvisible faces connected to the face v, if any */ + for (k = 0; k < num_nonvisible_faces; k++) + { + f0_sum = 0; + for (l = 0; l < d; l++) + f0_sum += f0[k * d + l]; + if (f0_sum == d - 1) + { + u_len++; + if (u_len == 1) + u = (int*)ch_malloc(u_len * sizeof(int)); + else + u = (int*)ch_realloc(u, u_len * sizeof(int)); + u[u_len - 1] = k; + } + } + for (k = 0; k < u_len; k++) + { + /* The boundary between the visible face v and the k(th) nonvisible + * face connected to the face v forms part of the horizon */ + count++; + if (count == 1) + horizon = (int*)ch_malloc(count * (d - 1) * sizeof(int)); + else + horizon = (int*)ch_realloc(horizon, count * (d - 1) * sizeof(int)); + for (l = 0; l < d; l++) + gVec[l] = nonvisible_faces[u[k] * d + l]; + for (l = 0, h = 0; l < d; l++) + { + if (f0[u[k] * d + l]) + { + horizon[(count - 1) * (d - 1) + h] = gVec[l]; + h++; + } + } + } + if (u_len != 0) + ch_free(u); + } + horizon_size1 = count; + for (j = 0, l = 0; j < nFaces; j++) + { + if (!visible_ind[j]) + { + /* Delete visible faces */ + for (k = 0; k < d; k++) + faces[l * d + k] = faces[j * d + k]; + + /* Delete the corresponding plane coefficients of the faces */ + for (k = 0; k < d; k++) + cf[l * d + k] = cf[j * d + k]; + df[l] = df[j]; + l++; + } + } + + /* Update the number of faces */ + nFaces = nFaces - num_visible_ind; + faces = (int*)ch_realloc(faces, nFaces * d * sizeof(int)); + cf = (CH_FLOAT*)ch_realloc(cf, nFaces * d * sizeof(CH_FLOAT)); + df = (CH_FLOAT*)ch_realloc(df, nFaces * sizeof(CH_FLOAT)); + + /* start is the first row of the new faces */ + start = nFaces; + + /* Add faces connecting horizon to the new point */ + n_newfaces = horizon_size1; + for (j = 0; j < n_newfaces; j++) + { + nFaces++; + faces = (int*)ch_realloc(faces, nFaces * d * sizeof(int)); + cf = (CH_FLOAT*)ch_realloc(cf, nFaces * d * sizeof(CH_FLOAT)); + df = (CH_FLOAT*)ch_realloc(df, nFaces * sizeof(CH_FLOAT)); + for (k = 0; k < d - 1; k++) + faces[(nFaces - 1) * d + k] = horizon[j * (d - 1) + k]; + faces[(nFaces - 1) * d + (d - 1)] = i; + + /* Calculate and store appropriately the plane coefficients of the faces + */ + for (k = 0; k < d; k++) + for (l = 0; l < d; l++) + p_s[k * d + l] + = points[(faces[(nFaces - 1) * d + k]) * (d + 1) + l]; + plane_3d(p_s, cfi, &dfi); + for (k = 0; k < d; k++) + cf[(nFaces - 1) * d + k] = cfi[k]; + df[(nFaces - 1)] = dfi; + if (nFaces > CH_MAX_NUM_FACES) + { + FUCKED = 1; + nFaces = 0; + break; + } + } + + /* Orient each new face properly */ + hVec = (int*)ch_malloc(nFaces * sizeof(int)); + hVec_mem_face = (int*)ch_malloc(nFaces * sizeof(int)); + for (j = 0; j < nFaces; j++) + hVec[j] = j; + for (k = start; k < nFaces; k++) + { + for (j = 0; j < d; j++) + face_s[j] = faces[k * d + j]; + sort_int(face_s, NULL, NULL, d, 0); + ismember(hVec, face_s, hVec_mem_face, nFaces, d); + num_p = 0; + for (j = 0; j < nFaces; j++) + if (!hVec_mem_face[j]) + num_p++; + pp = (int*)ch_malloc(num_p * sizeof(int)); + for (j = 0, l = 0; j < nFaces; j++) + { + if (!hVec_mem_face[j]) + { + pp[l] = hVec[j]; + l++; + } + } + index = 0; + detA = 0.0; + + /* While new point is coplanar, choose another point */ + while (detA == 0.0) + { + for (j = 0; j < d; j++) + for (l = 0; l < d + 1; l++) + A[j * (d + 1) + l] = points[(faces[k * d + j]) * (d + 1) + l]; + for (; j < d + 1; j++) + for (l = 0; l < d + 1; l++) + A[j * (d + 1) + l] = points[pp[index] * (d + 1) + l]; + index++; + detA = det_4x4(A); + } + + /* Orient faces so that each point on the original simplex can't see the + * opposite face */ + if (detA < 0.0) + { + /* If orientation is improper, reverse the order to change the volume + * sign */ + for (j = 0; j < d; j++) + face_tmp[j] = faces[k * d + j]; + for (j = 0, l = d - 2; j < d - 1; j++, l++) + faces[k * d + l] = face_tmp[d - j - 1]; + + /* Modify the plane coefficients of the properly oriented faces */ + for (j = 0; j < d; j++) + cf[k * d + j] = -cf[k * d + j]; + df[k] = -df[k]; + for (l = 0; l < d; l++) + for (j = 0; j < d + 1; j++) + A[l * (d + 1) + j] = points[(faces[k * d + l]) * (d + 1) + j]; + for (; l < d + 1; l++) + for (j = 0; j < d + 1; j++) + A[l * (d + 1) + j] = points[pp[index] * (d + 1) + j]; + } + ch_free(pp); + } + ch_free(horizon); + ch_free(f0); + ch_free(nonvisible_faces); + ch_free(visible); + ch_free(hVec); + ch_free(hVec_mem_face); + } + if (FUCKED) + { + break; + } + } + + /* output */ + if (FUCKED) + { + (*out_faces) = NULL; + (*nOut_faces) = 0; + } + else + { + (*out_faces) = (int*)ch_malloc(nFaces * d * sizeof(int)); + memcpy((*out_faces), faces, nFaces * d * sizeof(int)); + (*nOut_faces) = nFaces; + } + + /* clean-up */ + ch_free(visible_ind); + ch_free(points_cf); + ch_free(points_s); + ch_free(face_s); + ch_free(gVec); + ch_free(meanp); + ch_free(absdist); + ch_free(reldist); + ch_free(desReldist); + ch_free(ind); + ch_free(span); + ch_free(points); + ch_free(faces); + ch_free(aVec); + ch_free(cf); + ch_free(cfi); + ch_free(df); + ch_free(p_s); + ch_free(face_tmp); + ch_free(fVec); + ch_free(asfVec); + ch_free(bVec); + ch_free(A); +} + +inline void convhull_3d_export_obj( + ch_vertex* const vertices, + const int nVert, + int* const faces, + const int nFaces, + const int keepOnlyUsedVerticesFLAG, + char* const obj_filename) +{ + int i, j; + char path[256] = "\0"; + CV_STRNCPY(path, obj_filename, strlen(obj_filename)); + FILE* obj_file; +#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) + CV_STRCAT(path, ".obj"); + fopen_s(&obj_file, path, "wt"); +#else + obj_file = fopen(strcat(path, ".obj"), "wt"); +#endif + fprintf(obj_file, "o\n"); + CH_FLOAT scale; + ch_vec3 v1, v2, normal; + + /* export vertices */ + if (keepOnlyUsedVerticesFLAG) + { + for (i = 0; i < nFaces; i++) + for (j = 0; j < 3; j++) + fprintf( + obj_file, + "v %f %f %f\n", + vertices[faces[i * 3 + j]].x, + vertices[faces[i * 3 + j]].y, + vertices[faces[i * 3 + j]].z); + } + else + { + for (i = 0; i < nVert; i++) + fprintf( + obj_file, + "v %f %f %f\n", + vertices[i].x, + vertices[i].y, + vertices[i].z); + } + + /* export the face normals */ + for (i = 0; i < nFaces; i++) + { + /* calculate cross product between v1-v0 and v2-v0 */ + v1 = vertices[faces[i * 3 + 1]]; + v2 = vertices[faces[i * 3 + 2]]; + v1.x -= vertices[faces[i * 3]].x; + v1.y -= vertices[faces[i * 3]].y; + v1.z -= vertices[faces[i * 3]].z; + v2.x -= vertices[faces[i * 3]].x; + v2.y -= vertices[faces[i * 3]].y; + v2.z -= vertices[faces[i * 3]].z; + normal = cross(&v1, &v2); + + /* normalise to unit length */ + scale = ((CH_FLOAT)1.0) + / (ch_sqrt( + ch_pow(normal.x, (CH_FLOAT)2.0) + + ch_pow(normal.y, (CH_FLOAT)2.0) + + ch_pow(normal.z, (CH_FLOAT)2.0)) + + (CH_FLOAT)2.23e-9); + normal.x *= scale; + normal.y *= scale; + normal.z *= scale; + fprintf(obj_file, "vn %f %f %f\n", normal.x, normal.y, normal.z); + } + + /* export the face indices */ + if (keepOnlyUsedVerticesFLAG) + { + for (i = 0; i < nFaces; i++) + { + /* vertices are in same order as the faces, and normals are in order */ + fprintf( + obj_file, + "f %u//%u %u//%u %u//%u\n", + i * 3 + 1, + i + 1, + i * 3 + 1 + 1, + i + 1, + i * 3 + 2 + 1, + i + 1); + } + } + else + { + /* just normals are in order */ + for (i = 0; i < nFaces; i++) + { + fprintf( + obj_file, + "f %u//%u %u//%u %u//%u\n", + faces[i * 3] + 1, + i + 1, + faces[i * 3 + 1] + 1, + i + 1, + faces[i * 3 + 2] + 1, + i + 1); + } + } + fclose(obj_file); +} + +inline void convhull_3d_export_m( + ch_vertex* const vertices, + const int nVert, + int* const faces, + const int nFaces, + char* const m_filename) +{ + int i; + char path[256] = {"\0"}; + memcpy(path, m_filename, strlen(m_filename)); + FILE* m_file; +#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) + CV_STRCAT(path, ".m"); + fopen_s(&m_file, path, "wt"); +#else + m_file = fopen(strcat(path, ".m"), "wt"); +#endif + + /* save face indices and vertices for verification in matlab: */ + fprintf(m_file, "vertices = [\n"); + for (i = 0; i < nVert; i++) + fprintf( + m_file, "%f, %f, %f;\n", vertices[i].x, vertices[i].y, vertices[i].z); + fprintf(m_file, "];\n\n\n"); + fprintf(m_file, "faces = [\n"); + for (i = 0; i < nFaces; i++) + { + fprintf( + m_file, + " %u, %u, %u;\n", + faces[3 * i + 0] + 1, + faces[3 * i + 1] + 1, + faces[3 * i + 2] + 1); + } + fprintf(m_file, "];\n\n\n"); + fclose(m_file); +} + +inline void extractVerticesFromObjFile( + char* const obj_filename, ch_vertex** out_vertices, int* out_nVert) +{ + FILE* obj_file; +#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) + CV_STRCAT(obj_filename, ".obj"); + fopen_s(&obj_file, obj_filename, "r"); +#else + obj_file = fopen(strcat(obj_filename, ".obj"), "r"); +#endif + + /* determine number of vertices */ + unsigned int nVert = 0; + char line[256]; + while (fgets(line, sizeof(line), obj_file)) + { + char* vexists = strstr(line, "v "); + if (vexists != NULL) + nVert++; + } + (*out_nVert) = nVert; + (*out_vertices) = (ch_vertex*)ch_malloc(nVert * sizeof(ch_vertex)); + + /* extract the vertices */ + rewind(obj_file); + int i = 0; + int vertID, prev_char_isDigit, current_char_isDigit; + char vert_char[256] = {0}; + while (fgets(line, sizeof(line), obj_file)) + { + char* vexists = strstr(line, "v "); + if (vexists != NULL) + { + prev_char_isDigit = 0; + vertID = -1; + for (size_t j = 0; j < strlen(line) - 1; j++) + { + if (isdigit(line[j]) || line[j] == '.' || line[j] == '-' + || line[j] == '+' || line[j] == 'E' || line[j] == 'e') + { + vert_char[strlen(vert_char)] = line[j]; + current_char_isDigit = 1; + } + else + current_char_isDigit = 0; + if ((prev_char_isDigit && !current_char_isDigit) + || j == strlen(line) - 2) + { + vertID++; + if (vertID > 4) + { + /* not a valid file */ + ch_free((*out_vertices)); + (*out_vertices) = NULL; + (*out_nVert) = 0; + return; + } + (*out_vertices)[i].v[vertID] = (CH_FLOAT)atof(vert_char); + memset(vert_char, 0, 256 * sizeof(char)); + } + prev_char_isDigit = current_char_isDigit; + } + i++; + } + } +} + +#endif /* CONVHULL_3D_ENABLE */ diff --git a/dart/gui/OpenGLRenderInterface.cpp b/dart/gui/OpenGLRenderInterface.cpp index 5160a3c90f54a..907a5810176db 100644 --- a/dart/gui/OpenGLRenderInterface.cpp +++ b/dart/gui/OpenGLRenderInterface.cpp @@ -45,6 +45,7 @@ #include "dart/dynamics/Skeleton.hpp" #include "dart/gui/LoadOpengl.hpp" #include "dart/gui/OpenGLRenderInterface.hpp" +#include "dart/math/Icosphere.hpp" // Code taken from glut/lib/glut_shapes.c static GLUquadricObj* quadObj; @@ -237,24 +238,46 @@ void OpenGLRenderInterface::drawMultiSphere( } glPopMatrix(); } +} - if (spheres.size() < 2u) - return; +void OpenGLRenderInterface::drawMultiSphereConvexHull( + const std::vector>& spheres, + std::size_t subdivisions) +{ + // Create meshes of sphere and combine them into a single mesh + auto mesh = math::TriMeshf(); + for (const auto& sphere : spheres) + { + const double& radius = sphere.first; + const Eigen::Vector3d& center = sphere.second; - // Draw all the possible open cylinders that connects a pair of spheres in the - // list. - // - // TODO(JS): This is a workaround. The correct solution would be drawing the - // convex hull for the spheres, but we don't have a function computing convex - // hull yet. - for (auto i = 0u; i < spheres.size() - 1u; ++i) + auto icosphere = math::Icospheref(radius, subdivisions); + icosphere.translate(center.cast()); + + mesh += icosphere; + } + + // Create a convex hull from the combined mesh + auto convexHull = mesh.generateConvexHull(); + convexHull->computeVertexNormals(); + const auto& meshVertices = convexHull->getVertices(); + const auto& meshNormals = convexHull->getVertexNormals(); + const auto& meshTriangles = convexHull->getTriangles(); + assert(meshVertices.size() == meshNormals.size()); + + // Draw the triangles of the convex hull + glBegin(GL_TRIANGLES); + for (const auto& triangle : meshTriangles) { - for (auto j = i + 1u; j < spheres.size(); ++j) + for (auto i = 0u; i < 3; ++i) { - drawOpenCylinderConnectingTwoSpheres( - this, spheres[i], spheres[j], slices, stacks); + const auto& normal = meshNormals[triangle[i]]; + const auto& vertex = meshVertices[triangle[i]]; + glNormal3fv(normal.data()); + glVertex3fv(vertex.data()); } } + glEnd(); } void OpenGLRenderInterface::drawEllipsoid(const Eigen::Vector3d& _diameters) diff --git a/dart/gui/OpenGLRenderInterface.hpp b/dart/gui/OpenGLRenderInterface.hpp index 2e3fa047734f1..d04b4f91784ba 100644 --- a/dart/gui/OpenGLRenderInterface.hpp +++ b/dart/gui/OpenGLRenderInterface.hpp @@ -101,6 +101,9 @@ class OpenGLRenderInterface : public RenderInterface const std::vector>& spheres, int slices = 16, int stacks = 16) override; + void drawMultiSphereConvexHull( + const std::vector>& spheres, + std::size_t subdivisions) override; void drawEllipsoid(const Eigen::Vector3d& _diameters) override; void drawCube(const Eigen::Vector3d& _size) override; void drawOpenCylinder( diff --git a/dart/gui/RenderInterface.cpp b/dart/gui/RenderInterface.cpp index 9ef524f8c1bca..f23b57fb7b7d0 100644 --- a/dart/gui/RenderInterface.cpp +++ b/dart/gui/RenderInterface.cpp @@ -120,6 +120,13 @@ void RenderInterface::drawMultiSphere( // Do nothing } +void RenderInterface::drawMultiSphereConvexHull( + const std::vector>& /*spheres*/, + std::size_t /*subdivisions*/) +{ + // Do nothing +} + void RenderInterface::drawEllipsoid(const Eigen::Vector3d& /*_size*/) { } diff --git a/dart/gui/RenderInterface.hpp b/dart/gui/RenderInterface.hpp index 0ab1b3f60db53..2e07ecf86708c 100644 --- a/dart/gui/RenderInterface.hpp +++ b/dart/gui/RenderInterface.hpp @@ -106,10 +106,14 @@ class RenderInterface virtual void scale(const Eigen::Vector3d& _scale); // glScale virtual void drawSphere(double radius, int slices = 16, int stacks = 16); + DART_DEPRECATED(6.11) virtual void drawMultiSphere( const std::vector>& spheres, int slices = 16, int stacks = 16); + virtual void drawMultiSphereConvexHull( + const std::vector>& spheres, + std::size_t subdivisions); virtual void drawEllipsoid(const Eigen::Vector3d& _size); virtual void drawCube(const Eigen::Vector3d& _size); virtual void drawOpenCylinder( diff --git a/dart/gui/glut/SimWindow.cpp b/dart/gui/glut/SimWindow.cpp index 09926356b4f11..6a7610bae80fe 100644 --- a/dart/gui/glut/SimWindow.cpp +++ b/dart/gui/glut/SimWindow.cpp @@ -484,7 +484,7 @@ void SimWindow::drawShape( { const auto* multiSphere = static_cast(shape); - mRI->drawMultiSphere(multiSphere->getSpheres()); + mRI->drawMultiSphereConvexHull(multiSphere->getSpheres(), 3u); } else if (shape->is()) { diff --git a/dart/gui/osg/render/MeshShapeNode.cpp b/dart/gui/osg/render/MeshShapeNode.cpp index 3988c4eb08bae..1fa73f3fa0985 100644 --- a/dart/gui/osg/render/MeshShapeNode.cpp +++ b/dart/gui/osg/render/MeshShapeNode.cpp @@ -191,12 +191,6 @@ void MeshShapeNode::refresh() extractData(false); } -std::ostream& operator<<(std::ostream& str, const aiColor4D& c) -{ - str << c[0] << "\t " << c[1] << "\t " << c[2] << "\t " << c[3]; - return str; -} - //============================================================================== bool checkSpecularSanity(const aiColor4D& c) { diff --git a/dart/gui/osg/render/MultiSphereShapeNode.cpp b/dart/gui/osg/render/MultiSphereShapeNode.cpp index c33a611f3a02e..0ad19d9a8b653 100644 --- a/dart/gui/osg/render/MultiSphereShapeNode.cpp +++ b/dart/gui/osg/render/MultiSphereShapeNode.cpp @@ -32,15 +32,15 @@ #include #include +#include #include #include -#include - -#include "dart/gui/osg/Utils.hpp" -#include "dart/gui/osg/render/MultiSphereShapeNode.hpp" #include "dart/dynamics/MultiSphereConvexHullShape.hpp" #include "dart/dynamics/SimpleFrame.hpp" +#include "dart/gui/osg/Utils.hpp" +#include "dart/gui/osg/render/MultiSphereShapeNode.hpp" +#include "dart/math/Icosphere.hpp" namespace dart { namespace gui { @@ -68,7 +68,7 @@ class MultiSphereShapeGeode : public ShapeNode, public ::osg::Geode }; //============================================================================== -class MultiSphereShapeDrawable : public ::osg::ShapeDrawable +class MultiSphereShapeDrawable : public ::osg::Geometry { public: MultiSphereShapeDrawable( @@ -84,6 +84,9 @@ class MultiSphereShapeDrawable : public ::osg::ShapeDrawable dart::dynamics::MultiSphereConvexHullShape* mMultiSphereShape; dart::dynamics::VisualAspect* mVisualAspect; MultiSphereShapeGeode* mParent; + ::osg::ref_ptr<::osg::Vec3Array> mVertices; + ::osg::ref_ptr<::osg::Vec3Array> mNormals; + ::osg::ref_ptr<::osg::Vec4Array> mColors; }; //============================================================================== @@ -179,7 +182,12 @@ MultiSphereShapeDrawable::MultiSphereShapeDrawable( dart::dynamics::MultiSphereConvexHullShape* shape, dart::dynamics::VisualAspect* visualAspect, MultiSphereShapeGeode* parent) - : mMultiSphereShape(shape), mVisualAspect(visualAspect), mParent(parent) + : mMultiSphereShape(shape), + mVisualAspect(visualAspect), + mParent(parent), + mVertices(new ::osg::Vec3Array), + mNormals(new ::osg::Vec3Array), + mColors(new ::osg::Vec4Array) { refresh(true); } @@ -196,27 +204,68 @@ void MultiSphereShapeDrawable::refresh(bool firstTime) dart::dynamics::Shape::DYNAMIC_PRIMITIVE) || firstTime) { - ::osg::ref_ptr<::osg::CompositeShape> osg_shape = nullptr; - osg_shape = new ::osg::CompositeShape(); - + const auto subdivisions = 3; // TODO(JS): Make this configurable const auto& spheres = mMultiSphereShape->getSpheres(); + auto numVertices + = spheres.size() * math::Icosphered::getNumVertices(subdivisions); + mVertices->resize(numVertices); + + // Create meshes of sphere and combine them into a single mesh + auto mesh = math::TriMeshd(); for (const auto& sphere : spheres) { - ::osg::ref_ptr<::osg::Sphere> osg_sphere = nullptr; - osg_sphere = new ::osg::Sphere( - ::osg::Vec3(sphere.second.x(), sphere.second.y(), sphere.second.z()), - sphere.first); - osg_shape->addChild(osg_sphere); + const double& radius = sphere.first; + const Eigen::Vector3d& center = sphere.second; + + auto icosphere = math::Icosphered(radius, subdivisions); + icosphere.translate(center); + + mesh += icosphere; + } + + // Create a convex hull from the combined mesh + auto convexHull = mesh.generateConvexHull(); + convexHull->computeVertexNormals(); + const auto& meshVertices = convexHull->getVertices(); + const auto& meshNormals = convexHull->getVertexNormals(); + const auto& meshTriangles = convexHull->getTriangles(); + assert(meshVertices.size() == meshNormals.size()); + + // Convert the convex hull to OSG data types + mVertices->resize(meshVertices.size()); + mNormals->resize(meshVertices.size()); + for (auto i = 0u; i < meshVertices.size(); ++i) + { + const auto& v = meshVertices[i]; + const auto& n = meshNormals[i]; + (*mVertices)[i] = ::osg::Vec3(v[0], v[1], v[2]); + (*mNormals)[i] = ::osg::Vec3(n[0], n[1], n[2]); + } + setVertexArray(mVertices); + setNormalArray(mNormals, ::osg::Array::BIND_PER_VERTEX); + + ::osg::ref_ptr<::osg::DrawElementsUInt> drawElements + = new ::osg::DrawElementsUInt(GL_TRIANGLES); + drawElements->resize(3 * meshTriangles.size()); + for (auto i = 0u; i < meshTriangles.size(); ++i) + { + const auto& triangle = meshTriangles[i]; + (*drawElements)[3 * i] = triangle[0]; + (*drawElements)[3 * i + 1] = triangle[1]; + (*drawElements)[3 * i + 2] = triangle[2]; } + addPrimitiveSet(drawElements); - setShape(osg_shape); dirtyDisplayList(); } if (mMultiSphereShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR) || firstTime) { - setColor(eigToOsgVec4d(mVisualAspect->getRGBA())); + (*mColors).resize(1); + (*mColors)[0] = eigToOsgVec4f(mVisualAspect->getRGBA()); + setColorArray(mColors); + setColorBinding(::osg::Geometry::BIND_OVERALL); } } diff --git a/dart/math/Geometry.hpp b/dart/math/Geometry.hpp index af2b582b93fab..1794f9d74810f 100644 --- a/dart/math/Geometry.hpp +++ b/dart/math/Geometry.hpp @@ -532,6 +532,22 @@ SupportPolygon computeConvexHull(const SupportPolygon& _points); SupportPolygon computeConvexHull( std::vector& _originalIndices, const SupportPolygon& _points); +/// Generates a 3D convex hull given vertices and indices. +/// +/// \tparam S: The scalar type of the vertices. +/// \tparam Index: The index type of the triangles. +/// \param[in] vertices: The given vertices to generate a convex hull from. +/// \param[in] optimize: (Optional) Whether to discard vertices that are not +/// referred to in the resulted convex hull. The resulted indices will be +/// updated accordingly. +/// \return A tuple of the vertices and indices of the resulted convex hull. +template +std::tuple< + std::vector>, + std::vector>> +computeConvexHull3D( + const std::vector>& vertices, bool optimize = true); + /// Compute the centroid of a polygon, assuming the polygon is a convex hull Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull); @@ -638,4 +654,6 @@ class BoundingBox } // namespace math } // namespace dart +#include "dart/math/detail/Geometry-impl.hpp" + #endif // DART_MATH_GEOMETRY_HPP_ diff --git a/dart/math/Icosphere.hpp b/dart/math/Icosphere.hpp new file mode 100644 index 0000000000000..50413722ecaa3 --- /dev/null +++ b/dart/math/Icosphere.hpp @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_MATH_ICOSPHERE_HPP_ +#define DART_MATH_ICOSPHERE_HPP_ + +#include +#include + +#include + +#include "dart/math/TriMesh.hpp" + +namespace dart { +namespace math { + +/// The class Icosphere represents an icosphere where the subdivision and radius +/// are configurable. +template +class Icosphere : public TriMesh +{ +public: + // Type aliases + using S = S_; + using Base = TriMesh; + using Index = typename Base::Index; + using Vector3 = typename Base::Vector3; + using Triangle = typename Base::Triangle; + using Vertices = std::vector; + using Normals = typename Base::Normals; + using Triangles = std::vector; + + /// Returns the number of vertices of icosphere given subdivisions. + static std::size_t getNumVertices(std::size_t subdivisions); + + /// Returns the number of edges of icosphere given subdivisions. + static std::size_t getNumEdges(std::size_t subdivisions); + + /// Returns the number of triangles of icosphere given subdivisions. + static std::size_t getNumTriangles(std::size_t subdivisions); + + /// Returns vertices and faces of icosahedron given radius. + static std::pair computeIcosahedron(S radius); + + /// Construct an icosphere given radius and subdivisions. + /// + /// \param[in] radius: The radius of the icosphere. + /// \param[in] subdivisions: The number of subdividing an icosahedron. Passing + /// 1 generates icosahedron without subdividing. + Icosphere(S radius, std::size_t subdivisions); + + /// Returns the radius of the icosphere. + S getRadius() const; + + /// Returns the number of subdivisions of the icosphere. + std::size_t getNumSubdivisions() const; + +private: + /// Internal function to build icosphere given radius and subdivisions. + void build(); + + /// Radius of icosphere. + S mRadius; + + /// Number of subdividing an icosahedron. + std::size_t mSubdivisions; +}; + +using Icospheref = Icosphere; +using Icosphered = Icosphere; + +} // namespace math +} // namespace dart + +#include "dart/math/detail/Icosphere-impl.hpp" + +#endif // DART_MATH_ICOSPHERE_HPP_ diff --git a/dart/math/Mesh.hpp b/dart/math/Mesh.hpp new file mode 100644 index 0000000000000..f5efca1bdb9a5 --- /dev/null +++ b/dart/math/Mesh.hpp @@ -0,0 +1,108 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_MATH_MESH_HPP_ +#define DART_MATH_MESH_HPP_ + +#include + +#include + +namespace dart { +namespace math { + +/// Base class for meshes. +template +class Mesh +{ +public: + // Type aliases + using S = S_; + using Index = std::size_t; + using Vector3 = Eigen::Matrix; + using Vertices = std::vector; + using Normals = std::vector; + using Indices = std::vector; + + /// Destructor. + virtual ~Mesh(); + + /// Returns true if the mesh contains vertices. + bool hasVertices() const; + + /// Returns true if the mesh contains vertex normals. + bool hasVertexNormals() const; + + /// Returns the vertices of the mesh. + const Vertices& getVertices() const; + + /// Returns the vertex normals of the mesh. + const Normals& getVertexNormals() const; + + /// Clears all the vertices and vertex normals. + virtual void clear(); + + /// Returns true if the mesh has no vertices. + bool isEmpty() const; + + /// Translates the mesh vertices by adding \c translation to the vertices. + void translate(const Vector3& translation); + + /// Addition operator. + Mesh operator+(const Mesh& other) const; + + /// Addition assignment operator. + Mesh& operator+=(const Mesh& other); + +protected: + /// Default constructor. + Mesh(); + + /// Normalizes the vertex normals. + void normalizeVertexNormals(); + + /// Vertices of the mesh. + Vertices mVertices; + + /// Vertex normals of the mesh. + Normals mVertexNormals; +}; + +using Meshf = Mesh; +using Meshd = Mesh; + +} // namespace math +} // namespace dart + +#include "dart/math/detail/Mesh-impl.hpp" + +#endif // DART_MATH_MESH_HPP_ diff --git a/dart/math/TriMesh.cpp b/dart/math/TriMesh.cpp new file mode 100644 index 0000000000000..479cda85158a8 --- /dev/null +++ b/dart/math/TriMesh.cpp @@ -0,0 +1,41 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include "dart/math/TriMesh.hpp" + +namespace dart { +namespace math { + +template class TriMesh; + +} // namespace math +} // namespace dart diff --git a/dart/math/TriMesh.hpp b/dart/math/TriMesh.hpp new file mode 100644 index 0000000000000..134544724bac6 --- /dev/null +++ b/dart/math/TriMesh.hpp @@ -0,0 +1,121 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_MATH_TRIMESH_HPP_ +#define DART_MATH_TRIMESH_HPP_ + +#include + +#include "dart/math/Mesh.hpp" + +namespace dart { +namespace math { + +/// This class represents triangle meshes. +template +class TriMesh : public Mesh +{ +public: + // Type aliases + using S = S_; + using Base = Mesh; + using Index = typename Base::Index; + using Vector3 = typename Base::Vector3; + using Triangle = Eigen::Matrix; + using Vertices = typename Base::Vertices; + using Normals = typename Base::Normals; + using Triangles = std::vector; + + /// Default constructor. + TriMesh(); + + /// Destructor + ~TriMesh() override = default; + + /// Sets vertices and triangles. + void setTriangles(const Vertices& vertices, const Triangles& triangles); + + /// Computes vertex normals. + void computeVertexNormals(); + + /// Returns true if the mesh contains triangles. + bool hasTriangles() const; + + /// Returns true if the mesh contains triangle normals. + bool hasTriangleNormals() const; + + /// Returns the triangles of the mesh. + const Triangles& getTriangles() const; + + /// Returns the triangle normals of the mesh. + const Normals& getTriangleNormals() const; + + /// Clears all the data in the trimesh. + void clear() override; + + /// Addition operator. + TriMesh operator+(const TriMesh& other) const; + + /// Addition assignment operator. + TriMesh& operator+=(const TriMesh& other); + + /// Generates a convex hull that encloses the trimesh. + /// + /// \param[in] optimize: (Optional) Whether to discard vertices that are not + /// used in the convex hull. + std::shared_ptr> generateConvexHull(bool optimize = true) const; + +protected: + /// Computes triangle normals. + void computeTriangleNormals(); + + /// Normalizes triangle normals. + void normalizeTriangleNormals(); + + /// Triangle indices of the mesh. + Triangles mTriangles; + + /// Triangle normals of the mesh. + Normals mTriangleNormals; +}; + +extern template class TriMesh; + +using TriMeshf = TriMesh; +using TriMeshd = TriMesh; + +} // namespace math +} // namespace dart + +#include "dart/math/detail/TriMesh-impl.hpp" + +#endif // DART_MATH_TRIMESH_HPP_ diff --git a/dart/math/detail/Geometry-impl.hpp b/dart/math/detail/Geometry-impl.hpp new file mode 100644 index 0000000000000..21c466e58845b --- /dev/null +++ b/dart/math/detail/Geometry-impl.hpp @@ -0,0 +1,131 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_MATH_DETAIL_GEOMETRY_IMPL_HPP_ +#define DART_MATH_DETAIL_GEOMETRY_IMPL_HPP_ + +#include "dart/math/Geometry.hpp" + +#include + +#include "dart/external/convhull_3d/convhull_3d.h" + +namespace dart { +namespace math { + +//============================================================================== +template +std::tuple< + std::vector>, + std::vector>> +discardUnusedVertices( + const std::vector>& vertices, + const std::vector>& triangles) +{ + auto newVertices = std::vector>(); + auto newTriangles = std::vector>(); + newTriangles.resize(triangles.size()); + auto indexMap = std::unordered_map(); + auto newIndex = 0; + + for (auto i = 0u; i < triangles.size(); ++i) + { + const auto& triangle = triangles[i]; + auto& newTriangle = newTriangles[i]; + + for (auto j = 0u; j < 3; ++j) + { + const auto result + = indexMap.insert(std::make_pair(triangle[j], newIndex)); + const bool& inserted = result.second; + if (inserted) + { + newVertices.push_back(vertices[triangle[j]]); + newIndex++; + } + + newTriangle[j] = indexMap[triangle[j]]; + } + } + + return std::make_tuple(newVertices, newTriangles); +} + +//============================================================================== +template +std::tuple< + std::vector>, + std::vector>> +computeConvexHull3D( + const std::vector>& inputVertices, bool optimize) +{ + ch_vertex* vertices = new ch_vertex[inputVertices.size()]; + + for (auto i = 0u; i < inputVertices.size(); ++i) + { + const Eigen::Matrix& inputV = inputVertices[i]; + ch_vertex& v = vertices[i]; + v.x = inputV[0]; + v.y = inputV[1]; + v.z = inputV[2]; + } + + int* faces = nullptr; + int numFaces = 0; + + convhull_3d_build(vertices, inputVertices.size(), &faces, &numFaces); + + std::vector> eigenFaces; + eigenFaces.reserve(numFaces); + + for (auto i = 0; i < numFaces; ++i) + { + const auto index1 = faces[3 * i]; + const auto index2 = faces[3 * i + 1]; + const auto index3 = faces[3 * i + 2]; + + eigenFaces.emplace_back(index1, index2, index3); + } + + free(faces); + delete[] vertices; + + if (optimize) + return discardUnusedVertices(inputVertices, eigenFaces); + else + return std::make_pair(inputVertices, eigenFaces); +} + +} // namespace math +} // namespace dart + +#endif // DART_MATH_DETAIL_GEOMETRY_IMPL_HPP_ diff --git a/dart/math/detail/Icosphere-impl.hpp b/dart/math/detail/Icosphere-impl.hpp new file mode 100644 index 0000000000000..69a25a4a7c9a2 --- /dev/null +++ b/dart/math/detail/Icosphere-impl.hpp @@ -0,0 +1,221 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include "dart/math/Icosphere.hpp" + +#include + +#include "dart/math/Constants.hpp" + +namespace dart { +namespace math { + +//============================================================================== +template +std::size_t Icosphere::getNumVertices(std::size_t subdivisions) +{ + std::size_t numVertices = 12; + for (auto i = 0u; i < subdivisions; ++i) + numVertices += getNumEdges(i); + return numVertices; +} + +//============================================================================== +template +std::size_t Icosphere::getNumEdges(std::size_t subdivisions) +{ + return getNumTriangles(subdivisions) / 2 * 3; +} + +//============================================================================== +template +std::size_t Icosphere::getNumTriangles(std::size_t subdivisions) +{ + return 20 * std::pow(4, subdivisions); +} + +//============================================================================== +template +std::pair::Vertices, typename Icosphere::Triangles> +Icosphere::computeIcosahedron(S radius) +{ + constexpr S phi = constants::phi(); + const S unitX = 1 / std::sqrt(1 + phi * phi); + const S unitZ = unitX * phi; + + const S x = radius * unitX; + const S z = radius * unitZ; + + std::vector vertices = {{{-x, 0, z}, + {x, 0, z}, + {-x, 0, -z}, + {x, 0, -z}, + {0, z, x}, + {0, z, -x}, + {0, -z, x}, + {0, -z, -x}, + {z, x, 0}, + {-z, x, 0}, + {z, -x, 0}, + {-z, -x, 0}}}; + + static std::vector triangles + = {{{0, 4, 1}, {0, 9, 4}, {9, 5, 4}, {4, 5, 8}, {4, 8, 1}, + {8, 10, 1}, {8, 3, 10}, {5, 3, 8}, {5, 2, 3}, {2, 7, 3}, + {7, 10, 3}, {7, 6, 10}, {7, 11, 6}, {11, 0, 6}, {0, 1, 6}, + {6, 1, 10}, {9, 0, 11}, {9, 11, 2}, {9, 2, 5}, {7, 2, 11}}}; + + return std::make_pair(vertices, triangles); +} + +//============================================================================== +template +Icosphere::Icosphere(S radius, std::size_t subdivisions) + : mRadius(radius), mSubdivisions(subdivisions) +{ + static_assert( + std::is_floating_point::value, + "Scalar must be a floating point type."); + assert(radius > 0); + + build(); +} + +//============================================================================== +template +S Icosphere::getRadius() const +{ + return mRadius; +} + +//============================================================================== +template +std::size_t Icosphere::getNumSubdivisions() const +{ + return mSubdivisions; +} + +//============================================================================== +template +void Icosphere::build() +{ + // Reference: https://schneide.blog/2016/07/15/generating-an-icosphere-in-c/ + + // Create icosahedron + std::tie(this->mVertices, this->mTriangles) = computeIcosahedron(mRadius); + + // Return if no need to subdivide + if (mSubdivisions == 0) + return; + + // Create index map that is used for subdivision + using IndexMap = std::map, std::size_t>; + IndexMap midVertexIndices; + + // Create a temporary array of faces that is used for subdivision + std::vector tmpFaces; + if (mSubdivisions % 2) + { + this->mTriangles.reserve(getNumTriangles(mSubdivisions - 1)); + tmpFaces.reserve(getNumTriangles(mSubdivisions)); + } + else + { + this->mTriangles.reserve(getNumTriangles(mSubdivisions)); + tmpFaces.reserve(getNumTriangles(mSubdivisions - 1)); + } + + // Create more intermediate variables that are used for subdivision + std::vector* currFaces = &(this->mTriangles); + std::vector* newFaces = &tmpFaces; + std::array mid; + + // Subdivide icosahedron/icosphere iteratively. The key is to not duplicate + // the newly created vertices and faces during each subdivision. + for (std::size_t i = 0; i < mSubdivisions; ++i) + { + // Clear the array of faces that will store the faces of the subdivided + // isosphere in this iteration. This is because the faces of the previous + // isosphere are not reused. + (*newFaces).clear(); + midVertexIndices.clear(); + + // Iterate each face of the previous icosphere and divide the face into + // four new faces. + for (std::size_t j = 0; j < (*currFaces).size(); ++j) + { + const auto& outter = (*currFaces)[j]; + + // Create vertices on the middle of edges if not already created. + for (std::size_t k = 0; k < 3; ++k) + { + auto indexA = outter[k]; + auto indexB = outter[(k + 1) % 3]; + + // Sort indices to guarantee that the key is unique for the same pairs + // of indices. + if (indexA > indexB) + std::swap(indexA, indexB); + + // Check whether the mid vertex given index pair is already created. + const auto result = midVertexIndices.insert( + {{indexA, indexB}, this->mVertices.size()}); + const auto& inserted = result.second; + if (inserted) + { + // Create a vertex on the middle of the edge where the length of the + // vertex is equal to the radius of the icosphere. + const auto& v1 = this->mVertices[indexA]; + const auto& v2 = this->mVertices[indexB]; + this->mVertices.emplace_back(mRadius * (v1 + v2).normalized()); + } + + mid[k] = result.first->second; + } + + // Add four new faces. + (*newFaces).emplace_back(Triangle(outter[0], mid[0], mid[2])); + (*newFaces).emplace_back(Triangle(mid[0], outter[1], mid[1])); + (*newFaces).emplace_back(Triangle(mid[0], mid[1], mid[2])); + (*newFaces).emplace_back(Triangle(mid[2], mid[1], outter[2])); + } + + // Swap the arrays of faces. + std::swap(currFaces, newFaces); + } + + // Assign faces if needed. + this->mTriangles = *currFaces; +} + +} // namespace math +} // namespace dart diff --git a/dart/math/detail/Mesh-impl.hpp b/dart/math/detail/Mesh-impl.hpp new file mode 100644 index 0000000000000..54d320560d594 --- /dev/null +++ b/dart/math/detail/Mesh-impl.hpp @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_MATH_DETAIL_MESH_IMPL_HPP_ +#define DART_MATH_DETAIL_MESH_IMPL_HPP_ + +#include "dart/math/Mesh.hpp" + +namespace dart { +namespace math { + +//============================================================================== +template +Mesh::~Mesh() +{ + // Do nothing +} + +//============================================================================== +template +bool Mesh::hasVertices() const +{ + return !mVertices.empty(); +} + +//============================================================================== +template +bool Mesh::hasVertexNormals() const +{ + return hasVertices() && mVertices.size() == mVertexNormals.size(); +} + +//============================================================================== +template +const typename Mesh::Vertices& Mesh::getVertices() const +{ + return this->mVertices; +} + +//============================================================================== +template +const typename Mesh::Normals& Mesh::getVertexNormals() const +{ + return this->mVertexNormals; +} + +//============================================================================== +template +void Mesh::clear() +{ + mVertices.clear(); + mVertexNormals.clear(); +} + +//============================================================================== +template +bool Mesh::isEmpty() const +{ + return !(this->hasVertices()); +} + +//============================================================================== +template +void Mesh::translate(const Vector3& translation) +{ + for (auto& vertex : mVertices) + { + vertex += translation; + } +} + +//============================================================================== +template +Mesh Mesh::operator+(const Mesh& other) const +{ + return (Mesh(*this) += other); +} + +//============================================================================== +template +Mesh& Mesh::operator+=(const Mesh& other) +{ + if (other.isEmpty()) + return *this; + + // Insert vertex normals if both meshes have normals. Otherwise, clean the + // vertex normals. + if ((isEmpty() || hasVertexNormals()) && other.hasVertexNormals()) + { + mVertexNormals.insert( + mVertexNormals.end(), + other.mVertexNormals.begin(), + other.mVertexNormals.end()); + } + else + { + mVertexNormals.clear(); + } + + // Insert vertices + mVertices.insert( + mVertices.end(), other.mVertices.begin(), other.mVertices.end()); + + return *this; +} + +//============================================================================== +template +Mesh::Mesh() +{ + // Do nothing +} + +//============================================================================== +template +void Mesh::normalizeVertexNormals() +{ + for (auto& normal : mVertexNormals) + { + normal.normalize(); + } +} + +} // namespace math +} // namespace dart + +#endif // DART_MATH_DETAIL_MESH_IMPL_HPP_ diff --git a/dart/math/detail/TriMesh-impl.hpp b/dart/math/detail/TriMesh-impl.hpp new file mode 100644 index 0000000000000..bf15bbf0853ab --- /dev/null +++ b/dart/math/detail/TriMesh-impl.hpp @@ -0,0 +1,210 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_MATH_DETAIL_TRIMESH_IMPL_HPP_ +#define DART_MATH_DETAIL_TRIMESH_IMPL_HPP_ + +#include "dart/math/TriMesh.hpp" + +#include + +#include "dart/math/Geometry.hpp" + +namespace dart { +namespace math { + +//============================================================================== +template +TriMesh::TriMesh() +{ + // Do nothing +} + +//============================================================================== +template +void TriMesh::setTriangles( + const Vertices& vertices, const Triangles& triangles) +{ + clear(); + + this->mVertices = vertices; + mTriangles = triangles; +} + +//============================================================================== +template +void TriMesh::computeVertexNormals() +{ + computeTriangleNormals(); + + this->mVertexNormals.clear(); + this->mVertexNormals.resize(this->mVertices.size(), Vector3::Zero()); + + for (auto i = 0u; i < mTriangles.size(); ++i) + { + auto& triangle = mTriangles[i]; + this->mVertexNormals[triangle[0]] += mTriangleNormals[i]; + this->mVertexNormals[triangle[1]] += mTriangleNormals[i]; + this->mVertexNormals[triangle[2]] += mTriangleNormals[i]; + } + + this->normalizeVertexNormals(); +} + +//============================================================================== +template +bool TriMesh::hasTriangles() const +{ + return !mTriangles.empty(); +} + +//============================================================================== +template +bool TriMesh::hasTriangleNormals() const +{ + return hasTriangles() && mTriangles.size() == mTriangleNormals.size(); +} + +//============================================================================== +template +const typename TriMesh::Triangles& TriMesh::getTriangles() const +{ + return mTriangles; +} + +//============================================================================== +template +const typename TriMesh::Normals& TriMesh::getTriangleNormals() const +{ + return mTriangleNormals; +} + +//============================================================================== +template +void TriMesh::clear() +{ + mTriangles.clear(); + mTriangleNormals.clear(); + Base::clear(); +} + +//============================================================================== +template +TriMesh TriMesh::operator+(const TriMesh& other) const +{ + return (TriMesh(*this) += other); +} + +//============================================================================== +template +TriMesh& TriMesh::operator+=(const TriMesh& other) +{ + if (other.isEmpty()) + return *this; + + const auto oldNumVertices = this->mVertices.size(); + const auto oldNumTriangles = mTriangles.size(); + + Base::operator+=(other); + + // Insert triangle normals if both meshes have normals. Otherwise, clean the + // triangle normals. + if ((!hasTriangles() || hasTriangleNormals()) && other.hasTriangleNormals()) + { + mTriangleNormals.insert( + mTriangleNormals.end(), + other.mTriangleNormals.begin(), + other.mTriangleNormals.end()); + } + else + { + mTriangleNormals.clear(); + } + + const Triangle offset = Triangle::Constant(oldNumVertices); + mTriangles.resize(mTriangles.size() + other.mTriangles.size()); + for (auto i = 0u; i < other.mTriangles.size(); ++i) + { + mTriangles[i + oldNumTriangles] = other.mTriangles[i] + offset; + } + + return *this; +} + +//============================================================================== +template +std::shared_ptr> TriMesh::generateConvexHull(bool optimize) const +{ + auto triangles = Triangles(); + auto vertices = Vertices(); + std::tie(vertices, triangles) + = computeConvexHull3D(this->mVertices, optimize); + + auto mesh = std::make_shared>(); + mesh->setTriangles(vertices, triangles); + + return mesh; +} + +//============================================================================== +template +void TriMesh::computeTriangleNormals() +{ + mTriangleNormals.resize(mTriangles.size()); + + for (auto i = 0u; i < mTriangles.size(); ++i) + { + auto& triangle = mTriangles[i]; + const Vector3 v01 + = this->mVertices[triangle[1]] - this->mVertices[triangle[0]]; + const Vector3 v02 + = this->mVertices[triangle[2]] - this->mVertices[triangle[0]]; + mTriangleNormals[i] = v01.cross(v02); + } + + normalizeTriangleNormals(); +} + +//============================================================================== +template +void TriMesh::normalizeTriangleNormals() +{ + for (auto& normal : mTriangleNormals) + { + normal.normalize(); + } +} + +} // namespace math +} // namespace dart + +#endif // DART_MATH_DETAIL_TRIMESH_IMPL_HPP_ diff --git a/data/skel/shapes.skel b/data/skel/shapes.skel index cb54f17e4a25b..91dca0d77ee98 100644 --- a/data/skel/shapes.skel +++ b/data/skel/shapes.skel @@ -304,6 +304,10 @@ 0.075 0.075 0.0 0.0 + + 0.1 + 0 0.1 0 + 0.6 0.6 0.8 @@ -320,6 +324,10 @@ 0.075 0.075 0.0 0.0 + + 0.1 + 0 0.1 0 + diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 74050169d30db..f28eecf3a587b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -20,6 +20,7 @@ add_subdirectory(operational_space_control) if(HAVE_OCTOMAP) add_subdirectory(point_cloud) endif() +add_subdirectory(rigid_shapes) add_subdirectory(soft_bodies) add_subdirectory(tinkertoy) add_subdirectory(wam_ikfast) diff --git a/examples/rigid_shapes/CMakeLists.txt b/examples/rigid_shapes/CMakeLists.txt new file mode 100644 index 0000000000000..9bdca55b96845 --- /dev/null +++ b/examples/rigid_shapes/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5.1) + +get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME) + +project(${example_name}) + +set(required_components collision-bullet utils-urdf gui-osg) +set(required_libraries dart dart-collision-bullet dart-utils-urdf dart-gui-osg) + +if(DART_IN_SOURCE_BUILD) + dart_build_example_in_source(${example_name} LINK_LIBRARIES ${required_libraries}) + return() +endif() + +find_package(DART 6.6.0 REQUIRED COMPONENTS ${required_components} CONFIG) + +file(GLOB srcs "*.cpp" "*.hpp") +add_executable(${example_name} ${srcs}) +target_link_libraries(${example_name} PUBLIC ${required_libraries}) diff --git a/examples/rigid_shapes/README.md b/examples/rigid_shapes/README.md new file mode 100644 index 0000000000000..965b52cdf07b2 --- /dev/null +++ b/examples/rigid_shapes/README.md @@ -0,0 +1,20 @@ +This project is dependent on DART. Please make sure a proper version of DART is +installed before building this project. + +## Build Instructions + +From this directory: + + $ mkdir build + $ cd build + $ cmake .. + $ make + +## Execute Instructions + +Launch the executable from the build directory above: + + $ ./{generated_executable} + +Follow the instructions detailed in the console. + diff --git a/examples/rigid_shapes/main.cpp b/examples/rigid_shapes/main.cpp new file mode 100644 index 0000000000000..44ddd467b1b05 --- /dev/null +++ b/examples/rigid_shapes/main.cpp @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include +#include +#include +#include + +using namespace dart; + +int main() +{ + // Create world by reading a skel file + auto world = utils::SkelParser::readWorld("dart://sample/skel/shapes.skel"); + assert(world != NULL); + + // Use bullet collision detector for capsule and multi-sphere-convex-hull + // shapes + world->getConstraintSolver()->setCollisionDetector( + collision::BulletCollisionDetector::create()); + + // Wrap a WorldNode around it + ::osg::ref_ptr node + = new gui::osg::RealTimeWorldNode(world); + + // Create a Viewer and set it up with the WorldNode + auto viewer = gui::osg::Viewer(); + viewer.addWorldNode(node); + + viewer.addInstructionText("Press space to start free falling the box.\n"); + std::cout << viewer.getInstructions() << std::endl; + + // Set up the window to be 640x480 + viewer.setUpViewInWindow(0, 0, 640, 480); + + // Adjust the viewpoint of the Viewer + viewer.getCameraManipulator()->setHomePosition( + ::osg::Vec3(2.57f, 1.64f, 3.14f), + ::osg::Vec3(0.00f, 0.00f, 0.00f), + ::osg::Vec3(0, 1, 0)); + + // We need to re-dirty the CameraManipulator by passing it into the viewer + // again, so that the viewer knows to update its HomePosition setting + viewer.setCameraManipulator(viewer.getCameraManipulator()); + + // Begin running the application loop + viewer.run(); + + return 0; +} diff --git a/unittests/unit/CMakeLists.txt b/unittests/unit/CMakeLists.txt index 674f73b653ab8..f80b65e573096 100644 --- a/unittests/unit/CMakeLists.txt +++ b/unittests/unit/CMakeLists.txt @@ -11,6 +11,7 @@ dart_add_test("unit" test_ContactConstraint) dart_add_test("unit" test_Factory) dart_add_test("unit" test_GenericJoints) dart_add_test("unit" test_Geometry) +dart_add_test("unit" test_Icosphere) dart_add_test("unit" test_Inertia) dart_add_test("unit" test_Lemke) dart_add_test("unit" test_LocalResourceRetriever) @@ -20,6 +21,7 @@ dart_add_test("unit" test_Random) dart_add_test("unit" test_ScrewJoint) dart_add_test("unit" test_Signal) dart_add_test("unit" test_Subscriptions) +dart_add_test("unit" test_TriMesh) dart_add_test("unit" test_Uri) if(TARGET dart-optimizer-ipopt) diff --git a/unittests/unit/test_Icosphere.cpp b/unittests/unit/test_Icosphere.cpp new file mode 100644 index 0000000000000..c934f176edfa8 --- /dev/null +++ b/unittests/unit/test_Icosphere.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include +#include "dart/math/Icosphere.hpp" +#include "TestHelpers.hpp" + +using namespace dart; +using namespace math; + +//============================================================================== +TEST(IcosphereTests, NumOfVerticesAndTriangles) +{ + const double radius = 5.0; + + for (auto i = 0; i < 8; ++i) + { + const auto subdivisions = i; + const auto icosphere = Icosphered(radius, subdivisions); + const auto& vertices = icosphere.getVertices(); + const auto& triangles = icosphere.getTriangles(); + + EXPECT_EQ(vertices.size(), Icosphered::getNumVertices(subdivisions)); + EXPECT_EQ(triangles.size(), Icosphered::getNumTriangles(subdivisions)); + + for (const auto& v : vertices) + { + EXPECT_DOUBLE_EQ(v.norm(), radius); + } + } +} + +//============================================================================== +TEST(IcosphereTests, Constructor) +{ + auto s1 = Icosphered(1, 0); + EXPECT_FALSE(s1.isEmpty()); + EXPECT_DOUBLE_EQ(s1.getRadius(), 1); + EXPECT_EQ(s1.getNumSubdivisions(), 0); + + auto s2 = Icosphered(2, 3); + EXPECT_FALSE(s2.isEmpty()); + EXPECT_DOUBLE_EQ(s2.getRadius(), 2); + EXPECT_EQ(s2.getNumSubdivisions(), 3); +} diff --git a/unittests/unit/test_TriMesh.cpp b/unittests/unit/test_TriMesh.cpp new file mode 100644 index 0000000000000..e9033a9d30934 --- /dev/null +++ b/unittests/unit/test_TriMesh.cpp @@ -0,0 +1,148 @@ +/* + * Copyright (c) 2011-2021, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include +#include "dart/math/TriMesh.hpp" +#include "TestHelpers.hpp" + +using namespace dart; +using namespace math; + +//============================================================================== +TEST(TriMeshTests, DefaultConstructor) +{ + auto mesh = TriMeshd(); + EXPECT_FALSE(mesh.hasTriangles()); + EXPECT_FALSE(mesh.hasTriangleNormals()); + EXPECT_TRUE(mesh.isEmpty()); +} + +//============================================================================== +TEST(TriMeshTests, SetTriangles) +{ + auto mesh = TriMeshd(); + EXPECT_TRUE(mesh.isEmpty()); + + auto vertices = TriMeshd::Vertices(); + vertices.emplace_back(0, 0, 0); + vertices.emplace_back(1, 0, 0); + vertices.emplace_back(0, 1, 0); + auto triangles = TriMeshd::Triangles(); + triangles.emplace_back(0, 1, 2); + + mesh.setTriangles(vertices, triangles); + EXPECT_TRUE(mesh.hasTriangles()); + EXPECT_FALSE(mesh.hasTriangleNormals()); + EXPECT_FALSE(mesh.isEmpty()); + EXPECT_EQ(mesh.getVertices(), vertices); + EXPECT_EQ(mesh.getTriangles(), triangles); + + mesh.computeVertexNormals(); + EXPECT_TRUE(mesh.hasTriangles()); + EXPECT_TRUE(mesh.hasTriangleNormals()); + EXPECT_FALSE(mesh.isEmpty()); + + mesh.clear(); + EXPECT_FALSE(mesh.hasTriangles()); + EXPECT_FALSE(mesh.hasTriangleNormals()); + EXPECT_TRUE(mesh.isEmpty()); +} + +//============================================================================== +TEST(TriMeshTests, Operators) +{ + auto mesh1 = TriMeshd(); + auto mesh2 = TriMeshd(); + + auto vertices = TriMeshd::Vertices(); + vertices.emplace_back(0, 0, 0); + vertices.emplace_back(1, 0, 0); + vertices.emplace_back(0, 1, 0); + auto triangles = TriMeshd::Triangles(); + triangles.emplace_back(0, 1, 2); + + mesh1.setTriangles(vertices, triangles); + EXPECT_EQ(mesh1.getVertices().size(), 3); + EXPECT_EQ(mesh1.getTriangles().size(), 1); + mesh2.setTriangles(vertices, triangles); + EXPECT_EQ(mesh2.getVertices().size(), 3); + EXPECT_EQ(mesh2.getTriangles().size(), 1); + + auto mesh3 = mesh1 + mesh2; + EXPECT_EQ(mesh3.getVertices().size(), 6); + EXPECT_EQ(mesh3.getTriangles().size(), 2); + EXPECT_FALSE(mesh3.hasTriangleNormals()); + EXPECT_FALSE(mesh3.hasVertexNormals()); + + mesh1.computeVertexNormals(); + EXPECT_TRUE(mesh1.hasTriangleNormals()); + EXPECT_TRUE(mesh1.hasVertexNormals()); + EXPECT_FALSE(mesh2.hasTriangleNormals()); + EXPECT_FALSE(mesh2.hasVertexNormals()); + EXPECT_FALSE((mesh1 + mesh2).hasTriangleNormals()); + EXPECT_FALSE((mesh1 + mesh2).hasVertexNormals()); + + mesh2.computeVertexNormals(); + EXPECT_TRUE(mesh1.hasTriangleNormals()); + EXPECT_TRUE(mesh1.hasVertexNormals()); + EXPECT_TRUE(mesh2.hasTriangleNormals()); + EXPECT_TRUE(mesh2.hasVertexNormals()); + EXPECT_TRUE((mesh1 + mesh2).hasTriangleNormals()); + EXPECT_TRUE((mesh1 + mesh2).hasVertexNormals()); + + mesh1 += mesh2; + EXPECT_EQ(mesh1.getVertices().size(), 6); + EXPECT_EQ(mesh1.getTriangles().size(), 2); +} + +//============================================================================== +TEST(TriMeshTests, GenerateConvexHull) +{ + auto mesh = TriMeshd(); + EXPECT_TRUE(mesh.isEmpty()); + + auto emptyConvexHull = mesh.generateConvexHull(); + ASSERT_NE(emptyConvexHull, nullptr); + EXPECT_TRUE(emptyConvexHull->isEmpty()); + + auto vertices = TriMeshd::Vertices(); + vertices.emplace_back(0, 0, 0); + vertices.emplace_back(1, 0, 0); + vertices.emplace_back(0, 1, 0); + vertices.emplace_back(0, 0, 1); + mesh.setTriangles(vertices, {}); + + auto convexHull = mesh.generateConvexHull(); + ASSERT_NE(convexHull, nullptr); + EXPECT_EQ(convexHull->getVertices().size(), vertices.size()); + EXPECT_EQ(convexHull->getTriangles().size(), 4); +}