1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
|
from mathutils import *
def IsSamePoint(v31, v32, limitDistance):
if (v31 - v32).magnitude < limitDistance: return True
return False
class Plane:
@staticmethod
def XY():
p1 = Vector((0, 0, 0))
p2 = Vector((1, 0, 0))
p3 = Vector((0, 1, 0))
return Plane(p1, p2, p3)
# plane equation: (p - position).dot(normal) = 0
def __init__(self, P1, P2, P3):
self.normal = (P2 - P1).cross(P3 - P1)
self.normal.normalize()
self.position = P1
def CalcIntersectionPointLineSegment(self, PL1, PL2):
DL = PL2 - PL1
try: rvPar = ((self.position - PL1).dot(self.normal)) / (DL.dot(self.normal))
except: return None
return rvPar
def CalcNormalParameter(self, vector):
return (vector - self.position).dot(self.normal)
def CalcProjection(self, vector):
normalParameter = self.CalcNormalParameter(vector)
rvv3 = vector - (self.normal * normalParameter)
return [normalParameter, rvv3]
# http://geomalgorithms.com/a07-_distance.html
def CalcClosestPointLineSegments(v3P0, v3P1, v3Q0, v3Q1):
u = v3P1 - v3P0
v = v3Q1 - v3Q0
w0 = v3P0 - v3Q0
a = u.dot(u)
b = u.dot(v)
c = v.dot(v)
d = u.dot(w0)
e = v.dot(w0)
try: parP = (b * e - c * d) / (a * c - b * b)
except: return None
try: parQ = (a * e - b * d) / (a * c - b * b)
except: return None
return [parP, parQ]
def CalcIntersectionPointLineSegments(v3P0, v3P1, v3Q0, v3Q1, limitDistance):
rvList = CalcClosestPointLineSegments(v3P0, v3P1, v3Q0, v3Q1)
if rvList is None: return None
parP = rvList[0]
if parP < 0.0: return None
if parP > 1.0: return None
parQ = rvList[1]
if parQ < 0.0: return None
if parQ > 1.0: return None
pointP = v3P0 + ((v3P1 - v3P0) * parP)
pointQ = v3Q0 + ((v3Q1 - v3Q0) * parQ)
if not IsSamePoint(pointP, pointQ, limitDistance): return None
return [parP, parQ, pointP, pointQ]
def CalcIntersectionPointsLineSegmentsPOV(v3P0, v3P1, v3Q0, v3Q1, v3POV):
planeQ = Plane(v3POV, v3Q0, v3Q1)
parP = planeQ.CalcIntersectionPointLineSegment(v3P0, v3P1)
if parP is None: return None
if parP < 0.0: return None
if parP > 1.0: return None
planeP = Plane(v3POV, v3P0, v3P1)
parQ = planeP.CalcIntersectionPointLineSegment(v3Q0, v3Q1)
if parQ is None: return None
if parQ < 0.0: return None
if parQ > 1.0: return None
return [parP, parQ]
def CalcIntersectionPointsLineSegmentsDIR(v3P0, v3P1, v3Q0, v3Q1, v3DIR):
v3POV = v3Q0 + v3DIR
planeQ = Plane(v3POV, v3Q0, v3Q1)
parP = planeQ.CalcIntersectionPointLineSegment(v3P0, v3P1)
if parP is None: return None
if parP < 0.0: return None
if parP > 1.0: return None
v3POV = v3P0 + v3DIR
planeP = Plane(v3POV, v3P0, v3P1)
parQ = planeP.CalcIntersectionPointLineSegment(v3Q0, v3Q1)
if parQ is None: return None
if parQ < 0.0: return None
if parQ > 1.0: return None
return [parP, parQ]
def CalcRotationMatrix(v3From, v3To):
cross = v3From.cross(v3To)
try: angle = v3From.angle(v3To)
except: return Matrix.Identity(4)
return Matrix.Rotation(angle, 4, cross) # normalize axis?
def subdivide_cubic_bezier(p1, p2, p3, p4, t):
p12 = (p2 - p1) * t + p1
p23 = (p3 - p2) * t + p2
p34 = (p4 - p3) * t + p3
p123 = (p23 - p12) * t + p12
p234 = (p34 - p23) * t + p23
p1234 = (p234 - p123) * t + p123
return [p12, p123, p1234, p234, p34]
|
