From f1356a59fc0c22e555b936ae4c6841ad154a5ed7 Mon Sep 17 00:00:00 2001
From: Yuwei Xiao <xyw1105@126.com>
Date: Mon, 6 Apr 2020 16:45:42 +0800
Subject: [PATCH] ex4
---
4_collision/CMakeLists.txt | 43 ++++
4_collision/CollisionDetection.cpp | 191 ++++++++++++++++++
4_collision/CollisionDetection.h | 176 +++++++++++++++++
4_collision/CollisionSim.h | 302 +++++++++++++++++++++++++++++
4_collision/main.cpp | 94 +++++++++
5 files changed, 806 insertions(+)
create mode 100644 4_collision/CMakeLists.txt
create mode 100644 4_collision/CollisionDetection.cpp
create mode 100644 4_collision/CollisionDetection.h
create mode 100644 4_collision/CollisionSim.h
create mode 100644 4_collision/main.cpp
diff --git a/4_collision/CMakeLists.txt b/4_collision/CMakeLists.txt
new file mode 100644
index 0000000..f2a3b20
--- /dev/null
+++ b/4_collision/CMakeLists.txt
@@ -0,0 +1,43 @@
+
+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
diff --git a/4_collision/CollisionDetection.cpp b/4_collision/CollisionDetection.cpp
new file mode 100644
index 0000000..91cc8eb
--- /dev/null
+++ b/4_collision/CollisionDetection.cpp
@@ -0,0 +1,191 @@
+#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
diff --git a/4_collision/CollisionDetection.h b/4_collision/CollisionDetection.h
new file mode 100644
index 0000000..2a667cd
--- /dev/null
+++ b/4_collision/CollisionDetection.h
@@ -0,0 +1,176 @@
+#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
diff --git a/4_collision/CollisionSim.h b/4_collision/CollisionSim.h
new file mode 100644
index 0000000..38ffc18
--- /dev/null
+++ b/4_collision/CollisionSim.h
@@ -0,0 +1,302 @@
+#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
diff --git a/4_collision/main.cpp b/4_collision/main.cpp
new file mode 100644
index 0000000..e120f39
--- /dev/null
+++ b/4_collision/main.cpp
@@ -0,0 +1,94 @@
+#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
--
GitLab