home *** CD-ROM | disk | FTP | other *** search
/ vis-ftp.cs.umass.edu / vis-ftp.cs.umass.edu.tar / vis-ftp.cs.umass.edu / pub / Software / ASCENDER / ascendMar8.tar / UMass / BuildingFinder / Lisp / correct_angle.backup next >
Lisp/Scheme  |  1995-06-07  |  4KB  |  104 lines
  1. (in-package 'cme)
  2.  
  3. (defun lenvec-2d (vec)
  4.   (sqrt (+ (* (car vec) (car vec))
  5.        (* (cadr vec) (cadr vec)))))
  6.  
  7. (defun cos-angle-2d (vec1 vec2)
  8.   (/ (+ (* (car vec1) (car vec2))
  9.     (* (cadr vec1) (cadr vec2)))
  10.      (lenvec-2d vec1)
  11.      (lenvec-2d vec2)))
  12.  
  13. (defun backproj-aux (proj u v &optional (z 0.0))
  14.   (project-to-world
  15.       proj
  16.       (coerce u 'double-float)
  17.       (coerce v 'double-float)
  18.       (coerce z 'double-float)))
  19.  
  20. (defun make-vec-2d (x1 y1 x2 y2)
  21.   (list (- x2 x1) (- y2 y1)))
  22.  
  23. (defun farthest-point (basex basey ax ay bx by)
  24.   (if (< (lenvec-2d (make-vec-2d basex basey ax ay))
  25.      (lenvec-2d (make-vec-2d basex basey bx by)))
  26.       (list bx by)
  27.       (list ax ay)))
  28.  
  29. (defvar *return-struct2* (lci::make-double_2))
  30.  
  31. (defun fill-double2-struct (struct x y)
  32.   (setf (lci::double_2-d1 struct) (coerce x 'double-float))
  33.   (setf (lci::double_2-d2 struct) (coerce y 'double-float))
  34.   struct)
  35.    
  36. (defun is-corner-p (proj u1 v1 vertu vertv u2 v2 angle a2 b2)
  37.   (let* ((lsinangle (lenvec-2d (make-vec-2d vertu vertv u2 v2)))
  38.      (lcosangle (* lsinangle (tan angle)))
  39.      (du (* lcosangle a2))
  40.      (dv (* lcosangle b2))
  41.      (minu2 (- u2 du))
  42.      (minv2 (- v2 dv))
  43.      (maxu2 (+ u2 du))
  44.      (maxv2 (+ v2 dv)))
  45. #|  (format t "angle is ~d~%" angle)
  46.     (format t "lsinang, lcosang are ~d, ~d~%" lsinangle lcosangle)
  47.     (format t "vertu,v is ~a~%" (list vertu vertv))
  48.     (format t "u1,v1 is ~a~%" (list u1 v1))
  49.     (format t "minu2,v2 is ~a~%" (list minu2 minv2))
  50.     (format t "maxu2,v2 is ~a~%" (list maxu2 maxv2)) |#
  51.   (multiple-value-bind (vx vy) (backproj-aux proj vertu vertv)
  52.     (if (null vx)
  53.     (fill-double2-struct *return-struct2* -1.0 -1.0)
  54.       (multiple-value-bind (x1 y1) (backproj-aux proj u1 v1)
  55.       (multiple-value-bind (minx2 miny2) (backproj-aux proj minu2 minv2)
  56.       (multiple-value-bind (maxx2 maxy2) (backproj-aux proj maxu2 maxv2)
  57.       (let* ((basevec (make-vec-2d vx vy x1 y1))
  58.          (minvec (make-vec-2d vx vy minx2 miny2))
  59.          (maxvec (make-vec-2d vx vy maxx2 maxy2))
  60.          (cosangle1 (cos-angle-2d basevec minvec))
  61.          (cosangle2 (cos-angle-2d basevec maxvec)))
  62. #|    (format t "basevec is ~a~%" basevec)
  63.     (format t "minvec is ~a~%" minvec)
  64.     (format t "maxvec is ~a~%" maxvec)
  65.     (format t "angle1, angle2 is ~d, ~d~%" (acos cosangle1) (acos cosangle2))
  66.     (format t "cos angle test is ~d < 0 < ~d~%" (min cosangle1 cosangle2)
  67.         (max cosangle1 cosangle2)) |#
  68.     (if (plusp (* cosangle1 cosangle2))
  69.         (fill-double2-struct *return-struct2* -1.0 -1.0)
  70.         (fill-double2-struct *return-struct2* vertu vertv))))))))))
  71.  
  72.  
  73. (defun form-line-vector (u1 v1 u2 v2)
  74.   (let ((a (- v1 v2))
  75.     (b (- u2 u1))
  76.     (c (- (* u1 v2) (* u2 v1))))
  77.     (let ((scale (sqrt (+ (* a a) (* b b)))))
  78.       (values (/ a scale) (/ b scale) (/ c scale)))))
  79.  
  80. (defun line-intersect-point (a1 b1 c1 a2 b2 c2)
  81.   (let ((p (- (* b1 c2) (* b2 c1)))
  82.     (q (- (* c1 a2) (* c2 a1)))
  83.     (r (- (* a1 b2) (* a2 b1))))
  84.     (unless (= r 0)
  85.       (values (/ p r) (/ q r)))))
  86.  
  87. (defun lines-form-corner-p (proj au1 av1 au2 av2 bu1 bv1 bu2 bv2 angthreshold)
  88.   (multiple-value-bind (a1 b1 c1) (form-line-vector au1 av1 au2 av2)
  89.   (multiple-value-bind (a2 b2 c2) (form-line-vector bu1 bv1 bu2 bv2)
  90.   (multiple-value-bind (u v) (line-intersect-point a1 b1 c1 a2 b2 c2)
  91.     (if u
  92.     (let ((farapt (farthest-point u v au1 av1 au2 av2))
  93.           (farbpt (farthest-point u v bu1 bv1 bu2 bv2)))
  94.       (is-corner-p proj
  95.                (car farapt) (cadr farapt)
  96.                u v
  97.                (car farbpt) (cadr farbpt)
  98.                angthreshold a2 b2))
  99.         (fill-double2-struct *return-struct2* -1.0 -1.0))))))
  100.  
  101.  
  102.  
  103.  
  104.