From 8ee194a7919ea28b7c793878cf33e953b3355a30 Mon Sep 17 00:00:00 2001
From: Ivan Mahonin <bh@icystar.com>
Date: Nov 27 2019 15:51:06 +0000
Subject: collider


---

diff --git a/collider.cpp b/collider.cpp
new file mode 100644
index 0000000..1588c86
--- /dev/null
+++ b/collider.cpp
@@ -0,0 +1,43 @@
+
+
+#include "collider.h"
+
+
+Real Collider::distance_to_triangle(const Triangle &triangle, const Vector3 &pos, const Vector3 &dir) {
+    const Vector3 &n = triangle.normal;
+    const Vector3 &a = triangle.vertices[0];
+    const Vector3 &b = triangle.vertices[1];
+    const Vector3 &c = triangle.vertices[2];
+    
+    Real d = n * dir;
+    if (fabs(d) < 1e-5) return INFINITY;
+    if ((b - a).len() < 1e-5) return INFINITY;
+    if ((c - b).len() < 1e-5) return INFINITY;
+    if ((a - c).len() < 1e-5) return INFINITY;
+
+    d = 1/d;
+    //Real angle = fabs(atan(d));
+    Real l = (a - pos)*n*d;
+    
+    Vector3 p = pos + dir*l;
+    //Vector3 dp = n.cross(dir.cross(n));
+    
+    //Real th = 0, ch = 0, co = 0;
+    //tool.calc_collision(angle, th, ch, co);
+    //Real dist = sqrt(ch*ch + co*co);
+
+    Vector3 pp = p;// + dp*dist;
+    if (n.cross(b - a)*(pp - a) < 0) return INFINITY;
+    if (n.cross(c - b)*(pp - b) < 0) return INFINITY;
+    if (n.cross(a - c)*(pp - c) < 0) return INFINITY;
+    
+    return l;// - th;
+}
+
+Real Collider::distance_to_model(const Vector3 &pos, const Vector3 &dir) {
+    Real distance = INFINITY;
+    for(TriangleList::const_iterator i = model.triangles.begin(); i != model.triangles.end(); ++i)
+        distance = std::min(distance, distance_to_triangle(*i, pos, dir));
+    return distance;
+}
+
diff --git a/collider.h b/collider.h
new file mode 100644
index 0000000..36273da
--- /dev/null
+++ b/collider.h
@@ -0,0 +1,25 @@
+
+#ifndef COLLIDER_H
+#define COLLIDER_H
+
+
+#include "model.h"
+#include "tool.h"
+
+
+class Collider {
+public:
+    const Model &model;
+    const Tool &tool;
+
+    Collider(const Model &model, const Tool &tool):
+        model(model), tool(tool) { }
+    
+    Real distance_to_triangle(const Triangle &triangle, const Vector3 &pos, const Vector3 &dir);
+    Real distance_to_model(const Vector3 &pos, const Vector3 &dir);
+};
+
+
+#endif
+
+
diff --git a/geometry.h b/geometry.h
index 28f4e9d..7ff2c95 100644
--- a/geometry.h
+++ b/geometry.h
@@ -34,6 +34,22 @@ public:
     
     explicit Vector3(Real x = 0, Real y = 0, Real z = 0):
         x(x), y(y), z(z) { }
+    
+    Vector3 operator+(const Vector3 &v) const
+        { return Vector3(x+v.x, y+v.y, z+v.z); }
+    Vector3 operator-(const Vector3 &v) const
+        { return Vector3(x-v.x, y-v.y, z-v.z); }
+
+    Real operator*(const Vector3 &v) const
+        { return x*v.x + y*v.y + z*v.z; }
+    Vector3 operator*(Real k) const
+        { return Vector3(x*k, y*k, z*k); }
+
+    Vector3 cross(const Vector3 &v) const
+        { return Vector3(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x); }
+
+    Real len_sqr() const { return x*x + y*y + z*z; }
+    Real len() const { return sqrt(len_sqr()); }
 };
 
 
diff --git a/loader.cpp b/loader.cpp
index 0bee2d5..5f8af6b 100644
--- a/loader.cpp
+++ b/loader.cpp
@@ -6,7 +6,9 @@
 #include "loader.h"
 
 
-class Loader::Internal {
+namespace {
+    
+class LoaderTrack {
 public:
     Track &track;
     const std::string &filename;
@@ -19,7 +21,7 @@ public:
     bool newline;
     TrackPoint point;
     
-    Internal(Track &track, const std::string &filename):
+    LoaderTrack(Track &track, const std::string &filename):
         track(track),
         filename(filename),
         row(1),
@@ -150,10 +152,87 @@ public:
         std::cout << "loaded: " << filename << std::endl;
         return true;
     }
-};
+}; // LoaderTrack
+
+
+class LoaderModel {
+public:
+    Model &model;
+    const std::string &filename;
+    
+    FILE *f;
+    
+    LoaderModel(Model &model, const std::string &filename):
+        model(model),
+        filename(filename),
+        f()
+    { }
+
+    void error(const std::string &msg) {
+        std::cerr << "Error while loading model: " << filename
+                  << ": offset: " << (f ? ftell(f) : 0)
+                  << ": " << msg
+                  << std::endl;
+    }
+
+    template<typename T>
+    bool read(T &data) {
+        if (fread(&data, 1, sizeof(data), f) != sizeof(data)) {
+            error("fread failed");
+            return false;
+        }
+        return true;
+    }
+    
+    bool load() {
+        model.triangles.clear();
+
+        std::cout << "loading: " << filename << std::endl;
+        
+        f = fopen(filename.c_str(), "rb");
+        if (!f) {
+            error("cannot open file");
+            return false;
+        }
+        
+        char header[80] = {};
+        unsigned int count = 0;
+        float triangle_data[3+3*3] = {};
+        unsigned short attribute = 0;
+        Triangle t;
+        
+        bool success = false;
+        if (read(header) && read(count)) {
+            success = true;
+            for(unsigned int i = 0; i < count; ++i) {
+                if (!read(triangle_data) || !read(attribute))
+                    { success = false; break; }
+                for(int i = 0; i < 3; ++i) {
+                    t.normal.c[i] = triangle_data[i];
+                    for(int j = 0; j < 3; ++j)
+                        t.vertices[j].c[i] = triangle_data[3 + j*3 + i];
+                }
+                model.triangles.push_back(t);
+            }
+        }
+        
+        fclose(f);
+        
+        model.transform(Vector3(1000, 1000, 1000));
+
+        std::cout << "loaded: " << filename << std::endl;
+        return success;
+    }
+}; // LoaderModel
+
+} // namespace
 
 
 bool Loader::load(Track &track, const std::string &filename) {
-    return Internal(track, filename).load();
+    return LoaderTrack(track, filename).load();
+}
+
+bool Loader::load(Model &model, const std::string &filename) {
+    return LoaderModel(model, filename).load();
 }
 
diff --git a/loader.h b/loader.h
index a0c42be..0375d57 100644
--- a/loader.h
+++ b/loader.h
@@ -4,13 +4,13 @@
 
 
 #include "track.h"
+#include "model.h"
 
 
 class Loader {
-private:
-    class Internal;
 public:
     static bool load(Track &track, const std::string &filename);
+    static bool load(Model &model, const std::string &filename);
 };
 
 
diff --git a/main.cpp b/main.cpp
index d545c65..57a5a97 100644
--- a/main.cpp
+++ b/main.cpp
@@ -10,6 +10,8 @@
 #include "simulator.h"
 #include "tool.h"
 #include "loader.h"
+#include "collider.h"
+
 
 class Main {
 public:
@@ -75,10 +77,10 @@ public:
 
         glEnable(GL_LIGHT1);
         float light1[] = {-1, -1, -1, 0};
-        float color1[] = {1, 1, 1, 1};
+        float color[] = {1, 1, 1, 1};
         glLightfv(GL_LIGHT1, GL_POSITION, light1);
-        glLightfv(GL_LIGHT1, GL_DIFFUSE, color1);
-        glLightfv(GL_LIGHT1, GL_SPECULAR, color1);
+        glLightfv(GL_LIGHT1, GL_DIFFUSE, color);
+        glLightfv(GL_LIGHT1, GL_SPECULAR, color);
         
         glEnable(GL_COLOR_MATERIAL);
         
@@ -102,7 +104,17 @@ public:
         glMatrixMode(GL_MODELVIEW);
     }
     
+    void update_collider() {
+        if (scene->collider) {
+            Real l = scene->collider->distance_to_model(scene->tool_pos, scene->tool_dir);
+            if (!isinf(l))
+                scene->tool_pos = scene->tool_pos + scene->tool_dir*l;
+        }
+    }
+    
     void loop() {
+        update_collider();
+        
         bool quit = false;
         Uint32 prev_time = SDL_GetTicks();
         while(!quit) {
@@ -120,9 +132,21 @@ public:
                     case SDLK_ESCAPE:
                         quit = true;
                         break;
-                    case SDLK_r:
-                    case SDLK_g:
-                    case SDLK_b:
+                    case SDLK_w:
+                        scene->tool_pos.y += 0.5;
+                        update_collider();
+                        break;
+                    case SDLK_s:
+                        scene->tool_pos.y -= 0.5;
+                        update_collider();
+                        break;
+                    case SDLK_a:
+                        scene->tool_pos.x -= 0.5;
+                        update_collider();
+                        break;
+                    case SDLK_d:
+                        scene->tool_pos.x += 0.5;
+                        update_collider();
                         break;
                     default:
                         break;
@@ -161,12 +185,30 @@ int main(int argc, char **argv)
 {
     Main app;
     if (!app.init()) return -1;
+    
+    Model model;
     SimulatorXYZA simulator_xyza(400, 360);
-    if (argc > 1) {
+    FlatTool tool(1.5);
+    Collider collider(model, tool);
+    
+    Vector3 dir(1, 3, -3);
+    dir = dir*(1/dir.len());
+    
+    if (argc > 1 && std::string(argv[1]) != ".") {
         Loader::load(app.scene->track, argv[1]);
         app.scene->simulator = &simulator_xyza;
-        app.scene->simulator->simulate(app.scene->track, FlatTool(1.5));
+        app.scene->simulator->simulate(app.scene->track, tool);
     }
+
+    if (argc > 2 && std::string(argv[2]) != ".") {
+        Loader::load(model, argv[2]);
+        app.scene->model = &model;
+    }
+    
+    app.scene->tool = &tool;
+    app.scene->tool_dir = dir;
+    app.scene->collider = &collider;
+    
     app.loop();
     return 0;
 }
diff --git a/model.cpp b/model.cpp
new file mode 100644
index 0000000..755c4dc
--- /dev/null
+++ b/model.cpp
@@ -0,0 +1,38 @@
+
+#include <GL/gl.h>
+
+#include "model.h"
+
+
+void Model::transform(const Vector3 &scale, const Vector3 &move) {
+    for(TriangleList::iterator i = triangles.begin(); i != triangles.end(); ++i) {
+        for(int j = 0; j < 3; ++j) {
+            i->normal.c[j] *= scale.c[j];
+            for(int k = 0; k < 3; ++k) {
+                i->vertices[k].c[j] *= scale.c[j];
+                i->vertices[k].c[j] += move.c[j];
+            }
+        }
+        Real nl = i->normal.len();
+        nl = nl > 1e-5 ? 1/nl : 0.0;
+        i->normal = i->normal * nl;
+    }
+}
+
+
+void Model::draw() const {
+    glEnable(GL_LIGHTING);
+    
+    glColor4dv(color.c);
+    glBegin(GL_TRIANGLES);
+    for(TriangleList::const_iterator i = triangles.begin(); i != triangles.end(); ++i) {
+        glNormal3dv(i->normal.c);
+        glVertex3dv(i->vertices[0].c);
+        glVertex3dv(i->vertices[1].c);
+        glVertex3dv(i->vertices[2].c);
+    }
+    glEnd();
+
+    glDisable(GL_LIGHTING);
+}
+
diff --git a/model.h b/model.h
new file mode 100644
index 0000000..be6d1de
--- /dev/null
+++ b/model.h
@@ -0,0 +1,32 @@
+
+#ifndef MODEL_H
+#define MODEL_H
+
+
+#include <vector>
+
+#include "geometry.h"
+
+
+class Triangle {
+public:
+    Vector3 vertices[3];
+    Vector3 normal;
+};
+
+typedef std::vector<Triangle> TriangleList;
+
+class Model {
+public:
+    Vector4 color;
+    TriangleList triangles;
+    
+    Model(const Vector4 &color = Vector4(0.5, 0.5, 0.5, 0.5)):
+        color(color) { }
+    
+    void transform(const Vector3 &scale, const Vector3 &move = Vector3());
+    void draw() const;
+};
+
+#endif
+
diff --git a/scene.cpp b/scene.cpp
index bbb3fa2..b891fa0 100644
--- a/scene.cpp
+++ b/scene.cpp
@@ -5,6 +5,7 @@
 
 #include "scene.h"
 #include "simulator.h"
+#include "model.h"
 
 
 Scene::Scene():
@@ -15,7 +16,11 @@ Scene::Scene():
     time_forward(0.0),
     time_backward(20.0),
     time_speed(30.0),
-    simulator()
+    model(),
+    simulator(),
+    tool_dir(0, 0, -1),
+    tool(),
+    collider()
 { }
 
 Scene::~Scene() { }
@@ -156,7 +161,9 @@ void Scene::draw() {
 
     draw_stars();
     draw_axes();
+    if (model) model->draw();
     if (simulator) simulator->draw();
+    if (tool) tool->draw(tool_pos, tool_dir);
     draw_track();
     
     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
diff --git a/scene.h b/scene.h
index 1beae7f..3534fdd 100644
--- a/scene.h
+++ b/scene.h
@@ -5,7 +5,10 @@
 
 #include "track.h"
 
+class Model;
 class Simulator;
+class Tool;
+class Collider;
 
 class Scene {
 public:
@@ -19,8 +22,15 @@ public:
     Real time_backward;
     Real time_speed;
     
+    Model *model;
     Simulator *simulator;
     
+    Vector3 tool_pos;
+    Vector3 tool_dir;
+    Tool *tool;
+    
+    Collider *collider;
+    
     Scene();
     ~Scene();
     
diff --git a/tool.cpp b/tool.cpp
index 49d7d58..5d8ff1e 100644
--- a/tool.cpp
+++ b/tool.cpp
@@ -1,3 +1,57 @@
 
+#include <GL/gl.h>
+
 #include "tool.h"
 
+
+void FlatTool::draw(const Vector3 &pos, const Vector3 &dir) const {
+    const int segments = 16;
+    const Real len = 10*radius;
+
+    glPushMatrix();
+    glEnable(GL_NORMALIZE);
+    glEnable(GL_LIGHTING);
+    
+    glTranslated(pos.x, pos.y, pos.z);
+    Vector3 vx = dir.cross(Vector3(dir.y, dir.z, dir.x));
+    vx = vx*(1/vx.len());
+    Vector3 vy = vx.cross(dir);
+    
+    Real matrix[16] = {
+        vx.x,  vx.y,  vx.z,  0,
+        vy.x,  vy.y,  vy.z,  0,
+        dir.x, dir.y, dir.z, 0,
+        0, 0, 0, 1 };
+    glMultMatrixd(matrix);
+
+    glBegin(GL_TRIANGLE_STRIP);
+    for(int i = 0; i <= segments; ++i) {
+        Real a = 2*M_PI*i/segments;
+        Real c = cos(a);
+        Real s = sin(a);
+        glNormal3b(c, s, 0);
+        glVertex3d(c, s, 0);
+        glVertex3d(c, s, -len);
+    }
+    glEnd();
+
+    glBegin(GL_TRIANGLE_FAN);
+    glNormal3b(0, 0, 1);
+    for(int i = 0; i < segments; ++i) {
+        Real a = 2*M_PI*i/segments;
+        glVertex3d(cos(a), sin(a), 0);
+    }
+    glEnd();
+    
+    glBegin(GL_TRIANGLE_FAN);
+    glNormal3b(0, 0, -1);
+    for(int i = 0; i < segments; ++i) {
+        Real a = 2*M_PI*i/segments;
+        glVertex3d(cos(a), sin(a), -len);
+    }
+    glEnd();
+    
+    glDisable(GL_LIGHTING);
+    glDisable(GL_NORMALIZE);
+    glPopMatrix();
+}
diff --git a/tool.h b/tool.h
index 14d39cd..b506c07 100644
--- a/tool.h
+++ b/tool.h
@@ -13,12 +13,21 @@ public:
     virtual ~Tool() { }
     virtual Real get_radius() const = 0; // must be >= 0
     virtual Real get_height(Real offset) const = 0; // must be >= 0
+
+    virtual void calc_collision(
+        Real plane_angle,
+        Real &tool_height,
+        Real &collision_height,
+        Real &collision_offset ) const = 0;
+    
     virtual Real get_height_polar(Real distance, Real offset, Real angle) const { // must be >= distance
         // not exact solution, only for small angles
         if (distance < 1e-5) return 1e-5;
         Vector2 v(offset, tan(angle)*distance);
         return sqrt(distance*distance + v.y*v.y) + get_height(v.len());
     }
+    
+    virtual void draw(const Vector3 &pos, const Vector3 &dir) const = 0;
 };
 
 
@@ -33,6 +42,28 @@ public:
         { return radius; }
     Real get_height(Real offset) const  override
         { return fabs(offset) <= fabs(radius) ? 0 : INFINITY; }
+
+    void calc_collision(
+        Real plane_angle,
+        Real &tool_height,
+        Real &collision_height,
+        Real &collision_offset
+    ) const override
+    {
+        tool_height = 0;
+        collision_height = 0;
+        collision_offset = 0;
+        return;
+        
+        Real a = fabs(0.5*M_PI - plane_angle);
+        if (a >= 0.5*M_PI - 1e-5) return;
+        Real t = tan(a);
+        if (t <= 1e-5) return;
+
+        tool_height = collision_height = radius/t;
+        collision_offset = radius;
+    }
+        
     Real get_height_polar(Real distance, Real offset, Real angle) const override {
         if (distance < 1e-5) return 1e-5;
         if (!(fabs(offset) <= fabs(radius))) return INFINITY;
@@ -40,6 +71,8 @@ public:
         Real r2 = radius*radius - offset*offset;
         return t2 <= r2 ? sqrt(distance*distance + t2) : INFINITY;
     }
+
+    void draw(const Vector3 &pos, const Vector3 &dir) const override;
 };