fix part of bugs in 3-2_collision

3-2_collision/CollisionDetection.cpp

 #include "CollisionDetection.h"
 
+#include <set>
+#include <unordered_set>
+
 void CollisionDetection::computeBroadPhase(int broadPhaseMethod) {
     // compute possible collisions
     m_overlappingBodys.clear();
@@ -43,12 +46,21 @@ void CollisionDetection::computeNarrowPhase(int narrowPhaseMethod) {
     switch (narrowPhaseMethod) {
     case 0: {
         // exhaustive
+
+        // set of vertex indices that penetrate a face
+        std::set<int> m_penetratingVertices;
+        // set of pairs of vertex indices that represent a penetrating edge
+        std::set<std::pair<int, int>> m_penetratingEdges;
+
         // iterate through all pairs of possible collisions
-        for (auto overlap : m_overlappingBodys) {
+        for (auto &overlap : m_overlappingBodys) {
             std::vector<Contact> temp_contacts[2];
-            // compute intersection of a with b first and intersectino
+            // compute intersection of a with b first and intersection
             // of b with a and store results in temp_contacts
             for (int switcher = 0; switcher < 2; switcher++) {
+                m_penetratingEdges.clear();
+                m_penetratingVertices.clear();
+
                 RigidObject* a =
                     &m_objects[(!switcher) ? overlap.first
                                             : overlap.second];
@@ -93,17 +105,15 @@ void CollisionDetection::computeNarrowPhase(int narrowPhaseMethod) {
                                 break;
                             }
                             case ContactType::EDGEEDGE: {
-                                int orderedStart = std::min(start, end);
-                                int orderedEnd = std::max(start, end);
-                                auto ret = m_penetratingEdges.insert(
-                                    std::make_pair(orderedStart,
-                                                    orderedEnd));
+                                if (start > end) {
+                                    std::swap(start, end);
+                                }
+                                auto ret = m_penetratingEdges.emplace(start, end);
+
                                 // if not already in set
                                 if (ret.second) {
                                     Contact temp_col =
-                                        findEdgeEdgeCollision(
-                                            Va.row(orderedStart),
-                                            Va.row(orderedEnd), Vb, Fb);
+                                        findEdgeEdgeCollision(Va.row(start), Va.row(end), Vb, Fb);
                                     temp_col.a = a;
                                     temp_col.b = b;
                                     temp_col.type =
@@ -119,14 +129,12 @@ void CollisionDetection::computeNarrowPhase(int narrowPhaseMethod) {
                         }
                     }
                 }
-                m_penetratingVertices.clear();
-                m_penetratingEdges.clear();
             }
 
             // look for vertexFace
             bool found = false;
             for (int i = 0; i < 2; i++) {
-                for (auto cont : temp_contacts[i]) {
+                for (const auto &cont : temp_contacts[i]) {
                     if (cont.type == ContactType::VERTEXFACE) {
                         m_contacts.push_back(cont);
                         found = true;
@@ -175,7 +183,7 @@ void CollisionDetection::computeNarrowPhase(int narrowPhaseMethod) {
 
 void CollisionDetection::applyImpulse(double eps) {
     // compute impulse for all contacts
-    for (auto contact : m_contacts) {
+    for (const auto &contact : m_contacts) {
         Eigen::Vector3d vrel_vec = contact.a->getVelocity(contact.p) -
                                     contact.b->getVelocity(contact.p);
         double vrel = contact.n.dot(vrel_vec);

3-2_collision/CollisionDetection.h

@@ -2,7 +2,6 @@
 #define COLLISIONDETECTION_H
 
 #include <igl/ray_mesh_intersect.h>
-#include <set>
 #include <utility>
 #include <vector>
 #include "AABB.h"
@@ -139,8 +138,6 @@ class CollisionDetection {
 	void applyImpulse(double eps = 1.0);
 
     void clearDataStructures() {
-        m_penetratingEdges.clear();
-        m_penetratingVertices.clear();
         m_overlappingBodys.clear();
         m_contacts.clear();
     }
@@ -163,12 +160,6 @@ class CollisionDetection {
     // result of broadphase, pairs of objects with possible collisions
     std::vector<std::pair<size_t, size_t>> m_overlappingBodys;
 
-    // set of vertex indices that penetrate a face, used to avoid duplicates
-    std::set<int> m_penetratingVertices;
-    // set of pairs of vertex indices that represent a penetrating edge, used to
-    // avoid duplicates
-    std::set<std::pair<int, int>> m_penetratingEdges;
-
     // computed contact points
     std::vector<Contact> m_contacts;
 };