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