(require :geoclasses "geoclasses.l")
(provide :drawconst)

(defun draw-constraint (constraint &optional (a nil) (b nil)
				   (vwr geo:*viewer*) (arrow-length 30))
   (if (and a b)
       (dolist (clist constraint)
	 (if (and (eq (constraint-relation-constrained-body clist) a)
		  (eq (constraint-relation-constraining-body clist) b))
	     (dolist (c (constraint-relation-constraints clist))
	       (send c :draw-constraint vwr arrow-length))))
     (dolist (clist constraint)
       (dolist (c (constraint-relation-constraints clist))
	 (send c :draw-constraint vwr arrow-length)))))

(defun draw-cbodies (constraint &optional (arrow-length 5.0))
  (dolist (clist constraint)
    (draw (constraint-relation-constrained-body clist)
	  (constraint-relation-constraining-body clist))
    (dolist (c (constraint-relation-constraints clist))
      (send c :draw-constraint arrow-length))))

(defun draw-motion (motion a &rest b)
   (let (coords newbody newbodies m)
     (dolist (mlist motion)
       (dolist (m mlist)
         (setq coords (make-coords
		       :rot (make-matrix 3 3
			      (list (list 1.0 (- (elt m 5)) (elt m 4))
				    (list (elt m 5) 1.0 (- (elt m 3)))
				    (list (- (elt m 4)) (elt m 3) 1.0)))
		       :pos (subseq m 0 3)))
	 (setq newbody (copy-object a))
	 (send newbody :transform coords ':world)
	 (send newbody :worldcoords)
	 (read-char)
	 (cls)
	 (hidd a b)
	 (unix:sleep 1)
	 (hidd newbody b)
	 (push newbody newbodies))
       (read-char)
       (dolist (nb newbodies) (hidd nb)))))

(defun draw-force (force a b)
   (let (coords newbody newbodies f)
     (dolist (flist force)
       (dolist (f flist)
         (setq coords (make-coords
		       :rot (make-matrix 3 3
			      (list (list 1.0 (- (elt f 5)) (elt f 4))
				    (list (elt f 5) 1.0 (- (elt f 3)))
				    (list (- (elt f 4)) (elt f 3) 1.0)))
		       :pos (subseq f 0 3)))
	 (setq newbody (copy-object a))
	 (send newbody :transform coords ':world)
	 (send newbody :worldcoords)
	 (read-char)
	 (cls)
	 (hidd a b)
	 (unix:sleep 1)
	 (hidd newbody b)
	 (push newbody newbodies))
       (read-char)
       (dolist (nb newbodies) (hidd nb)))))

