;;; 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 '(distance distance-between-point-and-point distance-between-point-and-line-segment
	  distance-between-point-and-line  distance-between-line-segment-and-line-segment
	  distance-between-line-segment-and-line  distance-between-line-and-line  distance-squared
	  distance-squared-between-point-and-point  distance-squared-between-point-and-line-segment
	  distance-squared-between-point-and-line  distance-squared-between-line-and-line
	  distance-squared-between-line-segment-and-line  distance-squared-between-line-segment-and-line-segment
	  square-metric-distance-between-point-and-point  square-metric-distance-squared-between-point-and-point))
       

;;; To do: Add distance functions for rectangles and polygons.

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Distance functions
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun DISTANCE (object-1 object-2)
  "Returns the Euclidean distance between OBJECT-1 and OBJECT-2."
  (cond ((point-p object-1)
	 ;; OBJECT-1 is a point
	 (cond ((point-p object-2)
		(distance-between-point-and-point object-1 object-2))
	       ((line-segment-p object-2)
		(distance-between-point-and-line-segment object-1 object-2))
	       ((line-p object-2)
		(distance-between-point-and-line object-1 object-2))
	       (t
		(error
		  "Can't find the distance between ~A and ~A" object-1 object-2))))
	((point-p object-2)
	 ;; OBJECT-2 is a point, but OBJECT-1 is not
	 (distance object-2 object-1))
	((line-segment-p object-1)
	 ;; OBJECT-1 is a line segment
	 (cond ((line-segment-p object-2)
		(distance-between-line-segment-and-line-segment object-1 object-2))
	       ((line-p object-2)
		(distance-between-line-segment-and-line object-1 object-2))
	       (t
		(error
		  "Can't find the distance between ~A and ~A" object-1 object-2))))
	((line-segment-p object-2)
	 ;; OBJECT-2 is a line segment, but OBJECT-1 is not
	 (distance object-2 object-1))
	((line-p object-1)
	 ;; OBJECT-1 is a line
	 (cond ((line-p object-2)
		(distance-between-line-and-line object-1 object-2))
	       (t
		(error
		  "Can't find the distance between ~A and ~A" object-1 object-2))))
	(t
	 (error "Can't find the distance between ~A and ~A" object-1 object-2))))

(defun DISTANCE-BETWEEN-POINT-AND-POINT (point-1 point-2)
  "Returns the Euclidean distance between POINT-1 and POINT-2."
  (sqrt (distance-squared-between-point-and-point point-1 point-2)))

(defun DISTANCE-BETWEEN-POINT-AND-LINE-SEGMENT (point line-segment)
  "Returns the Euclidean distance between POINT and LINE-SEGMENT."
  (sqrt (distance-squared-between-point-and-line-segment point line-segment)))

(defun DISTANCE-BETWEEN-POINT-AND-LINE (point line)
  "Returns the Euclidean distance  between POINT and LINE."
  (abs (normal-directed-distance (line-point-1 line) point line)))

(defun DISTANCE-BETWEEN-LINE-SEGMENT-AND-LINE-SEGMENT
       (line-segment-1 line-segment-2)
  "Returns the Euclidean distance between LINE-SEGMENT-1 and LINE-SEGMENT-2."
  (sqrt (distance-squared-between-line-segment-and-line-segment line-segment-1 line-segment-2)))

(defun DISTANCE-BETWEEN-LINE-SEGMENT-AND-LINE (line-segment line)
  "Returns the Euclidean distance between LINE-SEGMENT and LINE."
  (sqrt (distance-squared-between-line-segment-and-line line-segment line)))

(defun DISTANCE-BETWEEN-LINE-AND-LINE (line-1 line-2)
  "Returns the Euclidean distance between LINE-1 and LINE-2."
  (if (intersect? line-1 line-2) 0
      (distance-between-point-and-line (line-point-1 line-1) line-2)))
      


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Distance-squared functions
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun DISTANCE-SQUARED (object-1 object-2)
  "Returns the square of the Euclidean distance between OBJECT-1 and OBJECT-2."
  (cond ((point-p object-1)
	 ;; OBJECT-1 is a point
	 (cond ((point-p object-2)
		(distance-squared-between-point-and-point object-1 object-2))
	       ((line-segment-p object-2)
		(distance-squared-between-point-and-line-segment object-1 object-2))
	       ((line-p object-2)
		(distance-squared-between-point-and-line object-1 object-2))
	       (t
		(error
		  "Can't find the distance-squared between ~A and ~A" object-1 object-2))))
	((point-p object-2)
	 ;; OBJECT-2 is a point, but OBJECT-1 is not
	 (distance-squared object-2 object-1))
	((line-segment-p object-1)
	 ;; OBJECT-1 is a line segment
	 (cond ((line-segment-p object-2)
		(distance-squared-between-line-segment-and-line-segment object-1 object-2))
	       ((line-p object-2)
		(distance-squared-between-line-segment-and-line object-1 object-2))
	       (t
		(error
		  "Can't find the distance-squared between ~A and ~A" object-1 object-2))))
	((line-segment-p object-2)
	 ;; OBJECT-2 is a line segment, but OBJECT-1 is not
	 (distance-squared object-2 object-1))
	((line-p object-1)
	 ;; OBJECT- is a line
	 (cond ((line-p object-2)
		(distance-squared-between-line-and-line object-1 object-2))
	       (t
		(error
		  "Can't find the distance-squared between ~A and ~A" object-1 object-2))))
	(t
	 (error "Can't find the distance-squared between ~A and ~A" object-1 object-2))))

(defun DISTANCE-SQUARED-BETWEEN-POINT-AND-POINT (point-1 point-2)
  "Returns the square of the Euclidean distance between POINT-1 and POINT-2."
  (+ (sq (- (point-x point-1) (point-x point-2)))
     (sq (- (point-y point-1) (point-y point-2)))))

(defun DISTANCE-SQUARED-BETWEEN-POINT-AND-LINE-SEGMENT (point line-segment)
  "Returns the square of the Euclidean distance between POINT and LINE-SEGMENT."
  ;; From "A Programmer's Geometry" by A. Bowyer and J. Woodwark
  (let* ((x (point-x point))
	 (y (point-y point))
	 (point-0 (line-segment-point-1 line-segment))
	 (x0 (point-x point-0))
	 (y0 (point-y point-0))
	 (point-1 (line-segment-point-2 line-segment))
	 (x1 (point-x point-1))
	 (y1 (point-y point-1))
	 (x-x0 (- x x0))
	 (x1-x0 (- x1 x0))
	 (y-y0 (- y y0))
	 (y1-y0 (- y1 y0))
	 ;; Consider line segment parameterized by k, k=0 => point-0, k = 1 => point-1
	 (k (/ (+ (* x-x0 x1-x0) (* y-y0 y1-y0))
	       (+ (sq x1-x0) (sq y1-y0)))))
    ;; Now, limit k to the range [0 1] so that it corresponds to the point on the LINE-SEGMENT closest to the POINT.
    (setf k (or (and (> k 1) 1)
		(and (< k 0) 0)
		k))
    ;; Now calculate the distance squared
    (+ (sq (- (* k x1-x0) x-x0))
       (sq (- (* k y1-y0) y-y0)))))

(defun DISTANCE-SQUARED-BETWEEN-POINT-AND-LINE (point line)
  "Returns the square of the Euclidean distance-squared between POINT and LINE."
  (sq (distance-between-point-and-line point line)))

(defun DISTANCE-SQUARED-BETWEEN-LINE-AND-LINE (line-1 line-2)
  "Returns the square of the Euclidean distance between LINE-1 and LINE-2."
  (sq (distance-between-line-and-line line-1 line-2)))

(defun DISTANCE-SQUARED-BETWEEN-LINE-SEGMENT-AND-LINE (line-segment line)
  "Returns the square of the Euclidean distance between LINE-SEGMENT and LINE."
  (if (intersect? line-segment line) 0
      (min (sq (normal-directed-distance (line-point-1 line) (line-segment-point-1 line-segment) line))
	   (sq (normal-directed-distance (line-point-1 line) (line-segment-point-2 line-segment) line)))))

(defun DISTANCE-SQUARED-BETWEEN-LINE-SEGMENT-AND-LINE-SEGMENT (line-segment-1 line-segment-2)
  "
  Purpose:  Find the square of the Euclidean distance between two line segments
	The result also holds if either or both of the line segments are degenerate.
  Returns:  Multiple-value-returns the square of the Euclidean distance between the line segments,
		the point on LINE-SEGMENT-1 closest to the second line segment, and
		the point on LINE-SEGMENT-2 closest to the first line segment.
  Comment:  One of the returned points will always be a line-segment endpoint since we are operating in 2D.
  "
  (declare (values distance-squared segment-1-closest-point segment-2-closest-point))
  ;; Initialize by checking point 1-a against LINE-SEGMENT-2
  (let ((pt (intersection-object line-segment-1 line-segment-2)))
    (if pt (values 0 pt pt)
	(let ((best-point-1 (line-segment-point-1 line-segment-1)))
	  (multiple-value-bind (best-distance best-point-2)
	      (distance-squared-between-point-and-line-segment (line-segment-point-1 line-segment-1) line-segment-2)
	    ;; Now compare point 1-b to LINE-SEGMENT-2
	    (multiple-value-bind (candidate-distance candidate-point)
		(distance-squared-between-point-and-line-segment (line-segment-point-2 line-segment-1) line-segment-2)
	      (and (< candidate-distance best-distance)
		   (setf best-distance candidate-distance best-point-1 (line-segment-point-2 line-segment-1)
			 best-point-2 candidate-point))
	      ;; Now compare point 2-a to LINE-SEGMENT-1
	      (multiple-value-setq (candidate-distance candidate-point)
		(distance-squared-between-point-and-line-segment (line-segment-point-1 line-segment-2) line-segment-1))
	      (and (< candidate-distance best-distance)
		   (setf best-distance candidate-distance best-point-1 candidate-point best-point-2
			 (line-segment-point-1 line-segment-2)))
	      ;; Now compare point 2-b to LINE-SEGMENT-1
	      (multiple-value-setq (candidate-distance candidate-point)
		(distance-squared-between-point-and-line-segment (line-segment-point-2 line-segment-2) line-segment-1))
	      (and (< candidate-distance best-distance)
		   (setf best-distance candidate-distance best-point-1 candidate-point best-point-2
			 (line-segment-point-2 line-segment-2))))
	    ;; Finally, return the closest distance and points
	    (values best-distance
		    best-point-1
		    best-point-2))))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Distance Functions (Non-Euclidean)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun SQUARE-METRIC-DISTANCE-BETWEEN-POINT-AND-POINT (point-1 point-2)
  "
  Purpose:  Find the Square-Metric distance between two points.
		(The Square-Metric distance is the max of delta-x and delta-y.)
  Args:	POINT-1	-- a point
	POINT-2	-- a point
  Returns:  The Square-Metric distance between POINT-1 and POINT-2
  "
  (declare (values square-metric-distance))
  (max (abs (- (point-x point-1) (point-x point-2)))
       (abs (- (point-y point-1) (point-y point-2)))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Distance-Squared Functions (Non-Euclidean)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun SQUARE-METRIC-DISTANCE-SQUARED-BETWEEN-POINT-AND-POINT (point-1 point-2)
  "
  Purpose:  Find the square of the Square-Metric distance between two points.
		(The Square-Metric distance is the max of delta-x and delta-y.)
  Args:	POINT-1	-- a point
	POINT-2	-- a point
  Returns:  The square of the Square-Metric distance between POINT-1 and POINT-2
  "
  (declare (values square-metric-distance-squared))
  (sq (square-metric-distance-between-point-and-point point-1 point-2)))

