#!/usr/bin/python
# -*- coding: utf-8 -*-

''' intersect2d.py

Purpose
  A simple 2D segments intersection library, shamelessly adapted from
  http://geomalgorithms.com/a05-_intersect-1.html#Line%20Intersections

Copyright
  Copyright 2001 softSurfer, 2012 Dan Sunday
  Copyright 2013 University of Lausanne, Switzerland

Authors
  Dan Sunday (Original C++ implementation)
  Thibault Genessay (Python translation)
	
License
  This code may be freely used and modified for any purpose
  providing that this copyright notice is included with it.
  SoftSurfer makes no warranty for this code, and cannot be held
  liable for any real or imagined damage resulting from its use.
  Users of this code must verify correctness for their application.

WTF note (or why this file exists in the first place)
  Intersecting finite line segments in the plane looks like an easy thing
  but it's not *that* easy given all corner cases. One can find lots and
  lots of "fastest implementation on earth" to solve this problem, although
  many (if not all) forget that correctness primes over speed.
  Further, many correct implementations can be found in huge packages
  such as SciPy, Pygame, SymPy, etc., which I don't want to depend upon.

WTF note about the WTF note above
  I you really love SciPy and wonder why I do not use it, please donate
  money to the project, and tell them to fix it. [If you have too much
  money, you could also donate a bit to boost (the C++ monstrosity)]. If they
  spend this money correctly (i.e. by attending architecture, design
  and [computer software] project management classes) I might one day
  consider them as "safe standards", and use them. In the meantime, I'm
  just going to stick with this kind of code snippets made by mathematicians
  who don't claim to know what a computer program is.

Implementation notes
  As opposed to the original code, which assumed a set of existing classes
  to handle points and vectors (with elementary vector/vector and
  scalar/vector operations defined), this module assumes that points are
  2-elements tuples/lists of floats, and segments are 2-elements tuples/lists
  of such points. The helper functions vsub(), vadd() and vsmul() serve
  that purpose.
'''

import math

SMALL_NUM = 0.00000001 # anything that avoids division overflow

# dot product (2D)	
def dot(u,v):
	return u[0] * v[0] + u[1] * v[1]
	
# perpendicular product (2D)	
def perp(u,v):
	return u[0] * v[1] - u[1] * v[0]
	
# vector subtraction (2D). Returns u-v
# Useful because we don't define (and assume the client will use) a
# 'vector' class
def vsub(u,v):
	return [u[0]-v[0], u[1]-v[1]]
# likewise, vector addition. Returns u+v
def vadd(u,v):
	return [u[0]+v[0], u[1]+v[1]]
# likewise, vector*scalar product. Returns v*s
def vsmul(v,s):
	return [v[0]*s, v[1]*s]
	
# Returns a copy of the input segment, using floats
# instead of whatever was passed in (typically ints)
# Is a no-op if input is all-floats already.
def floatify(seg):
	return [[float(c) for c in v] for v in seg]
	
def intersect2D_2Segments(s1in, s2in):
	'''
	find the 2D intersection of 2 finite segments
	
	Input:  two finite segments S1 and S2
	Return: None: disjoint (no intersect), or list of intersections points:
			[I0]: intersect  in unique point I0
		[I0, I1]: overlap  in segment from I0 to I1
	'''
	s1, s2 = [floatify(x) for x in (s1in, s2in)] # Make sure arguments are all floats
	u = vsub(s1[1], s1[0])
	v = vsub(s2[1], s2[0])
	w = vsub(s1[0], s2[0])
	D = perp(u,v)

	# test if  they are parallel (includes either being a point)
	if math.fabs(D) < SMALL_NUM:				# S1 and S2 are parallel
		if perp(u,w) != 0 or perp(v,w) != 0:
			return None							# they are NOT collinear

		# they are collinear or degenerate
		# check if they are degenerate  points
		du = dot(u,u)
		dv = dot(v,v)
		if du==0 and dv==0:						# both segments are points
			if s1[0] != s2[0]:					# they are distinct  points
				return None
			return s1[0]						# they are the same point

		if du == 0:								# S1 is a single point
			if inSegment(s1[0], s2) == 0: 		# but is not in S2
				return None
			return s1[0]

		if dv == 0: 							# S2 a single point
			if inSegment(s2[0], s1) == 0:		# but is not in S1
				return None
			return s2[0]

		# they are collinear segments - get  overlap (or not)
		# t0 and t1 are endpoints of S1 in eqn for S2
		w2 = vsub(s1[1], s2[0]) 
		if v[0] != 0:
			t0 = w[0] / v[0]
			t1 = w2[0] / v[0]
		else:
			t0 = w[1] / v[1]
			t1 = w2[1] / v[1]

		if t0 > t1: 							#  must have t0 smaller than t1
			# swap if not
			t=t0
			t0=t1
			t1=t

		if t0 > 1 or t1 < 0:
			return None							# NO overlap

		t0 = 0 if t0<0 else t0					# clip to min 0
		t1 = 1 if t1>1 else t1					# clip to max 1
		if t0 == t1:							# intersect is a point
			I0 = vadd(s2[0], vsmul(v, t0))
			return [I0]

		# they overlap in a valid subsegment
		I0 = vadd(s2[0], vsmul(v, t0))
		I1 = vadd(s2[0], vsmul(v, t1))
		return [I0, I1]

	# the segments are skew and may intersect in a point
	# get the intersect parameter for S1
	sI = perp(v,w) / D
	if sI < 0 or sI > 1:						# no intersect with S1
		return None

	# get the intersect parameter for S2
	tI = perp(u,w) / D
	if tI < 0 or tI > 1:						# no intersect with S2
		return None

	I0 = vadd(s1[0], vsmul(u, sI))				# compute S1 intersect point
	return [I0]



def inSegment(P, S):
	'''
	determine if a point is inside a segment
	
	Input:  a point P, and a collinear segment S
	Return: bool, True if P is inside S
	'''
	if S[0][0] != S[1][0]: 
		# S is not vertical
		if S[0][0] <= P[0] and P[0] <= S[1][0]:
			return True
		if S[0][0] >= P[0] and P[0] >= S[1][0]:
			return True

	else:
		# S is vertical, so test y coordinate
		if S[0][1] <= P[1] and P[1] <= S[1][1]:
			return True
		if S[0][1] >= P[1] and P[1] >= S[1][1]:
			return True

	return False

# About the big comment block below: this is the rest of the C++ code, which I've
# not translated to Python. But if I have the need someday, and if the internet
# collapses, I have the reference implementation. Yay!

# // intersect3D_SegmentPlane(): find the 3D intersection of a segment and a plane
# //    Input:  S = a segment, and Pn = a plane = {Point V0;  Vector n;}
# //    Output: *I0 = the intersect point (when it exists)
# //    Return: 0 = disjoint (no intersection)
# //            1 =  intersection in the unique point *I0
# //            2 = the  segment lies in the plane
# int
# intersect3D_SegmentPlane( Segment S, Plane Pn, Point* I )
# {
    # Vector    u = S.P1 - S.P0;
    # Vector    w = S.P0 - Pn.V0;

    # float     D = dot(Pn.n, u);
    # float     N = -dot(Pn.n, w);

    # if (math.fabs(D) < SMALL_NUM) {           // segment is parallel to plane
        # if (N == 0)                      // segment lies in plane
            # return 2;
        # else
            # return 0;                    // no intersection
    # }
    # // they are not parallel
    # // compute intersect param
    # float sI = N / D;
    # if (sI < 0 || sI > 1)
        # return 0;                        // no intersection

    # *I = S.P0 + sI * u;                  // compute segment intersect point
    # return 1;
# }
# //===================================================================
 


# // intersect3D_2Planes(): find the 3D intersection of two planes
# //    Input:  two planes Pn1 and Pn2
# //    Output: *L = the intersection line (when it exists)
# //    Return: 0 = disjoint (no intersection)
# //            1 = the two  planes coincide
# //            2 =  intersection in the unique line *L
# int
# intersect3D_2Planes( Plane Pn1, Plane Pn2, Line* L )
# {
    # Vector   u = Pn1.n * Pn2.n;          // cross product
    # float    ax = (u.x >= 0 ? u.x : -u.x);
    # float    ay = (u.y >= 0 ? u.y : -u.y);
    # float    az = (u.z >= 0 ? u.z : -u.z);

    # // test if the two planes are parallel
    # if ((ax+ay+az) < SMALL_NUM) {        // Pn1 and Pn2 are near parallel
        # // test if disjoint or coincide
        # Vector   v = Pn2.V0 -  Pn1.V0;
        # if (dot(Pn1.n, v) == 0)          // Pn2.V0 lies in Pn1
            # return 1;                    // Pn1 and Pn2 coincide
        # else
            # return 0;                    // Pn1 and Pn2 are disjoint
    # }

    # // Pn1 and Pn2 intersect in a line
    # // first determine max abs coordinate of cross product
    # int      maxc;                       // max coordinate
    # if (ax > ay) {
        # if (ax > az)
             # maxc =  1;
        # else maxc = 3;
    # }
    # else {
        # if (ay > az)
             # maxc =  2;
        # else maxc = 3;
    # }

    # // next, to get a point on the intersect line
    # // zero the max coord, and solve for the other two
    # Point    iP;                // intersect point
    # float    d1, d2;            // the constants in the 2 plane equations
    # d1 = -dot(Pn1.n, Pn1.V0);  // note: could be pre-stored  with plane
    # d2 = -dot(Pn2.n, Pn2.V0);  // ditto

    # switch (maxc) {             // select max coordinate
    # case 1:                     // intersect with x=0
        # iP.x = 0;
        # iP.y = (d2*Pn1.n.z - d1*Pn2.n.z) /  u.x;
        # iP.z = (d1*Pn2.n.y - d2*Pn1.n.y) /  u.x;
        # break;
    # case 2:                     // intersect with y=0
        # iP.x = (d1*Pn2.n.z - d2*Pn1.n.z) /  u.y;
        # iP.y = 0;
        # iP.z = (d2*Pn1.n.x - d1*Pn2.n.x) /  u.y;
        # break;
    # case 3:                     // intersect with z=0
        # iP.x = (d2*Pn1.n.y - d1*Pn2.n.y) /  u.z;
        # iP.y = (d1*Pn2.n.x - d2*Pn1.n.x) /  u.z;
        # iP.z = 0;
    # }
    # L->P0 = iP;
    # L->P1 = iP + u;
    # return 2;
# }
# //===================================================================