-
Notifications
You must be signed in to change notification settings - Fork 0
/
PointRotate.py
92 lines (84 loc) · 3.41 KB
/
PointRotate.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
## PointRotate.py Version 1.02
## Copyright (c) 2006 Bruce Vaughan, BV Detailing & Design, Inc.
## All rights reserved.
## NOT FOR SALE. The software is provided "as is" without any warranty.
#############################################################################
"""
Return a point rotated about an arbitrary axis in 3D.
Positive angles are counter-clockwise looking down the axis toward the origin.
The coordinate system is assumed to be right-hand.
Arguments: 'axis point 1', 'axis point 2', 'point to be rotated', 'angle of rotation (in radians)' >> 'new point'
Revision History:
Version 1.01 (11/11/06) - Revised function code
Version 1.02 (11/16/06) - Rewrote PointRotate3D function
Reference 'Rotate A Point About An Arbitrary Axis (3D)' - Paul Bourke
"""
def PointRotate3D(p1, p2, p0, theta):
from Point import Point
from math import cos, sin, sqrt
# Translate so axis is at origin
p = p0 - p1
# Initialize point q
q = Point(0.0,0.0,0.0)
N = (p2-p1)
Nm = sqrt(N.x**2 + N.y**2 + N.z**2)
# Rotation axis unit vector
n = Point(N.x/Nm, N.y/Nm, N.z/Nm)
# Matrix common factors
c = cos(theta)
t = (1 - cos(theta))
s = sin(theta)
X = n.x
Y = n.y
Z = n.z
# Matrix 'M'
d11 = t*X**2 + c
d12 = t*X*Y - s*Z
d13 = t*X*Z + s*Y
d21 = t*X*Y + s*Z
d22 = t*Y**2 + c
d23 = t*Y*Z - s*X
d31 = t*X*Z - s*Y
d32 = t*Y*Z + s*X
d33 = t*Z**2 + c
# |p.x|
# Matrix 'M'*|p.y|
# |p.z|
q.x = d11*p.x + d12*p.y + d13*p.z
q.y = d21*p.x + d22*p.y + d23*p.z
q.z = d31*p.x + d32*p.y + d33*p.z
# Translate axis and rotated point back to original location
return q + p1
## END PointRotate3D() ##########################
def test_PointRotate3D():
from Point import Point, PointLocate
from member import Member, MemberLocate
from param import dim_print, Prompt, yes_or_no, Warning, ClearSelection
from macrolib.angle import dtor
from macrolib.ExceptWarn import formatExceptionInfo
from macrolib.PrintPtList import formatPtList
a1 = 45.0
while 1:
ClearSelection()
try:
Warning("Rotate a Point About a Member Line - The rotation is REVERSED if the view " +\
"along the rotation axis is from the member LEFT end toward the member RIGHT end. " + \
"A right hand coordinate system is assumed. Positive angles are counter-clockwise " +\
"when looking down the axis toward the origin. The member LEFT end is the origin.")
mem1 = MemberLocate("Select member to rotate about")
pt1 = PointLocate("Pick point to rotate")
a1 = Prompt(a1, "Rotation angle")
pt2 = PointRotate3D(mem1.left.location, mem1.right.location, pt1, dtor(a1))
Warning("The selected point is %s, %s, %s \nThe new point is %s, %s, %s" % \
(dim_print(pt1.x), dim_print(pt1.y), dim_print(pt1.z), dim_print(pt2.x), dim_print(pt2.y), dim_print(pt2.z)))
print(formatPtList("Original Point and Rotated Point Coordinates:", [pt1, pt2]))
except:
Warning(formatExceptionInfo())
if not yes_or_no("Continue?"):
break
## END test_PointRotate3D() #####################
if __name__ == '__main__':
try:
test_PointRotate3D()
finally:
del test_PointRotate3D