;;; 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; Lowercase:Yes -*-

;;; Copyright 1988, 1989 (c) David J. Braunegg.  All rights reserved.
;;; Distribution of this file is unrestricted provided that this notice is not removed.

;;;; Purpose:  Implement a 2D convex hull.
;;;;
;;;; Author:  David J. Braunegg
;;;; Creation Date:  October 11, 1988
;;;; Modifications to released version:
;;;; Programmer		Date		Description of modifications
;;;; ________________	___________	_________________________________________________________________
;;;; 
;;;; 
;;; Comments
;;;;	1) The L-Convex Hull is defined in "Visual Navigation for a Mobile Robot: Building a Map of the Occupied Space from Sparse 3-D Stereo Data"
;;;;		by A. R. de Saint Vincent, 1987 IEEE International Conference on Robotics and Automation.
;;;;
;;;; To Do:
;;;;	The L-Convex functions could be made faster by bucketing the points and only searching those points in the neighboring buckets to the current point.
;;;;	 The bucket size would be the L-Convex search distance.


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Provide/Export/Require
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(export '(jarvis-march-convex-hull merge-convex-hulls intersect-segment-and-hull?
          remove-colinear-vertices l-convex-hull l-convex-hull-strict))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Convex Hull
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun JARVIS-MARCH-CONVEX-HULL (points)
  "
  Purpose:  Find the convex hull of POINTS.
  Args:	POINTS	
  Returns:  a list of points, in order, which form the convex hull of POINTS.
  Details:
	1) The algorithm used is based on the Jarvis March algorithm given on p. 104 of Computational Geometry by Preparata and Shamos.
	2) If more than two points are colinear on the hull, only the endpoints of that hull segment are included in the hull.
	4) Worst-case running time is O(n^2), actual time is O(hn), where n = # points and h = # hull points.
	5) If two points have the same x- and y-coordinates and that location is a hull vertex, one ot the points will arbitrarily be used.
	6) Degenerate cases are handled correctly.
  "
  (declare (values convex-hull))
  (let* ((extreme-point (extreme-point points))
	 ;; Keep the EXTREME-POINT so that we can use it for an end test,
	 ;; BUT, remove any other points with the same x- and y-coordinates as EXTREME-POINT from the point list so that they do not screw up the (eq) end test.
	 (remaining-points (cons extreme-point (remove extreme-point points :test #'point-equal) ))
	 (hull-points (list extreme-point)))
    ;; Jarvis march up the right side
    (loop do
      (loop for point in (cdr remaining-points)
	    with last-point = (first hull-points)
	    with current-point = (first remaining-points)
	    as side = (side last-point current-point point)
	    do
	(cond ((plusp side)
	       ;; POINT is to the right of the ray from LAST-POINT to CURRENT-POINT and so is a better hull candidate
	       (setf current-point point))
	      ((zerop side)
	       ;; POINT lies on the ray from LAST-POINT to CURRENT-POINT, so we want the point which is farther away from LAST-POINT.
	       ;; Or, CURRENT-POINT matches LAST-POINT, in which case we don't want it.
	       (when (> (distance-squared-between-point-and-point last-point point)
			(distance-squared-between-point-and-point last-point current-point))
		 (setf current-point point))))
	    finally (push current-point hull-points)
		    (setf remaining-points (remove current-point remaining-points :test #'eq)))
	  when (eq (first hull-points) extreme-point)
	    ;; We have closed the hull by getting back to the original point, so we are done.
	    return nil)
    ;; Return the convex hull.  The initial point will be duplicated at the end, so remove it from the beginning
    (cdr hull-points)))

(defun MERGE-CONVEX-HULLS (convex-hull-1 convex-hull-2)
  "
  Purpose:  Find the convex hull of POINTS.
  Args:	CONVEX-HULL-1	-- a list of  points which, in order, form a convex hull
	CONVEX-HULL-2	-- a list of  points which, in order, form a convex hull
  Returns:  a list of points, in order, which form the convex hull of the points contained in CONVEX-HULL-1 and CONVEX-HULL-2
  Details:
	1) If more than two points are colinear on the hull, only the endpoints of that hull segment are included in the hull.
	3) If two points have the same x- and y-coordinates and that location is a hull vertex, one ot the points will arbitrarily be used.
	4) Degenerate cases are handled correctly.
  "
  (jarvis-march-convex-hull (append convex-hull-1 convex-hull-2)))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; L-Convex Hull
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun L-CONVEX-HULL-STRICT (points max-arc-length-squared)
  "
  Purpose:  Find the L-Convex Hull of POINTS.
	This function uses the strict definition of L-Convexity, which can yield degenerate polygons.
  Args:	POINTS
	MAX-ARC-LENGTH-SQUARED	-- square of maximum length of an arc connecting two hull points.
  Returns:  a list of points, in order, which form the L-Convex Hull of POINTS.
  Details:
	1) The algorithm used is based on the L-Convex Hull algorithm of A. R. de Saint Vincent.
	2) If more than two points are colinear on the hull, only the endpoints of that hull segment are included in the hull.
	4) Worst-case running time is O(kn + km), where n = # points, m = # arcs in the GC (Graph Component) and k = # arcs in of the hull
	5) If two points have the same x- and y-coordinates and that location is a hull vertex, one ot the points will arbitrarily be used.
	6) Degenerate cases look lousy.
  "
  (declare (values l-convex-hull))
  (let* ((extreme-point (extreme-point points))
	 (hull-points (list extreme-point)))
    (loop do
      (push (find-l-convex-hull-point points hull-points max-arc-length-squared) hull-points)
	  when (point-equal (first hull-points) extreme-point)
	    ;; We have closed the hull by getting back to the original point, so we are done.
	    ;; (The initial point will be duplicated at the end, so remove it from the beginning.)
	    ;; Return the convex hull.
	    return (cdr hull-points))))

(defun L-CONVEX-HULL (points max-arc-length-squared)
  "
  Purpose:  Find the L-Convex Hull of POINTS.
	This version relaxes the L-Convex condition to obtain non-degenerate polygons by eliminating hull points which cause a concave degeneracy.
  Args:	POINTS
	MAX-ARC-LENGTH-SQUARED	-- square of maximum length of an arc connecting two hull points.
  Returns:  a list of points, in order, which form the L-Convex Hull of POINTS.
  Details:
	1) The algorithm used is based on the L-Convex Hull algorithm of A. R. de Saint Vincent.
	2) If more than two points are colinear on the hull, only the endpoints of that hull segment are included in the hull.
	4) Worst-case running time is O(kn + km), where n = # points, m = # arcs in the GC (Graph Component) and k = # arcs in of the hull
	5) If two points have the same x- and y-coordinates and that location is a hull vertex, one ot the points will arbitrarily be used.
	6) Degenerate cases are handled correctly.
  2D:  Any list elements after the y-coordinate of a point are ignored for calculations.
  "
  (declare (values l-convex-hull))
  (if (cddr points)
      ;; There are at least 3 points, so do full algorithm.  (With 3, might be colinear and therefore degenerate.  So, do all the work.)
      (let* ((extreme-point (extreme-point points))
	     (hull-points (list extreme-point))
	     (last-point-degenerate nil)
	     degenerate-point
	     best-point
	     degenerate-pointer
	     first-part)
	(loop do
	  (setf best-point (find-l-convex-hull-point points hull-points max-arc-length-squared))
	  (when last-point-degenerate
	    ;; Skip degenerate hull points which are concave
	    (setf first-part (cdr hull-points))
	    (setf degenerate-pointer (position degenerate-point first-part :test #'point-equal))
	    (when (and (not (point-equal (nth degenerate-pointer first-part) extreme-point))
		       (not (left-turn? (nth (1+ degenerate-pointer) first-part)
					(nth degenerate-pointer first-part)
					(nth (1- degenerate-pointer) first-part))))
	      (setf hull-points (cons (first hull-points) (delete degenerate-point first-part :test #'point-equal :count 1))))
	    (when (not (left-turn? (second hull-points) (first hull-points) best-point))
	      (setf hull-points (cdr hull-points))))
	  (and (point-equal best-point extreme-point)	;Don't miss point to left of extreme point (see notes 5/4/89)
	       (not (left-turn? (find-l-convex-hull-point points (cons best-point hull-points) max-arc-length-squared)
				best-point
				(first hull-points)))
	       ;; We have closed the hull by getting back to the original point, so we are done.
	       (return (remove-colinear-vertices hull-points)))
	  (if (member best-point hull-points :test #'point-equal)
	      ;; Was this point degenerate?
	      (setf last-point-degenerate t
		    degenerate-point best-point)
	      (setf last-point-degenerate nil))
	  ;; Add this point to the hull
	  (push best-point hull-points)))
      ;; There are only 1 or 2 points, so just return them
      points))

(defun FIND-L-CONVEX-HULL-POINT (points hull-points max-arc-length-squared)
  "
  Purpose:  Find the next L-Convex hull point from a set of points given the current hull.
  Args:	POINTS			-- list of points for which we are finding the hull
	HULL-POINTS		-- list of hull points found so far
	MAX-ARC-LENGTH-SQUARED	-- square of maximum length of a hull arc
  Returns:  The next L-Convex hull point.
  "
  (declare (values l-convex-hull-point))
  (let* ((last-point (first hull-points))
	 (two-points-ago (or (second hull-points) (diff last-point (make-point 0 1))))
	 (best-angle 7)				;7 > 2*PI
	 (best-last-point-to-point-distance-squared 0)
	 best-point
	 last-point-to-point-distance-squared
	 angle)
    (loop for point in points
	  do
      (cond ((or
	       ;; First a quick distance check
	       (> (square-metric-distance-squared-between-point-and-point last-point point) max-arc-length-squared)
	       ;; Now the more expensive Euclidean distance check
	       (and (setf last-point-to-point-distance-squared (distance-squared-between-point-and-point last-point point))
		    (> last-point-to-point-distance-squared max-arc-length-squared)))
	     ;; Distance to this point is too great for point to be on L-Convex Hull
	     ())
	    ((point-equal point last-point)
	     ())	     ;Ignore duplicate points
	    ((intersect-segment-and-hull? (make-line-segment last-point point) hull-points)
	     ())	     ;Can't use a segment that intersects the hull
	    ((or
	       ;; POINT forms a smaller angle with the last two points than our best current candidate, so keep it.
	       (and (setf angle (angle-2pi two-points-ago last-point point))
		    (or (when (< (abs angle) .01) (setf angle *short-2pi*)) t)
		    (< angle best-angle))
	       ;; POINT lies on the same line as our current best point, so we want the point which is closest to LAST-POINT.  (So that we end as soon as possible.)
	       (and (= angle best-angle)
		    (< last-point-to-point-distance-squared best-last-point-to-point-distance-squared)))
	     (setf best-point point
		   best-angle angle
		   best-last-point-to-point-distance-squared last-point-to-point-distance-squared)))
	  finally 
	    (return best-point))))

(defun REMOVE-COLINEAR-VERTICES (vertices)
  "
  Purpose:  Remove vertices from a polygon which actually lie on the line between the preceding and following vertices.
  Args:	VERTICES	-- a list of points which are the vertices of a polygon.
  Returns:  a list of points which constitute the polygon with only real vertices.
  "
  (declare (values vertices))
  (let ((new-vertices ())
	(last-vertex (first (last vertices)))
	last-new-vertices)
    (loop for partial-vertices on vertices
	  ;; Find a starting vertex which is not in line with the end vertices vertex and the next one.
	  when (not (three-points-colinear? (second partial-vertices) (first partial-vertices) last-vertex))
	    do
	      (push (first partial-vertices) new-vertices)
	      (setf last-new-vertices (first partial-vertices))
	      ;; Now, go through the vertices, keeping only those vertices which do not lie on the straight-line edge between the previous vertex and the next one.
	      (loop as vertex-1 in (cdr partial-vertices)
		    for vertex-2 in (cddr partial-vertices)
		    unless (three-points-colinear? (first new-vertices) vertex-1 vertex-2)
		      do (push vertex-1 new-vertices)
		    finally
		      ;; Make sure that the last vertex isn't on hte straight-line edge with the previous vertex and the first one.
		      (unless (three-points-colinear? (first new-vertices) vertex-1 last-new-vertices)
			      (push vertex-1 new-vertices)))
	      (loop-finish))
    new-vertices))

(defun INTERSECT-SEGMENT-AND-HULL? (segment hull-points)
  "
  Purpose:  Determine if a line segment intersects the L-Convex hull found so far.
  Args:	SEGMENT	-- a line segment
	HULL-POINTS	-- a list of points
  Returns:  T if SEGMENT intersects the hull.  However, an intersection of a segment endpoint and a hull point is allowed (Nil returned).
  "
  (declare (values boolean))
  (loop as hull-endpoint-1 in hull-points
	for hull-endpoint-2 in (cdr hull-points)
	as hull-segment = (make-line-segment hull-endpoint-1 hull-endpoint-2)
	as hull-segment-bounding-box = (bounding-box hull-segment)
	with segment-bounding-box = (bounding-box segment)
	with segment-endpoint-1 = (line-segment-point-1 segment)
	with segment-endpoint-2 = (line-segment-point-2 segment)
	when (intersect-rectangle-and-rectangle? segment-bounding-box hull-segment-bounding-box)
	  ;; The bounding boxes intersect, so maybe the segments do
	  do
	    (and (not (point-equal segment-endpoint-1 hull-endpoint-1))
		 (not (point-equal segment-endpoint-1 hull-endpoint-2))
		 (not (point-equal segment-endpoint-2 hull-endpoint-1))
		 (not (point-equal segment-endpoint-2 hull-endpoint-2))
		 ;; Endpoint intersections are OK.  This only matters when NEW-POINT is taken from previous endpoints.
		 ;; Note that there could be an intersection with a point on the hull segment that is not an endpoint.  That point could lie on hte hull
		 ;; segment but no be in the hull because the existing endpoints formed a longer segment.
		 (intersect-line-segment-and-line-segment? segment (make-line-segment hull-endpoint-1 hull-endpoint-2))
		 ;; The segment intersected this hull segment
		 (return t))))

