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?
    
    
