;;; 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.

;;;; File:  LINEARFIT.LSP
;;;;
;;;; Purpose:  Functions to perform a least-squares linear fit to a set of 2D data points.  (Slope-intercept or rho-theta line description.)
;;;;		Also code for drawing a rho-theta line.
;;;;
;;;; Author:  David J. Braunegg
;;;; Creation Date:  May 9, 1988
;;;; Modifications to released version:
;;;; Programmer		Date		Description of modifications
;;;; ________________	___________	_________________________________________________________________
;;;; 
;;;; 
;;;; Comments:  
;;;;	1) Reference: Robot Vision by B. K. P. Horn, Section 3.2, pp. 48--53


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Export
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(export '(least-squares-rho-theta
	  least-squares-slope-intercept
	  rho-theta-to-slope-intercept
	  slope-intercept-to-rho-theta
	  line-intersection-rho-theta
	  line-intersection-slope-intercept
	  closest-rho-theta-line-point
	  distance-between-point-and-rho-theta-line
	  distance-squared-between-point-and-rho-theta-line))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Rho-Theta Line Fitting
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun LEAST-SQUARES-RHO-THETA (point-list)
  "
  Purpose:  Fit a line to a point set, minimizing the sum of the distances from the points to the line.
  Args:	POINT-LIST	-- a list of point to which we fit a line.
  Returns:	  a list of (Line X Y), where
		Line	-- The line that best fits the points
		X-BAR	-- x-coordinate of center of area
		Y-BAR	-- y-coordinate of center of area
  "
  (declare (values (rho theta x-bar y-bar)))
  ;; Find the center of area of the points, which we will use as a point on the line.
  (let (x-bar y-bar a b c)
    (loop for (x y) in (mapcar #'point-coordinates-list point-list)
	  as count from 1
	  sum x into sum-x
	  sum y into sum-y
	  finally
	    (setf x-bar (float (/ sum-x count)))
	    (setf y-bar (float (/ sum-y count))))
    ;; Find the angle (slope) of the line
    (loop for (x y) in (mapcar #'point-coordinates-list point-list)
	  as x-prime = (- x x-bar)
	  as y-prime = (- y y-bar)
	  sum (sq x-prime) into almost-a
	  sum (* x-prime y-prime) into almost-b
	  sum (sq y-prime) into almost-c
	  finally
	    (setf a almost-a)
	    (setf b (* 2 almost-b))
	    (setf c almost-c))
    (let ((theta (/ (atan b (- a c)) 2.0)))
      (list (line-from-rho-theta (- (* y-bar (cos theta)) (* x-bar (sin theta))) theta) x-bar y-bar))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Slope-Intercept Line Fitting
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun LEAST-SQUARES-SLOPE-INTERCEPT (point-list)
  "
  Purpose:  Fit a line to a point set, minimizing the sum of the vertical (y) distances from the points to the line.
	The line determined has the form y = mx + b.
  Args:	POINT-LIST	-- a list of point to which we fit a line.
  Returns:  a line 
  Returns:	  a list of (Line X-BAR Y-BAR), where
		X-BAR	-- x-coordinate of center of area
		Y-BAR	-- y-coordinate of center of area
  "
  (declare (values (m b)))
  (loop for (x y) in (mapcar #'point-coordinates-list point-list)
	with n = (length point-list)
	sum x into sum-x
	sum y into sum-y
	sum (* x y) into sum-x-y
	sum (* x x) into sum-x-squared
	finally
	  (return
	    (list
	      (line-from-slope-intercept
		;; M
		(/ (float (- (* n sum-x-y) (* sum-x sum-y)))
		   (float (- (* n sum-x-squared) (* sum-x sum-x))))
		;; B
		(/ (float (- (* sum-y sum-x-squared) (* sum-x sum-x-y)))
		   (float (- (* n sum-x-squared) (* sum-x sum-x)))))
	      ;; X-BAR
	      (/ sum-x (float n))
	      ;; Y-BAR
	      (/ sum-y (float n))))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Line equation conversions: slope-intercept to rho-theta and vice-versa
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun RHO-THETA-TO-SLOPE-INTERCEPT (rho theta)
  "
  Purpose:  Convert rho-theta representation of a line to slope-intercept representation
  Args:	RHO	-- distance from origin to line
	THETA	-- angle of the line wrt x-axis, in radians, CCW positive
  Returns:  a list of the slope (m) and y-intercept (b) of the line
  "
  (declare (values (m b)))
  (list (tan theta) (/ rho (cos theta))))

(defun SLOPE-INTERCEPT-TO-RHO-THETA (m b)
  "
  Purpose:  Convert slope-intercept representation of a line to rho-theta representation
  Args:	M	-- slope of line
	B	-- y-intercept of line
  Returns:  a list of the distance from the origin to the line (rho) and the angle of the line wrt the x-axis (theta), in radians, CCW positive
  "
  (declare (values (rho theta)))
  (list (* b (/ 1 (sqrt (1+ (sq m))))) (atan m)))

(defun LINE-FROM-RHO-THETA (rho theta)
  "The line determined has the form x*sin(theta) + y*cos(theta) + rho = 0."
  (if (= 0 (cos theta)) (make-line (make-point 0 0) (make-point 0 1))	; vertical line
      (make-line (make-point 0 (/ rho (cos theta)))
		 (make-point 1 (/ (+ rho (sin theta)) (cos theta))))))

(defun LINE-FROM-SLOPE-INTERCEPT (m b)
  "The line determined has the form y = mx + b."
  (make-line (make-point 0 b)
	     (make-point 1 (+ m b))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Line intersections
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun LINE-INTERSECTION-RHO-THETA (rho-1 theta-1 rho-2 theta-2)
  "
  Purpose:  Find the intersection point of two lines
  Args:	RHO-1	-- distance from origin to line 1
	THETA-1	-- angle of line 1 wrt x-axis, in radians, CCW positive
	RHO-2	-- distance from origin to line 2
	THETA-2	-- angle of line 2 wrt x-axis, in radians, CCW positive
  Returns:  The intersection point
	NIL if the lines are parallel and not coincident, or
	T if the lines coincide
  Caveats:  If either of THETA-1 or THETA-2 is a double-float, we assume that the other is also
  Remarks:  Modified from edge-intersection-coordinates in WH:/HL/DMS/LISPM/HAPTICS/OBJECTS.LISP
  "
  (declare (values (x y)))
  (let ((t-1 (if (typep theta-1 'double-float) (mod theta-1 *2pi*) (mod theta-1 *short-2pi*)))
	(t-2 (if (typep theta-2 'double-float) (mod theta-2 *2pi*) (mod theta-2 *short-2pi*))))
    (cond
      ;; Check for parallel lines
      ((= t-1 t-2)
       ;; Equal angles
       (if (= rho-1 rho-2)
	   t
	   ()))
      ((or (and (typep theta-2 'double-float) (= t-1 (mod (+ theta-2 pi) *2pi*)))
	   (= t-1 (mod (+ t-2 *short-pi*) *short-2pi*)))
       ;; Opposite angles
       (if (= rho-1 (- rho-2))
	   t
	   ()))
      ;; Non-Parallel lines
      (t
       (let ((y (/ (- (* rho-2 (sin theta-1)) (* rho-1 (sin theta-2)))
		   (- (* (sin theta-1) (cos theta-2)) (* (sin theta-2) (cos theta-1))))))
	 (make-point
	   ;; x
	   (if (= (sin theta-1) 0)
	       (/ (- (* y (cos theta-2)) rho-2)
		  (sin theta-2))
	       (/ (- (* y (cos theta-1)) rho-1)
		  (sin theta-1)))
	   ;; y
	   y))))))

(defun LINE-INTERSECTION-SLOPE-INTERCEPT (m-1 b-1 m-2 b-2)
  "
  Purpose:  Find the intersection point of two lines
  Args:	M-1	-- slope of line 1
	B-1	-- y-intercept of line 1
	M-2	-- slope of line 2
	B-2	-- y-intercept of line 2
  Returns:  The intersection point, as a list (x y), or
	NIL if the lines are parallel and not coincident, or
	T if the lines coincide
  Caveat:  Since arbitrary vertical lines cannot be represented in slope-intercept form, if either slope is infinity an error is generated.
  "
  (declare (values (x y)))
  (when (or (= m-1 +1e) (= m-2 +1e) (= m-1 -1e) (= m-2 -1e))
    (error "Vertical lines are not allowed."))
  (cond
    ;; Check for parallel lines
    ((= m-1 m-2)
     ;; Equal slopes
     (if (= b-1 b-2)
	 t
	 ()))
    ;; Non-Parallel lines
    (t
     (let ((x (/ (- b-2 b-1) (- m-1 m-2))))
       (make-point
	 ;; X
	 x
	 ;; Y
	 (+ (* m-1 x) b-1))))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Closest point
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun CLOSEST-RHO-THETA-LINE-POINT (point rho-theta)
  "
  Purpose:  Find the point on a RHO-THETA line closest to a given POINT
  Args:	POINT
	RHO-THETA	-- a list (RHO THETA) describing the line
  Returns:  A point on the line
  Reference:  Robot Vision, p. 51
  "
  (declare (values point))
  ;; New method (from BKPH's book)
  (let* ((rho (first rho-theta))
	 (theta (second rho-theta))
	 (cos-theta (cos theta))
	 (sin-theta (sin theta))
	 (s (+ (* (point-x point) cos-theta) (* (point-y point) sin-theta))))
    (make-point (- (* s cos-theta) (* rho sin-theta))
		(+ (* rho cos-theta) (* s sin-theta)))))

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

(defun DISTANCE-BETWEEN-POINT-AND-RHO-THETA-LINE (point rho-theta)
  "
  Purpose:  Find the Euclidean distance between a point and a line.
  Args:	POINT
	RHO-THETA	-- a list (RHO THETA) describing the line
  Returns:  Multiple-value-returns the Euclidean distance between POINT and the line described by RHO-THETA
		and the point on the line closest to POINT.
  "
  (declare (values distance))
  (multiple-value-bind (dist-sq pt) (distance-squared-between-point-and-rho-theta-line point rho-theta)
    (values (sqrt dist-sq) pt)))

(defun DISTANCE-SQUARED-BETWEEN-POINT-AND-RHO-THETA-LINE (point rho-theta)
  "
  Purpose:  Find the square of the Euclidean distance between a point and a line.
  Args:	POINT
	RHO-THETA	-- a list (RHO THETA) describing the line
  Returns:  Multiple-value-returns the square of the Euclidean distance between POINT and the line described by RHO-THETA
		and the point on the line closest to POINT.
  "
  (declare (values distance-squared))
  (let ((closest-point (closest-rho-theta-line-point point rho-theta)))
    (values (distance-squared-between-point-and-point point closest-point) closest-point)))

