;;;; Functions for manipulating 2-dimensional points.
;;;  These are used in GRID-ENV and GRID-AGENT functions.


(defstruct (xy (:type list)) "A two-dimensional (i.e. x and y) point." x y)

(defun xy-p (arg) 
  "Is the argument a 2-D point?"
  (and (consp arg) (= (length arg) 2) (every #'numberp arg)))

(defun @ (x y) "Create a 2-D point" (make-xy :x x :y y))

(defun xy-equal (p q) (equal p q))

(defun xy-add (p q)
  "Add two points, component-wise."
  (@ (+ (xy-x p) (xy-x q)) (+ (xy-y p) (xy-y q))))

(defun xy-distance (p q)
  "The distance between two points."
  (sqrt (+ (square (- (xy-x p) (xy-x q)))
           (square (- (xy-y p) (xy-y q))))))

(defun x+y-distance (p q)
  "The 'city block distance' between two points."
  (+ (abs (- (xy-x p) (xy-x q)))
     (abs (- (xy-y p) (xy-y q)))))

(defun xy-between (xy1 xy2 xy3)
  "Predicate; return t iff xy1 is between xy2 and xy3. Points are collinear."
  (and (between (xy-x xy1) (xy-x xy2) (xy-x xy3))
       (between (xy-y xy1) (xy-y xy2) (xy-y xy3))))

(defun rotate (o a b c d)
  (let ((x (xy-x o))
        (y (xy-y o)))
    (@ (+ (* a x) (* b y)) (+ (* c x) (* d y)))))

(defun inside (l xmax ymax)
  "Is the point l inside a rectangle from 0,0 to xmax,ymax?"
  (let ((x (xy-x l)) (y (xy-y l)))
    (and (>= x 0) (>= y 0) (< x xmax) (< y ymax))))