Add function for projecting to screen

src/Math.cake

@@ -320,6 +320,47 @@
 (defun mat4-data (in (addr mat4) &return (addr float))
   (return (addr (at 0 (at 0 (path in > Elements))))))
 
+;; Returns whether the position is on screen
+;; This could be changed to return floating point if subpixel placement makes sense
+(defun mat4-world-to-screen-position
+    (view-projection-matrix mat4
+     screen-offset-x int
+     screen-offset-y int
+     screen-width int
+     screen-height int
+     world-position vec3
+     screen-x-out (addr int)
+     screen-y-out (addr int)
+     &return bool)
+  (when screen-x-out
+    (set (deref screen-x-out) 0))
+  (when screen-y-out
+    (set (deref screen-y-out) 0))
+  (var projected-vec4 vec4
+    (mat4-multiply-vec4 view-projection-matrix
+                        (vec4-create (vec-xyz world-position) 1.f)))
+  ;; Prevent divide-by-zero, and when W is greater than 0, the position is in front of the
+  ;; camera (negative W = behind camera)
+  (unless (> (field projected-vec4 W) 0.f)
+    (return false))
+
+  ;; Perspective divide
+  (each-in-range 3 axis
+    (set (at axis (field projected-vec4 Elements))
+         (/ (at axis (field projected-vec4 Elements)) (field projected-vec4 W))))
+
+  (var view-half-width int (/ screen-width 2))
+  (var view-half-height int (/ screen-height 2))
+  (when screen-x-out
+    (set (deref screen-x-out)
+         (+ screen-offset-x view-half-width
+            (* view-half-width (field projected-vec4 X)))))
+  (when screen-y-out
+    (set (deref screen-y-out)
+         (+ screen-offset-y view-half-height
+            (* view-half-height (negate (field projected-vec4 Y))))))
+  (return true))
+
 ;;
 ;; Quaternions
 ;;