Expose 3D dot function, add tolerance function

src/Math.cake

@@ -87,6 +87,7 @@
 
 (var-global g-up-axis (const vec3) (array 0.f 1.f 0.f))
 
+(def-c-function-alias vec3-dot HMM_DotVec3) ;; (a vec3 b vec3 &return float)
 (def-c-function-alias vec3-length HMM_LengthVec3) ;; (a vec3 &return float)
 (def-c-function-alias vec3-normalize HMM_NormalizeVec3) ;; (a vec3 &return vec3)
 (def-c-function-alias vec3-scale HMM_MultiplyVec3f) ;; (a vec3 scale float &return vec3)
@@ -183,6 +184,16 @@
     (set angle (negate angle)))
   (return angle))
 
+(defun vec3-equals-tolerance (vec-a vec3 vec-b vec3 tolerance float
+                              &return bool)
+  (var difference vec3 (vec3-subtract vec-a vec-b))
+  ;; TODO This doesn't quite solve the nan angles issue
+  (when (and (< (fabs (field difference X)) tolerance)
+             (< (fabs (field difference Y)) tolerance)
+             (< (fabs (field difference Z)) tolerance))
+    (return true))
+  (return false))
+
 ;; Expand vector to each of its components
 ;; TODO: Prevent multiple eval?
 (defmacro vec-xy (vec any)