00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 '''rotation matrix class
00024 '''
00025
00026 from math import sin, cos, sqrt, asin, atan2, pi, radians, acos, degrees
00027
00028 class Vector3:
00029 '''a vector'''
00030 def __init__(self, x=None, y=None, z=None):
00031 if x != None and y != None and z != None:
00032 self.x = float(x)
00033 self.y = float(y)
00034 self.z = float(z)
00035 elif x != None and len(x) == 3:
00036 self.x = float(x[0])
00037 self.y = float(x[1])
00038 self.z = float(x[2])
00039 elif x != None:
00040 raise ValueError('bad initialiser')
00041 else:
00042 self.x = float(0)
00043 self.y = float(0)
00044 self.z = float(0)
00045
00046 def __repr__(self):
00047 return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
00048 self.y,
00049 self.z)
00050
00051 def __eq__(self, v):
00052 return self.x == v.x and self.y == v.y and self.z == v.z
00053
00054 def __ne__(self, v):
00055 return not self == v
00056
00057 def close(self, v, tol=1e-7):
00058 return abs(self.x - v.x) < tol and \
00059 abs(self.y - v.y) < tol and \
00060 abs(self.z - v.z) < tol
00061
00062 def __add__(self, v):
00063 return Vector3(self.x + v.x,
00064 self.y + v.y,
00065 self.z + v.z)
00066
00067 __radd__ = __add__
00068
00069 def __sub__(self, v):
00070 return Vector3(self.x - v.x,
00071 self.y - v.y,
00072 self.z - v.z)
00073
00074 def __neg__(self):
00075 return Vector3(-self.x, -self.y, -self.z)
00076
00077 def __rsub__(self, v):
00078 return Vector3(v.x - self.x,
00079 v.y - self.y,
00080 v.z - self.z)
00081
00082 def __mul__(self, v):
00083 if isinstance(v, Vector3):
00084 '''dot product'''
00085 return self.x*v.x + self.y*v.y + self.z*v.z
00086 return Vector3(self.x * v,
00087 self.y * v,
00088 self.z * v)
00089
00090 __rmul__ = __mul__
00091
00092 def __div__(self, v):
00093 return Vector3(self.x / v,
00094 self.y / v,
00095 self.z / v)
00096
00097 def __mod__(self, v):
00098 '''cross product'''
00099 return Vector3(self.y*v.z - self.z*v.y,
00100 self.z*v.x - self.x*v.z,
00101 self.x*v.y - self.y*v.x)
00102
00103 def __copy__(self):
00104 return Vector3(self.x, self.y, self.z)
00105
00106 copy = __copy__
00107
00108 def length(self):
00109 return sqrt(self.x**2 + self.y**2 + self.z**2)
00110
00111 def zero(self):
00112 self.x = self.y = self.z = 0
00113
00114 def angle(self, v):
00115 '''return the angle between this vector and another vector'''
00116 return acos((self * v) / (self.length() * v.length()))
00117
00118 def normalized(self):
00119 return self.__div__(self.length())
00120
00121 def normalize(self):
00122 v = self.normalized()
00123 self.x = v.x
00124 self.y = v.y
00125 self.z = v.z
00126
00127 class Matrix3:
00128 '''a 3x3 matrix, intended as a rotation matrix'''
00129 def __init__(self, a=None, b=None, c=None):
00130 if a is not None and b is not None and c is not None:
00131 self.a = a.copy()
00132 self.b = b.copy()
00133 self.c = c.copy()
00134 else:
00135 self.identity()
00136
00137 def __repr__(self):
00138 return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % (
00139 self.a.x, self.a.y, self.a.z,
00140 self.b.x, self.b.y, self.b.z,
00141 self.c.x, self.c.y, self.c.z)
00142
00143 def identity(self):
00144 self.a = Vector3(1,0,0)
00145 self.b = Vector3(0,1,0)
00146 self.c = Vector3(0,0,1)
00147
00148 def transposed(self):
00149 return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
00150 Vector3(self.a.y, self.b.y, self.c.y),
00151 Vector3(self.a.z, self.b.z, self.c.z))
00152
00153
00154 def from_euler(self, roll, pitch, yaw):
00155 '''fill the matrix from Euler angles in radians'''
00156 cp = cos(pitch)
00157 sp = sin(pitch)
00158 sr = sin(roll)
00159 cr = cos(roll)
00160 sy = sin(yaw)
00161 cy = cos(yaw)
00162
00163 self.a.x = cp * cy
00164 self.a.y = (sr * sp * cy) - (cr * sy)
00165 self.a.z = (cr * sp * cy) + (sr * sy)
00166 self.b.x = cp * sy
00167 self.b.y = (sr * sp * sy) + (cr * cy)
00168 self.b.z = (cr * sp * sy) - (sr * cy)
00169 self.c.x = -sp
00170 self.c.y = sr * cp
00171 self.c.z = cr * cp
00172
00173
00174 def to_euler(self):
00175 '''find Euler angles (321 convention) for the matrix'''
00176 if self.c.x >= 1.0:
00177 pitch = pi
00178 elif self.c.x <= -1.0:
00179 pitch = -pi
00180 else:
00181 pitch = -asin(self.c.x)
00182 roll = atan2(self.c.y, self.c.z)
00183 yaw = atan2(self.b.x, self.a.x)
00184 return (roll, pitch, yaw)
00185
00186
00187 def to_euler312(self):
00188 '''find Euler angles (312 convention) for the matrix.
00189 See http://www.atacolorado.com/eulersequences.doc
00190 '''
00191 T21 = self.a.y
00192 T22 = self.b.y
00193 T23 = self.c.y
00194 T13 = self.c.x
00195 T33 = self.c.z
00196 yaw = atan2(-T21, T22)
00197 roll = asin(T23)
00198 pitch = atan2(-T13, T33)
00199 return (roll, pitch, yaw)
00200
00201 def from_euler312(self, roll, pitch, yaw):
00202 '''fill the matrix from Euler angles in radians in 312 convention'''
00203 c3 = cos(pitch)
00204 s3 = sin(pitch)
00205 s2 = sin(roll)
00206 c2 = cos(roll)
00207 s1 = sin(yaw)
00208 c1 = cos(yaw)
00209
00210 self.a.x = c1 * c3 - s1 * s2 * s3
00211 self.b.y = c1 * c2
00212 self.c.z = c3 * c2
00213 self.a.y = -c2*s1
00214 self.a.z = s3*c1 + c3*s2*s1
00215 self.b.x = c3*s1 + s3*s2*c1
00216 self.b.z = s1*s3 - s2*c1*c3
00217 self.c.x = -s3*c2
00218 self.c.y = s2
00219
00220 def __add__(self, m):
00221 return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c)
00222
00223 __radd__ = __add__
00224
00225 def __sub__(self, m):
00226 return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c)
00227
00228 def __rsub__(self, m):
00229 return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c)
00230
00231 def __mul__(self, other):
00232 if isinstance(other, Vector3):
00233 v = other
00234 return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z,
00235 self.b.x * v.x + self.b.y * v.y + self.b.z * v.z,
00236 self.c.x * v.x + self.c.y * v.y + self.c.z * v.z)
00237 elif isinstance(other, Matrix3):
00238 m = other
00239 return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x,
00240 self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y,
00241 self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z),
00242 Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x,
00243 self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y,
00244 self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z),
00245 Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x,
00246 self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y,
00247 self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z))
00248 v = other
00249 return Matrix3(self.a * v, self.b * v, self.c * v)
00250
00251 def __div__(self, v):
00252 return Matrix3(self.a / v, self.b / v, self.c / v)
00253
00254 def __neg__(self):
00255 return Matrix3(-self.a, -self.b, -self.c)
00256
00257 def __copy__(self):
00258 return Matrix3(self.a, self.b, self.c)
00259
00260 copy = __copy__
00261
00262 def rotate(self, g):
00263 '''rotate the matrix by a given amount on 3 axes'''
00264 temp_matrix = Matrix3()
00265 a = self.a
00266 b = self.b
00267 c = self.c
00268 temp_matrix.a.x = a.y * g.z - a.z * g.y
00269 temp_matrix.a.y = a.z * g.x - a.x * g.z
00270 temp_matrix.a.z = a.x * g.y - a.y * g.x
00271 temp_matrix.b.x = b.y * g.z - b.z * g.y
00272 temp_matrix.b.y = b.z * g.x - b.x * g.z
00273 temp_matrix.b.z = b.x * g.y - b.y * g.x
00274 temp_matrix.c.x = c.y * g.z - c.z * g.y
00275 temp_matrix.c.y = c.z * g.x - c.x * g.z
00276 temp_matrix.c.z = c.x * g.y - c.y * g.x
00277 self.a += temp_matrix.a
00278 self.b += temp_matrix.b
00279 self.c += temp_matrix.c
00280
00281 def normalize(self):
00282 '''re-normalise a rotation matrix'''
00283 error = self.a * self.b
00284 t0 = self.a - (self.b * (0.5 * error))
00285 t1 = self.b - (self.a * (0.5 * error))
00286 t2 = t0 % t1
00287 self.a = t0 * (1.0 / t0.length())
00288 self.b = t1 * (1.0 / t1.length())
00289 self.c = t2 * (1.0 / t2.length())
00290
00291 def trace(self):
00292 '''the trace of the matrix'''
00293 return self.a.x + self.b.y + self.c.z
00294
00295 def from_axis_angle(self, axis, angle):
00296 '''create a rotation matrix from axis and angle'''
00297 ux = axis.x
00298 uy = axis.y
00299 uz = axis.z
00300 ct = cos(angle)
00301 st = sin(angle)
00302 self.a.x = ct + (1-ct) * ux**2
00303 self.a.y = ux*uy*(1-ct) - uz*st
00304 self.a.z = ux*uz*(1-ct) + uy*st
00305 self.b.x = uy*ux*(1-ct) + uz*st
00306 self.b.y = ct + (1-ct) * uy**2
00307 self.b.z = uy*uz*(1-ct) - ux*st
00308 self.c.x = uz*ux*(1-ct) - uy*st
00309 self.c.y = uz*uy*(1-ct) + ux*st
00310 self.c.z = ct + (1-ct) * uz**2
00311
00312
00313 def from_two_vectors(self, vec1, vec2):
00314 '''get a rotation matrix from two vectors.
00315 This returns a rotation matrix which when applied to vec1
00316 will produce a vector pointing in the same direction as vec2'''
00317 angle = vec1.angle(vec2)
00318 cross = vec1 % vec2
00319 if cross.length() == 0:
00320
00321 return self.from_euler(0,0,angle)
00322 cross.normalize()
00323 return self.from_axis_angle(cross, angle)
00324
00325 def close(self, m, tol=1e-7):
00326 return self.a.close(m.a) and self.b.close(m.b) and self.c.close(m.c)
00327
00328 class Plane:
00329 '''a plane in 3 space, defined by a point and a vector normal'''
00330 def __init__(self, point=None, normal=None):
00331 if point is None:
00332 point = Vector3(0,0,0)
00333 if normal is None:
00334 normal = Vector3(0, 0, 1)
00335 self.point = point
00336 self.normal = normal
00337
00338 class Line:
00339 '''a line in 3 space, defined by a point and a vector'''
00340 def __init__(self, point=None, vector=None):
00341 if point is None:
00342 point = Vector3(0,0,0)
00343 if vector is None:
00344 vector = Vector3(0, 0, 1)
00345 self.point = point
00346 self.vector = vector
00347
00348 def plane_intersection(self, plane, forward_only=False):
00349 '''return point where line intersects with a plane'''
00350 l_dot_n = self.vector * plane.normal
00351 if l_dot_n == 0.0:
00352
00353 return None
00354 d = ((plane.point - self.point) * plane.normal) / l_dot_n
00355 if forward_only and d < 0:
00356 return None
00357 return (self.vector * d) + self.point
00358
00359
00360
00361 def test_euler():
00362 '''check that from_euler() and to_euler() are consistent'''
00363 m = Matrix3()
00364 from math import radians, degrees
00365 for r in range(-179, 179, 3):
00366 for p in range(-89, 89, 3):
00367 for y in range(-179, 179, 3):
00368 m.from_euler(radians(r), radians(p), radians(y))
00369 (r2, p2, y2) = m.to_euler()
00370 v1 = Vector3(r,p,y)
00371 v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
00372 diff = v1 - v2
00373 if diff.length() > 1.0e-12:
00374 print('EULER ERROR:', v1, v2, diff.length())
00375
00376
00377 def test_two_vectors():
00378 '''test the from_two_vectors() method'''
00379 import random
00380 for i in range(1000):
00381 v1 = Vector3(1, 0.2, -3)
00382 v2 = Vector3(random.uniform(-5,5), random.uniform(-5,5), random.uniform(-5,5))
00383 m = Matrix3()
00384 m.from_two_vectors(v1, v2)
00385 v3 = m * v1
00386 diff = v3.normalized() - v2.normalized()
00387 (r, p, y) = m.to_euler()
00388 if diff.length() > 0.001:
00389 print('err=%f' % diff.length())
00390 print("r/p/y = %.1f %.1f %.1f" % (
00391 degrees(r), degrees(p), degrees(y)))
00392 print(v1.normalized(), v2.normalized(), v3.normalized())
00393
00394 def test_plane():
00395 '''testing line/plane intersection'''
00396 print("testing plane/line maths")
00397 plane = Plane(Vector3(0,0,0), Vector3(0,0,1))
00398 line = Line(Vector3(0,0,100), Vector3(10, 10, -90))
00399 p = line.plane_intersection(plane)
00400 print(p)
00401
00402 if __name__ == "__main__":
00403 import doctest
00404 doctest.testmod()
00405 test_euler()
00406 test_two_vectors()