curve_tools/mathematics.py

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