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

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

(export '(normal cross dot diff add unit directed-distance normal-directed-distance cart-to-polar polar-to-cart side
	  left-turn?))

(defun NORMAL (v)
  (let ((v (g-coerce v 'point)))
    (make-point (point-y v) (- (point-x v)))))

(defun CROSS (v1 v2)
  (let ((v1 (g-coerce v1 'point))
	(v2 (g-coerce v2 'point)))
    (- (* (point-x v1) (point-y v2))
       (* (point-y v1) (point-x v2)))))

(defun DOT (v1 v2)
  (let ((v1 (g-coerce v1 'point))
	(v2 (g-coerce v2 'point)))
    (+ (* (point-x v1) (point-x v2))
       (* (point-y v1) (point-y v2)))))

(defun DIFF (v1 v2)
  (let ((v1 (g-coerce v1 'point))
	(v2 (g-coerce v2 'point)))
  (make-point (- (point-x v1) (point-x v2))
	       (- (point-y v1) (point-y v2)))))

(defun ADD (v1 v2)
  (let ((v1 (g-coerce v1 'point))
	(v2 (g-coerce v2 'point)))
    (make-point (+ (point-x v1) (point-x v2))
		(+ (point-y v1) (point-y v2)))))

(defun UNIT (v)
  (let ((v (g-coerce v 'point)))
    (let ((mag (sqrt (dot v v))))
      (if (< (abs mag) *epsilon*) nil (multiply v (/ 1.0 mag))))))

(defun DIRECTED-DISTANCE (pt1 pt2 line)		; Distance from pt1 to pt2
						; in direction from first pt in line to second
  (dot (unit line) (make-line pt1 pt2)))

(defun NORMAL-DIRECTED-DISTANCE (pt1 pt2 line)  ; distance from pt1 to pt2 in direction of
						; normal of line.
  (dot (diff pt2 pt1) (unit (normal (g-coerce line 'point)))))

(defun CART-TO-POLAR (pt)
  (values (sqrt (+ (square (point-x pt)) (square (point-y pt))))
	  (if (point-equal pt *ORIGIN*)
	      0
	      (atan (point-y pt) (point-x pt)))))

(defun POLAR-TO-CART (mag angle)
  (make-point (* mag (cos angle)) (* mag (sin angle))))

(defun SIDE (point-1 point-2 point-3)
  "
  Purpose:  Given the ray (POINT-1 --> POINT-2), determine on which side POINT-3 lies.
  Args:	POINT-1	-- a point (X Y)
	POINT-2	-- a point (X Y)
	POINT-3	-- a point (X Y)
  Returns:
	+1	if POINT-3 lies to the right of (POINT-1 --> POINT-2), i.e., POINT-1 to POINT-2 to POINT-3 is a right turn
	0	if POINT-3 is colinear with (POINT-1 --> POINT-2)
	-1	if POINT-3 lies to the left of (POINT-1 --> POINT-2)
	However, if any of the points coincide, zero (0) is returned.
  Method:  Find the sign of the dot product of (perpendicular to POINT-1 --> POINT-2) with (POINT-2 --> POINT-3).
	This is equivalent (with 4 fewer multiplies) to the method on p. 43 of Computational Geometry by Preparata and Shamos where we find the sign of the determinant
	| x1 y1 1 |
	| x2 y2 1 |
	| x3 y3 1 |
	with the sign reversed.
  "
  (declare (values sign))
  (let ((x2 (point-x point-2))
	(y2 (point-y point-2)))
    (~signum (+ (* (- y2 (point-y point-1)) (- (point-x point-3) x2))
	        (* (- (point-x point-1) x2) (- (point-y point-3) y2))))))

(defun LEFT-TURN? (point-1 point-2 point-3)
  "
  Purpose:  Determine if travelling from POINT-1 to POINT-2 to POINT-3 is a left turn.
	Alternatively, determine if POINT-3 is to the left of the ray from POINT-1 to POINT-2.
  Returns:  T if a left turn.  Nil if a right turn or the points are colinear
  2D:  Any list elements after the y-coordinate of a point are ignored for calculations.
  "
  (declare (values boolean))
  (minusp (side point-1 point-2 point-3)))

