Mercurial > hg-public > lasercut-scripts / file diff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/printrun-src/printrun/gl/trackball.py	Fri Jun 03 09:16:07 2016 +0200
@@ -0,0 +1,92 @@
+#!/usr/bin/env python
+
+# This file is part of the Printrun suite.
+#
+# Printrun is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# Printrun is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with Printrun.  If not, see <http://www.gnu.org/licenses/>.
+
+import math
+
+from pyglet.gl import GLdouble
+
+def cross(v1, v2):
+    return [v1[1] * v2[2] - v1[2] * v2[1],
+            v1[2] * v2[0] - v1[0] * v2[2],
+            v1[0] * v2[1] - v1[1] * v2[0]]
+
+def trackball(p1x, p1y, p2x, p2y, r):
+    TRACKBALLSIZE = r
+
+    if p1x == p2x and p1y == p2y:
+        return [0.0, 0.0, 0.0, 1.0]
+
+    p1 = [p1x, p1y, project_to_sphere(TRACKBALLSIZE, p1x, p1y)]
+    p2 = [p2x, p2y, project_to_sphere(TRACKBALLSIZE, p2x, p2y)]
+    a = cross(p2, p1)
+
+    d = map(lambda x, y: x - y, p1, p2)
+    t = math.sqrt(sum(map(lambda x: x * x, d))) / (2.0 * TRACKBALLSIZE)
+
+    if t > 1.0:
+        t = 1.0
+    if t < -1.0:
+        t = -1.0
+    phi = 2.0 * math.asin(t)
+
+    return axis_to_quat(a, phi)
+
+def axis_to_quat(a, phi):
+    lena = math.sqrt(sum(map(lambda x: x * x, a)))
+    q = map(lambda x: x * (1 / lena), a)
+    q = map(lambda x: x * math.sin(phi / 2.0), q)
+    q.append(math.cos(phi / 2.0))
+    return q
+
+def build_rotmatrix(q):
+    m = (GLdouble * 16)()
+    m[0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
+    m[1] = 2.0 * (q[0] * q[1] - q[2] * q[3])
+    m[2] = 2.0 * (q[2] * q[0] + q[1] * q[3])
+    m[3] = 0.0
+
+    m[4] = 2.0 * (q[0] * q[1] + q[2] * q[3])
+    m[5] = 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0])
+    m[6] = 2.0 * (q[1] * q[2] - q[0] * q[3])
+    m[7] = 0.0
+
+    m[8] = 2.0 * (q[2] * q[0] - q[1] * q[3])
+    m[9] = 2.0 * (q[1] * q[2] + q[0] * q[3])
+    m[10] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0])
+    m[11] = 0.0
+
+    m[12] = 0.0
+    m[13] = 0.0
+    m[14] = 0.0
+    m[15] = 1.0
+    return m
+
+
+def project_to_sphere(r, x, y):
+    d = math.sqrt(x * x + y * y)
+    if (d < r * 0.70710678118654752440):
+        return math.sqrt(r * r - d * d)
+    else:
+        t = r / 1.41421356237309504880
+        return t * t / d
+
+
+def mulquat(q1, rq):
+    return [q1[3] * rq[0] + q1[0] * rq[3] + q1[1] * rq[2] - q1[2] * rq[1],
+            q1[3] * rq[1] + q1[1] * rq[3] + q1[2] * rq[0] - q1[0] * rq[2],
+            q1[3] * rq[2] + q1[2] * rq[3] + q1[0] * rq[1] - q1[1] * rq[0],
+            q1[3] * rq[3] - q1[0] * rq[0] - q1[1] * rq[1] - q1[2] * rq[2]]