-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathpy3dmath.py
307 lines (231 loc) · 10.1 KB
/
py3dmath.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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
"""
@author: mwm
corresponds to the Vec3 class in Common/Math
"""
##UGLY TO PUT EVERYTHING IN __INIT__!
from __future__ import division, print_function #automatically do float division
import numpy as np
class Vec3:
def __init__(self, *args):
if len(args) == 0:
self.x = self.y = self.z = None
elif len(args) == 1:
inval = args[0]
if inval is None:
self.x = self.y = self.z = None
elif isinstance(inval,Vec3):
self.x = inval.x
self.y = inval.y
self.z = inval.z
elif type(inval).__name__ == 'ndarray':
inval = inval.flatten()
self.x = inval[0]
self.y = inval[1]
self.z = inval[2]
elif type(inval).__name__ == 'list':
self.x = inval[0]
self.y = inval[1]
self.z = inval[2]
elif type(inval).__name__ == 'matrix':
if inval.shape[0] > inval.shape[1]:
self.x = inval[0,0]
self.y = inval[1,0]
self.z = inval[2,0]
else:
self.x = inval[0,0]
self.y = inval[0,1]
self.z = inval[0,2]
else:
raise NameError('Unknown initialisation! (arg type)')
elif len(args) == 3:
self.x = args[0]
self.y = args[1]
self.z = args[2]
else:
raise NameError('Unknown initialisation! (num args)')
return
@classmethod
def from_list(cls, list):
return cls(list[0], list[1], list[2])
def norm2(self):
return (self.x**2+self.y**2+self.z**2)**(1/2)
def norm2Squared(self):
return self.x**2+self.y**2+self.z**2
def dot(self, other):
return self.x*other.x + self.y*other.y + self.z*other.z
def cross(self, other):
return Vec3([self.y*other.z - self.z*other.y,self.z*other.x - self.x*other.z,self.x*other.y - self.y*other.x])
def to_list(self):
return [self.x, self.y, self.z]
def to_array(self):
return np.array([self.to_list()]).T
def to_matrix(self):
return np.matrix([self.to_list()]).T
def to_unit_vector(self):
return self/self.norm2()
def to_cross_product_matrix(self):
return np.matrix([[0, -self.z, +self.y],
[+self.z, 0, -self.x],
[-self.y, +self.x, 0]])
def __add__(self, other):
return Vec3([self.x+other.x,self.y+other.y,self.z+other.z])
def __sub__(self, other):
return Vec3([self.x-other.x,self.y-other.y,self.z-other.z])
def __mul__(self, scalar):
return Vec3([self.x*scalar,self.y*scalar,self.z*scalar])
def __div__(self, scalar):
return Vec3([self.x/scalar,self.y/scalar,self.z/scalar])
def __rmul__(self, mulVal):
try:
#assume you're multiplying a 3x3 matrix:
if not mulVal.shape==(3,3):
print("Cannot multiply matrix of shape:", mulVal.shape())
raise
return Vec3(mulVal*self.to_array())
except:
#assume a scalar:
return Vec3([self.x*mulVal,self.y*mulVal,self.z*mulVal])
def __truediv__ (self, scalar):
return Vec3([self.x/scalar,self.y/scalar,self.z/scalar])
def __str__(self):
return '({0}, {1}, {2})'.format(self.x,self.y, self.z)
def __repr__(self):
return 'Vec3({0},{1},{2})'.format(self.x,self.y, self.z)
def __neg__(self):
return Vec3([-self.x, -self.y, -self.z])
#square brackets:
def __getitem__(self, index):
if index == 0:
return self.x
elif index == 1:
return self.y
elif index == 2:
return self.z
#square brackets:
def __setitem__(self, index, val):
if index == 0:
self.x = val
elif index == 1:
self.y = val
elif index == 2:
self.z = val
class Rotation:
def __init__(self, q0, q1, q2, q3):
self.q=np.array([q0,q1,q2,q3])
@classmethod
def from_list(cls, qList):
return cls(qList[0],qList[1],qList[2],qList[3])
def normalise(self):
self.q = self.q/np.sqrt(np.dot(self.q,self.q))
def inverse(self):
return Rotation(self.q[0],-self.q[1],-self.q[2],-self.q[3])
@classmethod
def from_axis_angle(cls, unitVector, angle):
return Rotation(np.cos(angle/2.),
np.sin(angle/2.)*unitVector[0],
np.sin(angle/2.)*unitVector[1],
np.sin(angle/2.)*unitVector[2])
@classmethod
def from_rotation_vector(cls, rotVec):
rotVec = Vec3(rotVec)
theta = rotVec.norm2()
if(theta < 4.84813681e-9):
return Rotation.identity() # less than one milli arc second :)
return Rotation.from_axis_angle(rotVec/theta,theta)
@classmethod
def from_vector_part_of_quaternion(cls, vec):
vec = Vec3(vec)
return Rotation(np.sqrt(1-vec[0]**2-vec[1]**2-vec[2]**2), vec[0], vec[1], vec[2])
@classmethod
def from_euler_YPR(cls, ypr):
y = ypr[0]
p = ypr[1]
r = ypr[2]
return Rotation(np.cos(0.5*y)*np.cos(0.5*p)*np.cos(0.5*r) + np.sin(0.5*y)*np.sin(0.5*p)*np.sin(0.5*r),
np.cos(0.5*y)*np.cos(0.5*p)*np.sin(0.5*r) - np.sin(0.5*y)*np.sin(0.5*p)*np.cos(0.5*r),
np.cos(0.5*y)*np.sin(0.5*p)*np.cos(0.5*r) + np.sin(0.5*y)*np.cos(0.5*p)*np.sin(0.5*r),
np.sin(0.5*y)*np.cos(0.5*p)*np.cos(0.5*r) - np.cos(0.5*y)*np.sin(0.5*p)*np.sin(0.5*r))
@classmethod
def from_rotation_matrix(cls, mat):
#(Zipfel, P.97-98)
rotAngle = np.arccos(0.5*(mat[0,0]+mat[1,1]+mat[2,2])-0.5)
rotVector = Vec3([mat[2,1]-mat[1,2],mat[0,2]-mat[2,0],mat[1,0]-mat[0,1]])*(1/(2*np.sin(rotAngle)))
return Rotation.from_axis_angle(rotVector,rotAngle)
@classmethod
def from_rodrigues_vector(cls, vec):
vec = Vec3(vec)
q = (1/np.sqrt(1+vec.norm2Squared()))*np.array([1, vec[0], vec[1], vec[2]])
return Rotation(q[0], q[1], q[2], q[3])
def to_rodrigues_vector(self):
"""Outputs the corresponding Rodrigues vector, as used e.g. by openCV"""
return Vec3(np.array([self.q[1],self.q[2],self.q[3]])/self.q[0])
def to_vector_part_of_quaternion(self):
if self.q[0] >= 0:
return Vec3(self.q[1], self.q[2], self.q[3])
else:
return -Vec3(self.q[1], self.q[2], self.q[3])
def to_euler_YPR(self):
y = np.arctan2(2.0*self.q[1]*self.q[2] + 2.0*self.q[0]*self.q[3], self.q[1]*self.q[1] + self.q[0]*self.q[0] - self.q[3]*self.q[3] - self.q[2]*self.q[2])
p = -np.arcsin(2.0*self.q[1]*self.q[3] - 2.0*self.q[0]*self.q[2])
r = np.arctan2(2.0*self.q[2]*self.q[3] + 2.0*self.q[0]*self.q[1], self.q[3]*self.q[3] - self.q[2]*self.q[2] - self.q[1]*self.q[1] + self.q[0]*self.q[0])
return np.array([y,p,r])
def to_list(self):
return [self.q[0], self.q[1], self.q[2], self.q[3]]
def to_array(self):
return np.array([self.to_list()]).T
def to_rotation_vector(self):
n = self.to_vector_part_of_quaternion()
theta = np.arcsin(n.norm2())*2
if(abs(theta) < 1e-15):
return Vec3(0,0,0)
return theta*n.to_unit_vector()
def to_rotation_matrix(self):
return self._rotation_matrix()
@classmethod
def identity(cls):
return Rotation(1,0,0,0)
def __mul__(self, rhs):
if(isinstance(rhs,Rotation)):
#apply successive rotations
c0 = rhs.q[0]*self.q[0] - rhs.q[1]*self.q[1] - rhs.q[2]*self.q[2] - rhs.q[3]*self.q[3]
c1 = rhs.q[1]*self.q[0] + rhs.q[0]*self.q[1] + rhs.q[3]*self.q[2] - rhs.q[2]*self.q[3]
c2 = rhs.q[2]*self.q[0] - rhs.q[3]*self.q[1] + rhs.q[0]*self.q[2] + rhs.q[1]*self.q[3]
c3 = rhs.q[3]*self.q[0] + rhs.q[2]*self.q[1] - rhs.q[1]*self.q[2] + rhs.q[0]*self.q[3]
return Rotation(c0,c1,c2,c3)
else:
#rotate the vector
return self.Rotate(rhs)
def Rotate(self, v):
if(isinstance(v,Vec3)):
return Vec3(self._rotation_matrix()*v.to_matrix())
return self._rotation_matrix()*v
def RotateBackwards(self, v):
vnp = np.matrix([v[0,0],v[1,0],v[2,0]]).T
vr = self.Inverse()._rotation_matrix()*vnp
return Vec3.FromNumpyMatrix(vr)
def _rotation_matrix(self):
r0=self.q[0]*self.q[0]
r1=self.q[1]*self.q[1]
r2=self.q[2]*self.q[2]
r3=self.q[3]*self.q[3]
R = np.matrix(np.zeros([3,3]))
R[0, 0] = r0 + r1 - r2 - r3
R[0, 1] = 2*self.q[1]*self.q[2] - 2*self.q[0]*self.q[3]
R[0, 2] = 2*self.q[1]*self.q[3] + 2*self.q[0]*self.q[2]
R[1, 0] = 2*self.q[1]*self.q[2] + 2*self.q[0]*self.q[3]
R[1, 1] = r0 - r1 + r2 - r3
R[1, 2] = 2*self.q[2]*self.q[3] - 2*self.q[0]*self.q[1]
R[2, 0] = 2*self.q[1]*self.q[3] - 2*self.q[0]*self.q[2]
R[2, 1] = 2*self.q[2]*self.q[3] + 2*self.q[0]*self.q[1]
R[2, 2] = r0 - r1 - r2 + r3
return R
def __str__(self):
return '{0},{1},{2},{3}'.format(self.q[0],self.q[1], self.q[2], self.q[3])
# r=self._rotation_matrix()
# return '{0}\t{1}\t{2}\n{3}\t{4}\t{5}\n{6}\t{7}\t{8} '.format(r[0,0],r[0,1],r[0,2],r[1,0],r[1,1],r[1,2],r[2,0],r[2,1],r[2,2])
def __repr__(self):
return 'Rotation({0},{1},{2},{3})'.format(self.q[0],self.q[1], self.q[2], self.q[3])
def PrintRotationMatrix(self):
r=self._rotation_matrix()
for i in range(0,3):
print(r[i,0], r[i,1], r[i,2])