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]
