Merge pull request #222 from kaveh808/opengl-mouse-picking-with-ray-c…

…asting

OpenGL mouse picking with ray casting

kons-9.asd

@@ -54,6 +54,9 @@
    (:file "src/kernel/scene-duplicate")
    (:file "src/kernel/protocol")
    (:file "src/kernel/clobber")
+   (:file "src/kernel/ray-triangle-intersect")
+   (:file "src/kernel/ray")
+   (:file "src/kernel/object-picking")
    (:file "src/kernel/main")
    ;; font libraries -- tmp until we use 3b-bmfont
    (:module "lib/JMC-font-libs/font-master"
@@ -115,7 +118,8 @@
 		 (:file "assertions")
  		 (:module "kernel"
  		  :components ((:file "utils")
-			       (:file "point-cloud")))
+			       (:file "point-cloud")
+                               (:file "ray-triangle-intersect")))
  		 (:file "entrypoint")))))
 
 (asdf:defsystem #:kons-9/api-docs

src/graphics/glfw/glfw-gui.lisp

@@ -42,6 +42,32 @@
   view)
 |#
 
+;;;; utils =====================================================================
+
+(let ((mouse-moved "undefined"))
+
+  (defun simple-click-disturbed ()
+    (setf mouse-moved t))
+
+  (defun simple-click-left-p (button action)
+    (when (eq button :left)
+      (when (eq action :press)
+        (setf mouse-moved nil))
+      (when (eq action :release)
+        (eq mouse-moved nil))))
+  )
+
+(defun win-to-screen-xy (win-x win-y)
+  ;; In `screen space` (also for OpenGL) origin is in the botton-left. In window
+  ;; space, origin is in the top-left. So, screen-x is same as win-x but we need
+  ;; to translate win-y to get screen-y.
+  (values
+   win-x
+   (- (second *window-size*) win-y)))
+
+(defun shift-key-p (modifier-keys)
+  (not (not (find :shift modifier-keys))))
+
 ;;;; scene-view ================================================================
 
 (defclass-kons-9 scene-view ()
@@ -301,6 +327,11 @@
   (when *display-ground-plane?*
     (draw-ground-plane))
 
+  ;; object picking
+
+  (when-pick-requested (ray multi-select)
+    (pick ray multi-select (scene view)))
+
   ;; display ui layer
 
   (2d-setup-projection (first *window-size*) (second *window-size*))
@@ -476,7 +507,10 @@
     (setf *current-mouse-pos-y* (second pos))
     (setf *current-mouse-modifier* (and mod-keys (car mod-keys)))
     (cond ((eq action :press)
-           (mouse-click (first pos) (second pos) button mod-keys)))))
+           (mouse-click (first pos) (second pos) button mod-keys)))
+    (when (simple-click-left-p button action)
+      (multiple-value-bind (x y) (win-to-screen-xy (first pos) (second pos))
+        (make-pick-request x y (shift-key-p mod-keys))))))
 
 (glfw:def-cursor-pos-callback cursor-position-callback (window x y)
   ;;  (format t "mouse x: ~a, y: ~a~%" x y)
@@ -488,7 +522,8 @@
       (cond ((eq action :press)
              (mouse-dragged x y dx dy))
             (t
-             (mouse-moved x y dx dy))))))
+             (mouse-moved x y dx dy)))))
+  (simple-click-disturbed))
 
 (defun register-choice-menu (menu x y)
   (setf *current-choice-menu-and-pos* (list menu x y)))

src/graphics/opengl/opengl.lisp

@@ -508,4 +508,18 @@
   (gl:enable :blend)
 )
 
+;;; ray  =======================================================================
 
+(defun gl-get-camera-position ()
+  (let* ((inverse-matrix (origin.dmat4:invert (gl:get-double :modelview-matrix)))
+         (position (origin.dmat4:get-translation inverse-matrix)))
+    (p-vec position)))
+
+(defun gl-unproject-to-far-plane (screen-x screen-y)
+  (multiple-value-bind (x y z)
+      (glu:un-project screen-x screen-y 1.d0)
+    (p! x y z)))
+
+(defun gl-get-picking-ray-coords (screen-x screen-y)
+  (values (gl-get-camera-position)
+          (gl-unproject-to-far-plane screen-x screen-y)))

src/kernel/object-picking.lisp

@@ -0,0 +1,74 @@
+(in-package #:kons-9)
+
+(defparameter *picking-enabled* t)
+(defparameter *picking-request* nil)
+(defparameter *picking-selector* nil)
+
+(defmacro make-pick-request (x y multi-select)
+  `(when *picking-enabled*
+     (setf *picking-request* (list ,x ,y ,multi-select))))
+
+(defmacro when-pick-requested ((ray multi-select) &body body)
+  `(when *picking-request*
+     (let ((,multi-select (elt *picking-request* 2))
+           (,ray (make-ray (elt *picking-request* 0)
+                           (elt *picking-request* 1))))
+       (setf *picking-request* nil)
+       ,@body)))
+
+;; Given a scene, the below macro gets the currently selected items. It then
+;; sets the scene selection to those items returned by the body form.
+(defmacro update-scene-selection ((current-selection scene) &body body)
+  (let ((g-scene (gensym))
+        (g-new-selection (gensym)))
+    `(let* ((,g-scene ,scene)
+            (,current-selection (copy-list (selected-shapes ,g-scene))))
+       (let ((,g-new-selection (progn ,@body)))
+         (when (listp ,g-new-selection)
+           (clear-selection ,g-scene)
+           (dolist (item ,g-new-selection)
+             (add-to-selection ,g-scene item)))))))
+
+(defun make-ray (screen-x screen-y)
+  (multiple-value-bind (from to) (gl-get-picking-ray-coords screen-x screen-y)
+    (make-instance 'ray :from from :to to)))
+
+(defun pick (ray multi-select scene)
+  (multiple-value-bind (xs-hit xs-miss) (intersect ray scene)
+    (update-scene-selection (current-selection scene)
+      (funcall (choose-picking-selector multi-select)
+               :xs-hit xs-hit
+               :xs-miss xs-miss
+               :xs-current current-selection
+               ))))
+
+(defun choose-picking-selector (multi-select)
+  (when (functionp *picking-selector*)
+    (return-from choose-picking-selector *picking-selector*))
+
+  (if multi-select
+      #'picking-selector-click-multi
+      #'picking-selector-click-1))
+
+;; picking selector functions ==================================================
+
+(let ((prev-xs-hit nil)
+      (i -1))
+  (defun picking-selector-click-1 (&key xs-hit xs-miss xs-current)
+    (declare (ignore xs-miss xs-current))
+    (flet ((next-i ()
+             (setf i (mod (+ 1 i) (length xs-hit)))
+             i))
+      (when (not (equal prev-xs-hit xs-hit))
+        (setf prev-xs-hit xs-hit)
+        (setf i -1))
+
+      (when (not (null xs-hit))
+        (list (elt xs-hit (next-i)))))))
+
+(defun picking-selector-click-multi (&key xs-hit xs-miss xs-current)
+  (declare (ignore xs-miss))
+  (let ((xs-hit-unselected (list-subtract xs-hit xs-current)))
+    (if (null xs-hit-unselected)
+        xs-current
+        (cons (car xs-hit-unselected) xs-current))))

src/kernel/polyhedron.lisp

@@ -118,6 +118,24 @@
 (defmethod face-points-array ((polyh polyhedron) (face list))
   (coerce (face-points-list polyh face) 'vector))
 
+(defmethod triangles-list ((polyh polyhedron) &key (matrix nil))
+  ;; TODO: this function will only work for convex polyhedrons but it should
+  ;; work for all cases.
+  (let ((triangles '())
+        (tri-polyh (if (is-triangulated-polyhedron? polyh)
+                       polyh
+                       (triangulate-polyhedron polyh))))
+    (flet ((transform-if (xs) (if matrix (transform-points xs matrix) xs)))
+      (do-array (_ face (faces tri-polyh))
+        (push (transform-if (face-points-array tri-polyh face)) triangles)))
+    triangles))
+
+(defmethod triangles-array ((polyh polyhedron) &key (matrix nil))
+  (coerce (triangles-list polyh :matrix matrix) 'vector))
+
+(defmethod triangles-world-array ((polyh polyhedron))
+  (triangles-array polyh :matrix (transform-matrix (transform polyh))))
+
 (defmethod reverse-face-normals ((polyh polyhedron))
   (dotimes (i (length (face-normals polyh)))
     (setf (aref (face-normals polyh) i) (p:negate (aref (face-normals polyh) i))))

src/kernel/ray-triangle-intersect.lisp

@@ -0,0 +1,81 @@
+(in-package #:kons-9)
+
+;;; Common Lisp port of the Moller-Trumbore ray-triangle intersection algorithm.
+;;; The original C code (raytri.c) authored by Tomas Moller was found here:
+;;; https://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/raytri/raytri.c
+
+;;; Function `intersect_triangle` from raytri.c has been ported below as
+;;; `intersect/triangle`.  Other optimized variations of this function still
+;;; remain to be ported to CL. Read more about the optimized variations in
+;;; Moller's insightful article here:
+;;; https://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/raytri/
+
+
+;; Ray-Triangle Intersection Test Routines
+;; Different optimizations of my and Ben Trumbore's
+;; code from journals of graphics tools (JGT)
+;; http://www.acm.org/jgt/
+;; by Tomas Moller, May 2000
+
+;; Copyright 2020 Tomas Akenine-Moller
+
+;; Permission is hereby granted, free of charge, to any person obtaining a copy
+;; of this software and associated documentation files (the "Software"), to deal
+;; in the Software without restriction, including without limitation the rights
+;; to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+;; copies of the Software, and to permit persons to whom the Software is
+;; furnished to do so, subject to the following conditions:
+
+;; The above copyright notice and this permission notice shall be included in
+;; all copies or substantial portions of the Software.
+
+;; THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+;; IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+;; FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+;; AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+;; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+;; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+;; SOFTWARE.
+
+(defun intersect/triangle (tri ray)
+  (let ((epsilon 0.000001))
+    (let ((orig (origin.geometry.ray:origin ray))
+          (dir (origin.geometry.ray:direction ray))
+          (vert0 (origin.geometry.triangle::a tri))
+          (vert1 (origin.geometry.triangle::b tri))
+          (vert2 (origin.geometry.triangle::c tri)))
+      (let* (
+             ;; find vectors for two edges sharing vert0
+             (edge1 (p:- vert1 vert0))
+             (edge2 (p:- vert2 vert0))
+             ;; begin calculating determinant - also used to calculate U
+             (pvec (p:cross dir edge2))
+             ;; if determinant is near zero, ray lies in plane of triangle
+             (det (p:dot edge1 pvec)))
+        (when (and (> det (- epsilon)) (< det epsilon))
+          (return-from intersect/triangle nil))
+        (let* ((inv-det (/ 1.0 det))
+               ;; calculate distance from vert0 to ray origin
+               (tvec (p:- orig vert0))
+               ;; calculate U and test bounds
+               (u (* (p:dot tvec pvec) inv-det)))
+          (when (or (< u 0.0) (> u 1.0))
+            (return-from intersect/triangle nil))
+          (let* (;;prepare to test V
+                 (qvec (p:cross tvec edge1))
+                 ;; calculate V parameter and test bounds
+                 (v (* (p:dot dir qvec) inv-det)))
+            (when (or (< v 0.0) (> (+ u v) 1.0))
+              (return-from intersect/triangle nil))
+            (let (;; calculate t, ray intersects triangle
+                  ;; (using `t_` since `t` clashes in Common Lisp)
+                  (t_ (* (p:dot edge2 qvec) inv-det)))
+              ;; The original C code returns 1 if intersection occurs and 0
+              ;; otherwise, while the actual values computed by the function,
+              ;; namely t, u, v are returned indirectly via pointer arguments
+              ;; whoose contents are populated by the function.
+
+              ;; Instead, we return `(values t u v)` if intersection occurs and
+              ;; nil otherwise.
+              (values t_ u v))))))))
+

src/kernel/ray.lisp

@@ -0,0 +1,59 @@
+(in-package #:kons-9)
+
+(defclass ray ()
+  ((from :initarg :from :reader from)
+   (to :initarg :to :reader to)))
+
+(defmethod print-object ((self ray) stream)
+  (print-unreadable-object (self stream :type t)
+    (format stream "~s - ~s" (from self) (to self))))
+
+(defun intersect-aabb (ray point-min point-max)
+  (origin.geometry:raycast-aabb
+   (origin.geometry.ray:ray :origin (from ray) :direction (to ray))
+   (origin.geometry.aabb:aabb-from-min/max
+    :min point-min :max point-max)))
+
+(defun intersect-triangle (ray p0 p1 p2)
+  (intersect/triangle
+   (origin.geometry.triangle:triangle p0 p1 p2)
+   (origin.geometry.ray:ray :origin (from ray) :direction (to ray))))
+
+(defun intersect-triangles (ray triangles)
+  (let ((min-distance nil))
+    (do-array (_ points triangles)
+      (let ((distance (intersect-triangle ray
+                                          (aref points 0)
+                                          (aref points 1)
+                                          (aref points 2))))
+        (when distance
+          (when (or (null min-distance) (< distance min-distance))
+            (setf min-distance distance)))))
+    min-distance))
+
+(defmethod intersect ((self ray) (shape shape))
+  (error "INTERSECT not implemented"))
+
+(defmethod intersect ((self ray) (polyh polyhedron))
+  (multiple-value-bind (lo hi) (get-bounds-world polyh)
+    (when (and lo hi)
+      (when (intersect-aabb self lo hi)
+        ;; before doing a more expensive operation of intersecting many
+        ;; triangles we first do a quick intersect with the shapes aabb (axis
+        ;; aligned bounding box). If the aabb does not intersect there is no use
+        ;; of intersecting with triangles.
+        (intersect-triangles self (triangles-world-array polyh))))))
+
+(defmethod intersect ((self ray) (scene scene))
+  (let ((xs-hit-distances '())
+        (xs-miss '())
+        (xs-all (find-shapes scene #'identity)))
+    (mapc (lambda (shape)
+            (let ((distance (intersect self shape)))
+              (if distance
+                  (push (cons distance shape) xs-hit-distances)
+                  (push shape xs-miss))))
+          xs-all)
+    (setf xs-hit-distances (stable-sort xs-hit-distances #'< :key #'car))
+    (let ((xs-hit (mapcar #'cdr xs-hit-distances)))
+      (values xs-hit xs-miss))))

src/kernel/shape.lisp

@@ -46,6 +46,12 @@
   (warn "Shape ~a does not have GET-BOUNDS defined. Using default bounds values." self)
   (values (p! -1 -1 -1) (p! 1 1 1)))
 
+(defmethod get-bounds-world ((self shape))
+  (multiple-value-bind (lo hi) (get-bounds self)
+    (let ((m (transform-matrix (transform self))))
+      (values (transform-point lo m)
+              (transform-point hi m)))))
+
 (defmethod center-at-origin ((self shape))
   (multiple-value-bind (bounds-lo bounds-hi)
       (get-bounds self)

src/kernel/utils.lisp

@@ -148,6 +148,11 @@
 (defun vec-last (vec)
   (aref vec (1- (length vec))))
 
+(defun list-subtract (list-1 list-2)
+  (remove-if
+   (lambda (el) (member el list-2))
+   list-1))
+
 ;;;; math ======================================================================
 
 (defconstant 2pi (* 2 pi))