curve_tools/mathematics.py

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