ODE precision conversion

borodust · Jan 28, 2017 · 99b85be · 99b85be
1 parent 119304f
commit 99b85be
Show file tree

Hide file tree

Showing 9 changed files with 66 additions and 39 deletions.
diff --git a/physics/contacts.lisp b/physics/contacts.lisp
@@ -8,16 +8,16 @@
 
 (defun contact-position (info)
   (c-let ((geom %ode:contact-geom :from (contact-geom info)))
-    (vec3 (geom :pos 0) (geom :pos 1) (geom :pos 2))))
+    (vec3 (f (geom :pos 0)) (f (geom :pos 1)) (f (geom :pos 2)))))
 
 
 (defun contact-normal (info)
   (c-let ((geom %ode:contact-geom :from (contact-geom info)))
-    (vec3 (geom :normal 0) (geom :normal 1) (geom :normal 2))))
+    (vec3 (f (geom :normal 0)) (f (geom :normal 1)) (f (geom :normal 2)))))
 
 
 (defun contact-depth (info)
-  (c-ref (contact-geom info) %ode:contact-geom :depth))
+  (f (c-ref (contact-geom info) %ode:contact-geom :depth)))
 
 
 (defun fill-contact-geom (contact-geom info)
@@ -27,6 +27,6 @@
 (defun fill-contact (contact info)
   (setf (%ode:contact.surface.mode contact) (mask 'contact-flags :approx0 :bounce)
         (%ode:contact.surface.mu contact) +infinity+
-        (%ode:contact.surface.bounce contact) 1.0)
+        (%ode:contact.surface.bounce contact) (ode-real 1.0))
   (fill-contact-geom (%ode:contact.geom contact) info)
   contact)
diff --git a/physics/geometry.lisp b/physics/geometry.lisp
@@ -43,7 +43,7 @@
                                 &key (radius (error ":radius missing")))
   (apply #'call-next-method
          this
-         :handle (make-geom-handle (%ode:create-sphere (space-of (universe)) radius))
+         :handle (make-geom-handle (%ode:create-sphere (space-of (universe)) (ode-real radius)))
          args))
 
 ;;;
@@ -57,9 +57,9 @@
   (apply #'call-next-method
          this
          :handle (make-geom-handle (%ode:create-box (space-of (universe))
-                                                    (x dimensions)
-                                                    (y dimensions)
-                                                    (z dimensions)))
+                                                    (ode-real (x dimensions))
+                                                    (ode-real (y dimensions))
+                                                    (ode-real (z dimensions))))
          args))
 
 
@@ -77,7 +77,10 @@
          this
          :handle (make-geom-handle
                   (%ode:create-plane (space-of (universe))
-                                     (x normal) (y normal) (z normal) offset))
+                                     (ode-real (x normal))
+                                     (ode-real (y normal))
+                                     (ode-real (z normal))
+                                     (ode-real offset)))
          args))
 
 ;;;
@@ -88,17 +91,25 @@
    (direction :reader direction-of)))
 
 
+(definline set-ray (ray position direction)
+  (%ode:geom-ray-set ray
+                     (ode-real (x position))
+                     (ode-real (y position))
+                     (ode-real (z position))
+                     (ode-real (x direction))
+                     (ode-real (y direction))
+                     (ode-real (z direction))))
+
+
 (defmethod initialize-instance ((this ray-geom) &rest args
                                 &key (position (vec3 0.0 0.0 0.0))
                                   (direction (error ":direction missing"))
                                   (length (error ":length missing")))
   (with-slots ((pos position) (dir direction)) this
-    (let ((ode-ray (%ode:create-ray (space-of (universe)) length)))
+    (let ((ode-ray (%ode:create-ray (space-of (universe)) (ode-real length))))
       (setf pos position
             dir direction)
-      (%ode:geom-ray-set ode-ray
-                         (x position) (y position) (z position)
-                         (x direction) (y direction) (z direction))
+      (set-ray ode-ray position direction)
       (apply #'call-next-method
              this
              :handle (make-geom-handle ode-ray)
@@ -108,14 +119,10 @@
 (defmethod (setf position-of) ((position vec3) (this ray-geom))
   (with-slots (direction (pos position)) this
     (setf pos position)
-    (%ode:geom-ray-set (handle-value-of this)
-                       (x position) (y position) (z position)
-                       (x direction) (y direction) (z direction))))
+    (set-ray (handle-value-of this) position direction)))
 
 
 (defmethod (setf direction-of) ((direction vec3) (this ray-geom))
   (with-slots (position (dir direction)) this
     (setf dir direction)
-    (%ode:geom-ray-set (handle-value-of this)
-                       (x position) (y position) (z position)
-                       (x direction) (y direction) (z direction))))
+    (set-ray (handle-value-of this) position direction)))
diff --git a/physics/joints.lisp b/physics/joints.lisp
@@ -23,8 +23,8 @@
        (defclass ,class-name (joint) ())
        (declaim (inline ,class-ctor-name))
        (defun ,class-ctor-name (this-body &optional that-body)
-         (make-joint (lambda (w)
-                       (c-fun ,joint-ctor-name w 0))
+         (make-joint (lambda (world)
+                       (c-fun ,joint-ctor-name world 0))
                      ',class-name (universe) this-body that-body)))))
 
 

diff --git a/physics/mass.lisp b/physics/mass.lisp
@@ -11,11 +11,12 @@
 
 (defun make-box-mass (total x y z)
   (let ((mass (make-instance 'mass)))
-    (%ode:mass-set-box-total (value-of mass) total x y z)
+    (%ode:mass-set-box-total (value-of mass) (ode-real total)
+                             (ode-real x) (ode-real y) (ode-real z))
     mass))
 
 
 (defun make-sphere-mass (total radius)
   (let ((mass (make-instance 'mass)))
-    (%ode:mass-set-sphere-total (value-of mass) total radius)
+    (%ode:mass-set-sphere-total (value-of mass) (ode-real total) (ode-real radius))
     mass))
diff --git a/physics/ode.lisp b/physics/ode.lisp
@@ -1,6 +1,9 @@
 (in-package :cl-bodge.physics)
 
 
+(define-constant +precision+ (if ode:+double-precision-p+ 0d0 0f0)
+  :test #'=)
+
 (defclass ode-object (foreign-object) ())
 
 
@@ -32,3 +35,7 @@
 
 (defmethod collide ((this collidable) (that collidable))
   nil)
+
+
+(definline ode-real (value)
+  (float value +precision+))
diff --git a/physics/rigid-body.lisp b/physics/rigid-body.lisp
@@ -38,13 +38,16 @@
 
 (defmethod (setf position-of) (value (this rigid-body))
   (declare (type vec3 value))
-  (%ode:body-set-position (handle-value-of this) (vref value 0) (vref value 1) (vref value 2)))
+  (%ode:body-set-position (handle-value-of this)
+                          (ode-real (vref value 0))
+                          (ode-real (vref value 1))
+                          (ode-real (vref value 2))))
 
 
 ;; fixme memory sink
 (defmethod position-of ((this rigid-body))
   (let ((ode-pos (%ode:body-get-position (handle-value-of this))))
-    (flet ((el (idx) #f(c-ref ode-pos %ode:real idx)))
+    (flet ((el (idx) (f (c-ref ode-pos %ode:real idx))))
       (vec3 (el 0) (el 1) (el 2)))))
 
 
@@ -59,9 +62,12 @@
     (macrolet ((init ()
                  `(mat3 ,@(loop for i from 0 below 3 append
                                (loop for j from 0 below 3 collect
-                                    `(float (c-ref m3 %ode:real ,(+ (* j 4) i)) 0f0))))))
+                                    `(f (c-ref m3 %ode:real ,(+ (* j 4) i))))))))
       (init))))
 
 
 (defun apply-force (rigid-body vec3)
-  (%ode:body-add-force (handle-value-of rigid-body) (vref vec3 0) (vref vec3 1) (vref vec3 2)))
+  (%ode:body-add-force (handle-value-of rigid-body)
+                       (ode-real (vref vec3 0))
+                       (ode-real (vref vec3 1))
+                       (ode-real (vref vec3 2))))
diff --git a/physics/system.lisp b/physics/system.lisp
@@ -34,4 +34,6 @@
 
 (define-system-function (setf gravity) physics-system (vec)
   (%ode:world-set-gravity (world-of (universe))
-                          (vref vec 0) (vref vec 1) (vref vec 2)))
+                          (ode-real (vref vec 0))
+                          (ode-real (vref vec 1))
+                          (ode-real (vref vec 2))))
diff --git a/physics/universe.lisp b/physics/universe.lisp
@@ -18,8 +18,8 @@
 
 (defmethod initialize-instance :after ((this universe) &key)
   (with-slots (world) this
-    (%ode:world-set-erp world 0.8)
-    (%ode:world-set-cfm world 0.01)))
+    (%ode:world-set-erp world (ode-real 0.8))
+    (%ode:world-set-cfm world (ode-real 0.01))))
 
 
 (defun make-universe ()
@@ -87,4 +87,4 @@
 (defun %observe-universe (universe seconds-since-last-observation)
   (with-slots (world) universe
     (with-contact-joint-group () universe
-      (%ode:world-quick-step world #f seconds-since-last-observation))))
+      (%ode:world-quick-step world (ode-real seconds-since-last-observation)))))
diff --git a/utils/utils.lisp b/utils/utils.lisp
@@ -2,14 +2,24 @@
 
 
 (eval-when (:compile-toplevel :load-toplevel :execute)
+  (defmacro definline (name lambda-list &body body)
+  `(progn
+     (declaim (inline ,name))
+     (defun ,name ,lambda-list ,@body)))
+
+
+  (definline f (value)
+    (float value 0f0))
+
+
   (set-dispatch-macro-character
    #\# #\f
    (lambda (stream key arg)
      (declare (ignore key arg))
      (let ((sexp (read stream t nil t)))
        (if (numberp sexp)
-           (float sexp 0f0)
-           `(float ,sexp 0f0))))))
+           (f sexp)
+           `(ge.util::f ,sexp))))))
 
 
 (defmacro when-debugging (&body body)
@@ -101,12 +111,6 @@
   (/ (get-internal-real-time) internal-time-units-per-second))
 
 
-(defmacro definline (name lambda-list &body body)
-  `(progn
-     (declaim (inline ,name))
-     (defun ,name ,lambda-list ,@body)))
-
-
 (defmacro ensure-not-null (value)
   (once-only ((v value))
     `(if (null ,v)