ex4

4_collision/CMakeLists.txt

+
+cmake_minimum_required(VERSION 3.1)
+project(4_collision)
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/../cmake)
+set(CMAKE_CXX_FLAGS "-Wall")
+
+# libigl
+option(LIBIGL_USE_STATIC_LIBRARY "Use libigl as static library" OFF)
+option(LIBIGL_WITH_ANTTWEAKBAR       "Use AntTweakBar"    OFF)
+option(LIBIGL_WITH_CGAL              "Use CGAL"           OFF)
+option(LIBIGL_WITH_COMISO            "Use CoMiso"         OFF)
+option(LIBIGL_WITH_CORK              "Use Cork"           OFF)
+option(LIBIGL_WITH_EMBREE            "Use Embree"         OFF)
+option(LIBIGL_WITH_LIM               "Use LIM"            OFF)
+option(LIBIGL_WITH_MATLAB            "Use Matlab"         OFF)
+option(LIBIGL_WITH_MOSEK             "Use MOSEK"          OFF)
+option(LIBIGL_WITH_OPENGL            "Use OpenGL"         ON)
+option(LIBIGL_WITH_OPENGL_GLFW       "Use GLFW"           ON)
+option(LIBIGL_WITH_OPENGL_GLFW_IMGUI "Use ImGui"          ON)
+option(LIBIGL_WITH_PNG               "Use PNG"            OFF)
+option(LIBIGL_WITH_PYTHON            "Use Python"         OFF)
+option(LIBIGL_WITH_TETGEN            "Use Tetgen"         OFF)
+option(LIBIGL_WITH_TRIANGLE          "Use Triangle"       OFF)
+option(LIBIGL_WITH_VIEWER            "Use OpenGL viewer"  ON)
+option(LIBIGL_WITH_XML               "Use XML"            OFF)
+
+if (NOT LIBIGL_FOUND)
+    find_package(LIBIGL REQUIRED QUIET)
+endif()
+
+# Add default project files
+file(GLOB LIBFILES ${PROJECT_SOURCE_DIR}/../include/*.*)
+source_group("Library Files" FILES ${LIBFILES})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../include)
+
+# Add your project files
+file(GLOB SRCFILES *.cpp)
+file(GLOB HFILES *.h)
+
+add_definitions(-DIGL_VIEWER_VIEWER_QUIET)
+add_executable(${PROJECT_NAME} ${SRCFILES} ${HFILES} ${LIBFILES} )
+target_link_libraries(${PROJECT_NAME} igl::core igl::opengl_glfw igl::opengl_glfw_imgui)
\ No newline at end of file

4_collision/CollisionDetection.cpp

+#include "CollisionDetection.h"
+
+void CollisionDetection::computeBroadPhase(int broadPhaseMethod) {
+    // compute possible collisions
+    m_overlappingBodys.clear();
+    
+	switch (broadPhaseMethod) {
+	case 0: { // none
+		for (size_t i = 0; i < m_objects.size(); i++) {
+			for (size_t j = i + 1; j < m_objects.size(); j++) {
+				m_overlappingBodys.push_back(std::make_pair(i, j));
+			}
+		}
+		break;
+	}
+     
+	case 1: {  // AABB
+        // compute bounding boxes
+        std::vector<AABB> aabbs(m_objects.size());
+        for (size_t i = 0; i < aabbs.size(); i++) {
+            aabbs[i].computeAABB(m_objects[i]);
+        }
+        for (size_t i = 0; i < m_objects.size(); i++) {
+            for (size_t j = i + 1; j < m_objects.size(); j++) {
+                // add pair of objects to possible collision if their
+                // bounding boxes overlap
+                if (aabbs[i].testCollision(aabbs[j])) {
+                    m_overlappingBodys.push_back(std::make_pair(i, j));
+                }
+            }
+        }
+        break;
+    }
+	
+	case 2: {
+		// TODO: implement other broad phase algorithm
+		break;
+	}
+	}
+}
+
+void CollisionDetection::computeNarrowPhase(int narrowPhaseMethod) {
+    switch (narrowPhaseMethod) {
+    case 0: {
+        // exhaustive
+        // iterate through all pairs of possible collisions
+        for (auto overlap : m_overlappingBodys) {
+            std::vector<Contact> temp_contacts[2];
+            // compute intersection of a with b first and intersectino
+            // of b with a and store results in temp_contacts
+            for (int switcher = 0; switcher < 2; switcher++) {
+                RigidObject* a =
+                    &m_objects[(!switcher) ? overlap.first
+                                            : overlap.second];
+                RigidObject* b =
+                    &m_objects[(!switcher) ? overlap.second
+                                            : overlap.first];
+
+                Eigen::MatrixXd Va, Vb;
+                Eigen::MatrixXi Fa, Fb;
+                a->getMesh(Va, Fa);
+                b->getMesh(Vb, Fb);
+
+                // iterate through all faces of first object
+                for (int face = 0; face < Fa.rows(); face++) {
+                    // iterate through all edges of given face
+                    for (size_t j = 0; j < 3; j++) {
+                        int start = Fa(face, j);
+                        int end = Fa(face, (j + 1) % 3);
+
+                        // check if there is a collision
+                        ContactType ct = isColliding(
+                            Va.row(start), Va.row(end), Vb, Fb);
+
+                        // find collision and check for duplicates
+                        switch (ct) {
+                            case ContactType::VERTEXFACE: {
+                                auto ret = m_penetratingVertices.insert(
+                                    Fa(face, j));
+                                // if not already in set
+                                if (ret.second) {
+                                    Contact temp_col =
+                                        findVertexFaceCollision(
+                                            Va.row(Fa(face, j)), Vb,
+                                            Fb);
+                                    temp_col.a = a;
+                                    temp_col.b = b;
+                                    temp_col.type =
+                                        ContactType::VERTEXFACE;
+                                    temp_contacts[switcher].push_back(
+                                        temp_col);
+                                }
+                                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 not already in set
+                                if (ret.second) {
+                                    Contact temp_col =
+                                        findEdgeEdgeCollision(
+                                            Va.row(orderedStart),
+                                            Va.row(orderedEnd), Vb, Fb);
+                                    temp_col.a = a;
+                                    temp_col.b = b;
+                                    temp_col.type =
+                                        ContactType::EDGEEDGE;
+                                    temp_contacts[switcher].push_back(
+                                        temp_col);
+                                }
+                                break;
+                            }
+                            case ContactType::NONE: {
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+
+            // look for vertexFace
+            bool found = false;
+            for (int i = 0; i < 2; i++) {
+                for (auto cont : temp_contacts[i]) {
+                    if (cont.type == ContactType::VERTEXFACE) {
+                        m_contacts.push_back(cont);
+                        found = true;
+                        break;
+                    }
+                }
+                if (found) {
+                    break;
+                }
+            }
+            if (found) {
+                continue;
+            }
+
+            // take single edgeedge if possible
+            if (temp_contacts[0].size() > 0 &&
+                temp_contacts[0].size() < temp_contacts[1].size()) {
+                for (int i = 0; i < temp_contacts[0].size(); i++) {
+                    m_contacts.push_back(temp_contacts[0][i]);
+                }
+            } else if (temp_contacts[1].size() > 0 &&
+                        temp_contacts[0].size() >
+                            temp_contacts[1].size()) {
+                for (int i = 0; i < temp_contacts[1].size(); i++) {
+                    m_contacts.push_back(temp_contacts[1][i]);
+                }
+            } else if (temp_contacts[0].size() > 0) {
+                for (int i = 0; i < temp_contacts[0].size(); i++) {
+                    m_contacts.push_back(temp_contacts[0][i]);
+                }
+            } else if (temp_contacts[1].size() > 0) {
+                for (int i = 0; i < temp_contacts[1].size(); i++) {
+                    m_contacts.push_back(temp_contacts[1][i]);
+                }
+            }
+        }
+        break;
+    }
+
+	case 1: {
+		// TODO: implement other narrow phase algorithm
+		break;
+	}
+    }
+}
+
+void CollisionDetection::applyImpulse(double eps) {
+    // compute impulse for all contacts
+    for (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);
+        if (vrel > 0) {
+            // bodies are moving apart
+            continue;
+        }
+
+		// TODO: compute impulse and update the following momentums
+		//contact.a->setLinearMomentum();
+		//contact.b->setLinearMomentum();
+		//contact.a->setAngularMomentum();
+		//contact.b->setAngularMomentum();
+    }
+}
\ No newline at end of file

4_collision/CollisionDetection.h

+#ifndef COLLISIONDETECTION_H
+#define COLLISIONDETECTION_H
+
+#include <igl/ray_mesh_intersect.h>
+#include <set>
+#include <utility>
+#include <vector>
+#include "AABB.h"
+#include "RigidObject.h"
+
+enum class ContactType { VERTEXFACE, EDGEEDGE, NONE };
+
+struct Contact {
+    RigidObject* a;      // body containing vertex
+    RigidObject* b;      // body containing face
+    Eigen::Vector3d n;   // world-space vertex location
+    Eigen::Vector3d p;   // outwards pointing normal of face
+    Eigen::Vector3d ea;  // edge direction for A
+    Eigen::Vector3d eb;  // edge direction for B
+
+    ContactType type;  // type of contact
+};
+
+class CollisionDetection {
+   public:
+    CollisionDetection(std::vector<RigidObject>& world) : m_objects(world) {}
+
+    // pass objects in scene to collision detection
+    void setObjects(std::vector<RigidObject>& world) { m_objects = world; }
+
+	void computeBroadPhase(int broadPhaseMethod);
+
+    // test if ray from start towards end intersects object with vertices V and
+    // faces F
+    ContactType isColliding(const Eigen::Vector3d& start,
+                            const Eigen::Vector3d& end,
+                            const Eigen::MatrixXd& V,
+                            const Eigen::MatrixXi& F) {
+        std::vector<igl::Hit> hits;
+        igl::ray_mesh_intersect(start, end - start, V, F, hits);
+
+        // count number of intersections with object that happen between start
+        // and end (so along the edge)
+        int cntr = 0;
+        for (auto hit : hits) {
+            if (hit.t > 0 && hit.t <= 1) {
+                cntr++;
+            }
+        }
+
+        // if hits is odd then the ray enters through one face and
+        // does not leave again through another, hence the starting point is
+        // inside the object, if the number of intersections between start and
+        // end of edge are even, then the edge entering through one and leaving
+        // through another face, hence the edge is intersection the object
+        ContactType ret = ContactType::NONE;
+        if (hits.size() % 2 == 1) {
+            ret = ContactType::VERTEXFACE;
+        }
+        if (cntr % 2 == 0 && cntr > 0) {
+            ret = ContactType::EDGEEDGE;
+        }
+        return ret;
+    }
+
+    // compute normal for all faces and use it to find closesest face to
+    // given vertex
+    Contact findVertexFaceCollision(const Eigen::Vector3d& vertex,
+                                    const Eigen::MatrixXd& V,
+                                    const Eigen::MatrixXi& F) {
+        double minDist = std::numeric_limits<double>::infinity();
+        Eigen::Vector3d minNormal;
+        for (int i = 0; i < F.rows(); i++) {
+            Eigen::Vector3d a = V.row(F(i, 0));
+            Eigen::Vector3d b = V.row(F(i, 1));
+            Eigen::Vector3d c = V.row(F(i, 2));
+
+            Eigen::Vector3d n = -(b - a).cross(c - a).normalized();
+
+            Eigen::Vector3d v = vertex - a;
+
+            double distance = v.dot(n);
+            // if vertex inside
+            if (distance >= 0) {
+                if (distance < minDist) {
+                    minDist = distance;
+                    minNormal = -n;
+                }
+            }
+        }
+        Contact ret;
+        ret.n = minNormal;
+        ret.p = vertex + minDist * minNormal;
+        return ret;
+    }
+
+    // loop over edges of faces and compute closest to given edge
+    Contact findEdgeEdgeCollision(const Eigen::Vector3d& start,
+                                  const Eigen::Vector3d& end,
+                                  const Eigen::MatrixXd& V,
+                                  const Eigen::MatrixXi& F) {
+        double minDist = std::numeric_limits<double>::infinity();
+        Contact ret;
+        for (int i = 0; i < F.rows(); i++) {
+            Eigen::Vector3d a = V.row(F(i, 0));
+            Eigen::Vector3d b = V.row(F(i, 1));
+            Eigen::Vector3d c = V.row(F(i, 2));
+
+            Eigen::Vector3d n_face = -(b - a).cross(c - a).normalized();
+
+            for (int j = 0; j < 3; j++) {
+                Eigen::Vector3d s = V.row(F(i, j));
+                Eigen::Vector3d e = V.row(F(i, (j + 1) % 3));
+
+                Eigen::Vector3d ea = end - start;
+                Eigen::Vector3d eb = e - s;
+                Eigen::Vector3d n =
+                    (ea).cross(eb);  // direction of shortest distance
+                double distance = n.dot(start - s) / n.norm();
+
+                Eigen::Vector3d plane_normal = n.cross(eb).normalized();
+                double t =
+                    (s - start).dot(plane_normal) / (ea.dot(plane_normal));
+                if (n_face.dot(n) < 0 && distance < 0 && -distance < minDist &&
+                    t >= 0 && t <= 1) {
+                    ret.ea = ea;
+                    ret.eb = eb;
+                    ret.n = n;
+                    ret.p = start + t * ea;
+                    minDist = -distance;
+                }
+            }
+        }
+        return ret;
+    }
+
+	void computeNarrowPhase(int narrowPhaseMethod);
+
+	void applyImpulse(double eps = 1.0);
+
+    void clearDataStructures() {
+        m_penetratingEdges.clear();
+        m_penetratingVertices.clear();
+        m_overlappingBodys.clear();
+        m_contacts.clear();
+    }
+
+    void computeCollisionDetection(int broadPhaseMethod = 0,
+                                   int narrowPhaseMethod = 0,
+                                   double eps = 1.0) {
+        clearDataStructures();
+
+        computeBroadPhase(broadPhaseMethod);
+
+        computeNarrowPhase(narrowPhaseMethod);
+
+        applyImpulse(eps);
+    }
+
+    std::vector<Contact> getContacts() { return m_contacts; }
+
+    std::vector<RigidObject>& m_objects;  // all objects in scene
+    // 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;
+};
+
+#endif

4_collision/CollisionSim.h

+#include "CollisionDetection.h"
+#include "Simulation.h"
+
+#include <deque>
+
+using namespace std;
+
+/*
+ */
+class CollisionSim : public Simulation {
+   public:
+    CollisionSim() : Simulation(), m_collisionDetection(m_objects) { init(); }
+
+    const int NUM_CUBES = 5;  // complexity of scene
+
+    virtual void init() override {
+        std::string path = "cube.off";
+        for (int i = 0; i < NUM_CUBES; i++) {
+            m_objects.push_back(RigidObject(path));
+        }
+		path = "cube_shallow.off";
+		for (int i = 0; i < 5; ++i) {
+			m_objects.push_back(RigidObject(path));
+		}
+		
+        m_collisionDetection.setObjects(m_objects);
+
+        m_dt = 1e-3 * 3;
+        m_gravity << 0, -9.81, 0;
+        m_mass = 1.0;
+        m_showContacts = false;
+        m_broadPhaseMethod = 0;
+        m_narrowPhaseMethod = 0;
+        m_eps = 1.0;
+
+        reset();
+    }
+
+    virtual void resetMembers() override {
+        for (auto &o : m_objects) {
+            o.reset();
+        }
+
+		double x1 = 5;
+		double y1 = 4;
+		m_objects[0].setPosition(Eigen::Vector3d(0, y1, 0));
+		m_objects[0].setRotation(
+			Eigen::Quaterniond(0, -0.3444844, -0.3444844, -0.8733046)); 
+		
+		Eigen::RowVector3d c0(204.0 / 255.0, 0, 0);
+		Eigen::RowVector3d c1(0, 128.0 / 255.0, 204.0 / 255.0);
+		for (int i = 1; i < NUM_CUBES; ++i) {
+			m_objects[i].setPosition(Eigen::Vector3d(x1, y1*(i+1), 0));
+			double a = double(i+1) / NUM_CUBES;
+			m_objects[i].setColors(c0*a + c1*(1 - a));
+		}
+
+        for (size_t i = 0; i < NUM_CUBES; i++) {
+            m_objects[i].setMass(m_mass);
+        }
+
+		for (int i = 0; i < 5; ++i) {
+			m_objects[i + NUM_CUBES].setScale(10);
+			m_objects[i + NUM_CUBES].setType(ObjType::STATIC);
+			m_objects[i + NUM_CUBES].setColors(Eigen::RowVector3d(0.5, 0.5, 0.5));
+			m_objects[i + NUM_CUBES].setMass(std::numeric_limits<double>::max());
+		}
+
+		m_objects[0 + NUM_CUBES].setPosition(
+			Eigen::Vector3d(0, -0.1, 0));
+
+		m_objects[1 + NUM_CUBES].setPosition(
+			Eigen::Vector3d(-10.1, 10, 0));
+		m_objects[1 + NUM_CUBES].setRotation(
+			Eigen::Quaterniond(0.7071, 0, 0, 0.7071));
+
+		m_objects[2 + NUM_CUBES].setPosition(
+			Eigen::Vector3d(10.1, 10, 0));
+		m_objects[2 + NUM_CUBES].setRotation(
+			Eigen::Quaterniond(0.7071, 0, 0, 0.7071));
+
+		m_objects[3 + NUM_CUBES].setPosition(
+			Eigen::Vector3d(0, 10, -10.1));
+		m_objects[3 + NUM_CUBES].setRotation(
+			Eigen::Quaterniond(0.5, 0.5, 0.5, 0.5));
+
+		m_objects[4 + NUM_CUBES].setPosition(
+			Eigen::Vector3d(0, 10, 10.1));
+		m_objects[4 + NUM_CUBES].setRotation(
+			Eigen::Quaterniond(0.5, 0.5, 0.5, 0.5));
+
+
+        updateVars();
+    }
+
+    virtual void updateRenderGeometry() override {
+        for (size_t i = 0; i < m_objects.size(); i++) {
+            RigidObject &o = m_objects[i];
+            if (o.getID() < 0) {
+                m_renderVs.emplace_back();
+                m_renderFs.emplace_back();
+            }
+
+            m_objects[i].getMesh(m_renderVs[i], m_renderFs[i]);
+        }
+    }
+
+    virtual bool advance() override {
+        // compute the collision detection
+        m_collisionDetection.computeCollisionDetection(m_broadPhaseMethod, m_narrowPhaseMethod, m_eps);
+
+        // apply forces (only gravity in this case)
+        for (auto &o : m_objects) {
+            o.applyForceToCOM(m_gravity);
+        }
+
+        for (auto &o : m_objects) {
+            // integrate velocities
+            o.setLinearMomentum(o.getLinearMomentum() + m_dt * o.getForce());
+            o.setAngularMomentum(o.getAngularMomentum() + m_dt * o.getTorque());
+            o.resetForce();
+            o.resetTorque();
+
+            // angular velocity (matrix)
+            Eigen::Vector3d w = o.getAngularVelocity();
+            Eigen::Quaterniond wq;
+            wq.w() = 0;
+            wq.vec() = w;
+
+            // integrate position and rotation
+            o.setPosition(o.getPosition() + m_dt * o.getLinearVelocity());
+            Eigen::Quaterniond q = o.getRotation();
+            Eigen::Quaterniond dq = wq * q;
+            Eigen::Quaterniond new_q;
+            new_q.w() = q.w() + 0.5 * m_dt * dq.w();
+            new_q.vec() = q.vec() + 0.5 * m_dt * dq.vec();
+            o.setRotation(new_q.normalized());
+        }
+        // advance time
+        m_time += m_dt;
+        m_step++;
+
+        return false;
+    }
+
+    virtual void renderRenderGeometry(
+        igl::opengl::glfw::Viewer &viewer) override {
+        for (size_t i = 0; i < m_objects.size(); i++) {
+            RigidObject &o = m_objects[i];
+            if (o.getID() < 0) {
+                int new_id = 0;
+                if (i > 0) {
+                    new_id = viewer.append_mesh();
+                    o.setID(new_id);
+                } else {
+                    o.setID(new_id);
+                }
+
+                size_t meshIndex = viewer.mesh_index(o.getID());
+				if (i >= NUM_CUBES) {
+					viewer.data_list[meshIndex].show_lines = true;
+					viewer.data_list[meshIndex].show_faces = false;
+				}
+				else {
+					viewer.data_list[meshIndex].show_lines = false;
+				}
+                viewer.data_list[meshIndex].set_face_based(true);
+                viewer.data_list[meshIndex].point_size = 2.0f;
+                viewer.data_list[meshIndex].clear();
+            }
+            size_t meshIndex = viewer.mesh_index(o.getID());
+
+            viewer.data_list[meshIndex].set_mesh(m_renderVs[i], m_renderFs[i]);
+            viewer.data_list[meshIndex].compute_normals();
+
+            Eigen::MatrixXd color;
+            o.getColors(color);
+            viewer.data_list[meshIndex].set_colors(color);
+        }
+
+        if (m_showContacts) {
+            // number of timesteps to keep showing collision
+            int delay = 10;
+
+            // clear old points
+            viewer.data_list[1].points = Eigen::MatrixXd(0, 6);
+            viewer.data_list[1].point_size = 10.0f;
+
+            // remove expired points
+            while (m_contactMemory.size() > 0 &&
+                   m_contactMemory.front().second + delay < m_step) {
+                m_contactMemory.pop_front();
+            }
+
+            // get new points and add them to memory
+            auto contacts = m_collisionDetection.getContacts();
+            for (auto &contact : contacts) {
+                m_contactMemory.push_back(std::make_pair(contact, m_step));
+            }
+
+            // show points
+            for (auto &contact_int_p : m_contactMemory) {
+                viewer.data_list[1].add_points(
+                    contact_int_p.first.p.transpose(),
+                    (contact_int_p.first.type == ContactType::EDGEEDGE)
+                        ? Eigen::RowVector3d(0, 1, 0)
+                        : Eigen::RowVector3d(0, 0, 1));
+            }
+        }
+    }
+
+#pragma region SettersAndGetters
+    /*
+     * Compute magnitude and direction of momentum and apply it to o
+     */
+    void updateVars() {
+        Eigen::Vector3d momentum;
+        momentum << std::sin(m_angle), std::cos(m_angle), 0;
+        momentum *= m_force;
+        m_objects[0].setLinearMomentum(momentum);
+    }
+
+    void setAngle(double a) {
+        m_angle = a;
+        updateVars();
+    }
+
+    void setForce(double f) {
+        m_force = f;
+        updateVars();
+    }
+
+    void setMass(double m) { m_mass = m; }
+
+    void showContacts(bool s) {
+        if (!s) {
+            m_contactMemory.clear();
+        }
+        m_showContacts = s;
+    }
+
+    void setBroadPhaseMethod(int m) { m_broadPhaseMethod = m; }
+    void setNarrowPhaseMethod(int m) { m_narrowPhaseMethod = m; }
+
+    void setEps(double eps) { m_eps = eps; }
+
+    Eigen::Vector3d getKineticEnergy() {
+        Eigen::Vector3d res;
+        res.setZero();
+        for (auto o : m_objects) {
+            if (o.getType() == ObjType::STATIC) continue;
+            Eigen::Vector3d rotE = 0.5 * o.getInertia().diagonal().cwiseProduct(
+                                             o.getAngularVelocity());
+            Eigen::Vector3d kinE =
+                0.5 * o.getMass() * o.getLinearVelocity().array().square();
+            res += rotE + kinE;
+        }
+        return res;
+    }
+
+    Eigen::Vector3d getLinearMomentum() {
+        Eigen::Vector3d res;
+        res.setZero();
+        for (auto o : m_objects) {
+            if (o.getType() == ObjType::STATIC) continue;
+            res += o.getLinearMomentum();
+        }
+        return res;
+    }
+
+    Eigen::Vector3d getAngularMomentum() {
+        Eigen::Vector3d res;
+        res.setZero();
+        for (auto o : m_objects) {
+            if (o.getType() == ObjType::STATIC) continue;
+            res += o.getAngularMomentum();
+        }
+        return res;
+    }
+
+#pragma endregion SettersAndGetters
+
+   private:
+    [[maybe_unused]] int m_method;  // id of integrator to be used (0: analytical, 1: explicit
+                   // euler, 2: semi-implicit euler)
+    double m_angle;
+    double m_force;
+    double m_mass;
+
+    Eigen::Vector3d m_gravity;
+
+    CollisionDetection m_collisionDetection;
+    int m_broadPhaseMethod;
+    int m_narrowPhaseMethod;
+    double m_eps;
+
+    std::vector<Eigen::MatrixXd> m_renderVs;  // vertex positions for rendering
+    std::vector<Eigen::MatrixXi> m_renderFs;  // face indices for rendering
+
+    bool m_showContacts;
+    std::deque<std::pair<Contact, int>> m_contactMemory;
+};
\ No newline at end of file

4_collision/main.cpp

+#include <igl/writeOFF.h>
+#include "CollisionSim.h"
+#include "Gui.h"
+
+class CollisionGui : public Gui {
+   public:
+    float m_angle = 1.047f;
+    float m_force = 10.0f;
+    float m_dt = 1e-1 * 3;
+    float m_mass = 1.0;
+    bool m_showContacts = false;
+    bool m_useAABB = false;
+    float m_eps = 0.6;
+
+    int m_maxHistory = 200;
+    std::vector<float> m_energy_history;
+
+    int m_selectedBroadPhase = 0;
+    const std::vector<char const *> m_broadphases = {"None", "AABB", "Own"};
+    int m_selectedNarrowPhase = 0;
+    const std::vector<char const *> m_narrowphases = {"Exhaustive", "Own"};
+
+    CollisionSim *p_CollisionSim = NULL;
+
+    CollisionGui() {
+        p_CollisionSim = new CollisionSim();
+        setSimulation(p_CollisionSim);
+
+        // show vertex velocity instead of normal
+        callback_clicked_vertex = [&](int clickedVertexIndex,
+                                      int clickedObjectIndex,
+                                      Eigen::Vector3d &pos,
+                                      Eigen::Vector3d &dir) {
+            RigidObject &o = p_CollisionSim->getObjects()[clickedObjectIndex];
+            pos = o.getVertexPosition(clickedVertexIndex);
+            dir = o.getVelocity(pos);
+        };
+        start();
+    }
+
+    virtual void updateSimulationParameters() override {
+        p_CollisionSim->setForce(m_force);
+        p_CollisionSim->setAngle(m_angle);
+        p_CollisionSim->setTimestep(m_dt);
+        p_CollisionSim->setMass(m_mass);
+        p_CollisionSim->setEps(m_eps);
+    }
+
+    virtual void clearSimulation() override {
+        p_CollisionSim->showContacts(false);
+        p_CollisionSim->showContacts(m_showContacts);
+    }
+
+    virtual void drawSimulationParameterMenu() override {
+        ImGui::SliderAngle("Angle", &m_angle, -180.0f, 180.0f);
+        ImGui::InputFloat("Force", &m_force, 0, 0);
+        ImGui::InputFloat("Mass", &m_mass, 0, 0);
+        ImGui::InputFloat("dt", &m_dt, 0, 0);
+        if (ImGui::Checkbox("Show contacts", &m_showContacts)) {
+            p_CollisionSim->showContacts(m_showContacts);
+        }
+        if (ImGui::Combo("Broadphase", &m_selectedBroadPhase,
+                         m_broadphases.data(), m_broadphases.size())) {
+            p_CollisionSim->setBroadPhaseMethod(m_selectedBroadPhase);
+        }
+        if (ImGui::Combo("Narrowphase", &m_selectedNarrowPhase,
+                         m_narrowphases.data(), m_narrowphases.size())) {
+            p_CollisionSim->setNarrowPhaseMethod(m_selectedNarrowPhase);
+        }
+        ImGui::InputFloat("eps", &m_eps, 0, 0);
+    }
+
+    virtual void drawSimulationStats() override {
+        Eigen::Vector3d E = p_CollisionSim->getKineticEnergy();
+        m_energy_history.push_back(E.cast<float>().cwiseAbs().sum());
+        if (m_energy_history.size() > m_maxHistory)
+            m_energy_history.erase(m_energy_history.begin(),
+                                   m_energy_history.begin() + 1);
+        ImGui::Text("E: %.3f, %.3f, %.3f", E(0), E(1), E(2));
+        ImGui::PlotLines("Total Energy", &m_energy_history[0],
+                         m_energy_history.size(), 0, NULL, 0, 1000,
+                         ImVec2(0, 200));
+        Eigen::Vector3d p = p_CollisionSim->getLinearMomentum();
+        ImGui::Text("M: %.3f, %.3f, %.3f", p(0), p(1), p(2));
+        Eigen::Vector3d l = p_CollisionSim->getAngularMomentum();
+        ImGui::Text("L: %.3f, %.3f, %.3f", l(0), l(1), l(2));
+    }
+};
+
+int main(int argc, char *argv[]) {
+    new CollisionGui();
+
+    return 0;
+}
\ No newline at end of file