[cells-cvs] CVS cells-ode

8 Feb 2008

      Update of /project/cells/cvsroot/cells-ode
In directory clnet:/tmp/cvs-serv32070

Added Files:
	bodies.lisp cells-ode.asd collision.lisp core.lisp geoms.lisp 
	joints.lisp mass.lisp objects.lisp ode-compat.lisp 
	package.lisp primitives.lisp simulate.lisp test-c-ode.lisp 
	types.lisp utility.lisp world.lisp 
Log Message:
initial ci

--- /project/cells/cvsroot/cells-ode/bodies.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/bodies.lisp	2008/02/08 18:09:31	1.1

(in-package :c-ode)

;;;
;;; body
;;;

(def-ode-model body ()
  ((position :type vector)
   (linear-vel :type vector)
   (angular-vel :type vector)
   (quaternion :type quaternion)

   (force :type vector)
   (torque :type vector)

   (mass :type mass :result-arg t)

   (auto-disable-flag :type bool)
   (auto-disable-linear-threshold)
   (auto-disable-angular-threshold)
   (auto-disable-steps :type int)
   (auto-disable-time)

   (finite-rotation-mode :type bool) ; 0 = infinitesimal, 1 = finite
   (finite-rotation-axis :type vector :result-arg t)

   (gravity-mode :type bool :initform (c-in t)))
  (:default-initargs
      :ode-id (call-ode body-create ((*world* object)))))

(defmethod initialize-instance :after ((self body) &rest initargs))

(defmethod ode-destroy ((self body))
  (call-ode body-destroy ((self object)))
  (call-next-method))

(defmethod echo-slots append ((self body))
  '(position linear-vel angular-vel quaternion))

;;;
;;; Forces
;;;

;;; add force or torque

(def-ode-method add-force ((self body) (force vector)))
(def-ode-method add-torque ((self body) (force vector)))
(def-ode-method add-rel-force ((self body) (force vector)))
(def-ode-method add-rel-torque ((self body) (force vector)))

;;; add force at a point

(def-ode-method add-force-at-pos ((self body) (force vector) (pos vector)))
(def-ode-method add-force-at-rel-pos ((self body) (force vector) (pos vector)))
(def-ode-method add-rel-force-at-pos ((self body) (force vector) (pos vector)))
(def-ode-method add-rel-force-at-rel-pos ((self body) (force vector) (pos vector)))

;;;
;;; coordinate transforms
;;; 

;;; get absolute velocity or position for a point

(def-ode-method get-rel-point-pos ((self body) (point vector) (result vector)))
(def-ode-method get-rel-point-vel ((self body) (point vector) (result vector)))
(def-ode-method get-point-vel ((self body) (point vector) (result vector)))

;;; get relative position for a point

(def-ode-method get-pos-rel-point ((self body) (point vector) (result vector)))

;;; rotate a vector to/from relative coordinates

(def-ode-method vector-to-world ((self body) (point vector) (result vector)))
(def-ode-method vector-from-world ((self body) (point vector) (result vector)))

;;;
;;; auto disabling
;;;

(def-ode-method enable ((self body)))
(def-ode-method disable ((self body)))
(def-ode-method is-enabled ((self body)) bool)
(def-ode-method set-auto-disable-defaults ((self body)))

;;;
;;; Joint handling
;;;

(def-ode-method get-num-joints ((self body)) number)
(def-ode-method get-joint ((self body) (index int)) object)

--- /project/cells/cvsroot/cells-ode/cells-ode.asd	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/cells-ode.asd	2008/02/08 18:09:31	1.1

(asdf:defsystem :cells-ode
  :name "cells-ode"
  :depends-on (:cells :cl-ode :utils-kt :cffi)
  :serial t
  :components
  ((:file "package")
   (:file "ode-compat")
   (:file "types" :depends-on ("package"))
   (:file "core" :depends-on ("types" "ode-compat"))
   (:file "objects" :depends-on ("core"))
   (:file "mass" :depends-on ("core"))
   (:file "world" :depends-on ("objects"))
   (:file "bodies" :depends-on ("objects"))
   (:file "geoms" :depends-on ("objects"))
   (:file "joints" :depends-on ("objects"))
   (:file "utility" :depends-on ("objects"))
   (:file "primitives" :depends-on ("geoms" "bodies" "mass"))
   (:file "collision" :depends-on ("objects"))
   (:file "simulate" :depends-on ("collision" "objects" "world"))
   (:file "test-c-ode" :depends-on ("simulate"))
   ))--- /project/cells/cvsroot/cells-ode/collision.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/collision.lisp	2008/02/08 18:09:31	1.1

;;; -----------------------------------------------------------------------------------------------
;;;     collision detection
;;; -----------------------------------------------------------------------------------------------

(in-package :c-ode)

;;;
;;; Spaces
;;; 

(def-ode-model space ()
  ((cleanup :type bool :initform (c-in t)) ; automatic cleanup
   (num-geoms :type int :read-only t))
  )

(defmethod ode-destroy ((self space))
  (call-ode space-destroy ((self object)))
  (call-next-method))

(defmethod echo-slots append ((self space))
  '(num-geoms))

;;; simple space

(def-ode-model simple-space (space)
  ()
  (:default-initargs
      :ode-id (call-ode simple-space-create (((null-pointer))))))

;;; hash space

(def-ode-model hash-space (space)
  ()
  (:default-initargs
      :ode-id (call-ode hash-space-create (((null-pointer))))))

(def-ode-method set-levels ((self hash-space) (minlevel int) (maxlevel int)))

;;; TODO (def-ode-method get-levels) ;; needs multiple return values

;;; quad tree space

(def-ode-model quad-tree-space (space)
  ()
  (:default-initargs
      :ode-id (error "Use mk-quad-tree-space to create a quad-tree-space")))

(defun mk-quad-tree-space (center extents depth)
  (make-instance 'quad-tree-space :ode-id (call-ode quad-tree-space-create (((null-pointer)) (center vector) (extents vector) (depth int)))))

;;;
;;; geom/space bookkeeping
;;;

(def-ode-method (add-geom :ode-name add) ((self space) (geom object)))
(def-ode-method (remove-geom :ode-name remove) ((self space) (geom object)))

(def-ode-method (query-geom :ode-name query) ((self space) (geom object)) bool)

(def-ode-method get-geom ((self space) (num int)) object)

(defmethod geoms ((self space))
  (bwhen (num (num-geoms self))
   (loop for i from 0 below num collecting (get-geom self i))))

;;;
;;; collision detection
;;; 

(defconstant +max-collision-contacts+ 256)
(defvar *collision-joint-group* nil "ODE joint group")

(def-ode-method (space-collide :ode-name collide) ((self space) data near-collision-callback))
(def-ode-fun space-collide2 ((geom-1 object) (geom-2 object) data near-collision-callback))
(def-ode-fun collide ((geom-1 object) (geom-2 object) (max-contacts int) contact (skip int)) int
  (format t "~&in collide~%")
  (let ((res (call-ode-fun)))
    (format t "~&called collide -- result ~a~%" res)
    res))

#+nil (collide (,geom-1
				      ,geom-2
				      ,max-contacts
				      (foreign-slot-value (mem-aref ,contacts 'ode:contact 0) 'ode:contact 'ode:geom)
				      (foreign-type-size 'ode:contact)))

(defmacro do-contacts ((contact geom-1 geom-2 &key (max-contacts +max-collision-contacts+)) &body body)
  (with-uniqs (contacts num-contacts)
    `(with-foreign-object (,contacts 'ode:contact ,max-contacts)
       (let ((,num-contacts (call-ode collide ((,geom-1 object) 
					       (,geom-2 object)
					       (,max-contacts int)
					       ((foreign-slot-value (mem-aref ,contacts 'ode:contact 0) 'ode:contact 'ode:geom))
					       ((foreign-type-size 'ode:contact))) int)

	       ))
	 (dotimes (i ,num-contacts)
	   (let ((,contact (mem-aref ,contacts 'ode:contact i)))
	     (flet ((mk-collision () (attach (mk-contact-joint *collision-joint-group* ,contact) (body ,geom-1) (body ,geom-2))))
	      ,@body)))))))

(eval-now!
 (defun ode-sym (sym)
   (intern (string sym) :ode))

 (defun make-with (type slots-and-types)
   (multiple-value-bind (slots types) (parse-typed-args slots-and-types)
     `(defmacro ,(intern-string 'with type) (,type (&optional ,@(mapcar #'(lambda (slot) `(,slot ',(gensym (string slot)))) slots)) &body body)
	(list 'with-foreign-slots (list ',(mapcar #'ode-sym slots) ,type ',(ode-sym type))
	      (append (list 'let (list ,@(mapcar #'(lambda (slot type) `(list ,slot ',(make-from-ode type nil (list (ode-sym slot))))) slots types)))
		      body))))))

(defmacro def-with-ode (type (&rest slots-and-types))
  (make-with type slots-and-types))

(def-with-ode contact (surface geom (f-dir-1 vector)))

(def-with-ode contact-geom ((pos vector) (normal vector) (g-1 object) (g-2 object) (depth number) (side-1 int) (side-2 int)))

(defmacro with-surface-parameters ((ode-surface geom-1 geom-2) select &body body)
  (let ((params '(mu slip-1 slip-2 soft-erp bounce bounce-vel soft-cfm)))
    (let ((ode-params (mapcar #'(lambda (sym) (intern (string sym) :ode)) params)))
      (with-uniqs mode
	`(with-foreign-slots (,(append ode-params '(ode:mode)) ,ode-surface ode:surface-parameters)
	   (let ,(append (list (list mode 0)) params)
	     (macrolet ((select-max (&rest params) `(progn ,@(mapcar #'(lambda (param) `(setf ,param (max (,param ,',geom-1) (,param ,',geom-2)))) ',params))))
	       ,select)
	     ,@(loop for sym in params for ode-sym in ode-params
		  collecting `(when ,sym
				(setf ,ode-sym ,@(make-convert sym 'number))
				(setf ,mode (logior ,mode
						    ,(intern (format nil "+CONTACT-~a+" (case sym
											  (bounce-vel 'bounce)
											  (mu 'approx-1)
											  (t sym)))
							     :ode)))))
	     (setf ,(intern "MODE" :ode) ,mode)
	     ,@body))))))

;;;
;;; collision detection callback
;;; 

(defcallback near-collision-callback :void ((data :pointer) (geom-id-1 ode:geom-id) (geom-id-2 ode:geom-id))
  (let ((geom-1 (lookup geom-id-1))
	(geom-2 (lookup geom-id-2)))
    (if (or (is-space geom-1) (is-space geom-2))
	(space-collide geom-1 geom-2 data (callback near-collision-callback))
	(progn
	  (format t "~&Colliding geoms ~a <--> ~a~%" (md-name geom-1) (md-name geom-2))
	  (do-contacts (contact geom-1 geom-2)
	    (with-contact contact (surface contact-geom friction-dir-1)
	      (with-contact-geom contact-geom (pos normal)
		(with-surface-parameters (surface geom-1 geom-2)
		    (progn (select-max mu bounce bounce-vel))
		  (mk-collision)))))))))

;;;
;;; high level collision detection routine

(defmacro with-collision ((space) &body body)
  `(let ((*collision-joint-group* (mk-joint-group (* +max-collision-contacts+ 1000))))
     (space-collide ,space (null-pointer) (callback near-collision-callback))
     ,@body
     (ode-destroy *collision-joint-group*)))
--- /project/cells/cvsroot/cells-ode/core.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/core.lisp	2008/02/08 18:09:31	1.1

(in-package :cells-ode)

;;;
;;; General purpose utilities
;;;

(eval-when (:compile-toplevel :load-toplevel :execute)
  (defmacro eval-now! (&body body)
    `(eval-when (:compile-toplevel :load-toplevel :execute)
       ,@body)))

(eval-now!
  (defun mk-list (var)
    (if (listp var) var (list var)))

  (defmacro nconcf (place add)
    `(setf ,place (nconc (mk-list ,place) (mk-list ,add))))

  (defmacro with-uniqs (syms &body body)
    `(let ,(mapcar #'(lambda (sym) `(,sym (gensym ,(concatenate 'string (string sym) "-")))) (mk-list syms))
       ,@body))

  (defmacro csetf (place value)
    (with-uniqs newval
      `(let ((,newval ,value))
	 (unless (eql ,newval ,place)
	   (setf ,place ,newval)))))

  (defmacro dohash ((obj hash-table) &body body)
    `(loop for ,obj being the hash-values of ,hash-table do ,@body))

  (defun denil (lst)
    (loop for x in lst if x collect x))
  (defun concat (&rest parts)
    (format nil "~:@(~{~@[~a~#[~:;-~]~]~}~)" (denil parts)))

  (defun intern-string (&rest strings)
    (intern (apply #'concat strings))))

;;; ODE function names

(eval-now!
 (defun setter (name slot)
   (intern-string name 'set slot))
 (defun getter (name slot)
   (intern-string name 'get slot)))

;;; deactivating an observer

;; later

;;;
;;; ODE model, method, function, call
;;; 

(defvar *dbg* nil)

(defmacro with-dbg (&body body)
  `(let ((*dbg* t))
     ,@body))

(eval-now!
  (defun make-call (fn ret-type args-and-types &optional (self 'self))
    (multiple-value-bind (args types) (parse-typed-args args-and-types)
      (let (par-list result-arg-type)
	(labels ((call-with (args types)
		   (let ((arg (car args))
			 (type (car types)))
		     (cond
		       ((not args)
			(let ((fn-call `(,(intern (string fn) :ode) ,@par-list)))
			  (if result-arg-type
			      `(progn ,fn-call result)
			      fn-call)))
		       ((eq arg 'result)
			(setf result-arg-type type)
			(nconcf par-list arg)
			(call-with (rest args) (rest types)))
		       (t
			(nconcf par-list (make-convert arg type))
			(make-with-ode arg type (list (call-with (rest args) (rest types))) self))))))	  
	  (let ((fn-call (call-with args types)))	    
	    (let ((fn-call-ret (bif (return-type (or ret-type result-arg-type))
				    (make-from-ode return-type (when result-arg-type 'result) (list fn-call))
				    fn-call)))
	      (with-uniqs result
		`(if *dbg*
		     (progn
		       (format t ,(format nil "~&~%Calling ~a (~~@{~~a~~#[~~:; ~~]~~}) ... " fn) ,@(remove 'result args))
		       (let ((,result ,fn-call-ret))
			 (format t "==> ~a~%" ,result)
			 ,result))
		     ,fn-call-ret))))))))

  (defun canonic-args-list (args-and-types)
    (mapcar #'mk-list args-and-types))

  (defun parse-typed-args (args-and-types)
    (loop for (arg type) in (canonic-args-list args-and-types)
       collect arg into args
       collect type into types
       finally (return (values args types))))

[110 lines skipped]
--- /project/cells/cvsroot/cells-ode/geoms.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/geoms.lisp	2008/02/08 18:09:31	1.1

[275 lines skipped]
--- /project/cells/cvsroot/cells-ode/joints.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/joints.lisp	2008/02/08 18:09:31	1.1

[472 lines skipped]
--- /project/cells/cvsroot/cells-ode/mass.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/mass.lisp	2008/02/08 18:09:31	1.1

[612 lines skipped]
--- /project/cells/cvsroot/cells-ode/objects.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/objects.lisp	2008/02/08 18:09:31	1.1

[732 lines skipped]
--- /project/cells/cvsroot/cells-ode/ode-compat.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/ode-compat.lisp	2008/02/08 18:09:31	1.1

[777 lines skipped]
--- /project/cells/cvsroot/cells-ode/package.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/package.lisp	2008/02/08 18:09:31	1.1

[787 lines skipped]
--- /project/cells/cvsroot/cells-ode/primitives.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/primitives.lisp	2008/02/08 18:09:31	1.1

[810 lines skipped]
--- /project/cells/cvsroot/cells-ode/simulate.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/simulate.lisp	2008/02/08 18:09:31	1.1

[862 lines skipped]
--- /project/cells/cvsroot/cells-ode/test-c-ode.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/test-c-ode.lisp	2008/02/08 18:09:31	1.1

[928 lines skipped]
--- /project/cells/cvsroot/cells-ode/types.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/types.lisp	2008/02/08 18:09:31	1.1

[1092 lines skipped]
--- /project/cells/cvsroot/cells-ode/utility.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/utility.lisp	2008/02/08 18:09:31	1.1

[1109 lines skipped]
--- /project/cells/cvsroot/cells-ode/world.lisp	2008/02/08 18:09:31	NONE
+++ /project/cells/cvsroot/cells-ode/world.lisp	2008/02/08 18:09:31	1.1

[1169 lines skipped]

[cells-cvs] CVS cells-ode

phildebrandt