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