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