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]