-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcamera.py
155 lines (118 loc) · 5.89 KB
/
camera.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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
import OpenGL
from OpenGL.GL import *
from OpenGL.GLU import *
import numpy as np
import math
from scipy.spatial.transform import Rotation
class Camera:
def __init__(self, name, pos, orient, active, lock=None):
self.name = name
self.pos = pos
self.orient = orient
self.active = active
self.lock = lock
self.offset_amount = 0
self.yaw = 0
self.pitch = 0
self.max_pitch = 60
self.min_pitch = -80
def get_name(self):
return self.name
def get_pos(self):
return self.pos
def set_pos(self, new_pos):
req_trans = new_pos - self.pos
glTranslate(req_trans[0], req_trans[1], req_trans[2])
self.pos = new_pos
def move(self, movement):
if not self.lock:
glTranslate((movement[0] * self.orient[0][0]) + (movement[1] * self.orient[1][0]) + (movement[2] * self.orient[2][0]),
(movement[0] * self.orient[0][1]) + (movement[1] * self.orient[1][1]) + (movement[2] * self.orient[2][1]),
(movement[0] * self.orient[0][2]) + (movement[1] * self.orient[1][2]) + (movement[2] * self.orient[2][2]))
self.pos = np.array([self.pos[0] + (movement[0] * self.orient[0][0]) + (movement[1] * self.orient[1][0]) + (movement[2] * self.orient[2][0]),
self.pos[1] + (movement[0] * self.orient[0][1]) + (movement[1] * self.orient[1][1]) + (movement[2] * self.orient[2][1]),
self.pos[2] + (movement[0] * self.orient[0][2]) + (movement[1] * self.orient[1][2]) + (movement[2] * self.orient[2][2])])
else:
# this handles zooming in and out
self.offset_amount += movement[2]
if self.offset_amount <= 0:
self.offset_amount -= movement[2]
def get_orient(self):
return self.orient
def get_textmtx(self):
return self.textmtx
def get_active(self):
return self.active
def activate(self):
self.active = True
def deactivate(self):
self.active = False
def get_lock(self):
return self.lock
def calculate_orientation_matrix(self):
# Calculate the 3x3 orientation matrix based on yaw and pitch
yaw_matrix = np.array([[np.cos(self.yaw), 0, -np.sin(self.yaw)],
[0, 1, 0],
[np.sin(self.yaw), 0, np.cos(self.yaw)]])
pitch_matrix = np.array([[1, 0, 0],
[0, np.cos(self.pitch), np.sin(self.pitch)],
[0, -np.sin(self.pitch), np.cos(self.pitch)]])
orientation_matrix = np.dot(yaw_matrix, pitch_matrix)
return orientation_matrix
def calculate_text_matrix(self):
# Calculate the 3x3 orientation matrix based on yaw and pitch
yaw_matrix = np.array([[np.cos(-self.yaw), 0, -np.sin(-self.yaw)],
[0, 1, 0],
[np.sin(-self.yaw), 0, np.cos(-self.yaw)]])
pitch_matrix = np.array([[1, 0, 0],
[0, np.cos(-self.pitch), np.sin(-self.pitch)],
[0, -np.sin(-self.pitch), np.cos(-self.pitch)]])
orientation_matrix = np.dot(pitch_matrix, yaw_matrix)
return orientation_matrix
def rotate(self, rotation):
yaw_delta = rotation[1]
pitch_delta = rotation[0]
if self.pitch > np.deg2rad(self.max_pitch) and pitch_delta > 0:
pitch_delta = 0
elif self.pitch < np.deg2rad(self.min_pitch) and pitch_delta < 0:
pitch_delta = 0
# Update yaw and pitch based on input deltas
self.yaw += yaw_delta
self.pitch += pitch_delta
# Keep the pitch within a certain range to avoid gimbal lock
self.pitch = np.clip(self.pitch, -np.pi / 2, np.pi / 2)
# Update the orientation matrix
self.orient = self.calculate_orientation_matrix()
self.orient[2] = np.cross(self.orient[0], self.orient[1])
self.textmtx = self.calculate_text_matrix()
self.textmtx[2] = np.cross(self.textmtx[0], self.textmtx[1])
# Use OpenGL's glRotate to update the view matrix
glTranslate(-self.pos[0], -self.pos[1], -self.pos[2])
glRotatef(np.degrees(yaw_delta), 0, 1, 0)
glRotatef(np.degrees(pitch_delta), np.cos(self.yaw), 0, np.sin(self.yaw))
glTranslate(self.pos[0], self.pos[1], self.pos[2])
def lock_to_target(self, target):
self.lock = target
self.set_pos(-self.lock.pos - self.orient[2] * self.offset_amount)
def unlock(self):
self.lock = None
def move_with_lock(self, dt):
if self.lock:
self.set_pos(-self.lock.pos - self.orient[2] * self.offset_amount)
#rot = np.array([self.lock.ang_vel[0], -self.lock.ang_vel[1], self.lock.ang_vel[2]])
#self.rotate(np.rad2deg(rot * dt))
def rotate_with_lock(self, dt):
if self.lock:
if np.linalg.norm(self.lock.ang_vel) > 0:
about_pos = -self.lock.pos
rotation = -self.lock.ang_vel * dt * 180 / 3.14159
glTranslate(-about_pos[0], -about_pos[1], -about_pos[2])
glRotate(-rotation[0], self.orient[0][0], self.orient[0][1], self.orient[0][2])
glTranslate(about_pos[0], about_pos[1], about_pos[2])
glTranslate(-about_pos[0], -about_pos[1], -about_pos[2])
glRotate(-rotation[1], self.orient[1][0], self.orient[1][1], self.orient[1][2])
glTranslate(about_pos[0], about_pos[1], about_pos[2])
glTranslate(-about_pos[0], -about_pos[1], -about_pos[2])
glRotate(-rotation[2], self.orient[2][0], self.orient[2][1], self.orient[2][2])
glTranslate(about_pos[0], about_pos[1], about_pos[2])
self.orient = self.lock.orient