rotation support

7 files changed, 115 insertions, 36 deletions

diff --git a/include/llmr/map/map.hpp b/include/llmr/map/map.hpp
index 7940eb32c1..bb3b288aa0 100644
--- a/include/llmr/map/map.hpp
+++ b/include/llmr/map/map.hpp
@@ -21,6 +21,7 @@ public:
     bool render();
     void moveBy(double dx, double dy);
     void scaleBy(double ds, double cx, double cy);
+    void rotateBy(double cx, double cy, double dx, double dy);
     void tileLoaded(tile *tile);
     void tileFailed(tile *tile);
 
diff --git a/include/llmr/map/transform.hpp b/include/llmr/map/transform.hpp
index 57d5379bfc..249ce9721e 100644
--- a/include/llmr/map/transform.hpp
+++ b/include/llmr/map/transform.hpp
@@ -14,9 +14,11 @@ public:
     // Relative changes
     void moveBy(double dx, double dy);
     void scaleBy(double ds, double cx, double cy);
+    void rotateBy(double cx, double cy, double dx, double dy);
 
     // Absolute changes
     void setScale(double scale);
+    void setAngle(double angle);
     void setZoom(double zoom);
     void setLonLat(double lon, double lat);
     // void setAngle(double angle);
@@ -31,19 +33,19 @@ private:
     double pixel_y() const;
 
 public:
-    // double x, y;
     uint32_t width;
     uint32_t height;
 
 
 private:
-    double x, y; // spherical mercator meters of the view center
+    double x, y; // pixel values of the map center in the current scale
     double angle;
     double scale;
 
-
+    // tile size
     const int32_t size = 512;
 
+    // cache values for spherical mercator math
     double zc, Bc, Cc;
 };
 
diff --git a/include/llmr/util/mat4.h b/include/llmr/util/mat4.h
index bdfc3b1715..484b4b0823 100644
--- a/include/llmr/util/mat4.h
+++ b/include/llmr/util/mat4.h
@@ -31,6 +31,7 @@ void mat4_identity(float out[16]);
 void mat4_ortho(float out[16], float left, float right, float bottom, float top, float near, float far);
 void mat4_copy(float out[16], float a[16]);
 void mat4_translate(float out[16], float a[16], float x, float y, float z);
+void mat4_rotate_z(float out[16], float a[16], float rad);
 void mat4_scale(float out[16], float a[16], float x, float y, float z);
 void mat4_multiply(float out[16], float a[16], float b[16]);
 
diff --git a/macosx/main.mm b/macosx/main.mm
index 864803354a..549cd2b059 100644
--- a/macosx/main.mm
+++ b/macosx/main.mm
@@ -72,8 +72,14 @@ public:
 
     static void mouseclick(GLFWwindow *window, int button, int action, int modifiers) {
         MapView *view = (MapView *)glfwGetWindowUserPointer(window);
-        if (button == GLFW_MOUSE_BUTTON_1) {
+        if (button == GLFW_MOUSE_BUTTON_LEFT) {
             view->tracking = action == GLFW_PRESS;
+        } else if (button == GLFW_MOUSE_BUTTON_RIGHT) {
+            view->rotating = action == GLFW_PRESS;
+            if (view->rotating) {
+                view->start_x = view->last_x;
+                view->start_y = view->last_y;
+            }
         }
     }
 
@@ -81,6 +87,8 @@ public:
         MapView *view = (MapView *)glfwGetWindowUserPointer(window);
         if (view->tracking) {
             view->map->moveBy(x - view->last_x, y - view->last_y);
+        } else if (view->rotating) {
+            view->map->rotateBy(view->start_x, view->start_y, x - view->last_x, y - view->last_y);
         }
         view->last_x = x;
         view->last_y = y;
@@ -133,6 +141,9 @@ public:
     double last_x, last_y;
     bool tracking;
 
+    double start_x, start_y;
+    bool rotating;
+
     GLFWwindow *window;
     llmr::platform *platform;
     llmr::map *map;
diff --git a/src/map/map.cpp b/src/map/map.cpp
index e01a9fc98f..5e8b17f60e 100644
--- a/src/map/map.cpp
+++ b/src/map/map.cpp
@@ -52,6 +52,11 @@ void map::scaleBy(double ds, double cx, double cy) {
     platform->restart();
 }
 
+void map::rotateBy(double cx, double cy, double dx, double dy) {
+    transform->rotateBy(cx, cy, dx, dy);
+    platform->restart();
+}
+
 
 bool map::render() {
     painter->clear();
diff --git a/src/map/transform.cpp b/src/map/transform.cpp
index 4fcac6892b..b1a0018e29 100644
--- a/src/map/transform.cpp
+++ b/src/map/transform.cpp
@@ -10,6 +10,7 @@ using namespace llmr;
 const double MAXEXTENT = 20037508.34;
 const double D2R = M_PI / 180.0;
 const double R2D = 180.0 / M_PI;
+const double M2PI = 2 * M_PI;
 const double A = 6378137;
 
 
@@ -22,22 +23,35 @@ transform::transform()
     angle(0.0),
     scale(1.0) {
     setScale(scale);
+    setAngle(angle);
 }
 
 void transform::moveBy(double dx, double dy) {
-    x += dx;
-    y += dy;
+    x += cos(angle) * dx + sin(angle) * dy;
+    y += cos(angle) * dy + sin(-angle) * dx;
 }
 
 void transform::scaleBy(double ds, double cx, double cy) {
+    const double dx = (cx - width / 2) * (1.0 - ds);
+    const double dy = (cy - height / 2) * (1.0 - ds);
+    const double fx = cos(angle) * dx + sin(angle) * dy;
+    const double fy = cos(angle) * dy + sin(-angle) * dx;
+
     scale *= ds;
-    x = x * ds + (cx - width / 2) * (1.0 - ds);
-    y = y * ds + (cy - height / 2) * (1.0 - ds);
+    x = (x * ds) + fx;
+    y = (y * ds) + fy;
+}
+
+void transform::rotateBy(double cx, double cy, double dx, double dy) {
+    const double distance = copysign(sqrt(dx * dx + dy * dy), dx);
+    setAngle(angle + D2R * distance);
 }
 
-// void transform::setAngle(double _angle) {
-//     angle = _angle;
-// }
+void transform::setAngle(double new_angle) {
+    angle = new_angle;
+    while (angle > M_PI) angle -= M2PI;
+    while (angle <= -M_PI) angle += M2PI;
+}
 
 void transform::setScale(double new_scale) {
     const double factor = new_scale / scale;
@@ -59,8 +73,6 @@ void transform::setLonLat(double lon, double lat) {
     const double f = fmin(fmax(sin(D2R * lat), -0.9999), 0.9999);
     x = -round(lon * Bc);
     y = round(0.5 * Cc * log((1 + f) / (1 - f)));
-
-    fprintf(stderr, "pos: %f/%f\n", x, y);
 }
 
 void transform::getLonLat(double& lon, double& lat) const {
@@ -83,9 +95,20 @@ void transform::matrixFor(float matrix[16], uint32_t tile_z, uint32_t tile_x, ui
     const double tile_size = scale * size / tile_scale;
 
     mat4_identity(matrix);
-    mat4_translate(matrix, matrix, pixel_x(), pixel_y(), -1);
+
+    mat4_translate(matrix, matrix, 0.5f * (float)width, 0.5f * (float)height, 0);
+    mat4_rotate_z(matrix, matrix, angle);
+    mat4_translate(matrix, matrix, -0.5f * (float)width, -0.5f * (float)height, 0);
+
+    mat4_translate(matrix, matrix, pixel_x(), pixel_y(), 0);
     mat4_translate(matrix, matrix, tile_x * tile_size, tile_y * tile_size, 0);
 
     // TODO: Get rid of the 8 (scaling from 4096 to 512 px tile size);
     mat4_scale(matrix, matrix, scale / tile_scale / 8, scale / tile_scale / 8, 1);
+
+    // Clipping plane
+    mat4_translate(matrix, matrix, 0, 0, -1);
+
+
+
 }
diff --git a/src/util/mat4.c b/src/util/mat4.c
index e3925ab3a1..ef74fc3f19 100644
--- a/src/util/mat4.c
+++ b/src/util/mat4.c
@@ -22,6 +22,8 @@
 
 #include <llmr/util/mat4.h>
 
+#include <math.h>
+
 void mat4_identity(float out[16]) {
     out[0] = 1.0f;
     out[1] = 0.0f;
@@ -83,29 +85,63 @@ void mat4_copy(float out[16], float a[16]) {
 }
 
 void mat4_translate(float out[16], float a[16], float x, float y, float z) {
-    float a00 = a[0], a01 = a[1], a02 = a[2], a03 = a[3],
-          a10 = a[4], a11 = a[5], a12 = a[6], a13 = a[7],
-          a20 = a[8], a21 = a[9], a22 = a[10], a23 = a[11],
-          a30 = a[12], a31 = a[13], a32 = a[14], a33 = a[15];
+    float a00, a01, a02, a03,
+          a10, a11, a12, a13,
+          a20, a21, a22, a23;
+
+    if (a == out) {
+        out[12] = a[0] * x + a[4] * y + a[8] * z + a[12];
+        out[13] = a[1] * x + a[5] * y + a[9] * z + a[13];
+        out[14] = a[2] * x + a[6] * y + a[10] * z + a[14];
+        out[15] = a[3] * x + a[7] * y + a[11] * z + a[15];
+    } else {
+        a00 = a[0]; a01 = a[1]; a02 = a[2]; a03 = a[3];
+        a10 = a[4]; a11 = a[5]; a12 = a[6]; a13 = a[7];
+        a20 = a[8]; a21 = a[9]; a22 = a[10]; a23 = a[11];
+
+        out[0] = a00; out[1] = a01; out[2] = a02; out[3] = a03;
+        out[4] = a10; out[5] = a11; out[6] = a12; out[7] = a13;
+        out[8] = a20; out[9] = a21; out[10] = a22; out[11] = a23;
+
+        out[12] = a00 * x + a10 * y + a20 * z + a[12];
+        out[13] = a01 * x + a11 * y + a21 * z + a[13];
+        out[14] = a02 * x + a12 * y + a22 * z + a[14];
+        out[15] = a03 * x + a13 * y + a23 * z + a[15];
+    }
+}
+
+void mat4_rotate_z(float out[16], float a[16], float rad) {
+    float s = sin(rad),
+          c = cos(rad),
+          a00 = a[0],
+          a01 = a[1],
+          a02 = a[2],
+          a03 = a[3],
+          a10 = a[4],
+          a11 = a[5],
+          a12 = a[6],
+          a13 = a[7];
+
+    if (a != out) { // If the source and destination differ, copy the unchanged last row
+        out[8]  = a[8];
+        out[9]  = a[9];
+        out[10] = a[10];
+        out[11] = a[11];
+        out[12] = a[12];
+        out[13] = a[13];
+        out[14] = a[14];
+        out[15] = a[15];
+    }
 
-    out[0] = a00 + a03 * x;
-    out[1] = a01 + a03 * y;
-    out[2] = a02 + a03 * z;
-    out[3] = a03;
-
-    out[4] = a10 + a13 * x;
-    out[5] = a11 + a13 * y;
-    out[6] = a12 + a13 * z;
-    out[7] = a13;
-
-    out[8] = a20 + a23 * x;
-    out[9] = a21 + a23 * y;
-    out[10] = a22 + a23 * z;
-    out[11] = a23;
-    out[12] = a30 + a33 * x;
-    out[13] = a31 + a33 * y;
-    out[14] = a32 + a33 * z;
-    out[15] = a33;
+    // Perform axis-specific matrix multiplication
+    out[0] = a00 * c + a10 * s;
+    out[1] = a01 * c + a11 * s;
+    out[2] = a02 * c + a12 * s;
+    out[3] = a03 * c + a13 * s;
+    out[4] = a10 * c - a00 * s;
+    out[5] = a11 * c - a01 * s;
+    out[6] = a12 * c - a02 * s;
+    out[7] = a13 * c - a03 * s;
 }
 
 void mat4_scale(float out[16], float a[16], float x, float y, float z) {