forked from sympy/sympy
-
Notifications
You must be signed in to change notification settings - Fork 1
/
plot_rotation.py
59 lines (44 loc) · 1.33 KB
/
plot_rotation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
from pyglet.gl import *
from math import sqrt, acos
def cross(a, b):
return (a[1]*b[2] - a[2]*b[1],
a[2]*b[0] - a[0]*b[2],
a[0]*b[1] - a[1]*b[0])
def dot(a, b):
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]
def mag(a):
return sqrt(a[0]**2 + a[1]**2 + a[2]**2)
def norm(a):
m = mag(a)
return (a[0]/m, a[1]/m, a[2]/m)
def get_sphere_mapping(x, y, width, height):
x = min([max([x,0]), width])
y = min([max([y,0]), height])
sr = sqrt( (width/2)**2 + (height/2)**2 )
#sr *= 1.5
sx = ( (x - width/2) / sr )
sy = ( (y - height/2) / sr )
sz = 1.0 - sx**2 - sy**2
if sz > 0.0:
sz = sqrt(sz)
return (sx, sy, sz)
else:
sz = 0
return norm( (sx, sy, sz) )
rad2deg = 180.0/3.141592
def get_spherical_rotatation(p1, p2, width, height, theta_multiplier):
v1 = get_sphere_mapping(p1[0], p1[1], width, height)
v2 = get_sphere_mapping(p2[0], p2[1], width, height)
d = min(max([dot(v1, v2), -1]), 1)
if abs(d - 1.0) < 0.000001:
return None
raxis = norm( cross(v1, v2) )
rtheta = theta_multiplier * rad2deg * acos(d)
#rtheta = 2.0 * rad2deg * acos(d)
glPushMatrix()
glLoadIdentity()
glRotatef(rtheta, *raxis)
mat = (c_float*16)()
glGetFloatv(GL_MODELVIEW_MATRIX, mat)
glPopMatrix()
return mat