;;; 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 '(direction parallel? parallel-vectors? vector-angle ordered-vector-angle angle-0 
          angle-2pi abs-angle-difference))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Parallelism
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun DIRECTION (object)
  "Returns a unit vector which defines the direction of OBJECT."
  (unit (g-coerce object 'point)))

(defun PARALLEL? (line-1 line-2)
  "Returns T if LINE-1 and LINE-2 are parallel.  If parallel, they may or may not be colinear."
  (let ((direction-1 (direction line-1))
	(direction-2 (direction line-2)))
    (parallel-vectors? direction-1 direction-2)))

(defun PARALLEL-VECTORS? (vector-1 vector-2)
  "Returns T if VECTOR-1 and VECTOR-2 are parallel.  The vectors are represented as points."
  (cond ((not (and (point-p vector-1) (point-p vector-2)))
	 (error "~A and ~A must both be points."))
	(t (let ((dot-1-2 (dot (unit vector-1) (unit vector-2))))
	     (or  (~= dot-1-2 1)
		  (~= dot-1-2 -1))))))

(defun VECTOR-ANGLE (vec1 vec2)
  "The angle formed by two vectors.  Their order doesn't matter, so the angle varies
   from 0 to PI."
  (let ((vec1 (unit vec1))
	(vec2 (unit vec2)))
    (atan (abs (dot (normal vec1) vec2)) (dot vec1 vec2))))

(defun ORDERED-VECTOR-ANGLE (vec1 vec2)
  "The angle found by rotating vec1 counter-clockwise to vec2.  Angle returned varies
   from -PI to PI.  A negative angle means that rotating the first vector clockwise by
   that angle brings it to the second vector."
  (let ((vec1 (unit vec1))
	(vec2 (unit vec2)))
    (atan (- (dot (normal vec1) vec2)) (dot vec1 vec2))))

(defun CONNECTED-LINE-ANGLE (ls1 ls2)
  "Finds the angle from the first line segment to the second, when the second point of ls1 =
   the first point of ls2"
  (ordered-vector-angle (multiply (g-coerce ls1 'point) -1) (g-coerce ls2 'point)))

(defun ANGLE-0 (point-1 point-2 point-3)
  "
  Purpose:  Find the angle POINT-1 POINT-2 POINT-3 in radians.
	This version uses the angle 0 and not 2*PI.
  Returns:  The angle, in radians, of POINT-1 POINT-2 POINT-3 in the range [0 2PI).
  Caveats:  If POINT-1 = POINT-2 or POINT-2 = POINT-3, an error will result.
  "
  (declare (values radians))
  (if (point-equal point-1 point-3)
      ;; This is an easy case that can be missed due to finite-precision math
      0
      ;; Normal case
      (let* (
	     ;; Get the vectors
	     (a (make-point (- (point-x point-1) (point-x point-2)) (- (point-y point-1) (point-y point-2))))
	     (b (make-point (- (point-x point-3) (point-x point-2)) (- (point-y point-3) (point-y point-2))))
	     ;; Now the cosine
	     (cos-theta (/ (dot a b) (norm a) (norm b))))
	;; Close points may lead to bad data, with |cosine(theta)| > 1, so we round to +/-1
	(cond ((>= cos-theta 1.0) 0)		;Easy case.  Use 0, not 2*PI.
	      ((<= cos-theta -1.0) *short-pi*)	;Easy case.  PI.
	      ((minusp (side point-1 point-2 point-3)) (- *short-2pi* (acos cos-theta)))	;PI < THETA < 2*PI
	      (t (acos cos-theta))))))		;0 < THETA < PI

(defun ANGLE-2PI (point-1 point-2 point-3)
  "
  Purpose:  Find the angle POINT-1 POINT-2 POINT-3 in radians.
	This version uses the angle 2*PI and not 0.
  Returns:  The angle, in radians, of POINT-1 POINT-2 POINT-3 in the range (0 2PI].
  Caveats:  If POINT-1 = POINT-2 or POINT-2 = POINT-3, an error will result.
  2D:  Any list elements after the y-coordinate of a point are ignored for calculations.
  "
  (declare (values radians))
  (if (point-equal point-1 point-3)
      ;; This is an easy case that can be missed due to finite-precision math
      *short-2pi*
      ;; Normal case
      (let* (
	     ;; Get the vectors
	     (a (diff point-1 point-2))
	     (b (diff point-3 point-2))
	     ;; Now the cosine
	     (cos-theta (/ (dot a b) (norm a) (norm b))))
	;; Close points may lead to bad data, with |cosine(theta)| > 1, so we round to +/-1
	(cond ((>= cos-theta 1.0) *short-2pi*)	;Easy case.  Use 2*PI, not 0.
	      ((<= cos-theta -1.0) *short-pi*)	;Easy case.  PI.
	      ((minusp (side point-1 point-2 point-3)) (- *short-2pi* (acos cos-theta)))	;PI < THETA < 2*PI
	      (t (acos cos-theta))))))		;0 < THETA < PI

(defun ABS-ANGLE-DIFFERENCE (a1 a2)
  "Given two angles, from 0 to 2pi, find the absolute difference between them."
  (let ((diff (abs (- a1 a2))))
    (min diff (- g:*2pi* diff))))