;;; This software is being provided to you, the LICENSEE, by the
;;; Massachusetts Institute of Technology (M.I.T.), and the NEC Research
;;; Institute (NECI) under the following license.  By obtaining, using
;;; and/or copying this software, you agree that you have read,
;;; understood, and will comply with these terms and conditions:
;;; 
;;; Permission to use, copy, modify and distribute, including the right to
;;; grant others rights to distribute at any tier, this software and its
;;; documentation for research purposes only and without fee or royalty is
;;; hereby granted, provided that you agree to comply with the following
;;; copyright notice and statements, including the disclaimer, and that
;;; the same appear on ALL copies of the software and documentation,
;;; including modifications that you make for internal use or for
;;; distribution:
;;; 
;;; Copyright 1995 by the Massachusetts Institute of Technology.  All
;;; rights reserved.  Copyright 1995 by NEC Research Institute Inc. All
;;; rights reserved
;;; 
;;; THIS SOFTWARE IS PROVIDED "AS IS", AND M.I.T. and NECI MAKE NO
;;; REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.  By way of example,
;;; but not limitation, M.I.T. and NECI MAKE NO REPRESENTATIONS OR
;;; WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY PARTICULAR PURPOSE OR
;;; THAT THE USE OF THE LICENSED SOFTWARE OR DOCUMENTATION WILL NOT
;;; INFRINGE ANY THIRD PARTY PATENTS, COPYRIGHTS, TRADEMARKS OR OTHER
;;; RIGHTS.
;;; 
;;; The name of the Massachusetts Institute of Technology or M.I.T., or
;;; NEC Research Institute, or NECI may NOT be used in advertising or
;;; publicity pertaining to distribution of the software.  Title to
;;; copyright in this software and any associated documentation shall at
;;; all times remain with M.I.T. and NECI, and USER agrees to preserve
;;; same.
;;; 

;;; -*- Mode: LISP; Base: 10; Package: 2DG; Syntax: Common-Lisp -*-
(in-package '2DG)

(export '(intersect? intersect-point-and-point? intersect-point-and-line-segment?
		      intersect-point-and-line? intersect-line-segment-and-line?
		      intersect-line-and-line? intersect-line-segment-and-line-segment?
		      intersection-object intersection-of-point-and-point
		      intersection-of-point-and-line-segment intersection-of-point-and-line
		      intersection-of-line-segment-and-line-segment
		      intersection-of-line-segment-and-line intersection-of-line-and-line
		      intersect-rectangle-and-rectangle?))


;;; To do: write intersect code for line/line-segments and polygon/rectangles.
;;; Write intersect-object code to include polygons and rectangles.

(defun INTERSECT? (object-1 object-2)
  "Returns T if OBJECT-1 and OBJECT-2 intersect."
  (cond ((or (null object-1) (null object-2))
	 ;; We include this case for calls where one of the objects is itself returned by a function
	 nil)
	((point-p object-1)
	 ;; OBJECT-1 is a point
	 (case (geometric-type object-2)
	   (point (intersect-point-and-point? object-1 object-2))
	   (line-segment (intersect-point-and-line-segment? object-1 object-2))
	   (line (intersect-point-and-line? object-1 object-2))
	   ((polygon rectangle) (inside? object-1 object-2))
	   (t (error "Can't determine if ~A and ~A intersect" object-1 object-2))))
	((point-p object-2)
	 ;; OBJECT-2 is a point, but OBJECT-1 is not
	 (intersect? object-2 object-1))
	((line-segment-p object-1)
	 ;; OBJECT-1 is a line segment
	 (cond ((line-segment-p object-2) (intersect-line-segment-and-line-segment? object-1 object-2))
	       ((line-p object-2) (intersect-line-segment-and-line? object-1 object-2))
	       ((rectangle-p object-2) (intersect-line-segment-and-rectangle? object-1 object-2))
	       (t (error "Can't determine if ~A and ~A intersect" object-1 object-2))))
	((line-segment-p object-2)
	 ;; OBJECT-2 is a line-segment, but OBJECT-1 is not
	 (intersect? object-2 object-1))
	((line-p object-1)
	 ;; OBJECT-1 is a line
	 (cond ((line-p object-2) (intersect-line-and-line? object-1 object-2))
	       (t (error "Can't determine if ~A and ~A intersect" object-1 object-2))))
	((line-p object-2)
	 ;; OBJECT-2 is a line, but OBJECT-1 is not
	 (intersect? object-2 object-1))
	((rectangle-p object-1)
	 (cond ((rectangle-p object-2) (intersect-rectangle-and-rectangle? object-1 object-2))
	       (t (error "Can't determine if ~A and ~A intersect" object-1 object-2))))
	(t
	 ;; OBJECT-1 is an unknown type
	 (error "Can't determine if ~A and ~A intersect" object-1 object-2))))

(defun INTERSECT-POINT-AND-POINT? (point-1 point-2)
  "Returns T if POINT-1 and POINT-2 coincide."
  (object-equal point-1 point-2))

(defun INTERSECT-POINT-AND-LINE-SEGMENT? (point line-segment)
  "
  Purpose:  Determines if POINT intersects LINE SEGMENT.
	If they do not intersect, but come within OK-DISTANCE of each other, we count that as an intersection.
	The test also holds if the line segment is degenerate.
  Args:	POINT	-- a list (X Y)
	LINE-SEGMENT	-- a list of two endpoints, each endpoint a list (X Y)
  Returns:  T if the point and line segment intersect
  "
  (declare (values boolean))
  ;; The distance function works for degenerate line segments.
  (~zerop (distance-between-point-and-line-segment point line-segment)))

(defun INTERSECT-POINT-AND-LINE? (point line)
  "Returns T if POINT intersects LINE."
  (colinear? point line))

(defun INTERSECT-LINE-SEGMENT-AND-LINE? (line-segment line)
  "Returns T if LINE-SEGMENT and LINE intersect.
   Returns T in degenerate case, where they could intersect, modulo *epsilon*,
   at the end point of the line segment."
  (or (colinear? line-segment line)
      (not (plusp (* (side (line-point-1 line) (line-point-2 line) (line-segment-point-1 line-segment))
		     (side (line-point-1 line) (line-point-2 line) (line-segment-point-2 line-segment)))))))

(defun INTERSECT-LINE-AND-LINE? (line-1 line-2)
  "Returns T if LINE-1 and LINE-2 intersect."
  (or (not (parallel? line-1 line-2))
      (colinear? line-1 line-2)))

(defun INTERSECT-LINE-SEGMENT-AND-LINE-SEGMENT? (line-segment-1 line-segment-2 &optional (ok-distance *epsilon*))
  "
  Purpose:  Determines if LINE-SEGMENT-1 intersects LINE-SEGMENT-2.
	If they do not intersect, but come within OK-DISTANCE of each other, we count that as an intersection.
	The test also holds for degenerate line segments.
  Args:	LINE-SEGMENT-1	-- a line-segment
	LINE-SEGMENT-2	-- a line-segment
	OK-DISTANCE	-- distance within which we declare an intersection
  Returns:  T if the segments intersect, modulo OK-DISTANCE
  "
  (declare (values boolean))
  ;; Make the endpoints easier to get at
  (let ((seg1-a (line-segment-point-1 line-segment-1))
	(seg1-b (line-segment-point-2 line-segment-1))
	(seg2-a (line-segment-point-1 line-segment-2))
	(seg2-b (line-segment-point-2 line-segment-2)))
    (if (point-equal seg1-a seg1-b)
	;; Segment 1 is degenerate.  (The distance function works for degenerate line segments.)
	(<= (distance-between-point-and-line-segment seg1-a line-segment-2) ok-distance)
	;; Segment 1 is not degenerate
	(if (point-equal seg2-a seg2-b)
	    ;; Segment 2 is degenerate
	    (<= (distance-between-point-and-line-segment seg2-a line-segment-1) ok-distance)
	    ;; Segment 2 is not degerate
	    (or
	      ;; Check if the endpoints of one segment fall on either side of the line of the other segment, and vice-versa.
	      ;; If the two segments are colinear, the endpoints fall on the line of the other line segment, so "on the line" isn't good enough.
	      (and (not (plusp (* (side seg1-a seg1-b seg2-a) (side seg1-a seg1-b seg2-b))))
		   (not (plusp (* (side seg2-a seg2-b seg1-a) (side seg2-a seg2-b seg1-b)))))
	      ;; Check for an endpoint intersecting the other segment
	      (<= (distance-between-point-and-line-segment seg1-a line-segment-2) ok-distance)
	      (<= (distance-between-point-and-line-segment seg1-b line-segment-2) ok-distance)
	      (<= (distance-between-point-and-line-segment seg2-a line-segment-1) ok-distance)
	      (<= (distance-between-point-and-line-segment seg2-b line-segment-1) ok-distance))))))

(defun INTERSECT-RECTANGLE-AND-RECTANGLE? (r1 r2) 
    (and
      ;; Overlap in x dimension?
      (<= (rectangle-min-x r1) (rectangle-max-x r2)) (<= (rectangle-min-x r2) (rectangle-max-x r1))
      ;; Overlap in y dimension?
      (<= (rectangle-min-y r1) (rectangle-max-y r2)) (<= (rectangle-min-y r2) (rectangle-max-y r1))))
 

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Intersection-object functions (return intersection object or NIL)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun INTERSECTION-OBJECT (object-1 object-2)
  "Returns the intersection object of OBJECT-1 and OBJECT-2, or NIL if they do not intersect."
  (cond ((or (null object-1) (null object-2))
	 ;; We include this case for calls where one of the objects is itself returned by a function
	 nil)
	((point-p object-1)
	 ;; OBJECT-1 is a point
	 (cond ((point-p object-2) (intersection-of-point-and-point object-1 object-2))
	       ((line-segment-p object-2) (intersection-of-point-and-line-segment object-1 object-2))
	       ((line-p object-2) (intersection-of-point-and-line object-1 object-2))
	       (t (error "Can't determine intersection between ~A and ~A" object-1 object-2))))
	((point-p object-2)
	 ;; OBJECT-2 is a point, but OBJECT-1 is not
	 (intersection-object object-2 object-1))
	((line-segment-p object-1)
	 ;; OBJECT-1 is a line segment
	 (cond ((line-segment-p object-2) (intersection-of-line-segment-and-line-segment object-1 object-2))
	       ((line-p object-2) (intersection-of-line-segment-and-line object-1 object-2))
	       (t (error "Can't determine intersection between ~A and ~A" object-1 object-2))))
	((line-segment-p object-2)
	 ;; OBJECT-2 is a line-segment, but OBJECT-1 is not
	 (intersection-object object-2 object-1))
	((line-p object-1)
	 ;; OBJECT-1 is a line
	 (cond ((line-p object-2) (intersection-of-line-and-line object-1 object-2))
	       (t (error "Can't determine intersection between ~A and ~A" object-1 object-2))))
	((line-p object-2)
	 ;; OBJECT-2 is a line, but OBJECT-1 is not
	 (intersection-object object-2 object-1))
	(t
	 ;; OBJECT-1 is an unknown type
	 (error "Can't determine intersection between ~A and ~A" object-1 object-2))))

(defun INTERSECTION-OF-POINT-AND-POINT (point-1 point-2)
  "Returns POINT-1 if POINT-1 and POINT-2 coincide."
  (when (object-equal point-1 point-2) point-1))

(defun INTERSECTION-OF-POINT-AND-LINE-SEGMENT (point line-segment)
  "Returns POINT if POINT and LINE-SEGMENT intersect."
  (when (intersect? point line-segment) point))

(defun INTERSECTION-OF-POINT-AND-LINE (point line)
  "Returns POINT if POINT and LINE intersect."
  (when (intersect? point line) point))

(defun INTERSECTION-OF-LINE-SEGMENT-AND-LINE-SEGMENT (line-segment-1 line-segment-2)
  "Returns the intersection point or line-segment of LINE-SEGMENT-1 and LINE-SEGMENT-2, or NIL if they do not intersect."
  (let ((intersection-point (intersection-of-line-and-line (g-coerce line-segment-1 'line) (g-coerce line-segment-2 'line))))
    (cond ((point-p intersection-point)		
	   ;; Does the intersection point lie in each segment?
	   (when (and (intersect-point-and-line-segment? intersection-point line-segment-1)
		      (intersect-point-and-line-segment? intersection-point line-segment-2))
	     intersection-point))
	  (intersection-point
	   ;; The lines are colinear.  See if they overlap.
	   (let ((point-1-1 (first-point line-segment-1))
		 (point-1-2 (second-point line-segment-1))
		 (point-2-1 (first-point line-segment-2))
		 (point-2-2 (second-point line-segment-2))
		 (intersection-endpoints ()))
	     ;; Find the overlap, if any, of the segments.
	     (when (intersect-point-and-line-segment? point-1-1 line-segment-2)
	       (push point-1-1 intersection-endpoints))
	     (when (intersect-point-and-line-segment? point-1-2 line-segment-2)
	       (push point-1-2 intersection-endpoints))
	     (when (intersect-point-and-line-segment? point-2-1 line-segment-1)
	       (push point-2-1 intersection-endpoints))
	     (when (intersect-point-and-line-segment? point-2-2 line-segment-1)
	       (push point-2-2 intersection-endpoints))
	     ;; Check the overlap
	     (case (length (setf intersection-endpoints (delete-duplicates intersection-endpoints :test #'object-equal)))
	       (0 nil)				;No overlap
	       (1 (first intersection-endpoints))	;Overlap at an enpoint
	       (2				;Two endpoints overlap
		 (make-line-segment (first intersection-endpoints) (second intersection-endpoints)))	;Normal overlap
	       (4 line-segment-1)		;Segments are equal
	       (otherwise (error "Things are pretty screwed up in INTERSECTION-OF-LINE-SEGMENT-AND-LINE-SEGMENT")))))
	  (t
	   ;; The lines are parallel and not colinear
	   nil))))

(defun INTERSECTION-OF-LINE-SEGMENT-AND-LINE (line-segment line)
  "Returns the intersection point or line-segment of LINE-SEGMENT and LINE, or NIL if they do not intersect."
  (let ((intersection-point (intersection-of-line-and-line (g-coerce line-segment 'line) line)))
    (cond ((point-p intersection-point)
	   ;; The lines intersect in a point.  Does the intersection point lie in the segment?
	   (when (intersect-point-and-line-segment? intersection-point line-segment)
	     intersection-point))
	  (intersection-point
	   ;; LINE-SEGMENT and LINE are colinear.  Return LINE-SEGMENT.
	   line-segment)
	  (t
	   ;; The lines are parallel and do not intersect.
	   nil))))


(defun INTERSECTION-OF-LINE-AND-LINE (line-1 line-2)
  "
  Purpose:  Find the intersection of two lines
  Args:	LINE-1	
	LINE-2	
  Returns:  a point if the lines intersect
	One of the lines if the lines are colinear
	Nil if the lines are parallel but not colinear
  "
  (declare (values point-or-boolean))
  (multiple-value-bind (x1 y1) (point-coordinates (line-point-1 line-1))
    (multiple-value-bind (x2 y2) (point-coordinates (line-point-2 line-1))
      (multiple-value-bind (x3 y3) (point-coordinates (line-point-1 line-2))
	(multiple-value-bind (x4 y4) (point-coordinates (line-point-2 line-2))
	  (let ((denominator (- (* (- x4 x3) (- y2 y1))
				(* (- y4 y3) (- x2 x1)))))
	    (if (~zerop denominator)
		;; Lines are parallel and maybe colinear
		(if (colinear? line-1 line-2) line-1 nil)
		;; Lines actually intersect
		(let ((beta (/ (- (* (- y3 y1) (- x2 x1))
				  (* (- x3 x1) (- y2 y1)))
			       denominator)))
		  (make-point (+ x3 (* beta (- x4 x3)))
			      (+ y3 (* beta (- y4 y3))))))))))))



