Math.py

from mathutils import *


def IsSamePoint(v31, v32, limitDistance):
    if (v31 - v32).magnitude < limitDistance: return True

    return False


class Plane:
    @staticmethod
    def XY():
        p1 = Vector((0, 0, 0))
        p2 = Vector((1, 0, 0))
        p3 = Vector((0, 1, 0))

        return Plane(p1, p2, p3)


    # plane equation: (p - position).dot(normal) = 0
    def __init__(self, P1, P2, P3):
        self.normal = (P2 - P1).cross(P3 - P1)
        self.normal.normalize()

        self.position = P1


    def CalcIntersectionPointLineSegment(self, PL1, PL2):
        DL = PL2 - PL1

        try: rvPar = ((self.position - PL1).dot(self.normal)) / (DL.dot(self.normal))
        except: return None

        return rvPar


    def CalcNormalParameter(self, vector):
        return (vector - self.position).dot(self.normal)


    def CalcProjection(self, vector):
        normalParameter = self.CalcNormalParameter(vector)

        rvv3 = vector - (self.normal * normalParameter)

        return [normalParameter, rvv3]



# http://geomalgorithms.com/a07-_distance.html
def CalcClosestPointLineSegments(v3P0, v3P1, v3Q0, v3Q1):
    u = v3P1 - v3P0
    v = v3Q1 - v3Q0

    w0 = v3P0 - v3Q0
    a = u.dot(u)
    b = u.dot(v)
    c = v.dot(v)
    d = u.dot(w0)
    e = v.dot(w0)


    try: parP = (b * e - c * d) / (a * c - b * b)
    except: return None

    try: parQ = (a * e - b * d) / (a * c - b * b)
    except: return None


    return [parP, parQ]


def CalcIntersectionPointLineSegments(v3P0, v3P1, v3Q0, v3Q1, limitDistance):
    rvList = CalcClosestPointLineSegments(v3P0, v3P1, v3Q0, v3Q1)
    if rvList is None: return None


    parP = rvList[0]
    if parP < 0.0: return None
    if parP > 1.0: return None

    parQ = rvList[1]
    if parQ < 0.0: return None
    if parQ > 1.0: return None


    pointP = v3P0 + ((v3P1 - v3P0) * parP)
    pointQ = v3Q0 + ((v3Q1 - v3Q0) * parQ)
    if not IsSamePoint(pointP, pointQ, limitDistance): return None

    return [parP, parQ, pointP, pointQ]


def CalcIntersectionPointsLineSegmentsPOV(v3P0, v3P1, v3Q0, v3Q1, v3POV):
    planeQ = Plane(v3POV, v3Q0, v3Q1)
    parP = planeQ.CalcIntersectionPointLineSegment(v3P0, v3P1)
    if parP is None: return None
    if parP < 0.0: return None
    if parP > 1.0: return None

    planeP = Plane(v3POV, v3P0, v3P1)
    parQ = planeP.CalcIntersectionPointLineSegment(v3Q0, v3Q1)
    if parQ is None: return None
    if parQ < 0.0: return None
    if parQ > 1.0: return None

    return [parP, parQ]


def CalcIntersectionPointsLineSegmentsDIR(v3P0, v3P1, v3Q0, v3Q1, v3DIR):
    v3POV = v3Q0 + v3DIR
    planeQ = Plane(v3POV, v3Q0, v3Q1)
    parP = planeQ.CalcIntersectionPointLineSegment(v3P0, v3P1)
    if parP is None: return None
    if parP < 0.0: return None
    if parP > 1.0: return None

    v3POV = v3P0 + v3DIR
    planeP = Plane(v3POV, v3P0, v3P1)
    parQ = planeP.CalcIntersectionPointLineSegment(v3Q0, v3Q1)
    if parQ is None: return None
    if parQ < 0.0: return None
    if parQ > 1.0: return None

    return [parP, parQ]



def CalcRotationMatrix(v3From, v3To):
    cross = v3From.cross(v3To)

    try: angle = v3From.angle(v3To)
    except: return Matrix.Identity(4)

    return Matrix.Rotation(angle, 4, cross)        # normalize axis?