{"action":"created","comment":{"url":"https://api.github.com/repos/dartsim/dart/pulls/comments/58490566","id":58490566,"diff_hunk":"@@ -37,231 +37,1344 @@\n \n #include \"dart/collision/fcl/FCLCollisionDetector.h\"\n \n-#include <vector>\n+#include <assimp/scene.h>\n \n-#include \"dart/dynamics/Shape.h\"\n-#include \"dart/dynamics/BodyNode.h\"\n-#include \"dart/dynamics/Skeleton.h\"\n-#include \"dart/collision/fcl/FCLCollisionNode.h\"\n-#include \"dart/collision/fcl/FCLTypes.h\"\n+#include <fcl/collision.h>\n+#include <fcl/collision_object.h>\n+#include <fcl/collision_data.h>\n+#include <fcl/BVH/BVH_model.h>\n+#include <fcl/broadphase/broadphase.h>\n+#include <fcl/shape/geometric_shapes.h>\n+#include <fcl/shape/geometric_shape_to_BVH_model.h>\n \n-#define FCL_VERSION_AT_LEAST(x,y,z) \\\n-  (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \\\n-  (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \\\n-  FCL_PATCH_VERSION >= z))))\n+#include \"dart/common/Console.h\"\n+#include \"dart/collision/CollisionObject.h\"\n+#include \"dart/collision/CollisionFilter.h\"\n+#include \"dart/collision/fcl/FCLTypes.h\"\n+#include \"dart/collision/fcl/FCLCollisionObject.h\"\n+#include \"dart/collision/fcl/FCLCollisionGroup.h\"\n+#include \"dart/collision/fcl/tri_tri_intersection_test.h\"\n+#include \"dart/dynamics/ShapeFrame.h\"\n+#include \"dart/dynamics/Shape.h\"\n+#include \"dart/dynamics/BoxShape.h\"\n+#include \"dart/dynamics/EllipsoidShape.h\"\n+#include \"dart/dynamics/CylinderShape.h\"\n+#include \"dart/dynamics/PlaneShape.h\"\n+#include \"dart/dynamics/MeshShape.h\"\n+#include \"dart/dynamics/SoftMeshShape.h\"\n \n namespace dart {\n namespace collision {\n \n-//==============================================================================\n-// Collision data stores the collision request and the result given by\n-// collision algorithm.\n-struct CollisionData\n+namespace {\n+\n+bool collisionCallback(fcl::CollisionObject* o1,\n+                       fcl::CollisionObject* o2,\n+                       void* cdata);\n+\n+void postProcessFCL(const fcl::CollisionResult& fclResult,\n+                    fcl::CollisionObject* o1,\n+                    fcl::CollisionObject* o2,\n+                    const Option& option,\n+                    Result& result);\n+\n+void postProcessDART(const fcl::CollisionResult& fclResult,\n+                     fcl::CollisionObject* o1,\n+                     fcl::CollisionObject* o2,\n+                     const Option& option,\n+                     Result& result);\n+\n+int evalContactPosition(const fcl::Contact& fclContact,\n+    const fcl::BVHModel<fcl::OBBRSS>* mesh1,\n+    const fcl::BVHModel<fcl::OBBRSS>* mesh2,\n+    const fcl::Transform3f& transform1,\n+    const fcl::Transform3f& transform2,\n+    Eigen::Vector3d* contactPosition);\n+\n+bool isColinear(const Eigen::Vector3d& pos1,\n+                const Eigen::Vector3d& pos2,\n+                const Eigen::Vector3d& pos3,\n+                double tol);\n+\n+int FFtest(\n+    const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3,\n+    const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3,\n+    fcl::Vec3f* res1, fcl::Vec3f* res2);\n+\n+double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3);\n+\n+void convertOption(const Option& fclOption, fcl::CollisionRequest& request);\n+\n+Contact convertContact(const fcl::Contact& fclContact,\n+                       fcl::CollisionObject* o1,\n+                       fcl::CollisionObject* o2);\n+\n+/// Collision data stores the collision request and the result given by\n+/// collision algorithm.\n+struct FCLCollisionCallbackData\n {\n-  CollisionData() { done = false; }\n+  /// FCL collision request\n+  fcl::CollisionRequest mFclRequest;\n+\n+  /// FCL collision result\n+  fcl::CollisionResult mFclResult;\n+\n+  /// Collision option of DART\n+  const Option& mOption;\n \n-  // Collision request\n-  fcl::CollisionRequest request;\n+  /// Collision result of DART\n+  Result* mResult;\n \n-  // Collision result\n-  fcl::CollisionResult result;\n+  FCLCollisionDetector::PrimitiveShape mPrimitiveShapeType;\n \n-  // FCL collision detector\n-  FCLCollisionDetector* collisionDetector;\n+  FCLCollisionDetector::ContactPointComputationMethod\n+  mContactPointComputationMethod;\n \n-  // Whether the collision iteration can stop\n+  /// Whether the collision iteration can stop\n   bool done;\n+\n+  /// Constructor\n+  FCLCollisionCallbackData(\n+      const Option& option,\n+      Result* result = nullptr,\n+      FCLCollisionDetector::PrimitiveShape type\n+          = FCLCollisionDetector::MESH,\n+      FCLCollisionDetector::ContactPointComputationMethod method\n+          = FCLCollisionDetector::DART)\n+    : mOption(option),\n+      mResult(result),\n+      mPrimitiveShapeType(type),\n+      mContactPointComputationMethod(method),\n+      done(false)\n+  {\n+    convertOption(mOption, mFclRequest);\n+\n+    mFclRequest.num_max_contacts = std::max(static_cast<size_t>(100u),\n+                                            mOption.maxNumContacts);\n+    // Since some contact points can be filtered out in the post process, we ask\n+    // more than the demend. 100 is randomly picked.\n+  }\n };\n \n //==============================================================================\n-bool collisionCallBack(fcl::CollisionObject* _o1,\n-                       fcl::CollisionObject* _o2,\n-                       void* _cdata)\n+// Create a cube mesh for collision detection\n+template<class BV>\n+fcl::BVHModel<BV>* createCube(float _sizeX, float _sizeY, float _sizeZ)\n+{\n+  int faces[6][4] =\n+  {\n+    {0, 1, 2, 3},\n+    {3, 2, 6, 7},\n+    {7, 6, 5, 4},\n+    {4, 5, 1, 0},\n+    {5, 6, 2, 1},\n+    {7, 4, 0, 3}\n+  };\n+  float v[8][3];\n+\n+  v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2;\n+  v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2;\n+  v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2;\n+  v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2;\n+  v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2;\n+  v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2;\n+\n+  fcl::BVHModel<BV>* model = new fcl::BVHModel<BV>;\n+  fcl::Vec3f p1, p2, p3;\n+  model->beginModel();\n+\n+  for (int i = 0; i < 6; i++)\n+  {\n+    p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);\n+    p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]);\n+    p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);\n+    model->addTriangle(p1, p2, p3);\n+\n+    p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);\n+    p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);\n+    p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]);\n+    model->addTriangle(p1, p2, p3);\n+  }\n+  model->endModel();\n+  return model;\n+}\n+\n+//==============================================================================\n+template<class BV>\n+fcl::BVHModel<BV>* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ)\n {\n-  CollisionData* cdata = static_cast<CollisionData*>(_cdata);\n-  const fcl::CollisionRequest& request = cdata->request;\n-  fcl::CollisionResult& result = cdata->result;\n-  FCLCollisionDetector* cd = cdata->collisionDetector;\n+  float v[59][3] =\n+  {\n+    {0, 0, 0},\n+    {0.135299, -0.461940, -0.135299},\n+    {0.000000, -0.461940, -0.191342},\n+    {-0.135299, -0.461940, -0.135299},\n+    {-0.191342, -0.461940, 0.000000},\n+    {-0.135299, -0.461940, 0.135299},\n+    {0.000000, -0.461940, 0.191342},\n+    {0.135299, -0.461940, 0.135299},\n+    {0.191342, -0.461940, 0.000000},\n+    {0.250000, -0.353553, -0.250000},\n+    {0.000000, -0.353553, -0.353553},\n+    {-0.250000, -0.353553, -0.250000},\n+    {-0.353553, -0.353553, 0.000000},\n+    {-0.250000, -0.353553, 0.250000},\n+    {0.000000, -0.353553, 0.353553},\n+    {0.250000, -0.353553, 0.250000},\n+    {0.353553, -0.353553, 0.000000},\n+    {0.326641, -0.191342, -0.326641},\n+    {0.000000, -0.191342, -0.461940},\n+    {-0.326641, -0.191342, -0.326641},\n+    {-0.461940, -0.191342, 0.000000},\n+    {-0.326641, -0.191342, 0.326641},\n+    {0.000000, -0.191342, 0.461940},\n+    {0.326641, -0.191342, 0.326641},\n+    {0.461940, -0.191342, 0.000000},\n+    {0.353553, 0.000000, -0.353553},\n+    {0.000000, 0.000000, -0.500000},\n+    {-0.353553, 0.000000, -0.353553},\n+    {-0.500000, 0.000000, 0.000000},\n+    {-0.353553, 0.000000, 0.353553},\n+    {0.000000, 0.000000, 0.500000},\n+    {0.353553, 0.000000, 0.353553},\n+    {0.500000, 0.000000, 0.000000},\n+    {0.326641, 0.191342, -0.326641},\n+    {0.000000, 0.191342, -0.461940},\n+    {-0.326641, 0.191342, -0.326641},\n+    {-0.461940, 0.191342, 0.000000},\n+    {-0.326641, 0.191342, 0.326641},\n+    {0.000000, 0.191342, 0.461940},\n+    {0.326641, 0.191342, 0.326641},\n+    {0.461940, 0.191342, 0.000000},\n+    {0.250000, 0.353553, -0.250000},\n+    {0.000000, 0.353553, -0.353553},\n+    {-0.250000, 0.353553, -0.250000},\n+    {-0.353553, 0.353553, 0.000000},\n+    {-0.250000, 0.353553, 0.250000},\n+    {0.000000, 0.353553, 0.353553},\n+    {0.250000, 0.353553, 0.250000},\n+    {0.353553, 0.353553, 0.000000},\n+    {0.135299, 0.461940, -0.135299},\n+    {0.000000, 0.461940, -0.191342},\n+    {-0.135299, 0.461940, -0.135299},\n+    {-0.191342, 0.461940, 0.000000},\n+    {-0.135299, 0.461940, 0.135299},\n+    {0.000000, 0.461940, 0.191342},\n+    {0.135299, 0.461940, 0.135299},\n+    {0.191342, 0.461940, 0.000000},\n+    {0.000000, -0.500000, 0.000000},\n+    {0.000000, 0.500000, 0.000000}\n+  };\n \n-  if(cdata->done)\n-    return true;\n+  int f[112][3] =\n+  {\n+    {1, 2, 9},\n+    {9, 2, 10},\n+    {2, 3, 10},\n+    {10, 3, 11},\n+    {3, 4, 11},\n+    {11, 4, 12},\n+    {4, 5, 12},\n+    {12, 5, 13},\n+    {5, 6, 13},\n+    {13, 6, 14},\n+    {6, 7, 14},\n+    {14, 7, 15},\n+    {7, 8, 15},\n+    {15, 8, 16},\n+    {8, 1, 16},\n+    {16, 1, 9},\n+    {9, 10, 17},\n+    {17, 10, 18},\n+    {10, 11, 18},\n+    {18, 11, 19},\n+    {11, 12, 19},\n+    {19, 12, 20},\n+    {12, 13, 20},\n+    {20, 13, 21},\n+    {13, 14, 21},\n+    {21, 14, 22},\n+    {14, 15, 22},\n+    {22, 15, 23},\n+    {15, 16, 23},\n+    {23, 16, 24},\n+    {16, 9, 24},\n+    {24, 9, 17},\n+    {17, 18, 25},\n+    {25, 18, 26},\n+    {18, 19, 26},\n+    {26, 19, 27},\n+    {19, 20, 27},\n+    {27, 20, 28},\n+    {20, 21, 28},\n+    {28, 21, 29},\n+    {21, 22, 29},\n+    {29, 22, 30},\n+    {22, 23, 30},\n+    {30, 23, 31},\n+    {23, 24, 31},\n+    {31, 24, 32},\n+    {24, 17, 32},\n+    {32, 17, 25},\n+    {25, 26, 33},\n+    {33, 26, 34},\n+    {26, 27, 34},\n+    {34, 27, 35},\n+    {27, 28, 35},\n+    {35, 28, 36},\n+    {28, 29, 36},\n+    {36, 29, 37},\n+    {29, 30, 37},\n+    {37, 30, 38},\n+    {30, 31, 38},\n+    {38, 31, 39},\n+    {31, 32, 39},\n+    {39, 32, 40},\n+    {32, 25, 40},\n+    {40, 25, 33},\n+    {33, 34, 41},\n+    {41, 34, 42},\n+    {34, 35, 42},\n+    {42, 35, 43},\n+    {35, 36, 43},\n+    {43, 36, 44},\n+    {36, 37, 44},\n+    {44, 37, 45},\n+    {37, 38, 45},\n+    {45, 38, 46},\n+    {38, 39, 46},\n+    {46, 39, 47},\n+    {39, 40, 47},\n+    {47, 40, 48},\n+    {40, 33, 48},\n+    {48, 33, 41},\n+    {41, 42, 49},\n+    {49, 42, 50},\n+    {42, 43, 50},\n+    {50, 43, 51},\n+    {43, 44, 51},\n+    {51, 44, 52},\n+    {44, 45, 52},\n+    {52, 45, 53},\n+    {45, 46, 53},\n+    {53, 46, 54},\n+    {46, 47, 54},\n+    {54, 47, 55},\n+    {47, 48, 55},\n+    {55, 48, 56},\n+    {48, 41, 56},\n+    {56, 41, 49},\n+    {2, 1, 57},\n+    {3, 2, 57},\n+    {4, 3, 57},\n+    {5, 4, 57},\n+    {6, 5, 57},\n+    {7, 6, 57},\n+    {8, 7, 57},\n+    {1, 8, 57},\n+    {49, 50, 58},\n+    {50, 51, 58},\n+    {51, 52, 58},\n+    {52, 53, 58},\n+    {53, 54, 58},\n+    {54, 55, 58},\n+    {55, 56, 58},\n+    {56, 49, 58}\n+  };\n \n-  // Filtering\n-  if (!cd->isCollidable(cd->findCollisionNode(_o1), cd->findCollisionNode(_o2)))\n-    return cdata->done;\n+  fcl::BVHModel<BV>* model = new fcl::BVHModel<BV>;\n+  fcl::Vec3f p1, p2, p3;\n+  model->beginModel();\n \n-  // Perform narrow-phase detection\n-  fcl::collide(_o1, _o2, request, result);\n+  for (int i = 0; i < 112; i++)\n+  {\n+    p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX,\n+        v[f[i][0]][1] * _sizeY,\n+        v[f[i][0]][2] * _sizeZ);\n+    p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX,\n+        v[f[i][1]][1] * _sizeY,\n+        v[f[i][1]][2] * _sizeZ);\n+    p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX,\n+        v[f[i][2]][1] * _sizeY,\n+        v[f[i][2]][2] * _sizeZ);\n+\n+    model->addTriangle(p1, p2, p3);\n+  }\n+\n+  model->endModel();\n+\n+  return model;\n+}\n+\n+#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4)\n+\n+//==============================================================================\n+template<class BV>\n+fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,\n+                                  double _height, int _slices, int _stacks)\n+{\n+  const int CACHE_SIZE = 240;\n+\n+  int i, j;\n+  float sinCache[CACHE_SIZE];\n+  float cosCache[CACHE_SIZE];\n+  float angle;\n+  float zBase;\n+  float zLow, zHigh;\n+  float sintemp, costemp;\n+  float deltaRadius;\n+  float radiusLow, radiusHigh;\n+\n+  if (_slices >= CACHE_SIZE) _slices = CACHE_SIZE-1;\n+\n+  if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0 ||\n+      _height < 0.0)\n+  {\n+    return nullptr;\n+  }\n+\n+  /* Center at CoM */\n+  zBase = -_height/2;\n+\n+  /* Compute delta */\n+  deltaRadius = _baseRadius - _topRadius;\n+\n+  /* Cache is the vertex locations cache */\n+  for (i = 0; i < _slices; i++)\n+  {\n+    angle = 2 * M_PI * i / _slices;\n+    sinCache[i] = sin(angle);\n+    cosCache[i] = cos(angle);\n+  }\n+\n+  sinCache[_slices] = sinCache[0];\n+  cosCache[_slices] = cosCache[0];\n+\n+  fcl::BVHModel<BV>* model = new fcl::BVHModel<BV>;\n+  fcl::Vec3f p1, p2, p3, p4;\n+\n+  model->beginModel();\n+\n+  /* Base of cylinder */\n+  sintemp = sinCache[0];\n+  costemp = cosCache[0];\n+  radiusLow = _baseRadius;\n+  zLow = zBase;\n+  p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow);\n+  for (i = 1; i < _slices; i++)\n+  {\n+    p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);\n+    p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow);\n+    model->addTriangle(p1, p2, p3);\n+  }\n+\n+  /* Body of cylinder */\n+  for (i = 0; i < _slices; i++)\n+  {\n+    for (j = 0; j < _stacks; j++)\n+    {\n+      zLow = j * _height / _stacks + zBase;\n+      zHigh = (j + 1) * _height / _stacks + zBase;\n+      radiusLow = _baseRadius\n+                  - deltaRadius * (static_cast<float>(j) / _stacks);\n+      radiusHigh = _baseRadius\n+                   - deltaRadius * (static_cast<float>(j + 1) / _stacks);\n+\n+      p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i],\n+                      zLow);\n+      p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1],\n+                      zLow);\n+      p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i],\n+                      zHigh);\n+      p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1],\n+                      zHigh);\n+\n+      model->addTriangle(p1, p2, p3);\n+      model->addTriangle(p2, p3, p4);\n+    }\n+  }\n+\n+  /* Top of cylinder */\n+  sintemp = sinCache[0];\n+  costemp = cosCache[0];\n+  radiusLow = _topRadius;\n+  zLow = zBase + _height;\n+  p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow);\n+  for (i = 1; i < _slices; i++)\n+  {\n+    p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);\n+    p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow);\n+    model->addTriangle(p1, p2, p3);\n+  }\n+\n+  model->endModel();\n+  return model;\n+}\n+\n+#endif\n+\n+//==============================================================================\n+template<class BV>\n+fcl::BVHModel<BV>* createMesh(float _scaleX, float _scaleY, float _scaleZ,\n+                              const aiScene* _mesh)\n+{\n+  // Create FCL mesh from Assimp mesh\n+\n+  assert(_mesh);\n+  fcl::BVHModel<BV>* model = new fcl::BVHModel<BV>;\n+  model->beginModel();\n+  for (size_t i = 0; i < _mesh->mNumMeshes; i++)\n+  {\n+    for (size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++)\n+    {\n+      fcl::Vec3f vertices[3];\n+      for (size_t k = 0; k < 3; k++)\n+      {\n+        const aiVector3D& vertex\n+            = _mesh->mMeshes[i]->mVertices[\n+              _mesh->mMeshes[i]->mFaces[j].mIndices[k]];\n+        vertices[k] = fcl::Vec3f(vertex.x * _scaleX,\n+                                 vertex.y * _scaleY,\n+                                 vertex.z * _scaleZ);\n+      }\n+      model->addTriangle(vertices[0], vertices[1], vertices[2]);\n+    }\n+  }\n+  model->endModel();\n+  return model;\n+}\n+\n+//==============================================================================\n+template<class BV>\n+fcl::BVHModel<BV>* createSoftMesh(const aiMesh* _mesh)\n+{\n+  // Create FCL mesh from Assimp mesh\n+\n+  assert(_mesh);\n+  fcl::BVHModel<BV>* model = new fcl::BVHModel<BV>;\n+  model->beginModel();\n+\n+  for (size_t i = 0; i < _mesh->mNumFaces; i++)\n+  {\n+    fcl::Vec3f vertices[3];\n+    for (size_t j = 0; j < 3; j++)\n+    {\n+      const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]];\n+      vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z);\n+    }\n+    model->addTriangle(vertices[0], vertices[1], vertices[2]);\n+  }\n+\n+  model->endModel();\n+  return model;\n+}\n+\n+} // anonymous namespace\n+\n+\n+\n+//==============================================================================\n+std::shared_ptr<FCLCollisionDetector> FCLCollisionDetector::create()\n+{\n+  return std::shared_ptr<FCLCollisionDetector>(new FCLCollisionDetector());\n+}\n+\n+//==============================================================================\n+FCLCollisionDetector::~FCLCollisionDetector()\n+{\n+  assert(mShapeMap.empty());\n+}\n+\n+//==============================================================================\n+const std::string& FCLCollisionDetector::getTypeStatic()\n+{\n+  static const std::string& type(\"FCL\");\n+\n+  return type;\n+}\n+\n+//==============================================================================\n+const std::string& FCLCollisionDetector::getType() const\n+{\n+  return FCLCollisionDetector::getTypeStatic();\n+}\n+\n+//==============================================================================\n+std::unique_ptr<CollisionGroup>\n+FCLCollisionDetector::createCollisionGroup()\n+{\n+  return common::make_unique<FCLCollisionGroup>(shared_from_this());\n+}\n+\n+//==============================================================================\n+bool FCLCollisionDetector::detect(\n+    CollisionGroup* group, const Option& option, Result& result)\n+{\n+  result.clear();\n+\n+  if (!group)\n+    return false;\n+\n+  if (group->getCollisionDetector()->getType()\n+      != FCLCollisionDetector::getTypeStatic())\n+  {\n+    return false;\n+  }\n+\n+  group->updateEngineData();\n+\n+  auto castedData = static_cast<FCLCollisionGroup*>(group);\n+  auto broadPhaseAlg = castedData->getFCLCollisionManager();\n+\n+  FCLCollisionCallbackData collData(\n+        option, &result, mPrimitiveShapeType,\n+        mContactPointComputationMethod);\n+  broadPhaseAlg->collide(&collData, collisionCallback);\n \n-  if (!request.enable_cost\n-      && (result.isCollision())\n-      && (result.numContacts() >= request.num_max_contacts))\n+  return result.isCollision();\n+}\n+\n+//==============================================================================\n+bool FCLCollisionDetector::detect(\n+    CollisionGroup* group1, CollisionGroup* group2,\n+    const Option& option, Result& result)\n+{\n+  result.clear();\n+\n+  if (!group1 || !group2)\n+    return false;\n+\n+  if (group1->getCollisionDetector()->getType()\n+      != FCLCollisionDetector::getTypeStatic())\n+  {\n+      return false;\n+  }\n+\n+  if (group2->getCollisionDetector()->getType()\n+      != FCLCollisionDetector::getTypeStatic())\n+  {\n+      return false;\n+  }","path":"dart/collision/fcl/FCLCollisionDetector.cpp","position":673,"original_position":673,"commit_id":"b66ed5fd7fde27bd0fea7cea8ac1a93ed74ed112","original_commit_id":"b66ed5fd7fde27bd0fea7cea8ac1a93ed74ed112","user":{"login":"mkoval","id":142251,"avatar_url":"https://avatars.githubusercontent.com/u/142251?v=3","gravatar_id":"","url":"https://api.github.com/users/mkoval","html_url":"https://github.com/mkoval","followers_url":"https://api.github.com/users/mkoval/followers","following_url":"https://api.github.com/users/mkoval/following{/other_user}","gists_url":"https://api.github.com/users/mkoval/gists{/gist_id}","starred_url":"https://api.github.com/users/mkoval/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/mkoval/subscriptions","organizations_url":"https://api.github.com/users/mkoval/orgs","repos_url":"https://api.github.com/users/mkoval/repos","events_url":"https://api.github.com/users/mkoval/events{/privacy}","received_events_url":"https://api.github.com/users/mkoval/received_events","type":"User","site_admin":false},"body":"I was expecting a check for\r\n```c++\r\ngroup1->getCollisionDetector() == group2->getCollisionDetector()\r\n```\r\nThe check that is implemented is less strict because it would allow me to pass in `CollisionGroup`s from two different `FCLCollisionDetector`s. Will that work? Is this intentional?","created_at":"2016-04-05T06:02:49Z","updated_at":"2016-04-05T06:02:49Z","html_url":"https://github.com/dartsim/dart/pull/631#discussion_r58490566","pull_request_url":"https://api.github.com/repos/dartsim/dart/pulls/631","_links":{"self":{"href":"https://api.github.com/repos/dartsim/dart/pulls/comments/58490566"},"html":{"href":"https://github.com/dartsim/dart/pull/631#discussion_r58490566"},"pull_request":{"href":"https://api.github.com/repos/dartsim/dart/pulls/631"}}},"pull_request":{"url":"https://api.github.com/repos/dartsim/dart/pulls/631","id":62586679,"html_url":"https://github.com/dartsim/dart/pull/631","diff_url":"https://github.com/dartsim/dart/pull/631.diff","patch_url":"https://github.com/dartsim/dart/pull/631.patch","issue_url":"https://api.github.com/repos/dartsim/dart/issues/631","number":631,"state":"open","locked":false,"title":"CollisionGroup class and refactoring of CollisionDetector","user":{"login":"jslee02","id":4038467,"avatar_url":"https://avatars.githubusercontent.com/u/4038467?v=3","gravatar_id":"","url":"https://api.github.com/users/jslee02","html_url":"https://github.com/jslee02","followers_url":"https://api.github.com/users/jslee02/followers","following_url":"https://api.github.com/users/jslee02/following{/other_user}","gists_url":"https://api.github.com/users/jslee02/gists{/gist_id}","starred_url":"https://api.github.com/users/jslee02/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/jslee02/subscriptions","organizations_url":"https://api.github.com/users/jslee02/orgs","repos_url":"https://api.github.com/users/jslee02/repos","events_url":"https://api.github.com/users/jslee02/events{/privacy}","received_events_url":"https://api.github.com/users/jslee02/received_events","type":"User","site_admin":false},"body":"This pull request introduces collision group feature with CollisionDetector refactoring. \r\n\r\nAn overview of the changes is:\r\n\r\n* `CollisionGroup` class is added. Collision detector used to containe collision objects in it and only could check collisions for the objects. So we couldn't be able to check collisions for arbitrary (distict) collision objects. `CollisionGroup` now plays the roll of collision object container, and collision detector can check collisions for object-object, object-group, and group-group.\r\n\r\n* `collision` is now only dependent on `Shape` class. So once we move `Shape` and the derived classes to `common` or `math` namespaces then `collision` would be completely independent on 'dynamics' namespace.\r\n\r\n* `FCLCollisionDetector` now takes the advantage of broad phase collision detectorion of FCL to avoid checking all pairs of objects.\r\n\r\n* Collision detector manages Shapes so that the collision detector doesn't create multiple collision detector specific collision object (e.g., btGImpactMeshShape, fcl::BVHModel<OBBRSS>) per Shape.\r\n\r\n\r\n\r\nThis pull request is not completed yet. The major issue remained is the ownership between `CollisionDetector` and `CollisionObject`. I'm considering apply the same mechanism of the customized shared pointer `BodyNodePtr` by making it general to be able to be used for `CollisionObject`.\r\n\r\nNote that this pull request includes the changes of #608 .","created_at":"2016-03-11T18:58:23Z","updated_at":"2016-04-05T06:02:49Z","closed_at":null,"merged_at":null,"merge_commit_sha":"1037ee542e71950d86caf32e05f1b8102a7d3822","assignee":null,"milestone":{"url":"https://api.github.com/repos/dartsim/dart/milestones/13","html_url":"https://github.com/dartsim/dart/milestones/DART%206.0.0","labels_url":"https://api.github.com/repos/dartsim/dart/milestones/13/labels","id":1199830,"number":13,"title":"DART 6.0.0","description":"Compiler requirements\r\n\r\n* Visual Studio 2015","creator":{"login":"mxgrey","id":1307217,"avatar_url":"https://avatars.githubusercontent.com/u/1307217?v=3","gravatar_id":"","url":"https://api.github.com/users/mxgrey","html_url":"https://github.com/mxgrey","followers_url":"https://api.github.com/users/mxgrey/followers","following_url":"https://api.github.com/users/mxgrey/following{/other_user}","gists_url":"https://api.github.com/users/mxgrey/gists{/gist_id}","starred_url":"https://api.github.com/users/mxgrey/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/mxgrey/subscriptions","organizations_url":"https://api.github.com/users/mxgrey/orgs","repos_url":"https://api.github.com/users/mxgrey/repos","events_url":"https://api.github.com/users/mxgrey/events{/privacy}","received_events_url":"https://api.github.com/users/mxgrey/received_events","type":"User","site_admin":false},"open_issues":10,"closed_issues":67,"state":"open","created_at":"2015-07-09T03:10:42Z","updated_at":"2016-04-04T23:14:38Z","due_on":"2016-04-15T04:00:00Z","closed_at":null},"commits_url":"https://api.github.com/repos/dartsim/dart/pulls/631/commits","review_comments_url":"https://api.github.com/repos/dartsim/dart/pulls/631/comments","review_comment_url":"https://api.github.com/repos/dartsim/dart/pulls/comments{/number}","comments_url":"https://api.github.com/repos/dartsim/dart/issues/631/comments","statuses_url":"https://api.github.com/repos/dartsim/dart/statuses/b66ed5fd7fde27bd0fea7cea8ac1a93ed74ed112","head":{"label":"dartsim:collision_group","ref":"collision_group","sha":"b66ed5fd7fde27bd0fea7cea8ac1a93ed74ed112","user":{"login":"dartsim","id":4191609,"avatar_url":"https://avatars.githubusercontent.com/u/4191609?v=3","gravatar_id":"","url":"https://api.github.com/users/dartsim","html_url":"https://github.com/dartsim","followers_url":"https://api.github.com/users/dartsim/followers","following_url":"https://api.github.com/users/dartsim/following{/other_user}","gists_url":"https://api.github.com/users/dartsim/gists{/gist_id}","starred_url":"https://api.github.com/users/dartsim/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/dartsim/subscriptions","organizations_url":"https://api.github.com/users/dartsim/orgs","repos_url":"https://api.github.com/users/dartsim/repos","events_url":"https://api.github.com/users/dartsim/events{/privacy}","received_events_url":"https://api.github.com/users/dartsim/received_events","type":"Organization","site_admin":false},"repo":{"id":2418592,"name":"dart","full_name":"dartsim/dart","owner":{"login":"dartsim","id":4191609,"avatar_url":"https://avatars.githubusercontent.com/u/4191609?v=3","gravatar_id":"","url":"https://api.github.com/users/dartsim","html_url":"https://github.com/dartsim","followers_url":"https://api.github.com/users/dartsim/followers","following_url":"https://api.github.com/users/dartsim/following{/other_user}","gists_url":"https://api.github.com/users/dartsim/gists{/gist_id}","starred_url":"https://api.github.com/users/dartsim/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/dartsim/subscriptions","organizations_url":"https://api.github.com/users/dartsim/orgs","repos_url":"https://api.github.com/users/dartsim/repos","events_url":"https://api.github.com/users/dartsim/events{/privacy}","received_events_url":"https://api.github.com/users/dartsim/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/dartsim/dart","description":"Dynamic Animation and Robotics Toolkit","fork":false,"url":"https://api.github.com/repos/dartsim/dart","forks_url":"https://api.github.com/repos/dartsim/dart/forks","keys_url":"https://api.github.com/repos/dartsim/dart/keys{/key_id}","collaborators_url":"https://api.github.com/repos/dartsim/dart/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/dartsim/dart/teams","hooks_url":"https://api.github.com/repos/dartsim/dart/hooks","issue_events_url":"https://api.github.com/repos/dartsim/dart/issues/events{/number}","events_url":"https://api.github.com/repos/dartsim/dart/events","assignees_url":"https://api.github.com/repos/dartsim/dart/assignees{/user}","branches_url":"https://api.github.com/repos/dartsim/dart/branches{/branch}","tags_url":"https://api.github.com/repos/dartsim/dart/tags","blobs_url":"https://api.github.com/repos/dartsim/dart/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/dartsim/dart/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/dartsim/dart/git/refs{/sha}","trees_url":"https://api.github.com/repos/dartsim/dart/git/trees{/sha}","statuses_url":"https://api.github.com/repos/dartsim/dart/statuses/{sha}","languages_url":"https://api.github.com/repos/dartsim/dart/languages","stargazers_url":"https://api.github.com/repos/dartsim/dart/stargazers","contributors_url":"https://api.github.com/repos/dartsim/dart/contributors","subscribers_url":"https://api.github.com/repos/dartsim/dart/subscribers","subscription_url":"https://api.github.com/repos/dartsim/dart/subscription","commits_url":"https://api.github.com/repos/dartsim/dart/commits{/sha}","git_commits_url":"https://api.github.com/repos/dartsim/dart/git/commits{/sha}","comments_url":"https://api.github.com/repos/dartsim/dart/comments{/number}","issue_comment_url":"https://api.github.com/repos/dartsim/dart/issues/comments{/number}","contents_url":"https://api.github.com/repos/dartsim/dart/contents/{+path}","compare_url":"https://api.github.com/repos/dartsim/dart/compare/{base}...{head}","merges_url":"https://api.github.com/repos/dartsim/dart/merges","archive_url":"https://api.github.com/repos/dartsim/dart/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/dartsim/dart/downloads","issues_url":"https://api.github.com/repos/dartsim/dart/issues{/number}","pulls_url":"https://api.github.com/repos/dartsim/dart/pulls{/number}","milestones_url":"https://api.github.com/repos/dartsim/dart/milestones{/number}","notifications_url":"https://api.github.com/repos/dartsim/dart/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/dartsim/dart/labels{/name}","releases_url":"https://api.github.com/repos/dartsim/dart/releases{/id}","deployments_url":"https://api.github.com/repos/dartsim/dart/deployments","created_at":"2011-09-19T21:53:51Z","updated_at":"2016-03-30T16:13:08Z","pushed_at":"2016-04-05T04:35:44Z","git_url":"git://github.com/dartsim/dart.git","ssh_url":"git@github.com:dartsim/dart.git","clone_url":"https://github.com/dartsim/dart.git","svn_url":"https://github.com/dartsim/dart","homepage":"http://dartsim.github.io/","size":112722,"stargazers_count":88,"watchers_count":88,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":true,"forks_count":63,"mirror_url":null,"open_issues_count":128,"forks":63,"open_issues":128,"watchers":88,"default_branch":"master"}},"base":{"label":"dartsim:master","ref":"master","sha":"7c04754e70ae7db33cfd8f47066b32a685e491f7","user":{"login":"dartsim","id":4191609,"avatar_url":"https://avatars.githubusercontent.com/u/4191609?v=3","gravatar_id":"","url":"https://api.github.com/users/dartsim","html_url":"https://github.com/dartsim","followers_url":"https://api.github.com/users/dartsim/followers","following_url":"https://api.github.com/users/dartsim/following{/other_user}","gists_url":"https://api.github.com/users/dartsim/gists{/gist_id}","starred_url":"https://api.github.com/users/dartsim/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/dartsim/subscriptions","organizations_url":"https://api.github.com/users/dartsim/orgs","repos_url":"https://api.github.com/users/dartsim/repos","events_url":"https://api.github.com/users/dartsim/events{/privacy}","received_events_url":"https://api.github.com/users/dartsim/received_events","type":"Organization","site_admin":false},"repo":{"id":2418592,"name":"dart","full_name":"dartsim/dart","owner":{"login":"dartsim","id":4191609,"avatar_url":"https://avatars.githubusercontent.com/u/4191609?v=3","gravatar_id":"","url":"https://api.github.com/users/dartsim","html_url":"https://github.com/dartsim","followers_url":"https://api.github.com/users/dartsim/followers","following_url":"https://api.github.com/users/dartsim/following{/other_user}","gists_url":"https://api.github.com/users/dartsim/gists{/gist_id}","starred_url":"https://api.github.com/users/dartsim/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/dartsim/subscriptions","organizations_url":"https://api.github.com/users/dartsim/orgs","repos_url":"https://api.github.com/users/dartsim/repos","events_url":"https://api.github.com/users/dartsim/events{/privacy}","received_events_url":"https://api.github.com/users/dartsim/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/dartsim/dart","description":"Dynamic Animation and Robotics Toolkit","fork":false,"url":"https://api.github.com/repos/dartsim/dart","forks_url":"https://api.github.com/repos/dartsim/dart/forks","keys_url":"https://api.github.com/repos/dartsim/dart/keys{/key_id}","collaborators_url":"https://api.github.com/repos/dartsim/dart/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/dartsim/dart/teams","hooks_url":"https://api.github.com/repos/dartsim/dart/hooks","issue_events_url":"https://api.github.com/repos/dartsim/dart/issues/events{/number}","events_url":"https://api.github.com/repos/dartsim/dart/events","assignees_url":"https://api.github.com/repos/dartsim/dart/assignees{/user}","branches_url":"https://api.github.com/repos/dartsim/dart/branches{/branch}","tags_url":"https://api.github.com/repos/dartsim/dart/tags","blobs_url":"https://api.github.com/repos/dartsim/dart/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/dartsim/dart/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/dartsim/dart/git/refs{/sha}","trees_url":"https://api.github.com/repos/dartsim/dart/git/trees{/sha}","statuses_url":"https://api.github.com/repos/dartsim/dart/statuses/{sha}","languages_url":"https://api.github.com/repos/dartsim/dart/languages","stargazers_url":"https://api.github.com/repos/dartsim/dart/stargazers","contributors_url":"https://api.github.com/repos/dartsim/dart/contributors","subscribers_url":"https://api.github.com/repos/dartsim/dart/subscribers","subscription_url":"https://api.github.com/repos/dartsim/dart/subscription","commits_url":"https://api.github.com/repos/dartsim/dart/commits{/sha}","git_commits_url":"https://api.github.com/repos/dartsim/dart/git/commits{/sha}","comments_url":"https://api.github.com/repos/dartsim/dart/comments{/number}","issue_comment_url":"https://api.github.com/repos/dartsim/dart/issues/comments{/number}","contents_url":"https://api.github.com/repos/dartsim/dart/contents/{+path}","compare_url":"https://api.github.com/repos/dartsim/dart/compare/{base}...{head}","merges_url":"https://api.github.com/repos/dartsim/dart/merges","archive_url":"https://api.github.com/repos/dartsim/dart/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/dartsim/dart/downloads","issues_url":"https://api.github.com/repos/dartsim/dart/issues{/number}","pulls_url":"https://api.github.com/repos/dartsim/dart/pulls{/number}","milestones_url":"https://api.github.com/repos/dartsim/dart/milestones{/number}","notifications_url":"https://api.github.com/repos/dartsim/dart/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/dartsim/dart/labels{/name}","releases_url":"https://api.github.com/repos/dartsim/dart/releases{/id}","deployments_url":"https://api.github.com/repos/dartsim/dart/deployments","created_at":"2011-09-19T21:53:51Z","updated_at":"2016-03-30T16:13:08Z","pushed_at":"2016-04-05T04:35:44Z","git_url":"git://github.com/dartsim/dart.git","ssh_url":"git@github.com:dartsim/dart.git","clone_url":"https://github.com/dartsim/dart.git","svn_url":"https://github.com/dartsim/dart","homepage":"http://dartsim.github.io/","size":112722,"stargazers_count":88,"watchers_count":88,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":true,"forks_count":63,"mirror_url":null,"open_issues_count":128,"forks":63,"open_issues":128,"watchers":88,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/dartsim/dart/pulls/631"},"html":{"href":"https://github.com/dartsim/dart/pull/631"},"issue":{"href":"https://api.github.com/repos/dartsim/dart/issues/631"},"comments":{"href":"https://api.github.com/repos/dartsim/dart/issues/631/comments"},"review_comments":{"href":"https://api.github.com/repos/dartsim/dart/pulls/631/comments"},"review_comment":{"href":"https://api.github.com/repos/dartsim/dart/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/dartsim/dart/pulls/631/commits"},"statuses":{"href":"https://api.github.com/repos/dartsim/dart/statuses/b66ed5fd7fde27bd0fea7cea8ac1a93ed74ed112"}}}}