source: fact/tools/pyscripts/pyfact/euclid.py@ 13538

Last change on this file since 13538 was 13029, checked in by neise, 14 years ago
class somewhere from the internet. used in coor.py for 2D vectors
File size: 70.7 KB
Line 
1#!/usr/bin/env python
2#
3# euclid graphics maths module
4#
5# Copyright (c) 2006 Alex Holkner
6# Alex.Holkner@mail.google.com
7#
8# This library is free software; you can redistribute it and/or modify it
9# under the terms of the GNU Lesser General Public License as published by the
10# Free Software Foundation; either version 2.1 of the License, or (at your
11# option) any later version.
12#
13# This library is distributed in the hope that it will be useful, but WITHOUT
14# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
15# FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
16# for more details.
17#
18# You should have received a copy of the GNU Lesser General Public License
19# along with this library; if not, write to the Free Software Foundation,
20# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21
22'''euclid graphics maths module
23
24Documentation and tests are included in the file "euclid.txt", or online
25at http://code.google.com/p/pyeuclid
26'''
27
28__docformat__ = 'restructuredtext'
29__version__ = '$Id$'
30__revision__ = '$Revision$'
31
32import math
33import operator
34import types
35
36# Some magic here. If _use_slots is True, the classes will derive from
37# object and will define a __slots__ class variable. If _use_slots is
38# False, classes will be old-style and will not define __slots__.
39#
40# _use_slots = True: Memory efficient, probably faster in future versions
41# of Python, "better".
42# _use_slots = False: Ordinary classes, much faster than slots in current
43# versions of Python (2.4 and 2.5).
44_use_slots = True
45
46# If True, allows components of Vector2 and Vector3 to be set via swizzling;
47# e.g. v.xyz = (1, 2, 3). This is much, much slower than the more verbose
48# v.x = 1; v.y = 2; v.z = 3, and slows down ordinary element setting as
49# well. Recommended setting is False.
50_enable_swizzle_set = False
51
52# Requires class to derive from object.
53if _enable_swizzle_set:
54 _use_slots = True
55
56# Implement _use_slots magic.
57class _EuclidMetaclass(type):
58 def __new__(cls, name, bases, dct):
59 if '__slots__' in dct:
60 dct['__getstate__'] = cls._create_getstate(dct['__slots__'])
61 dct['__setstate__'] = cls._create_setstate(dct['__slots__'])
62 if _use_slots:
63 return type.__new__(cls, name, bases + (object,), dct)
64 else:
65 if '__slots__' in dct:
66 del dct['__slots__']
67 return types.ClassType.__new__(types.ClassType, name, bases, dct)
68
69 @classmethod
70 def _create_getstate(cls, slots):
71 def __getstate__(self):
72 d = {}
73 for slot in slots:
74 d[slot] = getattr(self, slot)
75 return d
76 return __getstate__
77
78 @classmethod
79 def _create_setstate(cls, slots):
80 def __setstate__(self, state):
81 for name, value in state.items():
82 setattr(self, name, value)
83 return __setstate__
84
85__metaclass__ = _EuclidMetaclass
86
87class Vector2:
88 __slots__ = ['x', 'y']
89 __hash__ = None
90
91 def __init__(self, x=0, y=0):
92 self.x = x
93 self.y = y
94
95 def __copy__(self):
96 return self.__class__(self.x, self.y)
97
98 copy = __copy__
99
100 def __repr__(self):
101 return 'Vector2(%.2f, %.2f)' % (self.x, self.y)
102
103 def __eq__(self, other):
104 if isinstance(other, Vector2):
105 return self.x == other.x and \
106 self.y == other.y
107 else:
108 assert hasattr(other, '__len__') and len(other) == 2
109 return self.x == other[0] and \
110 self.y == other[1]
111
112 def __ne__(self, other):
113 return not self.__eq__(other)
114
115 def __nonzero__(self):
116 return self.x != 0 or self.y != 0
117
118 def __len__(self):
119 return 2
120
121 def __getitem__(self, key):
122 return (self.x, self.y)[key]
123
124 def __setitem__(self, key, value):
125 l = [self.x, self.y]
126 l[key] = value
127 self.x, self.y = l
128
129 def __iter__(self):
130 return iter((self.x, self.y))
131
132 def __getattr__(self, name):
133 try:
134 return tuple([(self.x, self.y)['xy'.index(c)] \
135 for c in name])
136 except ValueError:
137 raise AttributeError, name
138
139 if _enable_swizzle_set:
140 # This has detrimental performance on ordinary setattr as well
141 # if enabled
142 def __setattr__(self, name, value):
143 if len(name) == 1:
144 object.__setattr__(self, name, value)
145 else:
146 try:
147 l = [self.x, self.y]
148 for c, v in map(None, name, value):
149 l['xy'.index(c)] = v
150 self.x, self.y = l
151 except ValueError:
152 raise AttributeError, name
153
154 def __add__(self, other):
155 if isinstance(other, Vector2):
156 # Vector + Vector -> Vector
157 # Vector + Point -> Point
158 # Point + Point -> Vector
159 if self.__class__ is other.__class__:
160 _class = Vector2
161 else:
162 _class = Point2
163 return _class(self.x + other.x,
164 self.y + other.y)
165 else:
166 assert hasattr(other, '__len__') and len(other) == 2
167 return Vector2(self.x + other[0],
168 self.y + other[1])
169 __radd__ = __add__
170
171 def __iadd__(self, other):
172 if isinstance(other, Vector2):
173 self.x += other.x
174 self.y += other.y
175 else:
176 self.x += other[0]
177 self.y += other[1]
178 return self
179
180 def __sub__(self, other):
181 if isinstance(other, Vector2):
182 # Vector - Vector -> Vector
183 # Vector - Point -> Point
184 # Point - Point -> Vector
185 if self.__class__ is other.__class__:
186 _class = Vector2
187 else:
188 _class = Point2
189 return _class(self.x - other.x,
190 self.y - other.y)
191 else:
192 assert hasattr(other, '__len__') and len(other) == 2
193 return Vector2(self.x - other[0],
194 self.y - other[1])
195
196
197 def __rsub__(self, other):
198 if isinstance(other, Vector2):
199 return Vector2(other.x - self.x,
200 other.y - self.y)
201 else:
202 assert hasattr(other, '__len__') and len(other) == 2
203 return Vector2(other.x - self[0],
204 other.y - self[1])
205
206 def __mul__(self, other):
207 assert type(other) in (int, long, float)
208 return Vector2(self.x * other,
209 self.y * other)
210
211 __rmul__ = __mul__
212
213 def __imul__(self, other):
214 assert type(other) in (int, long, float)
215 self.x *= other
216 self.y *= other
217 return self
218
219 def __div__(self, other):
220 assert type(other) in (int, long, float)
221 return Vector2(operator.div(self.x, other),
222 operator.div(self.y, other))
223
224
225 def __rdiv__(self, other):
226 assert type(other) in (int, long, float)
227 return Vector2(operator.div(other, self.x),
228 operator.div(other, self.y))
229
230 def __floordiv__(self, other):
231 assert type(other) in (int, long, float)
232 return Vector2(operator.floordiv(self.x, other),
233 operator.floordiv(self.y, other))
234
235
236 def __rfloordiv__(self, other):
237 assert type(other) in (int, long, float)
238 return Vector2(operator.floordiv(other, self.x),
239 operator.floordiv(other, self.y))
240
241 def __truediv__(self, other):
242 assert type(other) in (int, long, float)
243 return Vector2(operator.truediv(self.x, other),
244 operator.truediv(self.y, other))
245
246
247 def __rtruediv__(self, other):
248 assert type(other) in (int, long, float)
249 return Vector2(operator.truediv(other, self.x),
250 operator.truediv(other, self.y))
251
252 def __neg__(self):
253 return Vector2(-self.x,
254 -self.y)
255
256 __pos__ = __copy__
257
258 def __abs__(self):
259 return math.sqrt(self.x ** 2 + \
260 self.y ** 2)
261
262 magnitude = __abs__
263
264 def magnitude_squared(self):
265 return self.x ** 2 + \
266 self.y ** 2
267
268 def normalize(self):
269 d = self.magnitude()
270 if d:
271 self.x /= d
272 self.y /= d
273 return self
274
275 def normalized(self):
276 d = self.magnitude()
277 if d:
278 return Vector2(self.x / d,
279 self.y / d)
280 return self.copy()
281
282 def dot(self, other):
283 assert isinstance(other, Vector2)
284 return self.x * other.x + \
285 self.y * other.y
286
287 def cross(self):
288 return Vector2(self.y, -self.x)
289
290 def reflect(self, normal):
291 # assume normal is normalized
292 assert isinstance(normal, Vector2)
293 d = 2 * (self.x * normal.x + self.y * normal.y)
294 return Vector2(self.x - d * normal.x,
295 self.y - d * normal.y)
296
297 def angle(self, other):
298 """Return the angle to the vector other"""
299 return math.acos(self.dot(other) / (self.magnitude()*other.magnitude()))
300
301 def project(self, other):
302 """Return one vector projected on the vector other"""
303 n = other.normalized()
304 return self.dot(n)*n
305
306class Vector3:
307 __slots__ = ['x', 'y', 'z']
308 __hash__ = None
309
310 def __init__(self, x=0, y=0, z=0):
311 self.x = x
312 self.y = y
313 self.z = z
314
315 def __copy__(self):
316 return self.__class__(self.x, self.y, self.z)
317
318 copy = __copy__
319
320 def __repr__(self):
321 return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
322 self.y,
323 self.z)
324
325 def __eq__(self, other):
326 if isinstance(other, Vector3):
327 return self.x == other.x and \
328 self.y == other.y and \
329 self.z == other.z
330 else:
331 assert hasattr(other, '__len__') and len(other) == 3
332 return self.x == other[0] and \
333 self.y == other[1] and \
334 self.z == other[2]
335
336 def __ne__(self, other):
337 return not self.__eq__(other)
338
339 def __nonzero__(self):
340 return self.x != 0 or self.y != 0 or self.z != 0
341
342 def __len__(self):
343 return 3
344
345 def __getitem__(self, key):
346 return (self.x, self.y, self.z)[key]
347
348 def __setitem__(self, key, value):
349 l = [self.x, self.y, self.z]
350 l[key] = value
351 self.x, self.y, self.z = l
352
353 def __iter__(self):
354 return iter((self.x, self.y, self.z))
355
356 def __getattr__(self, name):
357 try:
358 return tuple([(self.x, self.y, self.z)['xyz'.index(c)] \
359 for c in name])
360 except ValueError:
361 raise AttributeError, name
362
363 if _enable_swizzle_set:
364 # This has detrimental performance on ordinary setattr as well
365 # if enabled
366 def __setattr__(self, name, value):
367 if len(name) == 1:
368 object.__setattr__(self, name, value)
369 else:
370 try:
371 l = [self.x, self.y, self.z]
372 for c, v in map(None, name, value):
373 l['xyz'.index(c)] = v
374 self.x, self.y, self.z = l
375 except ValueError:
376 raise AttributeError, name
377
378
379 def __add__(self, other):
380 if isinstance(other, Vector3):
381 # Vector + Vector -> Vector
382 # Vector + Point -> Point
383 # Point + Point -> Vector
384 if self.__class__ is other.__class__:
385 _class = Vector3
386 else:
387 _class = Point3
388 return _class(self.x + other.x,
389 self.y + other.y,
390 self.z + other.z)
391 else:
392 assert hasattr(other, '__len__') and len(other) == 3
393 return Vector3(self.x + other[0],
394 self.y + other[1],
395 self.z + other[2])
396 __radd__ = __add__
397
398 def __iadd__(self, other):
399 if isinstance(other, Vector3):
400 self.x += other.x
401 self.y += other.y
402 self.z += other.z
403 else:
404 self.x += other[0]
405 self.y += other[1]
406 self.z += other[2]
407 return self
408
409 def __sub__(self, other):
410 if isinstance(other, Vector3):
411 # Vector - Vector -> Vector
412 # Vector - Point -> Point
413 # Point - Point -> Vector
414 if self.__class__ is other.__class__:
415 _class = Vector3
416 else:
417 _class = Point3
418 return Vector3(self.x - other.x,
419 self.y - other.y,
420 self.z - other.z)
421 else:
422 assert hasattr(other, '__len__') and len(other) == 3
423 return Vector3(self.x - other[0],
424 self.y - other[1],
425 self.z - other[2])
426
427
428 def __rsub__(self, other):
429 if isinstance(other, Vector3):
430 return Vector3(other.x - self.x,
431 other.y - self.y,
432 other.z - self.z)
433 else:
434 assert hasattr(other, '__len__') and len(other) == 3
435 return Vector3(other.x - self[0],
436 other.y - self[1],
437 other.z - self[2])
438
439 def __mul__(self, other):
440 if isinstance(other, Vector3):
441 # TODO component-wise mul/div in-place and on Vector2; docs.
442 if self.__class__ is Point3 or other.__class__ is Point3:
443 _class = Point3
444 else:
445 _class = Vector3
446 return _class(self.x * other.x,
447 self.y * other.y,
448 self.z * other.z)
449 else:
450 assert type(other) in (int, long, float)
451 return Vector3(self.x * other,
452 self.y * other,
453 self.z * other)
454
455 __rmul__ = __mul__
456
457 def __imul__(self, other):
458 assert type(other) in (int, long, float)
459 self.x *= other
460 self.y *= other
461 self.z *= other
462 return self
463
464 def __div__(self, other):
465 assert type(other) in (int, long, float)
466 return Vector3(operator.div(self.x, other),
467 operator.div(self.y, other),
468 operator.div(self.z, other))
469
470
471 def __rdiv__(self, other):
472 assert type(other) in (int, long, float)
473 return Vector3(operator.div(other, self.x),
474 operator.div(other, self.y),
475 operator.div(other, self.z))
476
477 def __floordiv__(self, other):
478 assert type(other) in (int, long, float)
479 return Vector3(operator.floordiv(self.x, other),
480 operator.floordiv(self.y, other),
481 operator.floordiv(self.z, other))
482
483
484 def __rfloordiv__(self, other):
485 assert type(other) in (int, long, float)
486 return Vector3(operator.floordiv(other, self.x),
487 operator.floordiv(other, self.y),
488 operator.floordiv(other, self.z))
489
490 def __truediv__(self, other):
491 assert type(other) in (int, long, float)
492 return Vector3(operator.truediv(self.x, other),
493 operator.truediv(self.y, other),
494 operator.truediv(self.z, other))
495
496
497 def __rtruediv__(self, other):
498 assert type(other) in (int, long, float)
499 return Vector3(operator.truediv(other, self.x),
500 operator.truediv(other, self.y),
501 operator.truediv(other, self.z))
502
503 def __neg__(self):
504 return Vector3(-self.x,
505 -self.y,
506 -self.z)
507
508 __pos__ = __copy__
509
510 def __abs__(self):
511 return math.sqrt(self.x ** 2 + \
512 self.y ** 2 + \
513 self.z ** 2)
514
515 magnitude = __abs__
516
517 def magnitude_squared(self):
518 return self.x ** 2 + \
519 self.y ** 2 + \
520 self.z ** 2
521
522 def normalize(self):
523 d = self.magnitude()
524 if d:
525 self.x /= d
526 self.y /= d
527 self.z /= d
528 return self
529
530 def normalized(self):
531 d = self.magnitude()
532 if d:
533 return Vector3(self.x / d,
534 self.y / d,
535 self.z / d)
536 return self.copy()
537
538 def dot(self, other):
539 assert isinstance(other, Vector3)
540 return self.x * other.x + \
541 self.y * other.y + \
542 self.z * other.z
543
544 def cross(self, other):
545 assert isinstance(other, Vector3)
546 return Vector3(self.y * other.z - self.z * other.y,
547 -self.x * other.z + self.z * other.x,
548 self.x * other.y - self.y * other.x)
549
550 def reflect(self, normal):
551 # assume normal is normalized
552 assert isinstance(normal, Vector3)
553 d = 2 * (self.x * normal.x + self.y * normal.y + self.z * normal.z)
554 return Vector3(self.x - d * normal.x,
555 self.y - d * normal.y,
556 self.z - d * normal.z)
557
558 def rotate_around(self, axis, theta):
559 """Return the vector rotated around axis through angle theta. Right hand rule applies"""
560
561 # Adapted from equations published by Glenn Murray.
562 # http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html
563 x, y, z = self.x, self.y,self.z
564 u, v, w = axis.x, axis.y, axis.z
565
566 # Extracted common factors for simplicity and efficiency
567 r2 = u**2 + v**2 + w**2
568 r = math.sqrt(r2)
569 ct = math.cos(theta)
570 st = math.sin(theta) / r
571 dt = (u*x + v*y + w*z) * (1 - ct) / r2
572 return Vector3((u * dt + x * ct + (-w * y + v * z) * st),
573 (v * dt + y * ct + ( w * x - u * z) * st),
574 (w * dt + z * ct + (-v * x + u * y) * st))
575
576 def angle(self, other):
577 """Return the angle to the vector other"""
578 return math.acos(self.dot(other) / (self.magnitude()*other.magnitude()))
579
580 def project(self, other):
581 """Return one vector projected on the vector other"""
582 n = other.normalized()
583 return self.dot(n)*n
584
585# a b c
586# e f g
587# i j k
588
589class Matrix3:
590 __slots__ = list('abcefgijk')
591
592 def __init__(self):
593 self.identity()
594
595 def __copy__(self):
596 M = Matrix3()
597 M.a = self.a
598 M.b = self.b
599 M.c = self.c
600 M.e = self.e
601 M.f = self.f
602 M.g = self.g
603 M.i = self.i
604 M.j = self.j
605 M.k = self.k
606 return M
607
608 copy = __copy__
609 def __repr__(self):
610 return ('Matrix3([% 8.2f % 8.2f % 8.2f\n' \
611 ' % 8.2f % 8.2f % 8.2f\n' \
612 ' % 8.2f % 8.2f % 8.2f])') \
613 % (self.a, self.b, self.c,
614 self.e, self.f, self.g,
615 self.i, self.j, self.k)
616
617 def __getitem__(self, key):
618 return [self.a, self.e, self.i,
619 self.b, self.f, self.j,
620 self.c, self.g, self.k][key]
621
622 def __setitem__(self, key, value):
623 L = self[:]
624 L[key] = value
625 (self.a, self.e, self.i,
626 self.b, self.f, self.j,
627 self.c, self.g, self.k) = L
628
629 def __mul__(self, other):
630 if isinstance(other, Matrix3):
631 # Caching repeatedly accessed attributes in local variables
632 # apparently increases performance by 20%. Attrib: Will McGugan.
633 Aa = self.a
634 Ab = self.b
635 Ac = self.c
636 Ae = self.e
637 Af = self.f
638 Ag = self.g
639 Ai = self.i
640 Aj = self.j
641 Ak = self.k
642 Ba = other.a
643 Bb = other.b
644 Bc = other.c
645 Be = other.e
646 Bf = other.f
647 Bg = other.g
648 Bi = other.i
649 Bj = other.j
650 Bk = other.k
651 C = Matrix3()
652 C.a = Aa * Ba + Ab * Be + Ac * Bi
653 C.b = Aa * Bb + Ab * Bf + Ac * Bj
654 C.c = Aa * Bc + Ab * Bg + Ac * Bk
655 C.e = Ae * Ba + Af * Be + Ag * Bi
656 C.f = Ae * Bb + Af * Bf + Ag * Bj
657 C.g = Ae * Bc + Af * Bg + Ag * Bk
658 C.i = Ai * Ba + Aj * Be + Ak * Bi
659 C.j = Ai * Bb + Aj * Bf + Ak * Bj
660 C.k = Ai * Bc + Aj * Bg + Ak * Bk
661 return C
662 elif isinstance(other, Point2):
663 A = self
664 B = other
665 P = Point2(0, 0)
666 P.x = A.a * B.x + A.b * B.y + A.c
667 P.y = A.e * B.x + A.f * B.y + A.g
668 return P
669 elif isinstance(other, Vector2):
670 A = self
671 B = other
672 V = Vector2(0, 0)
673 V.x = A.a * B.x + A.b * B.y
674 V.y = A.e * B.x + A.f * B.y
675 return V
676 else:
677 other = other.copy()
678 other._apply_transform(self)
679 return other
680
681 def __imul__(self, other):
682 assert isinstance(other, Matrix3)
683 # Cache attributes in local vars (see Matrix3.__mul__).
684 Aa = self.a
685 Ab = self.b
686 Ac = self.c
687 Ae = self.e
688 Af = self.f
689 Ag = self.g
690 Ai = self.i
691 Aj = self.j
692 Ak = self.k
693 Ba = other.a
694 Bb = other.b
695 Bc = other.c
696 Be = other.e
697 Bf = other.f
698 Bg = other.g
699 Bi = other.i
700 Bj = other.j
701 Bk = other.k
702 self.a = Aa * Ba + Ab * Be + Ac * Bi
703 self.b = Aa * Bb + Ab * Bf + Ac * Bj
704 self.c = Aa * Bc + Ab * Bg + Ac * Bk
705 self.e = Ae * Ba + Af * Be + Ag * Bi
706 self.f = Ae * Bb + Af * Bf + Ag * Bj
707 self.g = Ae * Bc + Af * Bg + Ag * Bk
708 self.i = Ai * Ba + Aj * Be + Ak * Bi
709 self.j = Ai * Bb + Aj * Bf + Ak * Bj
710 self.k = Ai * Bc + Aj * Bg + Ak * Bk
711 return self
712
713 def identity(self):
714 self.a = self.f = self.k = 1.
715 self.b = self.c = self.e = self.g = self.i = self.j = 0
716 return self
717
718 def scale(self, x, y):
719 self *= Matrix3.new_scale(x, y)
720 return self
721
722 def translate(self, x, y):
723 self *= Matrix3.new_translate(x, y)
724 return self
725
726 def rotate(self, angle):
727 self *= Matrix3.new_rotate(angle)
728 return self
729
730 # Static constructors
731 def new_identity(cls):
732 self = cls()
733 return self
734 new_identity = classmethod(new_identity)
735
736 def new_scale(cls, x, y):
737 self = cls()
738 self.a = x
739 self.f = y
740 return self
741 new_scale = classmethod(new_scale)
742
743 def new_translate(cls, x, y):
744 self = cls()
745 self.c = x
746 self.g = y
747 return self
748 new_translate = classmethod(new_translate)
749
750 def new_rotate(cls, angle):
751 self = cls()
752 s = math.sin(angle)
753 c = math.cos(angle)
754 self.a = self.f = c
755 self.b = -s
756 self.e = s
757 return self
758 new_rotate = classmethod(new_rotate)
759
760 def determinant(self):
761 return (self.a*self.f*self.k
762 + self.b*self.g*self.i
763 + self.c*self.e*self.j
764 - self.a*self.g*self.j
765 - self.b*self.e*self.k
766 - self.c*self.f*self.i)
767
768 def inverse(self):
769 tmp = Matrix3()
770 d = self.determinant()
771
772 if abs(d) < 0.001:
773 # No inverse, return identity
774 return tmp
775 else:
776 d = 1.0 / d
777
778 tmp.a = d * (self.f*self.k - self.g*self.j)
779 tmp.b = d * (self.c*self.j - self.b*self.k)
780 tmp.c = d * (self.b*self.g - self.c*self.f)
781 tmp.e = d * (self.g*self.i - self.e*self.k)
782 tmp.f = d * (self.a*self.k - self.c*self.i)
783 tmp.g = d * (self.c*self.e - self.a*self.g)
784 tmp.i = d * (self.e*self.j - self.f*self.i)
785 tmp.j = d * (self.b*self.i - self.a*self.j)
786 tmp.k = d * (self.a*self.f - self.b*self.e)
787
788 return tmp
789
790# a b c d
791# e f g h
792# i j k l
793# m n o p
794
795class Matrix4:
796 __slots__ = list('abcdefghijklmnop')
797
798 def __init__(self):
799 self.identity()
800
801 def __copy__(self):
802 M = Matrix4()
803 M.a = self.a
804 M.b = self.b
805 M.c = self.c
806 M.d = self.d
807 M.e = self.e
808 M.f = self.f
809 M.g = self.g
810 M.h = self.h
811 M.i = self.i
812 M.j = self.j
813 M.k = self.k
814 M.l = self.l
815 M.m = self.m
816 M.n = self.n
817 M.o = self.o
818 M.p = self.p
819 return M
820
821 copy = __copy__
822
823
824 def __repr__(self):
825 return ('Matrix4([% 8.2f % 8.2f % 8.2f % 8.2f\n' \
826 ' % 8.2f % 8.2f % 8.2f % 8.2f\n' \
827 ' % 8.2f % 8.2f % 8.2f % 8.2f\n' \
828 ' % 8.2f % 8.2f % 8.2f % 8.2f])') \
829 % (self.a, self.b, self.c, self.d,
830 self.e, self.f, self.g, self.h,
831 self.i, self.j, self.k, self.l,
832 self.m, self.n, self.o, self.p)
833
834 def __getitem__(self, key):
835 return [self.a, self.e, self.i, self.m,
836 self.b, self.f, self.j, self.n,
837 self.c, self.g, self.k, self.o,
838 self.d, self.h, self.l, self.p][key]
839
840 def __setitem__(self, key, value):
841 L = self[:]
842 L[key] = value
843 (self.a, self.e, self.i, self.m,
844 self.b, self.f, self.j, self.n,
845 self.c, self.g, self.k, self.o,
846 self.d, self.h, self.l, self.p) = L
847
848 def __mul__(self, other):
849 if isinstance(other, Matrix4):
850 # Cache attributes in local vars (see Matrix3.__mul__).
851 Aa = self.a
852 Ab = self.b
853 Ac = self.c
854 Ad = self.d
855 Ae = self.e
856 Af = self.f
857 Ag = self.g
858 Ah = self.h
859 Ai = self.i
860 Aj = self.j
861 Ak = self.k
862 Al = self.l
863 Am = self.m
864 An = self.n
865 Ao = self.o
866 Ap = self.p
867 Ba = other.a
868 Bb = other.b
869 Bc = other.c
870 Bd = other.d
871 Be = other.e
872 Bf = other.f
873 Bg = other.g
874 Bh = other.h
875 Bi = other.i
876 Bj = other.j
877 Bk = other.k
878 Bl = other.l
879 Bm = other.m
880 Bn = other.n
881 Bo = other.o
882 Bp = other.p
883 C = Matrix4()
884 C.a = Aa * Ba + Ab * Be + Ac * Bi + Ad * Bm
885 C.b = Aa * Bb + Ab * Bf + Ac * Bj + Ad * Bn
886 C.c = Aa * Bc + Ab * Bg + Ac * Bk + Ad * Bo
887 C.d = Aa * Bd + Ab * Bh + Ac * Bl + Ad * Bp
888 C.e = Ae * Ba + Af * Be + Ag * Bi + Ah * Bm
889 C.f = Ae * Bb + Af * Bf + Ag * Bj + Ah * Bn
890 C.g = Ae * Bc + Af * Bg + Ag * Bk + Ah * Bo
891 C.h = Ae * Bd + Af * Bh + Ag * Bl + Ah * Bp
892 C.i = Ai * Ba + Aj * Be + Ak * Bi + Al * Bm
893 C.j = Ai * Bb + Aj * Bf + Ak * Bj + Al * Bn
894 C.k = Ai * Bc + Aj * Bg + Ak * Bk + Al * Bo
895 C.l = Ai * Bd + Aj * Bh + Ak * Bl + Al * Bp
896 C.m = Am * Ba + An * Be + Ao * Bi + Ap * Bm
897 C.n = Am * Bb + An * Bf + Ao * Bj + Ap * Bn
898 C.o = Am * Bc + An * Bg + Ao * Bk + Ap * Bo
899 C.p = Am * Bd + An * Bh + Ao * Bl + Ap * Bp
900 return C
901 elif isinstance(other, Point3):
902 A = self
903 B = other
904 P = Point3(0, 0, 0)
905 P.x = A.a * B.x + A.b * B.y + A.c * B.z + A.d
906 P.y = A.e * B.x + A.f * B.y + A.g * B.z + A.h
907 P.z = A.i * B.x + A.j * B.y + A.k * B.z + A.l
908 return P
909 elif isinstance(other, Vector3):
910 A = self
911 B = other
912 V = Vector3(0, 0, 0)
913 V.x = A.a * B.x + A.b * B.y + A.c * B.z
914 V.y = A.e * B.x + A.f * B.y + A.g * B.z
915 V.z = A.i * B.x + A.j * B.y + A.k * B.z
916 return V
917 else:
918 other = other.copy()
919 other._apply_transform(self)
920 return other
921
922 def __imul__(self, other):
923 assert isinstance(other, Matrix4)
924 # Cache attributes in local vars (see Matrix3.__mul__).
925 Aa = self.a
926 Ab = self.b
927 Ac = self.c
928 Ad = self.d
929 Ae = self.e
930 Af = self.f
931 Ag = self.g
932 Ah = self.h
933 Ai = self.i
934 Aj = self.j
935 Ak = self.k
936 Al = self.l
937 Am = self.m
938 An = self.n
939 Ao = self.o
940 Ap = self.p
941 Ba = other.a
942 Bb = other.b
943 Bc = other.c
944 Bd = other.d
945 Be = other.e
946 Bf = other.f
947 Bg = other.g
948 Bh = other.h
949 Bi = other.i
950 Bj = other.j
951 Bk = other.k
952 Bl = other.l
953 Bm = other.m
954 Bn = other.n
955 Bo = other.o
956 Bp = other.p
957 self.a = Aa * Ba + Ab * Be + Ac * Bi + Ad * Bm
958 self.b = Aa * Bb + Ab * Bf + Ac * Bj + Ad * Bn
959 self.c = Aa * Bc + Ab * Bg + Ac * Bk + Ad * Bo
960 self.d = Aa * Bd + Ab * Bh + Ac * Bl + Ad * Bp
961 self.e = Ae * Ba + Af * Be + Ag * Bi + Ah * Bm
962 self.f = Ae * Bb + Af * Bf + Ag * Bj + Ah * Bn
963 self.g = Ae * Bc + Af * Bg + Ag * Bk + Ah * Bo
964 self.h = Ae * Bd + Af * Bh + Ag * Bl + Ah * Bp
965 self.i = Ai * Ba + Aj * Be + Ak * Bi + Al * Bm
966 self.j = Ai * Bb + Aj * Bf + Ak * Bj + Al * Bn
967 self.k = Ai * Bc + Aj * Bg + Ak * Bk + Al * Bo
968 self.l = Ai * Bd + Aj * Bh + Ak * Bl + Al * Bp
969 self.m = Am * Ba + An * Be + Ao * Bi + Ap * Bm
970 self.n = Am * Bb + An * Bf + Ao * Bj + Ap * Bn
971 self.o = Am * Bc + An * Bg + Ao * Bk + Ap * Bo
972 self.p = Am * Bd + An * Bh + Ao * Bl + Ap * Bp
973 return self
974
975 def transform(self, other):
976 A = self
977 B = other
978 P = Point3(0, 0, 0)
979 P.x = A.a * B.x + A.b * B.y + A.c * B.z + A.d
980 P.y = A.e * B.x + A.f * B.y + A.g * B.z + A.h
981 P.z = A.i * B.x + A.j * B.y + A.k * B.z + A.l
982 w = A.m * B.x + A.n * B.y + A.o * B.z + A.p
983 if w != 0:
984 P.x /= w
985 P.y /= w
986 P.z /= w
987 return P
988
989 def identity(self):
990 self.a = self.f = self.k = self.p = 1.
991 self.b = self.c = self.d = self.e = self.g = self.h = \
992 self.i = self.j = self.l = self.m = self.n = self.o = 0
993 return self
994
995 def scale(self, x, y, z):
996 self *= Matrix4.new_scale(x, y, z)
997 return self
998
999 def translate(self, x, y, z):
1000 self *= Matrix4.new_translate(x, y, z)
1001 return self
1002
1003 def rotatex(self, angle):
1004 self *= Matrix4.new_rotatex(angle)
1005 return self
1006
1007 def rotatey(self, angle):
1008 self *= Matrix4.new_rotatey(angle)
1009 return self
1010
1011 def rotatez(self, angle):
1012 self *= Matrix4.new_rotatez(angle)
1013 return self
1014
1015 def rotate_axis(self, angle, axis):
1016 self *= Matrix4.new_rotate_axis(angle, axis)
1017 return self
1018
1019 def rotate_euler(self, heading, attitude, bank):
1020 self *= Matrix4.new_rotate_euler(heading, attitude, bank)
1021 return self
1022
1023 def rotate_triple_axis(self, x, y, z):
1024 self *= Matrix4.new_rotate_triple_axis(x, y, z)
1025 return self
1026
1027 def transpose(self):
1028 (self.a, self.e, self.i, self.m,
1029 self.b, self.f, self.j, self.n,
1030 self.c, self.g, self.k, self.o,
1031 self.d, self.h, self.l, self.p) = \
1032 (self.a, self.b, self.c, self.d,
1033 self.e, self.f, self.g, self.h,
1034 self.i, self.j, self.k, self.l,
1035 self.m, self.n, self.o, self.p)
1036
1037 def transposed(self):
1038 M = self.copy()
1039 M.transpose()
1040 return M
1041
1042 # Static constructors
1043 def new(cls, *values):
1044 M = cls()
1045 M[:] = values
1046 return M
1047 new = classmethod(new)
1048
1049 def new_identity(cls):
1050 self = cls()
1051 return self
1052 new_identity = classmethod(new_identity)
1053
1054 def new_scale(cls, x, y, z):
1055 self = cls()
1056 self.a = x
1057 self.f = y
1058 self.k = z
1059 return self
1060 new_scale = classmethod(new_scale)
1061
1062 def new_translate(cls, x, y, z):
1063 self = cls()
1064 self.d = x
1065 self.h = y
1066 self.l = z
1067 return self
1068 new_translate = classmethod(new_translate)
1069
1070 def new_rotatex(cls, angle):
1071 self = cls()
1072 s = math.sin(angle)
1073 c = math.cos(angle)
1074 self.f = self.k = c
1075 self.g = -s
1076 self.j = s
1077 return self
1078 new_rotatex = classmethod(new_rotatex)
1079
1080 def new_rotatey(cls, angle):
1081 self = cls()
1082 s = math.sin(angle)
1083 c = math.cos(angle)
1084 self.a = self.k = c
1085 self.c = s
1086 self.i = -s
1087 return self
1088 new_rotatey = classmethod(new_rotatey)
1089
1090 def new_rotatez(cls, angle):
1091 self = cls()
1092 s = math.sin(angle)
1093 c = math.cos(angle)
1094 self.a = self.f = c
1095 self.b = -s
1096 self.e = s
1097 return self
1098 new_rotatez = classmethod(new_rotatez)
1099
1100 def new_rotate_axis(cls, angle, axis):
1101 assert(isinstance(axis, Vector3))
1102 vector = axis.normalized()
1103 x = vector.x
1104 y = vector.y
1105 z = vector.z
1106
1107 self = cls()
1108 s = math.sin(angle)
1109 c = math.cos(angle)
1110 c1 = 1. - c
1111
1112 # from the glRotate man page
1113 self.a = x * x * c1 + c
1114 self.b = x * y * c1 - z * s
1115 self.c = x * z * c1 + y * s
1116 self.e = y * x * c1 + z * s
1117 self.f = y * y * c1 + c
1118 self.g = y * z * c1 - x * s
1119 self.i = x * z * c1 - y * s
1120 self.j = y * z * c1 + x * s
1121 self.k = z * z * c1 + c
1122 return self
1123 new_rotate_axis = classmethod(new_rotate_axis)
1124
1125 def new_rotate_euler(cls, heading, attitude, bank):
1126 # from http://www.euclideanspace.com/
1127 ch = math.cos(heading)
1128 sh = math.sin(heading)
1129 ca = math.cos(attitude)
1130 sa = math.sin(attitude)
1131 cb = math.cos(bank)
1132 sb = math.sin(bank)
1133
1134 self = cls()
1135 self.a = ch * ca
1136 self.b = sh * sb - ch * sa * cb
1137 self.c = ch * sa * sb + sh * cb
1138 self.e = sa
1139 self.f = ca * cb
1140 self.g = -ca * sb
1141 self.i = -sh * ca
1142 self.j = sh * sa * cb + ch * sb
1143 self.k = -sh * sa * sb + ch * cb
1144 return self
1145 new_rotate_euler = classmethod(new_rotate_euler)
1146
1147 def new_rotate_triple_axis(cls, x, y, z):
1148 m = cls()
1149
1150 m.a, m.b, m.c = x.x, y.x, z.x
1151 m.e, m.f, m.g = x.y, y.y, z.y
1152 m.i, m.j, m.k = x.z, y.z, z.z
1153
1154 return m
1155 new_rotate_triple_axis = classmethod(new_rotate_triple_axis)
1156
1157 def new_look_at(cls, eye, at, up):
1158 z = (eye - at).normalized()
1159 x = up.cross(z).normalized()
1160 y = z.cross(x)
1161
1162 m = cls.new_rotate_triple_axis(x, y, z)
1163 m.d, m.h, m.l = eye.x, eye.y, eye.z
1164 return m
1165 new_look_at = classmethod(new_look_at)
1166
1167 def new_perspective(cls, fov_y, aspect, near, far):
1168 # from the gluPerspective man page
1169 f = 1 / math.tan(fov_y / 2)
1170 self = cls()
1171 assert near != 0.0 and near != far
1172 self.a = f / aspect
1173 self.f = f
1174 self.k = (far + near) / (near - far)
1175 self.l = 2 * far * near / (near - far)
1176 self.o = -1
1177 self.p = 0
1178 return self
1179 new_perspective = classmethod(new_perspective)
1180
1181 def determinant(self):
1182 return ((self.a * self.f - self.e * self.b)
1183 * (self.k * self.p - self.o * self.l)
1184 - (self.a * self.j - self.i * self.b)
1185 * (self.g * self.p - self.o * self.h)
1186 + (self.a * self.n - self.m * self.b)
1187 * (self.g * self.l - self.k * self.h)
1188 + (self.e * self.j - self.i * self.f)
1189 * (self.c * self.p - self.o * self.d)
1190 - (self.e * self.n - self.m * self.f)
1191 * (self.c * self.l - self.k * self.d)
1192 + (self.i * self.n - self.m * self.j)
1193 * (self.c * self.h - self.g * self.d))
1194
1195 def inverse(self):
1196 tmp = Matrix4()
1197 d = self.determinant();
1198
1199 if abs(d) < 0.001:
1200 # No inverse, return identity
1201 return tmp
1202 else:
1203 d = 1.0 / d;
1204
1205 tmp.a = d * (self.f * (self.k * self.p - self.o * self.l) + self.j * (self.o * self.h - self.g * self.p) + self.n * (self.g * self.l - self.k * self.h));
1206 tmp.e = d * (self.g * (self.i * self.p - self.m * self.l) + self.k * (self.m * self.h - self.e * self.p) + self.o * (self.e * self.l - self.i * self.h));
1207 tmp.i = d * (self.h * (self.i * self.n - self.m * self.j) + self.l * (self.m * self.f - self.e * self.n) + self.p * (self.e * self.j - self.i * self.f));
1208 tmp.m = d * (self.e * (self.n * self.k - self.j * self.o) + self.i * (self.f * self.o - self.n * self.g) + self.m * (self.j * self.g - self.f * self.k));
1209
1210 tmp.b = d * (self.j * (self.c * self.p - self.o * self.d) + self.n * (self.k * self.d - self.c * self.l) + self.b * (self.o * self.l - self.k * self.p));
1211 tmp.f = d * (self.k * (self.a * self.p - self.m * self.d) + self.o * (self.i * self.d - self.a * self.l) + self.c * (self.m * self.l - self.i * self.p));
1212 tmp.j = d * (self.l * (self.a * self.n - self.m * self.b) + self.p * (self.i * self.b - self.a * self.j) + self.d * (self.m * self.j - self.i * self.n));
1213 tmp.n = d * (self.i * (self.n * self.c - self.b * self.o) + self.m * (self.b * self.k - self.j * self.c) + self.a * (self.j * self.o - self.n * self.k));
1214
1215 tmp.c = d * (self.n * (self.c * self.h - self.g * self.d) + self.b * (self.g * self.p - self.o * self.h) + self.f * (self.o * self.d - self.c * self.p));
1216 tmp.g = d * (self.o * (self.a * self.h - self.e * self.d) + self.c * (self.e * self.p - self.m * self.h) + self.g * (self.m * self.d - self.a * self.p));
1217 tmp.k = d * (self.p * (self.a * self.f - self.e * self.b) + self.d * (self.e * self.n - self.m * self.f) + self.h * (self.m * self.b - self.a * self.n));
1218 tmp.o = d * (self.m * (self.f * self.c - self.b * self.g) + self.a * (self.n * self.g - self.f * self.o) + self.e * (self.b * self.o - self.n * self.c));
1219
1220 tmp.d = d * (self.b * (self.k * self.h - self.g * self.l) + self.f * (self.c * self.l - self.k * self.d) + self.j * (self.g * self.d - self.c * self.h));
1221 tmp.h = d * (self.c * (self.i * self.h - self.e * self.l) + self.g * (self.a * self.l - self.i * self.d) + self.k * (self.e * self.d - self.a * self.h));
1222 tmp.l = d * (self.d * (self.i * self.f - self.e * self.j) + self.h * (self.a * self.j - self.i * self.b) + self.l * (self.e * self.b - self.a * self.f));
1223 tmp.p = d * (self.a * (self.f * self.k - self.j * self.g) + self.e * (self.j * self.c - self.b * self.k) + self.i * (self.b * self.g - self.f * self.c));
1224
1225 return tmp;
1226
1227
1228class Quaternion:
1229 # All methods and naming conventions based off
1230 # http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions
1231
1232 # w is the real part, (x, y, z) are the imaginary parts
1233 __slots__ = ['w', 'x', 'y', 'z']
1234
1235 def __init__(self, w=1, x=0, y=0, z=0):
1236 self.w = w
1237 self.x = x
1238 self.y = y
1239 self.z = z
1240
1241 def __copy__(self):
1242 Q = Quaternion()
1243 Q.w = self.w
1244 Q.x = self.x
1245 Q.y = self.y
1246 Q.z = self.z
1247 return Q
1248
1249 copy = __copy__
1250
1251 def __repr__(self):
1252 return 'Quaternion(real=%.2f, imag=<%.2f, %.2f, %.2f>)' % \
1253 (self.w, self.x, self.y, self.z)
1254
1255 def __mul__(self, other):
1256 if isinstance(other, Quaternion):
1257 Ax = self.x
1258 Ay = self.y
1259 Az = self.z
1260 Aw = self.w
1261 Bx = other.x
1262 By = other.y
1263 Bz = other.z
1264 Bw = other.w
1265 Q = Quaternion()
1266 Q.x = Ax * Bw + Ay * Bz - Az * By + Aw * Bx
1267 Q.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By
1268 Q.z = Ax * By - Ay * Bx + Az * Bw + Aw * Bz
1269 Q.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw
1270 return Q
1271 elif isinstance(other, Vector3):
1272 w = self.w
1273 x = self.x
1274 y = self.y
1275 z = self.z
1276 Vx = other.x
1277 Vy = other.y
1278 Vz = other.z
1279 ww = w * w
1280 w2 = w * 2
1281 wx2 = w2 * x
1282 wy2 = w2 * y
1283 wz2 = w2 * z
1284 xx = x * x
1285 x2 = x * 2
1286 xy2 = x2 * y
1287 xz2 = x2 * z
1288 yy = y * y
1289 yz2 = 2 * y * z
1290 zz = z * z
1291 return other.__class__(\
1292 ww * Vx + wy2 * Vz - wz2 * Vy + \
1293 xx * Vx + xy2 * Vy + xz2 * Vz - \
1294 zz * Vx - yy * Vx,
1295 xy2 * Vx + yy * Vy + yz2 * Vz + \
1296 wz2 * Vx - zz * Vy + ww * Vy - \
1297 wx2 * Vz - xx * Vy,
1298 xz2 * Vx + yz2 * Vy + \
1299 zz * Vz - wy2 * Vx - yy * Vz + \
1300 wx2 * Vy - xx * Vz + ww * Vz)
1301 else:
1302 other = other.copy()
1303 other._apply_transform(self)
1304 return other
1305
1306 def __imul__(self, other):
1307 assert isinstance(other, Quaternion)
1308 Ax = self.x
1309 Ay = self.y
1310 Az = self.z
1311 Aw = self.w
1312 Bx = other.x
1313 By = other.y
1314 Bz = other.z
1315 Bw = other.w
1316 self.x = Ax * Bw + Ay * Bz - Az * By + Aw * Bx
1317 self.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By
1318 self.z = Ax * By - Ay * Bx + Az * Bw + Aw * Bz
1319 self.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw
1320 return self
1321
1322 def __abs__(self):
1323 return math.sqrt(self.w ** 2 + \
1324 self.x ** 2 + \
1325 self.y ** 2 + \
1326 self.z ** 2)
1327
1328 magnitude = __abs__
1329
1330 def magnitude_squared(self):
1331 return self.w ** 2 + \
1332 self.x ** 2 + \
1333 self.y ** 2 + \
1334 self.z ** 2
1335
1336 def identity(self):
1337 self.w = 1
1338 self.x = 0
1339 self.y = 0
1340 self.z = 0
1341 return self
1342
1343 def rotate_axis(self, angle, axis):
1344 self *= Quaternion.new_rotate_axis(angle, axis)
1345 return self
1346
1347 def rotate_euler(self, heading, attitude, bank):
1348 self *= Quaternion.new_rotate_euler(heading, attitude, bank)
1349 return self
1350
1351 def rotate_matrix(self, m):
1352 self *= Quaternion.new_rotate_matrix(m)
1353 return self
1354
1355 def conjugated(self):
1356 Q = Quaternion()
1357 Q.w = self.w
1358 Q.x = -self.x
1359 Q.y = -self.y
1360 Q.z = -self.z
1361 return Q
1362
1363 def normalize(self):
1364 d = self.magnitude()
1365 if d != 0:
1366 self.w /= d
1367 self.x /= d
1368 self.y /= d
1369 self.z /= d
1370 return self
1371
1372 def normalized(self):
1373 d = self.magnitude()
1374 if d != 0:
1375 Q = Quaternion()
1376 Q.w = self.w / d
1377 Q.x = self.x / d
1378 Q.y = self.y / d
1379 Q.z = self.z / d
1380 return Q
1381 else:
1382 return self.copy()
1383
1384 def get_angle_axis(self):
1385 if self.w > 1:
1386 self = self.normalized()
1387 angle = 2 * math.acos(self.w)
1388 s = math.sqrt(1 - self.w ** 2)
1389 if s < 0.001:
1390 return angle, Vector3(1, 0, 0)
1391 else:
1392 return angle, Vector3(self.x / s, self.y / s, self.z / s)
1393
1394 def get_euler(self):
1395 t = self.x * self.y + self.z * self.w
1396 if t > 0.4999:
1397 heading = 2 * math.atan2(self.x, self.w)
1398 attitude = math.pi / 2
1399 bank = 0
1400 elif t < -0.4999:
1401 heading = -2 * math.atan2(self.x, self.w)
1402 attitude = -math.pi / 2
1403 bank = 0
1404 else:
1405 sqx = self.x ** 2
1406 sqy = self.y ** 2
1407 sqz = self.z ** 2
1408 heading = math.atan2(2 * self.y * self.w - 2 * self.x * self.z,
1409 1 - 2 * sqy - 2 * sqz)
1410 attitude = math.asin(2 * t)
1411 bank = math.atan2(2 * self.x * self.w - 2 * self.y * self.z,
1412 1 - 2 * sqx - 2 * sqz)
1413 return heading, attitude, bank
1414
1415 def get_matrix(self):
1416 xx = self.x ** 2
1417 xy = self.x * self.y
1418 xz = self.x * self.z
1419 xw = self.x * self.w
1420 yy = self.y ** 2
1421 yz = self.y * self.z
1422 yw = self.y * self.w
1423 zz = self.z ** 2
1424 zw = self.z * self.w
1425 M = Matrix4()
1426 M.a = 1 - 2 * (yy + zz)
1427 M.b = 2 * (xy - zw)
1428 M.c = 2 * (xz + yw)
1429 M.e = 2 * (xy + zw)
1430 M.f = 1 - 2 * (xx + zz)
1431 M.g = 2 * (yz - xw)
1432 M.i = 2 * (xz - yw)
1433 M.j = 2 * (yz + xw)
1434 M.k = 1 - 2 * (xx + yy)
1435 return M
1436
1437 # Static constructors
1438 def new_identity(cls):
1439 return cls()
1440 new_identity = classmethod(new_identity)
1441
1442 def new_rotate_axis(cls, angle, axis):
1443 assert(isinstance(axis, Vector3))
1444 axis = axis.normalized()
1445 s = math.sin(angle / 2)
1446 Q = cls()
1447 Q.w = math.cos(angle / 2)
1448 Q.x = axis.x * s
1449 Q.y = axis.y * s
1450 Q.z = axis.z * s
1451 return Q
1452 new_rotate_axis = classmethod(new_rotate_axis)
1453
1454 def new_rotate_euler(cls, heading, attitude, bank):
1455 Q = cls()
1456 c1 = math.cos(heading / 2)
1457 s1 = math.sin(heading / 2)
1458 c2 = math.cos(attitude / 2)
1459 s2 = math.sin(attitude / 2)
1460 c3 = math.cos(bank / 2)
1461 s3 = math.sin(bank / 2)
1462
1463 Q.w = c1 * c2 * c3 - s1 * s2 * s3
1464 Q.x = s1 * s2 * c3 + c1 * c2 * s3
1465 Q.y = s1 * c2 * c3 + c1 * s2 * s3
1466 Q.z = c1 * s2 * c3 - s1 * c2 * s3
1467 return Q
1468 new_rotate_euler = classmethod(new_rotate_euler)
1469
1470 def new_rotate_matrix(cls, m):
1471 if m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] > 0.00000001:
1472 t = m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] + 1.0
1473 s = 0.5/math.sqrt(t)
1474
1475 return cls(
1476 s*t,
1477 (m[1*4 + 2] - m[2*4 + 1])*s,
1478 (m[2*4 + 0] - m[0*4 + 2])*s,
1479 (m[0*4 + 1] - m[1*4 + 0])*s
1480 )
1481
1482 elif m[0*4 + 0] > m[1*4 + 1] and m[0*4 + 0] > m[2*4 + 2]:
1483 t = m[0*4 + 0] - m[1*4 + 1] - m[2*4 + 2] + 1.0
1484 s = 0.5/math.sqrt(t)
1485
1486 return cls(
1487 (m[1*4 + 2] - m[2*4 + 1])*s,
1488 s*t,
1489 (m[0*4 + 1] + m[1*4 + 0])*s,
1490 (m[2*4 + 0] + m[0*4 + 2])*s
1491 )
1492
1493 elif m[1*4 + 1] > m[2*4 + 2]:
1494 t = -m[0*4 + 0] + m[1*4 + 1] - m[2*4 + 2] + 1.0
1495 s = 0.5/math.sqrt(t)
1496
1497 return cls(
1498 (m[2*4 + 0] - m[0*4 + 2])*s,
1499 (m[0*4 + 1] + m[1*4 + 0])*s,
1500 s*t,
1501 (m[1*4 + 2] + m[2*4 + 1])*s
1502 )
1503
1504 else:
1505 t = -m[0*4 + 0] - m[1*4 + 1] + m[2*4 + 2] + 1.0
1506 s = 0.5/math.sqrt(t)
1507
1508 return cls(
1509 (m[0*4 + 1] - m[1*4 + 0])*s,
1510 (m[2*4 + 0] + m[0*4 + 2])*s,
1511 (m[1*4 + 2] + m[2*4 + 1])*s,
1512 s*t
1513 )
1514 new_rotate_matrix = classmethod(new_rotate_matrix)
1515
1516 def new_interpolate(cls, q1, q2, t):
1517 assert isinstance(q1, Quaternion) and isinstance(q2, Quaternion)
1518 Q = cls()
1519
1520 costheta = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z
1521 if costheta < 0.:
1522 costheta = -costheta
1523 q1 = q1.conjugated()
1524 elif costheta > 1:
1525 costheta = 1
1526
1527 theta = math.acos(costheta)
1528 if abs(theta) < 0.01:
1529 Q.w = q2.w
1530 Q.x = q2.x
1531 Q.y = q2.y
1532 Q.z = q2.z
1533 return Q
1534
1535 sintheta = math.sqrt(1.0 - costheta * costheta)
1536 if abs(sintheta) < 0.01:
1537 Q.w = (q1.w + q2.w) * 0.5
1538 Q.x = (q1.x + q2.x) * 0.5
1539 Q.y = (q1.y + q2.y) * 0.5
1540 Q.z = (q1.z + q2.z) * 0.5
1541 return Q
1542
1543 ratio1 = math.sin((1 - t) * theta) / sintheta
1544 ratio2 = math.sin(t * theta) / sintheta
1545
1546 Q.w = q1.w * ratio1 + q2.w * ratio2
1547 Q.x = q1.x * ratio1 + q2.x * ratio2
1548 Q.y = q1.y * ratio1 + q2.y * ratio2
1549 Q.z = q1.z * ratio1 + q2.z * ratio2
1550 return Q
1551 new_interpolate = classmethod(new_interpolate)
1552
1553# Geometry
1554# Much maths thanks to Paul Bourke, http://astronomy.swin.edu.au/~pbourke
1555# ---------------------------------------------------------------------------
1556
1557class Geometry:
1558 def _connect_unimplemented(self, other):
1559 raise AttributeError, 'Cannot connect %s to %s' % \
1560 (self.__class__, other.__class__)
1561
1562 def _intersect_unimplemented(self, other):
1563 raise AttributeError, 'Cannot intersect %s and %s' % \
1564 (self.__class__, other.__class__)
1565
1566 _intersect_point2 = _intersect_unimplemented
1567 _intersect_line2 = _intersect_unimplemented
1568 _intersect_circle = _intersect_unimplemented
1569 _connect_point2 = _connect_unimplemented
1570 _connect_line2 = _connect_unimplemented
1571 _connect_circle = _connect_unimplemented
1572
1573 _intersect_point3 = _intersect_unimplemented
1574 _intersect_line3 = _intersect_unimplemented
1575 _intersect_sphere = _intersect_unimplemented
1576 _intersect_plane = _intersect_unimplemented
1577 _connect_point3 = _connect_unimplemented
1578 _connect_line3 = _connect_unimplemented
1579 _connect_sphere = _connect_unimplemented
1580 _connect_plane = _connect_unimplemented
1581
1582 def intersect(self, other):
1583 raise NotImplementedError
1584
1585 def connect(self, other):
1586 raise NotImplementedError
1587
1588 def distance(self, other):
1589 c = self.connect(other)
1590 if c:
1591 return c.length
1592 return 0.0
1593
1594def _intersect_point2_circle(P, C):
1595 return abs(P - C.c) <= C.r
1596
1597def _intersect_line2_line2(A, B):
1598 d = B.v.y * A.v.x - B.v.x * A.v.y
1599 if d == 0:
1600 return None
1601
1602 dy = A.p.y - B.p.y
1603 dx = A.p.x - B.p.x
1604 ua = (B.v.x * dy - B.v.y * dx) / d
1605 if not A._u_in(ua):
1606 return None
1607 ub = (A.v.x * dy - A.v.y * dx) / d
1608 if not B._u_in(ub):
1609 return None
1610
1611 return Point2(A.p.x + ua * A.v.x,
1612 A.p.y + ua * A.v.y)
1613
1614def _intersect_line2_circle(L, C):
1615 a = L.v.magnitude_squared()
1616 b = 2 * (L.v.x * (L.p.x - C.c.x) + \
1617 L.v.y * (L.p.y - C.c.y))
1618 c = C.c.magnitude_squared() + \
1619 L.p.magnitude_squared() - \
1620 2 * C.c.dot(L.p) - \
1621 C.r ** 2
1622 det = b ** 2 - 4 * a * c
1623 if det < 0:
1624 return None
1625 sq = math.sqrt(det)
1626 u1 = (-b + sq) / (2 * a)
1627 u2 = (-b - sq) / (2 * a)
1628 if not L._u_in(u1):
1629 u1 = max(min(u1, 1.0), 0.0)
1630 if not L._u_in(u2):
1631 u2 = max(min(u2, 1.0), 0.0)
1632
1633 # Tangent
1634 if u1 == u2:
1635 return Point2(L.p.x + u1 * L.v.x,
1636 L.p.y + u1 * L.v.y)
1637
1638 return LineSegment2(Point2(L.p.x + u1 * L.v.x,
1639 L.p.y + u1 * L.v.y),
1640 Point2(L.p.x + u2 * L.v.x,
1641 L.p.y + u2 * L.v.y))
1642
1643def _connect_point2_line2(P, L):
1644 d = L.v.magnitude_squared()
1645 assert d != 0
1646 u = ((P.x - L.p.x) * L.v.x + \
1647 (P.y - L.p.y) * L.v.y) / d
1648 if not L._u_in(u):
1649 u = max(min(u, 1.0), 0.0)
1650 return LineSegment2(P,
1651 Point2(L.p.x + u * L.v.x,
1652 L.p.y + u * L.v.y))
1653
1654def _connect_point2_circle(P, C):
1655 v = P - C.c
1656 v.normalize()
1657 v *= C.r
1658 return LineSegment2(P, Point2(C.c.x + v.x, C.c.y + v.y))
1659
1660def _connect_line2_line2(A, B):
1661 d = B.v.y * A.v.x - B.v.x * A.v.y
1662 if d == 0:
1663 # Parallel, connect an endpoint with a line
1664 if isinstance(B, Ray2) or isinstance(B, LineSegment2):
1665 p1, p2 = _connect_point2_line2(B.p, A)
1666 return p2, p1
1667 # No endpoint (or endpoint is on A), possibly choose arbitrary point
1668 # on line.
1669 return _connect_point2_line2(A.p, B)
1670
1671 dy = A.p.y - B.p.y
1672 dx = A.p.x - B.p.x
1673 ua = (B.v.x * dy - B.v.y * dx) / d
1674 if not A._u_in(ua):
1675 ua = max(min(ua, 1.0), 0.0)
1676 ub = (A.v.x * dy - A.v.y * dx) / d
1677 if not B._u_in(ub):
1678 ub = max(min(ub, 1.0), 0.0)
1679
1680 return LineSegment2(Point2(A.p.x + ua * A.v.x, A.p.y + ua * A.v.y),
1681 Point2(B.p.x + ub * B.v.x, B.p.y + ub * B.v.y))
1682
1683def _connect_circle_line2(C, L):
1684 d = L.v.magnitude_squared()
1685 assert d != 0
1686 u = ((C.c.x - L.p.x) * L.v.x + (C.c.y - L.p.y) * L.v.y) / d
1687 if not L._u_in(u):
1688 u = max(min(u, 1.0), 0.0)
1689 point = Point2(L.p.x + u * L.v.x, L.p.y + u * L.v.y)
1690 v = (point - C.c)
1691 v.normalize()
1692 v *= C.r
1693 return LineSegment2(Point2(C.c.x + v.x, C.c.y + v.y), point)
1694
1695def _connect_circle_circle(A, B):
1696 v = B.c - A.c
1697 d = v.magnitude()
1698 if A.r >= B.r and d < A.r:
1699 #centre B inside A
1700 s1,s2 = +1, +1
1701 elif B.r > A.r and d < B.r:
1702 #centre A inside B
1703 s1,s2 = -1, -1
1704 elif d >= A.r and d >= B.r:
1705 s1,s2 = +1, -1
1706 v.normalize()
1707 return LineSegment2(Point2(A.c.x + s1 * v.x * A.r, A.c.y + s1 * v.y * A.r),
1708 Point2(B.c.x + s2 * v.x * B.r, B.c.y + s2 * v.y * B.r))
1709
1710
1711class Point2(Vector2, Geometry):
1712 def __repr__(self):
1713 return 'Point2(%.2f, %.2f)' % (self.x, self.y)
1714
1715 def intersect(self, other):
1716 return other._intersect_point2(self)
1717
1718 def _intersect_circle(self, other):
1719 return _intersect_point2_circle(self, other)
1720
1721 def connect(self, other):
1722 return other._connect_point2(self)
1723
1724 def _connect_point2(self, other):
1725 return LineSegment2(other, self)
1726
1727 def _connect_line2(self, other):
1728 c = _connect_point2_line2(self, other)
1729 if c:
1730 return c._swap()
1731
1732 def _connect_circle(self, other):
1733 c = _connect_point2_circle(self, other)
1734 if c:
1735 return c._swap()
1736
1737class Line2(Geometry):
1738 __slots__ = ['p', 'v']
1739
1740 def __init__(self, *args):
1741 if len(args) == 3:
1742 assert isinstance(args[0], Point2) and \
1743 isinstance(args[1], Vector2) and \
1744 type(args[2]) == float
1745 self.p = args[0].copy()
1746 self.v = args[1] * args[2] / abs(args[1])
1747 elif len(args) == 2:
1748 if isinstance(args[0], Point2) and isinstance(args[1], Point2):
1749 self.p = args[0].copy()
1750 self.v = args[1] - args[0]
1751 elif isinstance(args[0], Point2) and isinstance(args[1], Vector2):
1752 self.p = args[0].copy()
1753 self.v = args[1].copy()
1754 else:
1755 raise AttributeError, '%r' % (args,)
1756 elif len(args) == 1:
1757 if isinstance(args[0], Line2):
1758 self.p = args[0].p.copy()
1759 self.v = args[0].v.copy()
1760 else:
1761 raise AttributeError, '%r' % (args,)
1762 else:
1763 raise AttributeError, '%r' % (args,)
1764
1765 if not self.v:
1766 raise AttributeError, 'Line has zero-length vector'
1767
1768 def __copy__(self):
1769 return self.__class__(self.p, self.v)
1770
1771 copy = __copy__
1772
1773 def __repr__(self):
1774 return 'Line2(<%.2f, %.2f> + u<%.2f, %.2f>)' % \
1775 (self.p.x, self.p.y, self.v.x, self.v.y)
1776
1777 p1 = property(lambda self: self.p)
1778 p2 = property(lambda self: Point2(self.p.x + self.v.x,
1779 self.p.y + self.v.y))
1780
1781 def _apply_transform(self, t):
1782 self.p = t * self.p
1783 self.v = t * self.v
1784
1785 def _u_in(self, u):
1786 return True
1787
1788 def intersect(self, other):
1789 return other._intersect_line2(self)
1790
1791 def _intersect_line2(self, other):
1792 return _intersect_line2_line2(self, other)
1793
1794 def _intersect_circle(self, other):
1795 return _intersect_line2_circle(self, other)
1796
1797 def connect(self, other):
1798 return other._connect_line2(self)
1799
1800 def _connect_point2(self, other):
1801 return _connect_point2_line2(other, self)
1802
1803 def _connect_line2(self, other):
1804 return _connect_line2_line2(other, self)
1805
1806 def _connect_circle(self, other):
1807 return _connect_circle_line2(other, self)
1808
1809class Ray2(Line2):
1810 def __repr__(self):
1811 return 'Ray2(<%.2f, %.2f> + u<%.2f, %.2f>)' % \
1812 (self.p.x, self.p.y, self.v.x, self.v.y)
1813
1814 def _u_in(self, u):
1815 return u >= 0.0
1816
1817class LineSegment2(Line2):
1818 def __repr__(self):
1819 return 'LineSegment2(<%.2f, %.2f> to <%.2f, %.2f>)' % \
1820 (self.p.x, self.p.y, self.p.x + self.v.x, self.p.y + self.v.y)
1821
1822 def _u_in(self, u):
1823 return u >= 0.0 and u <= 1.0
1824
1825 def __abs__(self):
1826 return abs(self.v)
1827
1828 def magnitude_squared(self):
1829 return self.v.magnitude_squared()
1830
1831 def _swap(self):
1832 # used by connect methods to switch order of points
1833 self.p = self.p2
1834 self.v *= -1
1835 return self
1836
1837 length = property(lambda self: abs(self.v))
1838
1839class Circle(Geometry):
1840 __slots__ = ['c', 'r']
1841
1842 def __init__(self, center, radius):
1843 assert isinstance(center, Vector2) and type(radius) == float
1844 self.c = center.copy()
1845 self.r = radius
1846
1847 def __copy__(self):
1848 return self.__class__(self.c, self.r)
1849
1850 copy = __copy__
1851
1852 def __repr__(self):
1853 return 'Circle(<%.2f, %.2f>, radius=%.2f)' % \
1854 (self.c.x, self.c.y, self.r)
1855
1856 def _apply_transform(self, t):
1857 self.c = t * self.c
1858
1859 def intersect(self, other):
1860 return other._intersect_circle(self)
1861
1862 def _intersect_point2(self, other):
1863 return _intersect_point2_circle(other, self)
1864
1865 def _intersect_line2(self, other):
1866 return _intersect_line2_circle(other, self)
1867
1868 def connect(self, other):
1869 return other._connect_circle(self)
1870
1871 def _connect_point2(self, other):
1872 return _connect_point2_circle(other, self)
1873
1874 def _connect_line2(self, other):
1875 c = _connect_circle_line2(self, other)
1876 if c:
1877 return c._swap()
1878
1879 def _connect_circle(self, other):
1880 return _connect_circle_circle(other, self)
1881
1882# 3D Geometry
1883# -------------------------------------------------------------------------
1884
1885def _connect_point3_line3(P, L):
1886 d = L.v.magnitude_squared()
1887 assert d != 0
1888 u = ((P.x - L.p.x) * L.v.x + \
1889 (P.y - L.p.y) * L.v.y + \
1890 (P.z - L.p.z) * L.v.z) / d
1891 if not L._u_in(u):
1892 u = max(min(u, 1.0), 0.0)
1893 return LineSegment3(P, Point3(L.p.x + u * L.v.x,
1894 L.p.y + u * L.v.y,
1895 L.p.z + u * L.v.z))
1896
1897def _connect_point3_sphere(P, S):
1898 v = P - S.c
1899 v.normalize()
1900 v *= S.r
1901 return LineSegment3(P, Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z))
1902
1903def _connect_point3_plane(p, plane):
1904 n = plane.n.normalized()
1905 d = p.dot(plane.n) - plane.k
1906 return LineSegment3(p, Point3(p.x - n.x * d, p.y - n.y * d, p.z - n.z * d))
1907
1908def _connect_line3_line3(A, B):
1909 assert A.v and B.v
1910 p13 = A.p - B.p
1911 d1343 = p13.dot(B.v)
1912 d4321 = B.v.dot(A.v)
1913 d1321 = p13.dot(A.v)
1914 d4343 = B.v.magnitude_squared()
1915 denom = A.v.magnitude_squared() * d4343 - d4321 ** 2
1916 if denom == 0:
1917 # Parallel, connect an endpoint with a line
1918 if isinstance(B, Ray3) or isinstance(B, LineSegment3):
1919 return _connect_point3_line3(B.p, A)._swap()
1920 # No endpoint (or endpoint is on A), possibly choose arbitrary
1921 # point on line.
1922 return _connect_point3_line3(A.p, B)
1923
1924 ua = (d1343 * d4321 - d1321 * d4343) / denom
1925 if not A._u_in(ua):
1926 ua = max(min(ua, 1.0), 0.0)
1927 ub = (d1343 + d4321 * ua) / d4343
1928 if not B._u_in(ub):
1929 ub = max(min(ub, 1.0), 0.0)
1930 return LineSegment3(Point3(A.p.x + ua * A.v.x,
1931 A.p.y + ua * A.v.y,
1932 A.p.z + ua * A.v.z),
1933 Point3(B.p.x + ub * B.v.x,
1934 B.p.y + ub * B.v.y,
1935 B.p.z + ub * B.v.z))
1936
1937def _connect_line3_plane(L, P):
1938 d = P.n.dot(L.v)
1939 if not d:
1940 # Parallel, choose an endpoint
1941 return _connect_point3_plane(L.p, P)
1942 u = (P.k - P.n.dot(L.p)) / d
1943 if not L._u_in(u):
1944 # intersects out of range, choose nearest endpoint
1945 u = max(min(u, 1.0), 0.0)
1946 return _connect_point3_plane(Point3(L.p.x + u * L.v.x,
1947 L.p.y + u * L.v.y,
1948 L.p.z + u * L.v.z), P)
1949 # Intersection
1950 return None
1951
1952def _connect_sphere_line3(S, L):
1953 d = L.v.magnitude_squared()
1954 assert d != 0
1955 u = ((S.c.x - L.p.x) * L.v.x + \
1956 (S.c.y - L.p.y) * L.v.y + \
1957 (S.c.z - L.p.z) * L.v.z) / d
1958 if not L._u_in(u):
1959 u = max(min(u, 1.0), 0.0)
1960 point = Point3(L.p.x + u * L.v.x, L.p.y + u * L.v.y, L.p.z + u * L.v.z)
1961 v = (point - S.c)
1962 v.normalize()
1963 v *= S.r
1964 return LineSegment3(Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z),
1965 point)
1966
1967def _connect_sphere_sphere(A, B):
1968 v = B.c - A.c
1969 d = v.magnitude()
1970 if A.r >= B.r and d < A.r:
1971 #centre B inside A
1972 s1,s2 = +1, +1
1973 elif B.r > A.r and d < B.r:
1974 #centre A inside B
1975 s1,s2 = -1, -1
1976 elif d >= A.r and d >= B.r:
1977 s1,s2 = +1, -1
1978
1979 v.normalize()
1980 return LineSegment3(Point3(A.c.x + s1* v.x * A.r,
1981 A.c.y + s1* v.y * A.r,
1982 A.c.z + s1* v.z * A.r),
1983 Point3(B.c.x + s2* v.x * B.r,
1984 B.c.y + s2* v.y * B.r,
1985 B.c.z + s2* v.z * B.r))
1986
1987def _connect_sphere_plane(S, P):
1988 c = _connect_point3_plane(S.c, P)
1989 if not c:
1990 return None
1991 p2 = c.p2
1992 v = p2 - S.c
1993 v.normalize()
1994 v *= S.r
1995 return LineSegment3(Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z),
1996 p2)
1997
1998def _connect_plane_plane(A, B):
1999 if A.n.cross(B.n):
2000 # Planes intersect
2001 return None
2002 else:
2003 # Planes are parallel, connect to arbitrary point
2004 return _connect_point3_plane(A._get_point(), B)
2005
2006def _intersect_point3_sphere(P, S):
2007 return abs(P - S.c) <= S.r
2008
2009def _intersect_line3_sphere(L, S):
2010 a = L.v.magnitude_squared()
2011 b = 2 * (L.v.x * (L.p.x - S.c.x) + \
2012 L.v.y * (L.p.y - S.c.y) + \
2013 L.v.z * (L.p.z - S.c.z))
2014 c = S.c.magnitude_squared() + \
2015 L.p.magnitude_squared() - \
2016 2 * S.c.dot(L.p) - \
2017 S.r ** 2
2018 det = b ** 2 - 4 * a * c
2019 if det < 0:
2020 return None
2021 sq = math.sqrt(det)
2022 u1 = (-b + sq) / (2 * a)
2023 u2 = (-b - sq) / (2 * a)
2024 if not L._u_in(u1):
2025 u1 = max(min(u1, 1.0), 0.0)
2026 if not L._u_in(u2):
2027 u2 = max(min(u2, 1.0), 0.0)
2028 return LineSegment3(Point3(L.p.x + u1 * L.v.x,
2029 L.p.y + u1 * L.v.y,
2030 L.p.z + u1 * L.v.z),
2031 Point3(L.p.x + u2 * L.v.x,
2032 L.p.y + u2 * L.v.y,
2033 L.p.z + u2 * L.v.z))
2034
2035def _intersect_line3_plane(L, P):
2036 d = P.n.dot(L.v)
2037 if not d:
2038 # Parallel
2039 return None
2040 u = (P.k - P.n.dot(L.p)) / d
2041 if not L._u_in(u):
2042 return None
2043 return Point3(L.p.x + u * L.v.x,
2044 L.p.y + u * L.v.y,
2045 L.p.z + u * L.v.z)
2046
2047def _intersect_plane_plane(A, B):
2048 n1_m = A.n.magnitude_squared()
2049 n2_m = B.n.magnitude_squared()
2050 n1d2 = A.n.dot(B.n)
2051 det = n1_m * n2_m - n1d2 ** 2
2052 if det == 0:
2053 # Parallel
2054 return None
2055 c1 = (A.k * n2_m - B.k * n1d2) / det
2056 c2 = (B.k * n1_m - A.k * n1d2) / det
2057 return Line3(Point3(c1 * A.n.x + c2 * B.n.x,
2058 c1 * A.n.y + c2 * B.n.y,
2059 c1 * A.n.z + c2 * B.n.z),
2060 A.n.cross(B.n))
2061
2062class Point3(Vector3, Geometry):
2063 def __repr__(self):
2064 return 'Point3(%.2f, %.2f, %.2f)' % (self.x, self.y, self.z)
2065
2066 def intersect(self, other):
2067 return other._intersect_point3(self)
2068
2069 def _intersect_sphere(self, other):
2070 return _intersect_point3_sphere(self, other)
2071
2072 def connect(self, other):
2073 return other._connect_point3(self)
2074
2075 def _connect_point3(self, other):
2076 if self != other:
2077 return LineSegment3(other, self)
2078 return None
2079
2080 def _connect_line3(self, other):
2081 c = _connect_point3_line3(self, other)
2082 if c:
2083 return c._swap()
2084
2085 def _connect_sphere(self, other):
2086 c = _connect_point3_sphere(self, other)
2087 if c:
2088 return c._swap()
2089
2090 def _connect_plane(self, other):
2091 c = _connect_point3_plane(self, other)
2092 if c:
2093 return c._swap()
2094
2095class Line3:
2096 __slots__ = ['p', 'v']
2097
2098 def __init__(self, *args):
2099 if len(args) == 3:
2100 assert isinstance(args[0], Point3) and \
2101 isinstance(args[1], Vector3) and \
2102 type(args[2]) == float
2103 self.p = args[0].copy()
2104 self.v = args[1] * args[2] / abs(args[1])
2105 elif len(args) == 2:
2106 if isinstance(args[0], Point3) and isinstance(args[1], Point3):
2107 self.p = args[0].copy()
2108 self.v = args[1] - args[0]
2109 elif isinstance(args[0], Point3) and isinstance(args[1], Vector3):
2110 self.p = args[0].copy()
2111 self.v = args[1].copy()
2112 else:
2113 raise AttributeError, '%r' % (args,)
2114 elif len(args) == 1:
2115 if isinstance(args[0], Line3):
2116 self.p = args[0].p.copy()
2117 self.v = args[0].v.copy()
2118 else:
2119 raise AttributeError, '%r' % (args,)
2120 else:
2121 raise AttributeError, '%r' % (args,)
2122
2123 # XXX This is annoying.
2124 #if not self.v:
2125 # raise AttributeError, 'Line has zero-length vector'
2126
2127 def __copy__(self):
2128 return self.__class__(self.p, self.v)
2129
2130 copy = __copy__
2131
2132 def __repr__(self):
2133 return 'Line3(<%.2f, %.2f, %.2f> + u<%.2f, %.2f, %.2f>)' % \
2134 (self.p.x, self.p.y, self.p.z, self.v.x, self.v.y, self.v.z)
2135
2136 p1 = property(lambda self: self.p)
2137 p2 = property(lambda self: Point3(self.p.x + self.v.x,
2138 self.p.y + self.v.y,
2139 self.p.z + self.v.z))
2140
2141 def _apply_transform(self, t):
2142 self.p = t * self.p
2143 self.v = t * self.v
2144
2145 def _u_in(self, u):
2146 return True
2147
2148 def intersect(self, other):
2149 return other._intersect_line3(self)
2150
2151 def _intersect_sphere(self, other):
2152 return _intersect_line3_sphere(self, other)
2153
2154 def _intersect_plane(self, other):
2155 return _intersect_line3_plane(self, other)
2156
2157 def connect(self, other):
2158 return other._connect_line3(self)
2159
2160 def _connect_point3(self, other):
2161 return _connect_point3_line3(other, self)
2162
2163 def _connect_line3(self, other):
2164 return _connect_line3_line3(other, self)
2165
2166 def _connect_sphere(self, other):
2167 return _connect_sphere_line3(other, self)
2168
2169 def _connect_plane(self, other):
2170 c = _connect_line3_plane(self, other)
2171 if c:
2172 return c
2173
2174class Ray3(Line3):
2175 def __repr__(self):
2176 return 'Ray3(<%.2f, %.2f, %.2f> + u<%.2f, %.2f, %.2f>)' % \
2177 (self.p.x, self.p.y, self.p.z, self.v.x, self.v.y, self.v.z)
2178
2179 def _u_in(self, u):
2180 return u >= 0.0
2181
2182class LineSegment3(Line3):
2183 def __repr__(self):
2184 return 'LineSegment3(<%.2f, %.2f, %.2f> to <%.2f, %.2f, %.2f>)' % \
2185 (self.p.x, self.p.y, self.p.z,
2186 self.p.x + self.v.x, self.p.y + self.v.y, self.p.z + self.v.z)
2187
2188 def _u_in(self, u):
2189 return u >= 0.0 and u <= 1.0
2190
2191 def __abs__(self):
2192 return abs(self.v)
2193
2194 def magnitude_squared(self):
2195 return self.v.magnitude_squared()
2196
2197 def _swap(self):
2198 # used by connect methods to switch order of points
2199 self.p = self.p2
2200 self.v *= -1
2201 return self
2202
2203 length = property(lambda self: abs(self.v))
2204
2205class Sphere:
2206 __slots__ = ['c', 'r']
2207
2208 def __init__(self, center, radius):
2209 assert isinstance(center, Vector3) and type(radius) == float
2210 self.c = center.copy()
2211 self.r = radius
2212
2213 def __copy__(self):
2214 return self.__class__(self.c, self.r)
2215
2216 copy = __copy__
2217
2218 def __repr__(self):
2219 return 'Sphere(<%.2f, %.2f, %.2f>, radius=%.2f)' % \
2220 (self.c.x, self.c.y, self.c.z, self.r)
2221
2222 def _apply_transform(self, t):
2223 self.c = t * self.c
2224
2225 def intersect(self, other):
2226 return other._intersect_sphere(self)
2227
2228 def _intersect_point3(self, other):
2229 return _intersect_point3_sphere(other, self)
2230
2231 def _intersect_line3(self, other):
2232 return _intersect_line3_sphere(other, self)
2233
2234 def connect(self, other):
2235 return other._connect_sphere(self)
2236
2237 def _connect_point3(self, other):
2238 return _connect_point3_sphere(other, self)
2239
2240 def _connect_line3(self, other):
2241 c = _connect_sphere_line3(self, other)
2242 if c:
2243 return c._swap()
2244
2245 def _connect_sphere(self, other):
2246 return _connect_sphere_sphere(other, self)
2247
2248 def _connect_plane(self, other):
2249 c = _connect_sphere_plane(self, other)
2250 if c:
2251 return c
2252
2253class Plane:
2254 # n.p = k, where n is normal, p is point on plane, k is constant scalar
2255 __slots__ = ['n', 'k']
2256
2257 def __init__(self, *args):
2258 if len(args) == 3:
2259 assert isinstance(args[0], Point3) and \
2260 isinstance(args[1], Point3) and \
2261 isinstance(args[2], Point3)
2262 self.n = (args[1] - args[0]).cross(args[2] - args[0])
2263 self.n.normalize()
2264 self.k = self.n.dot(args[0])
2265 elif len(args) == 2:
2266 if isinstance(args[0], Point3) and isinstance(args[1], Vector3):
2267 self.n = args[1].normalized()
2268 self.k = self.n.dot(args[0])
2269 elif isinstance(args[0], Vector3) and type(args[1]) == float:
2270 self.n = args[0].normalized()
2271 self.k = args[1]
2272 else:
2273 raise AttributeError, '%r' % (args,)
2274
2275 else:
2276 raise AttributeError, '%r' % (args,)
2277
2278 if not self.n:
2279 raise AttributeError, 'Points on plane are colinear'
2280
2281 def __copy__(self):
2282 return self.__class__(self.n, self.k)
2283
2284 copy = __copy__
2285
2286 def __repr__(self):
2287 return 'Plane(<%.2f, %.2f, %.2f>.p = %.2f)' % \
2288 (self.n.x, self.n.y, self.n.z, self.k)
2289
2290 def _get_point(self):
2291 # Return an arbitrary point on the plane
2292 if self.n.z:
2293 return Point3(0., 0., self.k / self.n.z)
2294 elif self.n.y:
2295 return Point3(0., self.k / self.n.y, 0.)
2296 else:
2297 return Point3(self.k / self.n.x, 0., 0.)
2298
2299 def _apply_transform(self, t):
2300 p = t * self._get_point()
2301 self.n = t * self.n
2302 self.k = self.n.dot(p)
2303
2304 def intersect(self, other):
2305 return other._intersect_plane(self)
2306
2307 def _intersect_line3(self, other):
2308 return _intersect_line3_plane(other, self)
2309
2310 def _intersect_plane(self, other):
2311 return _intersect_plane_plane(self, other)
2312
2313 def connect(self, other):
2314 return other._connect_plane(self)
2315
2316 def _connect_point3(self, other):
2317 return _connect_point3_plane(other, self)
2318
2319 def _connect_line3(self, other):
2320 return _connect_line3_plane(other, self)
2321
2322 def _connect_sphere(self, other):
2323 return _connect_sphere_plane(other, self)
2324
2325 def _connect_plane(self, other):
2326 return _connect_plane_plane(other, self)
2327
Note: See TracBrowser for help on using the repository browser.