Added group_id to collision detection

Added group_id to collision detection
- updated collision detection broad_phase to skip node/edges in the same group
- updated CollisionConstraint to receive the group_ids from OptimizationProblem
   - TODO: cleanup some of the methods/tests

Improve validation of json to indicate when invalid parameters are passed.

Tested saw with large min-epsilon ... seems to work fine.

cmake/FixingCollisionsDownloadExternal.cmake

@@ -69,6 +69,6 @@ endfunction()
 function(download_json)
     custom_download_project(json
         GIT_REPOSITORY https://github.com/nlohmann/json
-        GIT_TAG v3.5.0
+        GIT_TAG v3.7.0
     )
 endfunction()

fixtures/rigid_bodies/square_falling_onto_saw.json

@@ -1,6 +1,19 @@
 {
     "scene_type": "rigid_body_problem",
+    "distance_barrier_constraint": {
+        "custom_initial_epsilon": 0.1,
+        "detection_method": "hash_grid",
+        "use_distance_hashgrid": true,
+        "extend_collision_set": false,
+        "custom_hashgrid_cellsize": -1
+    },
+    "barrier_solver": {
+        "inner_solver": "newton_solver",
+        "min_barrier_epsilon": 1e-2,
+        "max_iterations": 0
+    },
     "rigid_body_problem": {
+        "collision_eps": 0.0,
         "rigid_bodies": [{
             "vertices": [
                 [2, 3],

fixtures/rigid_bodies/step_square_falling_diagonally_onto_floor.json

@@ -5,7 +5,7 @@
     "distance_barrier_constraint": {
         "custom_initial_epsilon": 0.5,
         "detection_method": "hash_grid",
-        "use_hash_grid": false,
+        "use_distance_hashgrid": false,
         "extend_collision_set": false
     },
     "rigid_body_problem": {

fixtures/rigid_bodies/step_square_falling_diagonally_onto_floor_touching.json

@@ -1,18 +1,19 @@
 {
     "scene_type": "rigid_body_problem",
     "timestep_size": 1.0,
-    "gravity": [0.0, 0.0, 0.0],
     "viewport_bbox": {
         "min": [-12, -3.0],
         "max": [12, 5.0]
     },
     "distance_barrier_constraint": {
         "custom_initial_epsilon": 0.5,
         "detection_method": "hash_grid",
-        "use_hash_grid": false,
-        "extend_collision_set": false
+        "use_distance_hashgrid": true,
+        "extend_collision_set": false,
+        "custom_hashgrid_cellsize": -1
     },
     "rigid_body_problem": {
+        "collision_eps": 0.0,
         "rigid_bodies": [{
             "vertices": [
                 [-1, 0.000001],

src/SimState.cpp

@@ -33,17 +33,19 @@ bool SimState::load_scene(const std::string& filename)
 
     if (input.good()) {
         scene_file = filename;
-        json scene = json::parse(input);
-        init(scene);
-
-        return true;
+        json scene = json::parse(input, nullptr, false);
+        if (scene.is_discarded()) {
+            spdlog::error("Invalid Json file");
+            return false;
+        }
+        return init(scene);
     }
     return false;
 }
 
 bool SimState::reload_scene() { return load_scene(scene_file); }
 
-void SimState::init(const nlohmann::json& args_in)
+bool SimState::init(const nlohmann::json& args_in)
 {
     using namespace nlohmann;
 
@@ -57,7 +59,7 @@ void SimState::init(const nlohmann::json& args_in)
               "update_constraint_set": true,
               "use_chain_functional": false,
               "gravity":[0.0,0.0,0.0],
-              "collision_eps": 2.0
+              "collision_eps": 0.0
          },
         "particles_problem":{
             "vertices":[],
@@ -69,7 +71,7 @@ void SimState::init(const nlohmann::json& args_in)
             "constraint": "distance_barrier_constraint",
             "update_constraint_set": true,
             "gravity":[0.0,0.0],
-            "collision_eps": 2.0
+            "collision_eps": 0.0
          },
         "barrier_solver": {
             "inner_solver": "newton_solver",
@@ -103,28 +105,51 @@ void SimState::init(const nlohmann::json& args_in)
             "initial_epsilon":"min_toi",
             "custom_initial_epsilon":1.0,
             "detection_method": "hash_grid",
-            "extend_collision_set": true
+            "extend_collision_set": true,
+            "custom_hashgrid_cellsize":-1
         },
        "distance_barrier_constraint":{
            "custom_initial_epsilon":0.5,
            "detection_method": "hash_grid",
-           "use_hash_grid": true,
-           "extend_collision_set": false
+           "use_distance_hashgrid": true,
+           "active_constraint_scale" : 1.5,
+           "extend_collision_set": false,
+           "custom_hashgrid_cellsize":-1
        },
        "volume_constraint":{
            "detection_method": "hash_grid",
            "extend_collision_set": true,
-           "volume_epsilon": 1e-6
+           "volume_epsilon": 1e-6,
+           "custom_hashgrid_cellsize":-1
        },
         "timestep_size": 0.1,
         "viewport_bbox": {"min":[0,0],"max":[0,0]}
     })"_json;
     // clang-format on
 
+    // check that incomming json doesn't have any unkown keys
+    // to avoid stupid bugs
+    auto patch = json::diff(args, args_in);
+    bool valid = true;
+    for (auto& op : patch) {
+        if (op["op"].get<std::string>().compare("add") == 0) {
+            auto new_path = json::json_pointer(op["path"].get<std::string>());
+            if (args_in[new_path.parent_pointer()].is_array()) {
+                valid = true;
+            } else {
+                valid = false;
+                spdlog::error("Unknown key in json path={}",
+                    op["path"].get<std::string>());
+            }
+        }
+    }
+    if (!valid) {
+        return false;
+    }
+
     args.merge_patch(args_in);
     m_timestep_size = args["timestep_size"].get<double>();
 
-
     auto problem_name = args["scene_type"].get<std::string>();
 
     // Config CCD CONSTRAINT
@@ -160,6 +185,8 @@ void SimState::init(const nlohmann::json& args_in)
 
     vertices_sequence.clear();
     vertices_sequence.push_back(problem_ptr->vertices());
+
+    return true;
 }
 
 nlohmann::json SimState::get_active_config()
@@ -209,9 +236,8 @@ bool SimState::solve_collision()
     }
 
     ccd::opt::OptimizationResults result;
-    QUICK_PROFILE("solve_collisions",
-        result = ccd_solver_ptr->solve(*problem_ptr);
-    );
+    QUICK_PROFILE(
+        "solve_collisions", result = ccd_solver_ptr->solve(*problem_ptr););
 
     m_step_has_collision = problem_ptr->take_step(result.x, m_timestep_size);
 
@@ -245,13 +271,13 @@ void SimState::collision_resolution_step()
         m_step_has_collision ? "unsolved" : "solved");
 }
 
-
-void SimState::save_simulation(const std::string& filename){
+void SimState::save_simulation(const std::string& filename)
+{
     nlohmann::json results;
     results["args"] = args;
     results["active_args"] = get_active_config();
     std::vector<nlohmann::json> vs;
-    for (auto&v: vertices_sequence) {
+    for (auto& v : vertices_sequence) {
         vs.push_back(io::to_json(v));
     }
     results["animation"] = nlohmann::json();
@@ -262,7 +288,6 @@ void SimState::save_simulation(const std::string& filename){
     o << std::setw(4) << results << std::endl;
 }
 
-
 void SimState::get_collision_gradient(Eigen::MatrixXd& fgrad)
 {
     if (m_dirty_constraints) {

src/SimState.hpp

@@ -15,7 +15,7 @@ class SimState {
 
     bool load_scene(const std::string& filename);
     bool reload_scene();
-    void init(const nlohmann::json& args);
+    bool init(const nlohmann::json& args);
 
     void simulation_step();
     bool solve_collision();

src/ccd/collision_detection.hpp

@@ -48,8 +48,19 @@ static const char* DetectionMethodNames[] = { "BRUTE_FORCE", "HASH_GRID" };
  *          vertex index and the time of impact are stored in ev_impacts.
  */
 void detect_edge_vertex_collisions(const Eigen::MatrixXd& vertices,
-    const Eigen::MatrixXd& displacements, const Eigen::MatrixX2i& edges,
-    EdgeVertexImpacts& ev_impacts, DetectionMethod method = BRUTE_FORCE,
+    const Eigen::MatrixXd& displacements,
+    const Eigen::MatrixX2i& edges,
+    const Eigen::VectorXi& group_ids,
+    EdgeVertexImpacts& ev_impacts,
+    DetectionMethod method = BRUTE_FORCE,
+    bool reset_impacts = true);
+
+/// @brief backward compatibility with old definition
+void detect_edge_vertex_collisions(const Eigen::MatrixXd& vertices,
+    const Eigen::MatrixXd& displacements,
+    const Eigen::MatrixX2i& edges,
+    EdgeVertexImpacts& ev_impacts,
+    DetectionMethod method = BRUTE_FORCE,
     bool reset_impacts = true);
 
 /**
@@ -73,7 +84,9 @@ void detect_edge_vertex_collisions(const Eigen::MatrixXd& vertices,
  *          vertex index and the time of impact are stored in ev_impacts.
  */
 void detect_edge_vertex_collisions_brute_force(const Eigen::MatrixXd& vertices,
-    const Eigen::MatrixXd& displacements, const Eigen::MatrixX2i& edges,
+    const Eigen::MatrixXd& displacements,
+    const Eigen::MatrixX2i& edges,
+    const Eigen::VectorXi& group_ids,
     EdgeVertexCandidates& ev_candidates);
 
 /**
@@ -97,7 +110,9 @@ void detect_edge_vertex_collisions_brute_force(const Eigen::MatrixXd& vertices,
  *          vertex index and the time of impact are stored in ev_impacts.
  */
 void detect_edge_vertex_collisions_hash_map(const Eigen::MatrixXd& vertices,
-    const Eigen::MatrixXd& displacements, const Eigen::MatrixX2i& edges,
+    const Eigen::MatrixXd& displacements,
+    const Eigen::MatrixX2i& edges,
+    const Eigen::VectorXi& group_ids,
     EdgeVertexCandidates& ev_candidates);
 
 /**
@@ -119,9 +134,12 @@ void detect_edge_vertex_collisions_hash_map(const Eigen::MatrixXd& vertices,
  * @param[out] ev_impacts  List of impacts on to which new impacts are pushed.
  */
 void detect_edge_vertex_collisions_narrow_phase(const Eigen::Vector2d& Vi,
-    const Eigen::Vector2d& Vj, const Eigen::Vector2d& Vk,
-    const Eigen::Vector2d& Ui, const Eigen::Vector2d& Uj,
-    const Eigen::Vector2d& Uk, const EdgeVertexCandidate& ev_candidate,
+    const Eigen::Vector2d& Vj,
+    const Eigen::Vector2d& Vk,
+    const Eigen::Vector2d& Ui,
+    const Eigen::Vector2d& Uj,
+    const Eigen::Vector2d& Uk,
+    const EdgeVertexCandidate& ev_candidate,
     EdgeVertexImpacts& ev_impacts);
 
 /**
@@ -148,9 +166,13 @@ void detect_edge_vertex_collisions_narrow_phase(const Eigen::Vector2d& Vi,
  *          impact points spatial parameter along the edge is stored in alpha.
  */
 bool compute_edge_vertex_time_of_impact(const Eigen::Vector2d& Vi,
-    const Eigen::Vector2d& Vj, const Eigen::Vector2d& Vk,
-    const Eigen::Vector2d& Ui, const Eigen::Vector2d& Uj,
-    const Eigen::Vector2d& Uk, double& toi, double& alpha,
+    const Eigen::Vector2d& Vj,
+    const Eigen::Vector2d& Vk,
+    const Eigen::Vector2d& Ui,
+    const Eigen::Vector2d& Uj,
+    const Eigen::Vector2d& Uk,
+    double& toi,
+    double& alpha,
     const double tolerance = 1e-8);
 
 /**
@@ -177,9 +199,13 @@ bool compute_edge_vertex_time_of_impact(const Eigen::Vector2d& Vi,
  *         the valid spatial parameter in alpha.
  */
 bool temporal_parameterization_to_spatial(const Eigen::Vector2d& Vi,
-    const Eigen::Vector2d& Vj, const Eigen::Vector2d& Vk,
-    const Eigen::Vector2d& Ui, const Eigen::Vector2d& Uj,
-    const Eigen::Vector2d& Uk, const double t, double& alpha,
+    const Eigen::Vector2d& Vj,
+    const Eigen::Vector2d& Vk,
+    const Eigen::Vector2d& Ui,
+    const Eigen::Vector2d& Uj,
+    const Eigen::Vector2d& Uk,
+    const double t,
+    double& alpha,
     const double tolerance = 1e-8);
 
 } // namespace ccd

src/ccd/collision_detection_broad_phase.cpp

@@ -13,8 +13,24 @@ namespace ccd {
 
 // Find all edge-vertex collisions in one time step.
 void detect_edge_vertex_collisions(const Eigen::MatrixXd& vertices,
-    const Eigen::MatrixXd& displacements, const Eigen::MatrixX2i& edges,
-    EdgeVertexImpacts& ev_impacts, DetectionMethod method, bool reset_impacts)
+    const Eigen::MatrixXd& displacements,
+    const Eigen::MatrixX2i& edges,
+    EdgeVertexImpacts& ev_impacts,
+    DetectionMethod method,
+    bool reset_impacts)
+
+{
+    return detect_edge_vertex_collisions(vertices, displacements, edges,
+        Eigen::VectorXi(), ev_impacts, method, reset_impacts);
+}
+
+void detect_edge_vertex_collisions(const Eigen::MatrixXd& vertices,
+    const Eigen::MatrixXd& displacements,
+    const Eigen::MatrixX2i& edges,
+    const Eigen::VectorXi& group_ids,
+    EdgeVertexImpacts& ev_impacts,
+    DetectionMethod method,
+    bool reset_impacts)
 {
     assert(vertices.size() == displacements.size());
     assert(method == DetectionMethod::BRUTE_FORCE
@@ -36,11 +52,11 @@ void detect_edge_vertex_collisions(const Eigen::MatrixXd& vertices,
         switch (method) {
             case BRUTE_FORCE:
                 detect_edge_vertex_collisions_brute_force(
-                    vertices, displacements, edges, ev_candidates);
+                    vertices, displacements, edges, group_ids, ev_candidates);
                 break;
             case HASH_GRID:
                 detect_edge_vertex_collisions_hash_map(
-                    vertices, displacements, edges, ev_candidates);
+                    vertices, displacements, edges, group_ids, ev_candidates);
                 break;
         },
         ProfiledPoint::DETECTING_COLLISIONS_BROAD_PHASE);
@@ -63,17 +79,26 @@ void detect_edge_vertex_collisions(const Eigen::MatrixXd& vertices,
 // Find all edge-vertex collisions in one time step using brute-force
 // comparisons of all edges and all vertices.
 void detect_edge_vertex_collisions_brute_force(const Eigen::MatrixXd& vertices,
-    const Eigen::MatrixXd& /*displacements*/, const Eigen::MatrixX2i& edges,
+    const Eigen::MatrixXd& /*displacements*/,
+    const Eigen::MatrixX2i& edges,
+    const Eigen::VectorXi& group_ids,
     EdgeVertexCandidates& ev_candidates)
 {
+    const bool check_group = group_ids.size() > 0;
     // Loop over all edges
     for (int edge_index = 0; edge_index < edges.rows(); edge_index++) {
         // Loop over all vertices
         for (int vertex_index = 0; vertex_index < vertices.rows();
              vertex_index++) {
             // Check that the vertex is not an endpoint of the edge
-            if (vertex_index != edges(edge_index, 0)
-                && vertex_index != edges(edge_index, 1)) {
+            bool is_endpoint = vertex_index == edges(edge_index, 0)
+                || vertex_index == edges(edge_index, 1);
+            bool same_group = false;
+            if (check_group) {
+                same_group = group_ids(vertex_index)
+                    == group_ids(edges(edge_index, 0));
+            }
+            if (!is_endpoint && !same_group) {
                 ev_candidates.push_back(
                     EdgeVertexCandidate(edge_index, vertex_index));
             }
@@ -84,7 +109,9 @@ void detect_edge_vertex_collisions_brute_force(const Eigen::MatrixXd& vertices,
 // Find all edge-vertex collisions in one time step using spatial-hashing to
 // only compare points and edge in the same cells.
 void detect_edge_vertex_collisions_hash_map(const Eigen::MatrixXd& vertices,
-    const Eigen::MatrixXd& displacements, const Eigen::MatrixX2i& edges,
+    const Eigen::MatrixXd& displacements,
+    const Eigen::MatrixX2i& edges,
+    const Eigen::VectorXi& group_ids,
     EdgeVertexCandidates& ev_candidates)
 {
     HashGrid hashgrid;
@@ -94,7 +121,7 @@ void detect_edge_vertex_collisions_hash_map(const Eigen::MatrixXd& vertices,
 
     // Assume checking if vertex is and end-point of the edge is done by
     // `hashgrid.getVertexEdgePairs(...)`.
-    hashgrid.getVertexEdgePairs(edges, ev_candidates);
+    hashgrid.getVertexEdgePairs(edges, group_ids, ev_candidates);
 }
 
 } // namespace ccd