Wed, 20 Jan 2021 10:17:01 +0100
updated pronterface.py losing my own changes
15 | 1 | # This file is part of the Printrun suite. |
2 | # | |
3 | # Printrun is free software: you can redistribute it and/or modify | |
4 | # it under the terms of the GNU General Public License as published by | |
5 | # the Free Software Foundation, either version 3 of the License, or | |
6 | # (at your option) any later version. | |
7 | # | |
8 | # Printrun is distributed in the hope that it will be useful, | |
9 | # but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | # GNU General Public License for more details. | |
12 | # | |
13 | # You should have received a copy of the GNU General Public License | |
14 | # along with Printrun. If not, see <http://www.gnu.org/licenses/>. | |
15 | ||
16 | import math | |
17 | ||
18 | from pyglet.gl import GLdouble | |
19 | ||
20 | def cross(v1, v2): | |
21 | return [v1[1] * v2[2] - v1[2] * v2[1], | |
22 | v1[2] * v2[0] - v1[0] * v2[2], | |
23 | v1[0] * v2[1] - v1[1] * v2[0]] | |
24 | ||
25 | def trackball(p1x, p1y, p2x, p2y, r): | |
26 | TRACKBALLSIZE = r | |
27 | ||
28 | if p1x == p2x and p1y == p2y: | |
29 | return [0.0, 0.0, 0.0, 1.0] | |
30 | ||
31 | p1 = [p1x, p1y, project_to_sphere(TRACKBALLSIZE, p1x, p1y)] | |
32 | p2 = [p2x, p2y, project_to_sphere(TRACKBALLSIZE, p2x, p2y)] | |
33 | a = cross(p2, p1) | |
34 | ||
35 | d = map(lambda x, y: x - y, p1, p2) | |
46 | 36 | t = math.sqrt(sum(x * x for x in d)) / (2.0 * TRACKBALLSIZE) |
15 | 37 | |
38 | if t > 1.0: | |
39 | t = 1.0 | |
40 | if t < -1.0: | |
41 | t = -1.0 | |
42 | phi = 2.0 * math.asin(t) | |
43 | ||
44 | return axis_to_quat(a, phi) | |
45 | ||
46 | def axis_to_quat(a, phi): | |
46 | 47 | lena = math.sqrt(sum(x * x for x in a)) |
48 | q = [x * (1 / lena) for x in a] | |
49 | q = [x * math.sin(phi / 2.0) for x in q] | |
15 | 50 | q.append(math.cos(phi / 2.0)) |
51 | return q | |
52 | ||
53 | def build_rotmatrix(q): | |
54 | m = (GLdouble * 16)() | |
55 | m[0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]) | |
56 | m[1] = 2.0 * (q[0] * q[1] - q[2] * q[3]) | |
57 | m[2] = 2.0 * (q[2] * q[0] + q[1] * q[3]) | |
58 | m[3] = 0.0 | |
59 | ||
60 | m[4] = 2.0 * (q[0] * q[1] + q[2] * q[3]) | |
61 | m[5] = 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]) | |
62 | m[6] = 2.0 * (q[1] * q[2] - q[0] * q[3]) | |
63 | m[7] = 0.0 | |
64 | ||
65 | m[8] = 2.0 * (q[2] * q[0] - q[1] * q[3]) | |
66 | m[9] = 2.0 * (q[1] * q[2] + q[0] * q[3]) | |
67 | m[10] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]) | |
68 | m[11] = 0.0 | |
69 | ||
70 | m[12] = 0.0 | |
71 | m[13] = 0.0 | |
72 | m[14] = 0.0 | |
73 | m[15] = 1.0 | |
74 | return m | |
75 | ||
76 | ||
77 | def project_to_sphere(r, x, y): | |
78 | d = math.sqrt(x * x + y * y) | |
79 | if (d < r * 0.70710678118654752440): | |
80 | return math.sqrt(r * r - d * d) | |
81 | else: | |
82 | t = r / 1.41421356237309504880 | |
83 | return t * t / d | |
84 | ||
85 | ||
86 | def mulquat(q1, rq): | |
87 | return [q1[3] * rq[0] + q1[0] * rq[3] + q1[1] * rq[2] - q1[2] * rq[1], | |
88 | q1[3] * rq[1] + q1[1] * rq[3] + q1[2] * rq[0] - q1[0] * rq[2], | |
89 | q1[3] * rq[2] + q1[2] * rq[3] + q1[0] * rq[1] - q1[1] * rq[0], | |
90 | q1[3] * rq[3] - q1[0] * rq[0] - q1[1] * rq[1] - q1[2] * rq[2]] |