curve_tools/mathematics.py

   1 from mathutils import *
   2
   3
   4 def IsSamePoint(v31, v32, limitDistance):
   5     if (v31 - v32).magnitude < limitDistance: return True
   6
   7     return False
   8
   9
  10 class 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
  51 def 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
  73 def 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
  94 def 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
 110 def 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
 129 def 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
 138 def 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]