diff options
| author | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
|---|---|---|
| committer | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
| commit | c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 (patch) | |
| tree | 277dd280daf10bf77013236b8edfa5f88708c7e0 /libs/ode-0.16.1/ode | |
| parent | 1cf9cc3408af7008451f9133fb95af66a9697d15 (diff) | |
add ode
Diffstat (limited to 'libs/ode-0.16.1/ode')
229 files changed, 96423 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/Makefile.am b/libs/ode-0.16.1/ode/Makefile.am new file mode 100644 index 0000000..656e7a4 --- /dev/null +++ b/libs/ode-0.16.1/ode/Makefile.am @@ -0,0 +1,6 @@ +SUBDIRS = src doc +if ENABLE_DEMOS + SUBDIRS += demo +endif + +#EXTRA_DIST = doc diff --git a/libs/ode-0.16.1/ode/Makefile.in b/libs/ode-0.16.1/ode/Makefile.in new file mode 100644 index 0000000..eae84a5 --- /dev/null +++ b/libs/ode-0.16.1/ode/Makefile.in @@ -0,0 +1,643 @@ +# Makefile.in generated by automake 1.15 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994-2014 Free Software Foundation, Inc. + +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ +VPATH = @srcdir@ +am__is_gnu_make = { \ + if test -z '$(MAKELEVEL)'; then \ + false; \ + elif test -n '$(MAKE_HOST)'; then \ + true; \ + elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \ + true; \ + else \ + false; \ + fi; \ +} +am__make_running_with_option = \ + case $${target_option-} in \ + ?) ;; \ + *) echo "am__make_running_with_option: internal error: invalid" \ + "target option '$${target_option-}' specified" >&2; \ + exit 1;; \ + esac; \ + has_opt=no; \ + sane_makeflags=$$MAKEFLAGS; \ + if $(am__is_gnu_make); then \ + sane_makeflags=$$MFLAGS; \ + else \ + case $$MAKEFLAGS in \ + *\\[\ \ ]*) \ + bs=\\; \ + sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \ + | sed "s/$$bs$$bs[$$bs $$bs ]*//g"`;; \ + esac; \ + fi; \ + skip_next=no; \ + strip_trailopt () \ + { \ + flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \ + }; \ + for flg in $$sane_makeflags; do \ + test $$skip_next = yes && { skip_next=no; continue; }; \ + case $$flg in \ + *=*|--*) continue;; \ + -*I) strip_trailopt 'I'; skip_next=yes;; \ + -*I?*) strip_trailopt 'I';; \ + -*O) strip_trailopt 'O'; skip_next=yes;; \ + -*O?*) strip_trailopt 'O';; \ + -*l) strip_trailopt 'l'; skip_next=yes;; \ + -*l?*) strip_trailopt 'l';; \ + -[dEDm]) skip_next=yes;; \ + -[JT]) skip_next=yes;; \ + esac; \ + case $$flg in \ + *$$target_option*) has_opt=yes; break;; \ + esac; \ + done; \ + test $$has_opt = yes +am__make_dryrun = (target_option=n; $(am__make_running_with_option)) +am__make_keepgoing = (target_option=k; $(am__make_running_with_option)) +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ +@ENABLE_DEMOS_TRUE@am__append_1 = demo +subdir = ode +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \ + $(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \ + $(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \ + $(top_srcdir)/m4/pkg.m4 $(top_srcdir)/configure.ac +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +DIST_COMMON = $(srcdir)/Makefile.am $(am__DIST_COMMON) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/ode/src/config.h +CONFIG_CLEAN_FILES = +CONFIG_CLEAN_VPATH_FILES = +AM_V_P = $(am__v_P_@AM_V@) +am__v_P_ = $(am__v_P_@AM_DEFAULT_V@) +am__v_P_0 = false +am__v_P_1 = : +AM_V_GEN = $(am__v_GEN_@AM_V@) +am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@) +am__v_GEN_0 = @echo " GEN " $@; +am__v_GEN_1 = +AM_V_at = $(am__v_at_@AM_V@) +am__v_at_ = $(am__v_at_@AM_DEFAULT_V@) +am__v_at_0 = @ +am__v_at_1 = +SOURCES = +DIST_SOURCES = +RECURSIVE_TARGETS = all-recursive check-recursive cscopelist-recursive \ + ctags-recursive dvi-recursive html-recursive info-recursive \ + install-data-recursive install-dvi-recursive \ + install-exec-recursive install-html-recursive \ + install-info-recursive install-pdf-recursive \ + install-ps-recursive install-recursive installcheck-recursive \ + installdirs-recursive pdf-recursive ps-recursive \ + tags-recursive uninstall-recursive +am__can_run_installinfo = \ + case $$AM_UPDATE_INFO_DIR in \ + n|no|NO) false;; \ + *) (install-info --version) >/dev/null 2>&1;; \ + esac +RECURSIVE_CLEAN_TARGETS = mostlyclean-recursive clean-recursive \ + distclean-recursive maintainer-clean-recursive +am__recursive_targets = \ + $(RECURSIVE_TARGETS) \ + $(RECURSIVE_CLEAN_TARGETS) \ + $(am__extra_recursive_targets) +AM_RECURSIVE_TARGETS = $(am__recursive_targets:-recursive=) TAGS CTAGS \ + distdir +am__tagged_files = $(HEADERS) $(SOURCES) $(TAGS_FILES) $(LISP) +# Read a list of newline-separated strings from the standard input, +# and print each of them once, without duplicates. Input order is +# *not* preserved. +am__uniquify_input = $(AWK) '\ + BEGIN { nonempty = 0; } \ + { items[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in items) print i; }; } \ +' +# Make sure the list of sources is unique. This is necessary because, +# e.g., the same source file might be shared among _SOURCES variables +# for different programs/libraries. +am__define_uniq_tagged_files = \ + list='$(am__tagged_files)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | $(am__uniquify_input)` +ETAGS = etags +CTAGS = ctags +DIST_SUBDIRS = src doc demo +am__DIST_COMMON = $(srcdir)/Makefile.in README TODO +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +am__relativize = \ + dir0=`pwd`; \ + sed_first='s,^\([^/]*\)/.*$$,\1,'; \ + sed_rest='s,^[^/]*/*,,'; \ + sed_last='s,^.*/\([^/]*\)$$,\1,'; \ + sed_butlast='s,/*[^/]*$$,,'; \ + while test -n "$$dir1"; do \ + first=`echo "$$dir1" | sed -e "$$sed_first"`; \ + if test "$$first" != "."; then \ + if test "$$first" = ".."; then \ + dir2=`echo "$$dir0" | sed -e "$$sed_last"`/"$$dir2"; \ + dir0=`echo "$$dir0" | sed -e "$$sed_butlast"`; \ + else \ + first2=`echo "$$dir2" | sed -e "$$sed_first"`; \ + if test "$$first2" = "$$first"; then \ + dir2=`echo "$$dir2" | sed -e "$$sed_rest"`; \ + else \ + dir2="../$$dir2"; \ + fi; \ + dir0="$$dir0"/"$$first"; \ + fi; \ + fi; \ + dir1=`echo "$$dir1" | sed -e "$$sed_rest"`; \ + done; \ + reldir="$$dir2" +ACLOCAL = @ACLOCAL@ +ALLOCA = @ALLOCA@ +AMTAR = @AMTAR@ +AM_DEFAULT_VERBOSITY = @AM_DEFAULT_VERBOSITY@ +AR = @AR@ +AS = @AS@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CCD_CFLAGS = @CCD_CFLAGS@ +CCD_LIBS = @CCD_LIBS@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXCPP = @CXXCPP@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DLLTOOL = @DLLTOOL@ +DOXYGEN = @DOXYGEN@ +DSYMUTIL = @DSYMUTIL@ +DUMPBIN = @DUMPBIN@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +EXTRA_LIBTOOL_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ +FGREP = @FGREP@ +GL_LIBS = @GL_LIBS@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LD = @LD@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LIBSTDCXX = @LIBSTDCXX@ +LIBTOOL = @LIBTOOL@ +LIPO = @LIPO@ +LN_S = @LN_S@ +LTLIBOBJS = @LTLIBOBJS@ +LT_SYS_LIBRARY_PATH = @LT_SYS_LIBRARY_PATH@ +MAKEINFO = @MAKEINFO@ +MANIFEST_TOOL = @MANIFEST_TOOL@ +MKDIR_P = @MKDIR_P@ +NM = @NM@ +NMEDIT = @NMEDIT@ +OBJDUMP = @OBJDUMP@ +OBJEXT = @OBJEXT@ +ODE_PRECISION = @ODE_PRECISION@ +ODE_VERSION = @ODE_VERSION@ +ODE_VERSION_INFO = @ODE_VERSION_INFO@ +OTOOL = @OTOOL@ +OTOOL64 = @OTOOL64@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_URL = @PACKAGE_URL@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +PKG_CONFIG = @PKG_CONFIG@ +PKG_CONFIG_LIBDIR = @PKG_CONFIG_LIBDIR@ +PKG_CONFIG_PATH = @PKG_CONFIG_PATH@ +RANLIB = @RANLIB@ +SED = @SED@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +VERSION = @VERSION@ +WINDRES = @WINDRES@ +X11_CFLAGS = @X11_CFLAGS@ +X11_LIBS = @X11_LIBS@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_AR = @ac_ct_AR@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +ac_ct_DUMPBIN = @ac_ct_DUMPBIN@ +ac_ct_WINDRES = @ac_ct_WINDRES@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +build = @build@ +build_alias = @build_alias@ +build_cpu = @build_cpu@ +build_os = @build_os@ +build_vendor = @build_vendor@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host = @host@ +host_alias = @host_alias@ +host_cpu = @host_cpu@ +host_os = @host_os@ +host_vendor = @host_vendor@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +runstatedir = @runstatedir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +subdirs = @subdirs@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +top_build_prefix = @top_build_prefix@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +SUBDIRS = src doc $(am__append_1) +all: all-recursive + +.SUFFIXES: +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \ + && { if test -f $@; then exit 0; else break; fi; }; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(am__aclocal_m4_deps): + +mostlyclean-libtool: + -rm -f *.lo + +clean-libtool: + -rm -rf .libs _libs + +# This directory's subdirectories are mostly independent; you can cd +# into them and run 'make' without going through this Makefile. +# To change the values of 'make' variables: instead of editing Makefiles, +# (1) if the variable is set in 'config.status', edit 'config.status' +# (which will cause the Makefiles to be regenerated when you run 'make'); +# (2) otherwise, pass the desired values on the 'make' command line. +$(am__recursive_targets): + @fail=; \ + if $(am__make_keepgoing); then \ + failcom='fail=yes'; \ + else \ + failcom='exit 1'; \ + fi; \ + dot_seen=no; \ + target=`echo $@ | sed s/-recursive//`; \ + case "$@" in \ + distclean-* | maintainer-clean-*) list='$(DIST_SUBDIRS)' ;; \ + *) list='$(SUBDIRS)' ;; \ + esac; \ + for subdir in $$list; do \ + echo "Making $$target in $$subdir"; \ + if test "$$subdir" = "."; then \ + dot_seen=yes; \ + local_target="$$target-am"; \ + else \ + local_target="$$target"; \ + fi; \ + ($(am__cd) $$subdir && $(MAKE) $(AM_MAKEFLAGS) $$local_target) \ + || eval $$failcom; \ + done; \ + if test "$$dot_seen" = "no"; then \ + $(MAKE) $(AM_MAKEFLAGS) "$$target-am" || exit 1; \ + fi; test -z "$$fail" + +ID: $(am__tagged_files) + $(am__define_uniq_tagged_files); mkid -fID $$unique +tags: tags-recursive +TAGS: tags + +tags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + set x; \ + here=`pwd`; \ + if ($(ETAGS) --etags-include --version) >/dev/null 2>&1; then \ + include_option=--etags-include; \ + empty_fix=.; \ + else \ + include_option=--include; \ + empty_fix=; \ + fi; \ + list='$(SUBDIRS)'; for subdir in $$list; do \ + if test "$$subdir" = .; then :; else \ + test ! -f $$subdir/TAGS || \ + set "$$@" "$$include_option=$$here/$$subdir/TAGS"; \ + fi; \ + done; \ + $(am__define_uniq_tagged_files); \ + shift; \ + if test -z "$(ETAGS_ARGS)$$*$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + if test $$# -gt 0; then \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + "$$@" $$unique; \ + else \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$unique; \ + fi; \ + fi +ctags: ctags-recursive + +CTAGS: ctags +ctags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + $(am__define_uniq_tagged_files); \ + test -z "$(CTAGS_ARGS)$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && $(am__cd) $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) "$$here" +cscopelist: cscopelist-recursive + +cscopelist-am: $(am__tagged_files) + list='$(am__tagged_files)'; \ + case "$(srcdir)" in \ + [\\/]* | ?:[\\/]*) sdir="$(srcdir)" ;; \ + *) sdir=$(subdir)/$(srcdir) ;; \ + esac; \ + for i in $$list; do \ + if test -f "$$i"; then \ + echo "$(subdir)/$$i"; \ + else \ + echo "$$sdir/$$i"; \ + fi; \ + done >> $(top_builddir)/cscope.files + +distclean-tags: + -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d "$(distdir)/$$file"; then \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \ + else \ + test -f "$(distdir)/$$file" \ + || cp -p $$d/$$file "$(distdir)/$$file" \ + || exit 1; \ + fi; \ + done + @list='$(DIST_SUBDIRS)'; for subdir in $$list; do \ + if test "$$subdir" = .; then :; else \ + $(am__make_dryrun) \ + || test -d "$(distdir)/$$subdir" \ + || $(MKDIR_P) "$(distdir)/$$subdir" \ + || exit 1; \ + dir1=$$subdir; dir2="$(distdir)/$$subdir"; \ + $(am__relativize); \ + new_distdir=$$reldir; \ + dir1=$$subdir; dir2="$(top_distdir)"; \ + $(am__relativize); \ + new_top_distdir=$$reldir; \ + echo " (cd $$subdir && $(MAKE) $(AM_MAKEFLAGS) top_distdir="$$new_top_distdir" distdir="$$new_distdir" \\"; \ + echo " am__remove_distdir=: am__skip_length_check=: am__skip_mode_fix=: distdir)"; \ + ($(am__cd) $$subdir && \ + $(MAKE) $(AM_MAKEFLAGS) \ + top_distdir="$$new_top_distdir" \ + distdir="$$new_distdir" \ + am__remove_distdir=: \ + am__skip_length_check=: \ + am__skip_mode_fix=: \ + distdir) \ + || exit 1; \ + fi; \ + done +check-am: all-am +check: check-recursive +all-am: Makefile +installdirs: installdirs-recursive +installdirs-am: +install: install-recursive +install-exec: install-exec-recursive +install-data: install-data-recursive +uninstall: uninstall-recursive + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-recursive +install-strip: + if test -z '$(STRIP)'; then \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + install; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \ + fi +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-recursive + +clean-am: clean-generic clean-libtool mostlyclean-am + +distclean: distclean-recursive + -rm -f Makefile +distclean-am: clean-am distclean-generic distclean-tags + +dvi: dvi-recursive + +dvi-am: + +html: html-recursive + +html-am: + +info: info-recursive + +info-am: + +install-data-am: + +install-dvi: install-dvi-recursive + +install-dvi-am: + +install-exec-am: + +install-html: install-html-recursive + +install-html-am: + +install-info: install-info-recursive + +install-info-am: + +install-man: + +install-pdf: install-pdf-recursive + +install-pdf-am: + +install-ps: install-ps-recursive + +install-ps-am: + +installcheck-am: + +maintainer-clean: maintainer-clean-recursive + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-recursive + +mostlyclean-am: mostlyclean-generic mostlyclean-libtool + +pdf: pdf-recursive + +pdf-am: + +ps: ps-recursive + +ps-am: + +uninstall-am: + +.MAKE: $(am__recursive_targets) install-am install-strip + +.PHONY: $(am__recursive_targets) CTAGS GTAGS TAGS all all-am check \ + check-am clean clean-generic clean-libtool cscopelist-am ctags \ + ctags-am distclean distclean-generic distclean-libtool \ + distclean-tags distdir dvi dvi-am html html-am info info-am \ + install install-am install-data install-data-am install-dvi \ + install-dvi-am install-exec install-exec-am install-html \ + install-html-am install-info install-info-am install-man \ + install-pdf install-pdf-am install-ps install-ps-am \ + install-strip installcheck installcheck-am installdirs \ + installdirs-am maintainer-clean maintainer-clean-generic \ + mostlyclean mostlyclean-generic mostlyclean-libtool pdf pdf-am \ + ps ps-am tags tags-am uninstall uninstall-am + +.PRECIOUS: Makefile + + +#EXTRA_DIST = doc + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/libs/ode-0.16.1/ode/README b/libs/ode-0.16.1/ode/README new file mode 100644 index 0000000..dd4596f --- /dev/null +++ b/libs/ode-0.16.1/ode/README @@ -0,0 +1,158 @@ +Dynamics Library. +================= + +CONVENTIONS +----------- + +matrix storage +-------------- + +matrix operations like factorization are expensive, so we must store the data +in a way that is most useful to the matrix code. we want the ability to update +the dynamics library without recompiling applications, e.g. so users can take +advantage of new floating point hardware. so we must settle on a single +format. because of the prevalence of 4-way SIMD, the format is this: store +the matrix by rows or columns, and each column is rounded up to a multiple of +4 elements. the extra "padding" elements at the end of each row/column are set +to 0. this is called the "standard format". to indicate if the data is stored +by rows or columns, we will say "standard row format" or "standard column +format". hopefully this decision will remain good in the future, as more and +more processors have 4-way SIMD, and 3D graphics always needs fast 4x4 +matrices. + +exception: matrices that have only one column or row (vectors), are always +stored as consecutive elements in standard row format, i.e. there is no +interior padding, only padding at the end. + +thus: all 3x1 floating point vectors are stored as 4x1 vectors: (x,x,x,0). +also: all 6x1 spatial velocities and accelerations are split into 3x1 position + and angular components, which are stored as contiguous 4x1 vectors. + +ALL matrices are stored by in standard row format. + + +arguments +--------- + +3x1 vector arguments to set() functions are supplied as x,y,z. +3x1 vector result arguments to get() function are pointers to arrays. +larger vectors are always supplied and returned as pointers. +all coordinates are in the global frame except where otherwise specified. +output-only arguments are usually supplied at the end. + + +memory allocation +----------------- + +with many C/C++ libraries memory allocation is a difficult problem to solve. +who allocates the memory? who frees it? must objects go on the heap or can +they go on the stack or in static storage? to provide the maximum flexibility, +the dynamics and collision libraries do not do their own memory allocation. +you must pass in pointers to externally allocated chunks of the right sizes. +the body, joint and colllision object structures are all exported, so you +can make instances of those structure and pass pointers to them. + +there are helper functions which allocate objects out of areans, in case you +need loots of dynamic creation and deletion. + +BUT!!! this ties us down to the body/joint/collision representation. + +a better approach is to supply custom memory allocation functions +(e.g. dlAlloc() etc). + + +C versus C++ ... ? +------------------ + +everything should be C linkable, and there should be C header files for +everything. but we want to develop in C++. so do this: + * all comments are "//". automatically convert to /**/ for distribution. + * structures derived from other structures --> automatically convert? + + +WORLDS +------ + +might want better terminology here. + +the dynamics world (DWorld) is a list of systems. each system corresponds to +one or more bodies, or perhaps some other kinds of physical object. +each system corresponds to one or more objects in the collision world +(there does not have to be a one-to-one correspondence between bodies and +collision objects). + +systems are simulated separately, perhaps using completely different +techniques. we must do something special when systems collide. +systems collide when collision objects belonging to system A touch +collision objects belonging to system B. + +for each collision point, the system must provide matrix equation data +that is used to compute collision forces. once those forces are computed, +the system must incorporate the forces into its timestep. +PROBLEM: what if we intertwine the LCP problems of the two systems - then +this simple approach wont work. + +the dynamics world contains two kinds of objects: bodies and joints. +joints connect two bodies together. + +the world contains one of more partitions. each partition is a collection of +bodies and joints such that each body is attached (through one or more joints) +to every other body. + +Joints +------ + +a joint can be connected to one or two bodies. +if the joint is only connected to one body, joint.node[1].body == 0. +joint.node[0].body is always valid. + + +Linkage +------- + +this library will always be statically linked with the app, for these reasons: + * collision space is selected at compile time, it adds data to the geom + objects. + + +Optimization +------------ + +doubles must be aligned on 8 byte boundaries! + + +MinGW on Windows issues +----------------------- + +* the .rc file for drawstuff needs a different include, try winresrc.h. + +* it seems we can't have both main() and WinMain() without the entry point + defaulting to main() and having resource loading problems. this screws up + what i was trying to do in the drawstuff library. perhaps main2() ? + +* remember to compile resources to COFF format RES files. + + + +Collision +--------- + +to plug in your own collision handling, replace (some of?) these functions +with your own. collision should be a separate library that you can link in +or not. your own library can call components in this collision library, e.g. +if you want polymorphic spaces instead of a single statically called space. + +creating an object will automatically register the appropriate +class (if necessary). how can we ensure that the minimum amount of code is +linked in? e.g. only one space handler, and sphere-sphere and sphere-box and +box-box collision code (if spheres and boxes instanced). + +the user creates a collision space, and for each dynamics object that is +created a collision object is inserted into the space. the collision +object's pos and R pointers are set to the corresponding dynamics +variables. + +there should be utility functions which create the dynamics and collision +objects at the same time, e.g. dMakeSphere(). + +collision objects and dynamics objects keep pointers to each other. diff --git a/libs/ode-0.16.1/ode/TODO b/libs/ode-0.16.1/ode/TODO new file mode 100644 index 0000000..cf6cdaa --- /dev/null +++ b/libs/ode-0.16.1/ode/TODO @@ -0,0 +1,698 @@ + +@@@'s + + +TODO for COLLISION +------------------ + +box-box collision: adjust generated face-face contact points by depth/2 to +be more fair. + +what happens when a GeomTransform's encapsulated object is manipulated, +e.g. position changed. should this be disallowed? should a GeomTransform +behave like a space and propagate dirtyness upwards? + +make sure that when we are using a large space for static environmental geoms, +that there is not excessive AABB computation when geoms are added/removed from +the space. the space AABB is pretty much guaranteed to cover everything, so +there's no need to compute/test the AABB in this case. + +hash space: implement collide2() efficiently instead of the current +simple-space-like brute-force approach. + +hash space: incremental scheme, so we dont have to rebuild the data structures +for geoms that don't move. + +disabled geoms (remove from all collision considerations) ... isn't this the +same as just taking it out of its enclosing group/space? + +integrate: + dRay + triangle collider - get latest tri collider code from erwin + erwin's quadtree space + +tests: + all aspects of collision API + + dGeomSetBody(0) maintains body-geom linked list properly. + + simple space: instantiate lots of non-moving geoms (i.e. environmental + geoms and make sure that we're still able to collide efficiently. + make sure AABB computation is efficient, or can be made efficient + through proper use of the API. + + test C interface support for making new classes. + make sure the dxGeom::aabbTest() function behaves as advertised. + + testing for contact point consistency: test for things that + would cause the dynamics to fail or become unstable + + test for: small adjustment in geom position causes a big jump in the + contact point set (bad for dynamics). + + test for: if contact constraints observed then it's impossible + (or hard) to move the objects so that the penetration is + increased. relax this when only a subset of the contact points are + returned. + + test for consistency, e.g. the boundary of geoms X and Y can + be defined by intersecting with a point, so test the intersection of X + and Y by comparing with the point tests. + + check that contact points are in the collision volume + + all existing space tests, and more. + +demos: + test_buggy: make a terrain out of non-moving geoms. use heirarchical + groups to get efficient collision, even with the simple space. + +go though the new collision docs and make sure the behavior that is described +there is actually implemented. + +multi-resolution hash table: + the current implementation rebuilds a new hash table each time + collide() is called. we don't keep any state between calls. this is + wasteful if there are unmoving objects in the space. + + make sure we prevent multiple collision callbacks for the same pair + + better virtual address function. + + the collision search can perhaps be optimized - as we search + chains we can come across other candidate intersections at + other levels, perhaps we should do the intersection check + straight away? --> save on list searching time only, which is + not too significant. + +collision docs: + optimization guide: whenever a single geom changes in a simple space, + the space AABB has to be recomputed by examining EVERY geom. + document this, or find a better behavior. + + + +TODO BEFORE NEXT RELEASE +------------------------ + +g++ needed for compiling tests using gcc 3.2 ? what is the problem? + +add joint feedback info from lambda, so that we can get motor forces etc. +need a way to map constraint indexes to what they mean. + +track down and fix the occasional popping/jumping problem in test_boxstack, +especially when boxes are piled on top of each other. find out if this is +caused by a configuration singularity or whether there is a bug in LCP. +i need to add some kind of diagnostic tool to help resolve these kinds of +problems. + +fixup ground plane jitter and shadow jumping in drawstuff. + +the inertias/COMs don't appear to be totally correct for the boxstack demo. +fix up, and add a mode that shows the effective mass box (for a given density). + +Improve box-box collision, especially for face-face contact (3 contact points). +Improve cylinder-box collision (2 contact points). + +windows DLL building and unix shared libs. libtool? +also MSVC project files. + +dBodyGetPointVel() + +contrib directory - all stuff in ~/3/ode + +functions to allow systems to be copied/cloned + dBodyTransplant (b, world) + dTransplantIsland (b, world) + dBodyCopy (bdest, bsrc) + dJointCopy (jdest, jsrc) -- what about body connections? + dCloneBody() + dCloneJoint() + dCloseBodyAndJointList() + dCloneIsland() + +this collision rule: + // no contacts if both geoms on the same body, and the body is not 0 + if (g1->body == g2->body && g1->body) return 0; +needs to be replaced. sometimes we want no collision when both bodies are 0, +but this wont work for geomgroup-to-environment. avoid stupid stuff like + dGeomSetBody (geom_group, (dBodyID) 1); +this also causes "failed-to-report" errors in the space test. + +Expose type-specific collision functions? + +Automatic code optimization process. + +joint limit spongyness: interacts with powered joints badly, because when the +limit is reached full power is applied. fix or doc. + +various hinge2 functions may not function correctly if axis1 and axis2 are not +perpendicular. in particular the getAngle() and getAngleRate() functions +probably will give bogus answers. + +slow step function will not respect the joint getinfo2 functions calling +addTorque() because it reads the force/torque accumulators before the +getinfo2 functions are called. + +spaces need multiple lists of objects that can never overlap. objects in these +lists are never tested against each other. + +deleting a body a joint is attached to should adjust the joint to only have +one body attached. currently the connected joints have *both* their body +attachments removed. BUT, dont do this if the dJOINT_TWOBODIES flag is set +on the joint. + +document error, mem and math functions. + +Web pages + credits section + projects using ODE + +update C++ interface? use SWIG? + +collision exclusion groups - exclude if obj1.n == obj2.n ? + +make sure the amotor joint can be used with just one body. at the moment it +only allows two-body attachments. + +implement dJointGetAMotorAngleRate() + +erwin says: Should the GeomGroup have a cleanupmode as the GeomTransform has? + +erwin says: http://q12.org/pipermail/ode/2002-January/000766.html + and http://q12.org/pipermail/ode/2001-December/000753.html + +rename duplicate filenames (object.h?) - some environments can't handle this. + +naming inconsistency: dCreateSphere() should be dSphereCreate() (etc...) to +match the rest of the API. + + +TODO +---- + +joint allocation in joint groups. allocation size should be rounded up using +dEFFICIENT_SIZE, to properly align all the data members. + +all dAlloc() allocations should be aligned using dEFFICIENT_SIZE() ??? + +automatic body & joint disabling / enabling. + +sometimes getting LCP infinite loops. + +function to get the entire island of bodies/joints + +joints: + hinge2 joint - implement trail, i.e. non-convergent steering and wheel + axes. + + erp individually settable for each joint? + + more joints: + angular3 (constrian full angle not position) + fixed path 1 (point must follow fixed path, etc etc) + - other fixed path joints. + linear a (point in 1 body fixed to plane of other) + linear b (point in 1 body fixed to line on other) + linear c (line in 1 body fixed to plane on other) + linear d (line in 1 body fixed to line on other) - like + prismatic but orientation along line can change + Relative-Path-Relative-Oriention Joint (set all dofs of 2 + bodies relative to each other) + spring (with natural length) + universal (2 kinds) + various angular relationships + + when attaching joints to static env, provision to move attachment + point (e.g. give it a linear/angular velocity). this can be used + instead of a FPFO joint on a body in many cases. + also do this with contacts to static env, to allow for contacts to + *moving* objects in the static env. + + interpretation of erp: is it (1) the error reduction per timestep, + (2) or a time constant independent of timestep?? if it's (2) then + perhaps this should be universal - this is already the meaning for + the suspension. + + hinge2 suspension: + suspension limits + suspension limit restitution and spongyness?? + +use autoconf? set paths in makefile? + +no-arg init functions, for andy + +explore: do joint parameters need to be set for the joint to be setup +correctly, or should set some proper body-dependent params when it is +attached? this is only really an issue for joints that have no parameters to +set, such as the fixed joint. + +dAlloc() should take an arena parameters which is stored in dWorld. + +debugging mode should use dASSERT2 that prints a descriptive error message +on error, not just the file:line or function. use dASSERT for internal +consistency checking. + +when vectors and matrices are initialized, we must ensure that the padding +elements are set to 0. this is going to be a problem everywhere! + +don't use 3-vectors anywhere. use SIMD friendly 4-vectors. + +make sure all data in body/joint etc objects is aligned well for single +precision SIMD (i.e. all vectors start on a 16 byte boundary). + +think about more complicated uses of collision, e.g. a single geom representing +an articulated structure. + +bodyGroup? (like joint group but for bodies). systemGroup? + +check the overhead of resizing Array<>s as elements are pushed on to them. + +replace alloca() with dPushFrame(), dPopFrame(), and dAlloca() ? allow for +the possibility of allocating in non-stack memory ? + +make sure that we can set mass parameters with non-zero center of mass. +if this is done after the body position is set, the position is adjusted. +if this is done before the body position is set, what do we do when the +pos is set? does the pos always refer to the center of mass from the user's +point of view? + +consider splitting solver into functions, which can be optimized separately. +might make things go faster. + +faster code for islands with a single body? faster code for dynamically +symmetric bodies? + +rotation.cpp functions that set matrices should also set padding elements. + +lcp solver must return (L,d) and some other information, so we can re-solve +for other right hand sides later on, but using the same complimentarity +solution so there are no integrator discontinuities. + +dSetZero() - make fast inline functions for fixed n e.g. (1-4). + +need proper `sticky' friction, i.e. compensation for numerical slip. + +on windows, make sure gcc-compiles libs can be linked with VC++ apps. need +to make sure some C++ runtime bits are present? + +kill all references to dArray<> (in geom.cpp). + +need testing code to test all joints with body-to-static-env + +copy stack.cpp, memory.cpp stuff to reuse + +dFactorLDLT() is not so efficient for matrix sizes < block size, e.g. +redundant calls, zero loads, adds etc + +contacts: cheaper friction: viscous friction? one step delay friction force. + +in geom.cpp, for objects that are never meant to collide, dCollide() will +always try to find the collider functions, which wastes a bit of time. + +geom.cpp:dCollideG() - handle special case of colliding 2 groups more +efficiently. + +timer reporting function: + void timerReport (void (*printFunction)(char *, ...)); + +disabled bodies stored in a separate list, so they are never traversed at all, +for speed when there are many disabled bodies. + + +MAYBE +----- + +new implementation for joint groups that is not so system dependent. +maybe individual contacts are reusable? in this case contact information +should be settable in the contact joints. max_size arg is really annoying. + +consider making anchor,axis, (everything) into a joint parameter and setting +them with a consistent interface. also consider overload the joint functions +so they are not distinguished by joint type?? + +collision memory optimizations? + +collision: support for persistent contact information? + +multiply reference tri list data so that it can be cloned + if the tri-list geoms could support rot/pos + transformations then we could have several tri-lists pointing to the + same vertex information. + +height fields + +pre-converted collision data -- Creating a hash space and associated +opcode tree structures may take significant amounts of time for a +large world with many 10s of thousands of triangles. Any chance of +pre-building that off-line and passing a memory block pointer to the +collision system? + +putting objects in multiple spaces -- If it was possible to add +objects to more than one space, you could do collision queries other +than 1vsN and NvsN. That flexibility might be useful when you want to +only collide against a subset of the space. For example, a camera +system may want to collide some rays with occlusion walls but the +occlusion walls may also need to be in the game-level space to bounce +against. + + +ALWAYS +------ + +make sure functions check their arguments in debug mode (e.g. using dASSERT). +make sure joint/geom functions check for the specific object type. + +vectors alloca()ed on the stack must have the correct alignment, use ALLOCA16. + +library should have no global constructors, as it might be used with C linkage. + +use `const' in function arguments. blah. + + + +DON'T BOTHER +------------ + +warning if user tries to set mass params with nonzero center of mass. + + + +DONE +---- + +check: when contact attached with (body1,0) and (0,body1), check that polarity +on depth and error info is okay for the two cases. + +set a better convention for which is the 1st and 2nd body in a joint, because +sometimes we get things swapped (because of the way the joint nodes are used). + +hinge and prismatic, attachment to static environment. + +turn macros into C++ inline functions? what about C users? + +remove `space' argument to geom creation functions? make user add it? +or just remove it from dCreateGeom() ? <-- did this one. + +test_chain should be in C, not C++. but first must remove global constructors. + +add more functionality to C++ interface - dMass, dSpace, dGeom + +there should be functions to delete groups of bodies/joints in one go - this +will be more efficient than deleting them one at a time, because less +partitioning tests will be needed. + +should we expose body and joint object structures so that the user can +explicitly allocate them locally, or e.g. on the stack? makes allocating +temporary contact constraints easier. NO --> helps data hiding and therefore +library binary compatability. + +joints: + hinge & slider - DONE + measure angle, rate - DONE + power - DONE + joint limits - DONE + mixed powered+limited joints, powering away from limit - DONE + + hinge2 - DONE + steering angle and rate measurement - DONE + steering limits - DONE + steering motor - DONE + wheel motor - DONE + wheel angle rate measurement - DONE + + optional hinge2 suspension: - DONE + alignment of B&S part to given axis - DONE + global framework for giving epsilon and gamma - DONE + + toss away r-motor, make power & stuff specific to joint - DONE + it's just easier that way + + joint code reuse: - DONE + use standard functions to set velocity (c), limits (lo,hi), + spongyness (epsilon) etc, this prevents these functions from + proliferating + + implicit spring framework - actually allow joints to return a value `k' + such that J*vnew = c + k*f, where f = force needed to achieve + vnew - DONE + + contact slip - DONE + contact erp & cfm parameters (not just "softness") - DONE + + hinge2: when we lock back wheels along the steering axis, there is no + error correction if they get out of alignment - DONE, just use high + and low limits. + + joint limit spongyness: erp and cfm for joint set from world (global) + values when joint created. - DONE + + joint limit restitution - DONE + +check inertia transformations, e.g. by applying steering torque to a thin +wheel --> actually, i made test_I + +more comprehensive random number comparisons between slow and fast methods. + - random PD inertia (not just diagonal). + - random velocity + - random joint error (make joints then move bodies a bit) + +check that J*vnew=c (slow step already does this, but it doesn't equal zero +for some reason! - actually, when LCP constraint limits are reached, it wont!) + +tons of things in lcp.cpp (@@@), especially speed optimizations. also, we +wanted to do index block switching and index block updates to take advantage +of the outer product trick ... but this is not worth the effort i think. + +lcp.cpp: if lo=hi=0, check operation. can we switch from NL <-> NH without +going through C? --> done. + +andy says: still having trouble with those resource files.. +drawstuff.res doesn't seem to build or be found under cygwin gcc. + +DOC how bodies and geoms associated then resolved in contact callback ... not +really necessary. + +fix the "memory leak" in geom.cpp + +library should have no global constructors, as it might be used with C linkage. + --> as long as test_chain1 works, there are none. + +DOC cfm, the derivation and what it means. + --> partially done, could be better + +joint "get type" function + +andy says: in ode/src/error.cpp _snprintf() and _vsnprintf() are missing +in testode: finite and isnan are missing. copysign is missing + russ: okay here's the problem: i have Makefile.platform files for + VC++, MinGW, but not Cygwin. Cygwin uses the unix-like functions + for everything, but the VC++/MinGW configs assumes the MS C-runtime + functions. this is easy to fix, except i need to install Cygwin + which is a pain to do over MinGW. argh. + +build on linux - assumptions made about location of X11 lib, opengl etc. + +implement: dBodyAddForceAtPos,dBodyAddRelForceAtPos,dBodyAddRelForceAtRelPos, + dBodyGetPointPos,dBodyGetPointVel,dBodyGetPointRelVel + +dJointAttach(), allow both bodies to be 0 to put the joint into limbo. + +space near-callback should be given potentially intersecting objects 100 at a +time instead of 1 at a time, to save on calling costs ... which are trivial, +so we don't bother to do this. + +doccer: @func{} also refs second etc function in function *list*. + +make sure joints can return 0 from GetInfo1, i.e. no constraints or "inactive" +joint, and the step functions will handle it. + +when attaching contact with (0,body), instead of setting the reverse flag +on the joint and checking it in getInfo2(), we should just reverse the normal +straight away ... ? + --> trouble is, dJointAttach() knows nothing about what kind of joint + it is attaching. + +hinge2 needs to be attached to two bodies for it to work, make sure this is +always the case. --> assertion added in dJointAttach(). + +if two joints connect to the same two bodies, check that the fast solver +works! -> it should. + +functions to get all the joints/bodies a body/joint is connected to. + +If I don't have the GCC libraries installed, HUGE_VALF is undefined. + +fix capped cylinder - capped cylinder collision so that two contacts can +be generated. + +transformation geometry object. + +joint groups should also be destroyed by destroying the world --> naaahhh. + +DONT DO THIS: body/joint creators with world = 0 --> not inserted into any +world. allow bodies/joints to be detached from a world (this is what happens +to grouped joints when a world is destroyed). + can bodies and joints be linked together when not attached to world?? + what happens when we have an island of b/j, some of which are not in + world? soln: dont keep lists of b/j in the world, just infer it from + the islands? + +body & joint disabling / enabling + +start a change log. + +collision flags - 0xffff mask. + +dBodyGetFiniteRotationMode() / ...Axis() + +dBodyAddForceAtRelPos() + +ball & socket joint limits and motors. + +auto-build env on windows: 3 compilers, debug/release, short/double = +12 combinations --> auto logs. + +handle infinities better: HUGE_VALF is not commanly defined, it seems. +get rid of the __USE_ISOC9X macro in common.h +perhaps just use a "big" number instead of the actual IEEE infinity, it's +more portable anyway. + --> new config system + +dCloseODE() - tidy up *all* allocated memory, esp in geom.cpp. used to keep +leak detectors happy. + +extra API to get lambda and J'*lambda from last timestep. + +better stack implementation that is not so system dependent. but how will +we do dynamic page allocation? do we even need to? + + +all collision files will now be collision_*, not geom_* + +check exported global symbols - no C++ mangling. + +rename dSphere etc to dxSphere etc. + +C interface support for making new classes. + +make sure DLL-ized stuff preserved ... but class numbers should no longer be +exported. + +point geom ( = sphere of radius 0 ) + +geoms stored in doubly linked lists in space (fast removal). + +bodies need to keep geoms pointers and call dGeomMoved() in dBodySetPosition() +etc and world step. PROBLEM: links dynamics and collision together too much, +makes it hard to extract ODE collision ... unless we say: dGeomMoved() and +dGeomID must be supplied by the new collision library! + +dCollide() should take spaces as arguments - it should call dSpaceCollide2() +with its own callback that puts all found contacts in the array, stopping +when there is no more space left in the array. + +dxSpace::getGeom() - the geom numbers will change as geoms are dirtied - find +some other numbering scheme, or document this behavior. + +the 'placeable' property - objects that should not ever be attached to bodies +should flag an error when setBody etc are called. + +dGeomSetBody(0) - DOC: the position and orientation of the body will be +preserved. in this case the geom should NOT be dirtied (dGeomMoved() should +not be called). + +DOC: dGeomGetBodyNext() as part of dynamics/collision interface + +groups/spaces are subclasses of geom. + +groups/spaces can contain other groups/spaces. geom can be owned by a +group/space. collision handling: + geom-geom : standard collision function + geom-group : special space code + group-group : n^2 tests (or n space tests) - hard to optimize because + of disjoint space representations. + group internal : normal space internal-collision code + +groups/spaces can be told that some objects never move, i.e. that the objects +are locked. should we lock the whole space? + locking: the AABB for the object is not recalculated + +groups/spaces can be told that the internal contents self-intersect or not. +actually an old ODE group is the equivalent of an old ODE simple space. + - just call dCollide() or not. + +the group doesn't get passed to the space callback any more ... only the +intersecting geoms get passed? maybe the callback can initiate the extra +intersection tests itself? (because we want programmable flexibility to +determine what gets intersected and what doesn't) + - NO + +infrastructure to indicate when an object has moved (and thus its AABB needs +to be recalculated) + +space enumeration functions. make sure that there are no additions or deletions +while enumeration is taking place. + - documented the behavior, didn't disallow it + +cache the AABB in the dxGeom? (for non-moving objects) - perhaps keep a +pointer to separately allocated space? ... no + +DOC: dGeomGetClass() is a first-class geom function, not in the "User +defined classes" section. it returns a constant that can be checked +against dSphereClass etc. + +remove dxGeom dependence on dBodyID? ... not yet + +dBase -> dxBase + +allow a geom to be inserted into multiple spaces? need this to optimize some +kinds of tests ... no + +update docs. + +make CHECK_NOT_LOCKED an assert. + +DOC: "Calling these functions on a non-placeable geom results in a +runtime error." ...in the debug build only? + +non-placeable geoms should not allocate dxPosR. perhaps pass a dGeom +constructor arg that says 'placeable' or not - this also sets the +GEOM_PLACEABLE flag. + +GeomTransform: + final_pos and final_R valid if no GEOM_AABB_BAD flag!!! + fix up this code, esp use of ComputeTX(). + +Space incompatibilities: no dSpaceDestroy(), dGeomDestroy() does not +take a dSpaceID ... dSpaceDestroy() added. + +GeomGroup incompatibilities: + dCollide() used to take a GeomGroup and would return all the contact + points for all the intersecting objects. now you have to call + dSpaceCollide2() and get a callback for each one. + need to provide old behavior. + +simple space optimization: we should keep the precomputed AABB for the +non-moving geoms around, so that when the other geoms move we can just +compute the AABBs for those geoms and then combine it with the non-moving AABB. + --> too hard! + +collision build options: old and new + +tidyups for collision: + * rationalize what stuff goes in what source files, and file names + * minimize set of header files that all collision* sources use - after + all changes. + * update ode-cpp stuff (C++ interface header files). + +porting guide: + ODE list email + + dGeomGetSpaceAABB() deleted + + dGeomGetClass (geom_group); used to return a unique type for + GeomGroups, but now it returns dSimpleSpaceID. + +tidyups: update DLL declarations. + diff --git a/libs/ode-0.16.1/ode/demo/Makefile.am b/libs/ode-0.16.1/ode/demo/Makefile.am new file mode 100644 index 0000000..5d02b87 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/Makefile.am @@ -0,0 +1,75 @@ +AM_CPPFLAGS = -I$(top_srcdir)/include \ + -I$(top_builddir)/include \ + -DDRAWSTUFF_TEXTURE_PATH="\"$(abs_top_srcdir)/drawstuff/textures\"" + +if X11 +AM_LDFLAGS = $(X_PRE_LIBS) $(X_LIBS) $(X_EXTRA_LIBS) +endif + +# On Windows, GL_LIBS must go after libdrawstuff.la. +LDADD = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la @GL_LIBS@ + +noinst_HEADERS = basket_geom.h bunny_geom.h convex_bunny_geom.h convex_prism.h \ + icosahedron_geom.h halton235_geom.h texturepath.h world_geom3.h + +AM_DEFAULT_SOURCE_EXT = .cpp + +noinst_PROGRAMS = \ + demo_boxstack \ + demo_buggy \ + demo_cards \ + demo_chain1 \ + demo_chain2 \ + demo_collision \ + demo_convex \ + demo_crash \ + demo_cylvssphere \ + demo_dball \ + demo_dhinge \ + demo_transmission \ + demo_feedback \ + demo_friction \ + demo_gyroscopic \ + demo_gyro2 \ + demo_heightfield \ + demo_hinge \ + demo_I \ + demo_jointPR \ + demo_joints \ + demo_jointPU \ + demo_kinematic \ + demo_motion \ + demo_motor \ + demo_ode \ + demo_piston \ + demo_plane2d \ + demo_rfriction \ + demo_slider \ + demo_space \ + demo_space_stress \ + demo_step \ + demo_tracks + +demo_chain1_SOURCES = demo_chain1.c +demo_chain1_LDADD = $(LDADD) -lstdc++ + + +if TRIMESH +noinst_PROGRAMS += \ + demo_basket \ + demo_cyl \ + demo_moving_trimesh \ + demo_moving_convex \ + demo_trimesh + +AM_CPPFLAGS += -DdTRIMESH_ENABLED +endif + + + +if WIN32 +resources.o: $(top_srcdir)/drawstuff/src/resources.rc $(top_srcdir)/drawstuff/src/resource.h + @WINDRES@ $(top_srcdir)/drawstuff/src/resources.rc -o resources.o +LDADD += resources.o +endif diff --git a/libs/ode-0.16.1/ode/demo/Makefile.in b/libs/ode-0.16.1/ode/demo/Makefile.in new file mode 100644 index 0000000..368a5d1 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/Makefile.in @@ -0,0 +1,1133 @@ +# Makefile.in generated by automake 1.15 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994-2014 Free Software Foundation, Inc. + +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ + + +VPATH = @srcdir@ +am__is_gnu_make = { \ + if test -z '$(MAKELEVEL)'; then \ + false; \ + elif test -n '$(MAKE_HOST)'; then \ + true; \ + elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \ + true; \ + else \ + false; \ + fi; \ +} +am__make_running_with_option = \ + case $${target_option-} in \ + ?) ;; \ + *) echo "am__make_running_with_option: internal error: invalid" \ + "target option '$${target_option-}' specified" >&2; \ + exit 1;; \ + esac; \ + has_opt=no; \ + sane_makeflags=$$MAKEFLAGS; \ + if $(am__is_gnu_make); then \ + sane_makeflags=$$MFLAGS; \ + else \ + case $$MAKEFLAGS in \ + *\\[\ \ ]*) \ + bs=\\; \ + sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \ + | sed "s/$$bs$$bs[$$bs $$bs ]*//g"`;; \ + esac; \ + fi; \ + skip_next=no; \ + strip_trailopt () \ + { \ + flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \ + }; \ + for flg in $$sane_makeflags; do \ + test $$skip_next = yes && { skip_next=no; continue; }; \ + case $$flg in \ + *=*|--*) continue;; \ + -*I) strip_trailopt 'I'; skip_next=yes;; \ + -*I?*) strip_trailopt 'I';; \ + -*O) strip_trailopt 'O'; skip_next=yes;; \ + -*O?*) strip_trailopt 'O';; \ + -*l) strip_trailopt 'l'; skip_next=yes;; \ + -*l?*) strip_trailopt 'l';; \ + -[dEDm]) skip_next=yes;; \ + -[JT]) skip_next=yes;; \ + esac; \ + case $$flg in \ + *$$target_option*) has_opt=yes; break;; \ + esac; \ + done; \ + test $$has_opt = yes +am__make_dryrun = (target_option=n; $(am__make_running_with_option)) +am__make_keepgoing = (target_option=k; $(am__make_running_with_option)) +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ +noinst_PROGRAMS = demo_boxstack$(EXEEXT) demo_buggy$(EXEEXT) \ + demo_cards$(EXEEXT) demo_chain1$(EXEEXT) demo_chain2$(EXEEXT) \ + demo_collision$(EXEEXT) demo_convex$(EXEEXT) \ + demo_crash$(EXEEXT) demo_cylvssphere$(EXEEXT) \ + demo_dball$(EXEEXT) demo_dhinge$(EXEEXT) \ + demo_transmission$(EXEEXT) demo_feedback$(EXEEXT) \ + demo_friction$(EXEEXT) demo_gyroscopic$(EXEEXT) \ + demo_gyro2$(EXEEXT) demo_heightfield$(EXEEXT) \ + demo_hinge$(EXEEXT) demo_I$(EXEEXT) demo_jointPR$(EXEEXT) \ + demo_joints$(EXEEXT) demo_jointPU$(EXEEXT) \ + demo_kinematic$(EXEEXT) demo_motion$(EXEEXT) \ + demo_motor$(EXEEXT) demo_ode$(EXEEXT) demo_piston$(EXEEXT) \ + demo_plane2d$(EXEEXT) demo_rfriction$(EXEEXT) \ + demo_slider$(EXEEXT) demo_space$(EXEEXT) \ + demo_space_stress$(EXEEXT) demo_step$(EXEEXT) \ + demo_tracks$(EXEEXT) $(am__EXEEXT_1) +@TRIMESH_TRUE@am__append_1 = \ +@TRIMESH_TRUE@ demo_basket \ +@TRIMESH_TRUE@ demo_cyl \ +@TRIMESH_TRUE@ demo_moving_trimesh \ +@TRIMESH_TRUE@ demo_moving_convex \ +@TRIMESH_TRUE@ demo_trimesh + +@TRIMESH_TRUE@am__append_2 = -DdTRIMESH_ENABLED +@WIN32_TRUE@am__append_3 = resources.o +subdir = ode/demo +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \ + $(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \ + $(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \ + $(top_srcdir)/m4/pkg.m4 $(top_srcdir)/configure.ac +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +DIST_COMMON = $(srcdir)/Makefile.am $(noinst_HEADERS) \ + $(am__DIST_COMMON) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/ode/src/config.h +CONFIG_CLEAN_FILES = +CONFIG_CLEAN_VPATH_FILES = +@TRIMESH_TRUE@am__EXEEXT_1 = demo_basket$(EXEEXT) demo_cyl$(EXEEXT) \ +@TRIMESH_TRUE@ demo_moving_trimesh$(EXEEXT) \ +@TRIMESH_TRUE@ demo_moving_convex$(EXEEXT) \ +@TRIMESH_TRUE@ demo_trimesh$(EXEEXT) +PROGRAMS = $(noinst_PROGRAMS) +demo_I_SOURCES = demo_I.cpp +demo_I_OBJECTS = demo_I.$(OBJEXT) +demo_I_LDADD = $(LDADD) +demo_I_DEPENDENCIES = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +AM_V_lt = $(am__v_lt_@AM_V@) +am__v_lt_ = $(am__v_lt_@AM_DEFAULT_V@) +am__v_lt_0 = --silent +am__v_lt_1 = +demo_basket_SOURCES = demo_basket.cpp +demo_basket_OBJECTS = demo_basket.$(OBJEXT) +demo_basket_LDADD = $(LDADD) +demo_basket_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_boxstack_SOURCES = demo_boxstack.cpp +demo_boxstack_OBJECTS = demo_boxstack.$(OBJEXT) +demo_boxstack_LDADD = $(LDADD) +demo_boxstack_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_buggy_SOURCES = demo_buggy.cpp +demo_buggy_OBJECTS = demo_buggy.$(OBJEXT) +demo_buggy_LDADD = $(LDADD) +demo_buggy_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_cards_SOURCES = demo_cards.cpp +demo_cards_OBJECTS = demo_cards.$(OBJEXT) +demo_cards_LDADD = $(LDADD) +demo_cards_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +am_demo_chain1_OBJECTS = demo_chain1.$(OBJEXT) +demo_chain1_OBJECTS = $(am_demo_chain1_OBJECTS) +am__DEPENDENCIES_1 = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_chain1_DEPENDENCIES = $(am__DEPENDENCIES_1) +demo_chain2_SOURCES = demo_chain2.cpp +demo_chain2_OBJECTS = demo_chain2.$(OBJEXT) +demo_chain2_LDADD = $(LDADD) +demo_chain2_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_collision_SOURCES = demo_collision.cpp +demo_collision_OBJECTS = demo_collision.$(OBJEXT) +demo_collision_LDADD = $(LDADD) +demo_collision_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_convex_SOURCES = demo_convex.cpp +demo_convex_OBJECTS = demo_convex.$(OBJEXT) +demo_convex_LDADD = $(LDADD) +demo_convex_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_crash_SOURCES = demo_crash.cpp +demo_crash_OBJECTS = demo_crash.$(OBJEXT) +demo_crash_LDADD = $(LDADD) +demo_crash_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_cyl_SOURCES = demo_cyl.cpp +demo_cyl_OBJECTS = demo_cyl.$(OBJEXT) +demo_cyl_LDADD = $(LDADD) +demo_cyl_DEPENDENCIES = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_cylvssphere_SOURCES = demo_cylvssphere.cpp +demo_cylvssphere_OBJECTS = demo_cylvssphere.$(OBJEXT) +demo_cylvssphere_LDADD = $(LDADD) +demo_cylvssphere_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_dball_SOURCES = demo_dball.cpp +demo_dball_OBJECTS = demo_dball.$(OBJEXT) +demo_dball_LDADD = $(LDADD) +demo_dball_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_dhinge_SOURCES = demo_dhinge.cpp +demo_dhinge_OBJECTS = demo_dhinge.$(OBJEXT) +demo_dhinge_LDADD = $(LDADD) +demo_dhinge_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_feedback_SOURCES = demo_feedback.cpp +demo_feedback_OBJECTS = demo_feedback.$(OBJEXT) +demo_feedback_LDADD = $(LDADD) +demo_feedback_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_friction_SOURCES = demo_friction.cpp +demo_friction_OBJECTS = demo_friction.$(OBJEXT) +demo_friction_LDADD = $(LDADD) +demo_friction_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_gyro2_SOURCES = demo_gyro2.cpp +demo_gyro2_OBJECTS = demo_gyro2.$(OBJEXT) +demo_gyro2_LDADD = $(LDADD) +demo_gyro2_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_gyroscopic_SOURCES = demo_gyroscopic.cpp +demo_gyroscopic_OBJECTS = demo_gyroscopic.$(OBJEXT) +demo_gyroscopic_LDADD = $(LDADD) +demo_gyroscopic_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_heightfield_SOURCES = demo_heightfield.cpp +demo_heightfield_OBJECTS = demo_heightfield.$(OBJEXT) +demo_heightfield_LDADD = $(LDADD) +demo_heightfield_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_hinge_SOURCES = demo_hinge.cpp +demo_hinge_OBJECTS = demo_hinge.$(OBJEXT) +demo_hinge_LDADD = $(LDADD) +demo_hinge_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_jointPR_SOURCES = demo_jointPR.cpp +demo_jointPR_OBJECTS = demo_jointPR.$(OBJEXT) +demo_jointPR_LDADD = $(LDADD) +demo_jointPR_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_jointPU_SOURCES = demo_jointPU.cpp +demo_jointPU_OBJECTS = demo_jointPU.$(OBJEXT) +demo_jointPU_LDADD = $(LDADD) +demo_jointPU_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_joints_SOURCES = demo_joints.cpp +demo_joints_OBJECTS = demo_joints.$(OBJEXT) +demo_joints_LDADD = $(LDADD) +demo_joints_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_kinematic_SOURCES = demo_kinematic.cpp +demo_kinematic_OBJECTS = demo_kinematic.$(OBJEXT) +demo_kinematic_LDADD = $(LDADD) +demo_kinematic_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_motion_SOURCES = demo_motion.cpp +demo_motion_OBJECTS = demo_motion.$(OBJEXT) +demo_motion_LDADD = $(LDADD) +demo_motion_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_motor_SOURCES = demo_motor.cpp +demo_motor_OBJECTS = demo_motor.$(OBJEXT) +demo_motor_LDADD = $(LDADD) +demo_motor_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_moving_convex_SOURCES = demo_moving_convex.cpp +demo_moving_convex_OBJECTS = demo_moving_convex.$(OBJEXT) +demo_moving_convex_LDADD = $(LDADD) +demo_moving_convex_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_moving_trimesh_SOURCES = demo_moving_trimesh.cpp +demo_moving_trimesh_OBJECTS = demo_moving_trimesh.$(OBJEXT) +demo_moving_trimesh_LDADD = $(LDADD) +demo_moving_trimesh_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_ode_SOURCES = demo_ode.cpp +demo_ode_OBJECTS = demo_ode.$(OBJEXT) +demo_ode_LDADD = $(LDADD) +demo_ode_DEPENDENCIES = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_piston_SOURCES = demo_piston.cpp +demo_piston_OBJECTS = demo_piston.$(OBJEXT) +demo_piston_LDADD = $(LDADD) +demo_piston_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_plane2d_SOURCES = demo_plane2d.cpp +demo_plane2d_OBJECTS = demo_plane2d.$(OBJEXT) +demo_plane2d_LDADD = $(LDADD) +demo_plane2d_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_rfriction_SOURCES = demo_rfriction.cpp +demo_rfriction_OBJECTS = demo_rfriction.$(OBJEXT) +demo_rfriction_LDADD = $(LDADD) +demo_rfriction_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_slider_SOURCES = demo_slider.cpp +demo_slider_OBJECTS = demo_slider.$(OBJEXT) +demo_slider_LDADD = $(LDADD) +demo_slider_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_space_SOURCES = demo_space.cpp +demo_space_OBJECTS = demo_space.$(OBJEXT) +demo_space_LDADD = $(LDADD) +demo_space_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_space_stress_SOURCES = demo_space_stress.cpp +demo_space_stress_OBJECTS = demo_space_stress.$(OBJEXT) +demo_space_stress_LDADD = $(LDADD) +demo_space_stress_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_step_SOURCES = demo_step.cpp +demo_step_OBJECTS = demo_step.$(OBJEXT) +demo_step_LDADD = $(LDADD) +demo_step_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_tracks_SOURCES = demo_tracks.cpp +demo_tracks_OBJECTS = demo_tracks.$(OBJEXT) +demo_tracks_LDADD = $(LDADD) +demo_tracks_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_transmission_SOURCES = demo_transmission.cpp +demo_transmission_OBJECTS = demo_transmission.$(OBJEXT) +demo_transmission_LDADD = $(LDADD) +demo_transmission_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_trimesh_SOURCES = demo_trimesh.cpp +demo_trimesh_OBJECTS = demo_trimesh.$(OBJEXT) +demo_trimesh_LDADD = $(LDADD) +demo_trimesh_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +AM_V_P = $(am__v_P_@AM_V@) +am__v_P_ = $(am__v_P_@AM_DEFAULT_V@) +am__v_P_0 = false +am__v_P_1 = : +AM_V_GEN = $(am__v_GEN_@AM_V@) +am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@) +am__v_GEN_0 = @echo " GEN " $@; +am__v_GEN_1 = +AM_V_at = $(am__v_at_@AM_V@) +am__v_at_ = $(am__v_at_@AM_DEFAULT_V@) +am__v_at_0 = @ +am__v_at_1 = +DEFAULT_INCLUDES = -I.@am__isrc@ -I$(top_builddir)/ode/src +depcomp = $(SHELL) $(top_srcdir)/depcomp +am__depfiles_maybe = depfiles +am__mv = mv -f +COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \ + $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) +LTCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CC) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CFLAGS) $(CFLAGS) +AM_V_CC = $(am__v_CC_@AM_V@) +am__v_CC_ = $(am__v_CC_@AM_DEFAULT_V@) +am__v_CC_0 = @echo " CC " $@; +am__v_CC_1 = +CCLD = $(CC) +LINK = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CCLD) $(AM_CFLAGS) $(CFLAGS) \ + $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CCLD = $(am__v_CCLD_@AM_V@) +am__v_CCLD_ = $(am__v_CCLD_@AM_DEFAULT_V@) +am__v_CCLD_0 = @echo " CCLD " $@; +am__v_CCLD_1 = +CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \ + $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) +LTCXXCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CXX) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CXXFLAGS) $(CXXFLAGS) +AM_V_CXX = $(am__v_CXX_@AM_V@) +am__v_CXX_ = $(am__v_CXX_@AM_DEFAULT_V@) +am__v_CXX_0 = @echo " CXX " $@; +am__v_CXX_1 = +CXXLD = $(CXX) +CXXLINK = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CXXLD) $(AM_CXXFLAGS) \ + $(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CXXLD = $(am__v_CXXLD_@AM_V@) +am__v_CXXLD_ = $(am__v_CXXLD_@AM_DEFAULT_V@) +am__v_CXXLD_0 = @echo " CXXLD " $@; +am__v_CXXLD_1 = +SOURCES = demo_I.cpp demo_basket.cpp demo_boxstack.cpp demo_buggy.cpp \ + demo_cards.cpp $(demo_chain1_SOURCES) demo_chain2.cpp \ + demo_collision.cpp demo_convex.cpp demo_crash.cpp demo_cyl.cpp \ + demo_cylvssphere.cpp demo_dball.cpp demo_dhinge.cpp \ + demo_feedback.cpp demo_friction.cpp demo_gyro2.cpp \ + demo_gyroscopic.cpp demo_heightfield.cpp demo_hinge.cpp \ + demo_jointPR.cpp demo_jointPU.cpp demo_joints.cpp \ + demo_kinematic.cpp demo_motion.cpp demo_motor.cpp \ + demo_moving_convex.cpp demo_moving_trimesh.cpp demo_ode.cpp \ + demo_piston.cpp demo_plane2d.cpp demo_rfriction.cpp \ + demo_slider.cpp demo_space.cpp demo_space_stress.cpp \ + demo_step.cpp demo_tracks.cpp demo_transmission.cpp \ + demo_trimesh.cpp +DIST_SOURCES = demo_I.cpp demo_basket.cpp demo_boxstack.cpp \ + demo_buggy.cpp demo_cards.cpp $(demo_chain1_SOURCES) \ + demo_chain2.cpp demo_collision.cpp demo_convex.cpp \ + demo_crash.cpp demo_cyl.cpp demo_cylvssphere.cpp \ + demo_dball.cpp demo_dhinge.cpp demo_feedback.cpp \ + demo_friction.cpp demo_gyro2.cpp demo_gyroscopic.cpp \ + demo_heightfield.cpp demo_hinge.cpp demo_jointPR.cpp \ + demo_jointPU.cpp demo_joints.cpp demo_kinematic.cpp \ + demo_motion.cpp demo_motor.cpp demo_moving_convex.cpp \ + demo_moving_trimesh.cpp demo_ode.cpp demo_piston.cpp \ + demo_plane2d.cpp demo_rfriction.cpp demo_slider.cpp \ + demo_space.cpp demo_space_stress.cpp demo_step.cpp \ + demo_tracks.cpp demo_transmission.cpp demo_trimesh.cpp +am__can_run_installinfo = \ + case $$AM_UPDATE_INFO_DIR in \ + n|no|NO) false;; \ + *) (install-info --version) >/dev/null 2>&1;; \ + esac +HEADERS = $(noinst_HEADERS) +am__tagged_files = $(HEADERS) $(SOURCES) $(TAGS_FILES) $(LISP) +# Read a list of newline-separated strings from the standard input, +# and print each of them once, without duplicates. Input order is +# *not* preserved. +am__uniquify_input = $(AWK) '\ + BEGIN { nonempty = 0; } \ + { items[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in items) print i; }; } \ +' +# Make sure the list of sources is unique. This is necessary because, +# e.g., the same source file might be shared among _SOURCES variables +# for different programs/libraries. +am__define_uniq_tagged_files = \ + list='$(am__tagged_files)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | $(am__uniquify_input)` +ETAGS = etags +CTAGS = ctags +am__DIST_COMMON = $(srcdir)/Makefile.in $(top_srcdir)/depcomp +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +ACLOCAL = @ACLOCAL@ +ALLOCA = @ALLOCA@ +AMTAR = @AMTAR@ +AM_DEFAULT_VERBOSITY = @AM_DEFAULT_VERBOSITY@ +AR = @AR@ +AS = @AS@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CCD_CFLAGS = @CCD_CFLAGS@ +CCD_LIBS = @CCD_LIBS@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXCPP = @CXXCPP@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DLLTOOL = @DLLTOOL@ +DOXYGEN = @DOXYGEN@ +DSYMUTIL = @DSYMUTIL@ +DUMPBIN = @DUMPBIN@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +EXTRA_LIBTOOL_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ +FGREP = @FGREP@ +GL_LIBS = @GL_LIBS@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LD = @LD@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LIBSTDCXX = @LIBSTDCXX@ +LIBTOOL = @LIBTOOL@ +LIPO = @LIPO@ +LN_S = @LN_S@ +LTLIBOBJS = @LTLIBOBJS@ +LT_SYS_LIBRARY_PATH = @LT_SYS_LIBRARY_PATH@ +MAKEINFO = @MAKEINFO@ +MANIFEST_TOOL = @MANIFEST_TOOL@ +MKDIR_P = @MKDIR_P@ +NM = @NM@ +NMEDIT = @NMEDIT@ +OBJDUMP = @OBJDUMP@ +OBJEXT = @OBJEXT@ +ODE_PRECISION = @ODE_PRECISION@ +ODE_VERSION = @ODE_VERSION@ +ODE_VERSION_INFO = @ODE_VERSION_INFO@ +OTOOL = @OTOOL@ +OTOOL64 = @OTOOL64@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_URL = @PACKAGE_URL@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +PKG_CONFIG = @PKG_CONFIG@ +PKG_CONFIG_LIBDIR = @PKG_CONFIG_LIBDIR@ +PKG_CONFIG_PATH = @PKG_CONFIG_PATH@ +RANLIB = @RANLIB@ +SED = @SED@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +VERSION = @VERSION@ +WINDRES = @WINDRES@ +X11_CFLAGS = @X11_CFLAGS@ +X11_LIBS = @X11_LIBS@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_AR = @ac_ct_AR@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +ac_ct_DUMPBIN = @ac_ct_DUMPBIN@ +ac_ct_WINDRES = @ac_ct_WINDRES@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +build = @build@ +build_alias = @build_alias@ +build_cpu = @build_cpu@ +build_os = @build_os@ +build_vendor = @build_vendor@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host = @host@ +host_alias = @host_alias@ +host_cpu = @host_cpu@ +host_os = @host_os@ +host_vendor = @host_vendor@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +runstatedir = @runstatedir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +subdirs = @subdirs@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +top_build_prefix = @top_build_prefix@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include \ + -DDRAWSTUFF_TEXTURE_PATH="\"$(abs_top_srcdir)/drawstuff/textures\"" \ + $(am__append_2) +@X11_TRUE@AM_LDFLAGS = $(X_PRE_LIBS) $(X_LIBS) $(X_EXTRA_LIBS) + +# On Windows, GL_LIBS must go after libdrawstuff.la. +LDADD = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la @GL_LIBS@ $(am__append_3) +noinst_HEADERS = basket_geom.h bunny_geom.h convex_bunny_geom.h convex_prism.h \ + icosahedron_geom.h halton235_geom.h texturepath.h world_geom3.h + +AM_DEFAULT_SOURCE_EXT = .cpp +demo_chain1_SOURCES = demo_chain1.c +demo_chain1_LDADD = $(LDADD) -lstdc++ +all: all-am + +.SUFFIXES: +.SUFFIXES: .c .cpp .lo .o .obj +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \ + && { if test -f $@; then exit 0; else break; fi; }; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/demo/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/demo/Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(am__aclocal_m4_deps): + +clean-noinstPROGRAMS: + @list='$(noinst_PROGRAMS)'; test -n "$$list" || exit 0; \ + echo " rm -f" $$list; \ + rm -f $$list || exit $$?; \ + test -n "$(EXEEXT)" || exit 0; \ + list=`for p in $$list; do echo "$$p"; done | sed 's/$(EXEEXT)$$//'`; \ + echo " rm -f" $$list; \ + rm -f $$list + +demo_I$(EXEEXT): $(demo_I_OBJECTS) $(demo_I_DEPENDENCIES) $(EXTRA_demo_I_DEPENDENCIES) + @rm -f demo_I$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_I_OBJECTS) $(demo_I_LDADD) $(LIBS) + +demo_basket$(EXEEXT): $(demo_basket_OBJECTS) $(demo_basket_DEPENDENCIES) $(EXTRA_demo_basket_DEPENDENCIES) + @rm -f demo_basket$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_basket_OBJECTS) $(demo_basket_LDADD) $(LIBS) + +demo_boxstack$(EXEEXT): $(demo_boxstack_OBJECTS) $(demo_boxstack_DEPENDENCIES) $(EXTRA_demo_boxstack_DEPENDENCIES) + @rm -f demo_boxstack$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_boxstack_OBJECTS) $(demo_boxstack_LDADD) $(LIBS) + +demo_buggy$(EXEEXT): $(demo_buggy_OBJECTS) $(demo_buggy_DEPENDENCIES) $(EXTRA_demo_buggy_DEPENDENCIES) + @rm -f demo_buggy$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_buggy_OBJECTS) $(demo_buggy_LDADD) $(LIBS) + +demo_cards$(EXEEXT): $(demo_cards_OBJECTS) $(demo_cards_DEPENDENCIES) $(EXTRA_demo_cards_DEPENDENCIES) + @rm -f demo_cards$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_cards_OBJECTS) $(demo_cards_LDADD) $(LIBS) + +demo_chain1$(EXEEXT): $(demo_chain1_OBJECTS) $(demo_chain1_DEPENDENCIES) $(EXTRA_demo_chain1_DEPENDENCIES) + @rm -f demo_chain1$(EXEEXT) + $(AM_V_CCLD)$(LINK) $(demo_chain1_OBJECTS) $(demo_chain1_LDADD) $(LIBS) + +demo_chain2$(EXEEXT): $(demo_chain2_OBJECTS) $(demo_chain2_DEPENDENCIES) $(EXTRA_demo_chain2_DEPENDENCIES) + @rm -f demo_chain2$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_chain2_OBJECTS) $(demo_chain2_LDADD) $(LIBS) + +demo_collision$(EXEEXT): $(demo_collision_OBJECTS) $(demo_collision_DEPENDENCIES) $(EXTRA_demo_collision_DEPENDENCIES) + @rm -f demo_collision$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_collision_OBJECTS) $(demo_collision_LDADD) $(LIBS) + +demo_convex$(EXEEXT): $(demo_convex_OBJECTS) $(demo_convex_DEPENDENCIES) $(EXTRA_demo_convex_DEPENDENCIES) + @rm -f demo_convex$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_convex_OBJECTS) $(demo_convex_LDADD) $(LIBS) + +demo_crash$(EXEEXT): $(demo_crash_OBJECTS) $(demo_crash_DEPENDENCIES) $(EXTRA_demo_crash_DEPENDENCIES) + @rm -f demo_crash$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_crash_OBJECTS) $(demo_crash_LDADD) $(LIBS) + +demo_cyl$(EXEEXT): $(demo_cyl_OBJECTS) $(demo_cyl_DEPENDENCIES) $(EXTRA_demo_cyl_DEPENDENCIES) + @rm -f demo_cyl$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_cyl_OBJECTS) $(demo_cyl_LDADD) $(LIBS) + +demo_cylvssphere$(EXEEXT): $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_DEPENDENCIES) $(EXTRA_demo_cylvssphere_DEPENDENCIES) + @rm -f demo_cylvssphere$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_LDADD) $(LIBS) + +demo_dball$(EXEEXT): $(demo_dball_OBJECTS) $(demo_dball_DEPENDENCIES) $(EXTRA_demo_dball_DEPENDENCIES) + @rm -f demo_dball$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_dball_OBJECTS) $(demo_dball_LDADD) $(LIBS) + +demo_dhinge$(EXEEXT): $(demo_dhinge_OBJECTS) $(demo_dhinge_DEPENDENCIES) $(EXTRA_demo_dhinge_DEPENDENCIES) + @rm -f demo_dhinge$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_dhinge_OBJECTS) $(demo_dhinge_LDADD) $(LIBS) + +demo_feedback$(EXEEXT): $(demo_feedback_OBJECTS) $(demo_feedback_DEPENDENCIES) $(EXTRA_demo_feedback_DEPENDENCIES) + @rm -f demo_feedback$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_feedback_OBJECTS) $(demo_feedback_LDADD) $(LIBS) + +demo_friction$(EXEEXT): $(demo_friction_OBJECTS) $(demo_friction_DEPENDENCIES) $(EXTRA_demo_friction_DEPENDENCIES) + @rm -f demo_friction$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_friction_OBJECTS) $(demo_friction_LDADD) $(LIBS) + +demo_gyro2$(EXEEXT): $(demo_gyro2_OBJECTS) $(demo_gyro2_DEPENDENCIES) $(EXTRA_demo_gyro2_DEPENDENCIES) + @rm -f demo_gyro2$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_gyro2_OBJECTS) $(demo_gyro2_LDADD) $(LIBS) + +demo_gyroscopic$(EXEEXT): $(demo_gyroscopic_OBJECTS) $(demo_gyroscopic_DEPENDENCIES) $(EXTRA_demo_gyroscopic_DEPENDENCIES) + @rm -f demo_gyroscopic$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_gyroscopic_OBJECTS) $(demo_gyroscopic_LDADD) $(LIBS) + +demo_heightfield$(EXEEXT): $(demo_heightfield_OBJECTS) $(demo_heightfield_DEPENDENCIES) $(EXTRA_demo_heightfield_DEPENDENCIES) + @rm -f demo_heightfield$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_heightfield_OBJECTS) $(demo_heightfield_LDADD) $(LIBS) + +demo_hinge$(EXEEXT): $(demo_hinge_OBJECTS) $(demo_hinge_DEPENDENCIES) $(EXTRA_demo_hinge_DEPENDENCIES) + @rm -f demo_hinge$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_hinge_OBJECTS) $(demo_hinge_LDADD) $(LIBS) + +demo_jointPR$(EXEEXT): $(demo_jointPR_OBJECTS) $(demo_jointPR_DEPENDENCIES) $(EXTRA_demo_jointPR_DEPENDENCIES) + @rm -f demo_jointPR$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_jointPR_OBJECTS) $(demo_jointPR_LDADD) $(LIBS) + +demo_jointPU$(EXEEXT): $(demo_jointPU_OBJECTS) $(demo_jointPU_DEPENDENCIES) $(EXTRA_demo_jointPU_DEPENDENCIES) + @rm -f demo_jointPU$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_jointPU_OBJECTS) $(demo_jointPU_LDADD) $(LIBS) + +demo_joints$(EXEEXT): $(demo_joints_OBJECTS) $(demo_joints_DEPENDENCIES) $(EXTRA_demo_joints_DEPENDENCIES) + @rm -f demo_joints$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_joints_OBJECTS) $(demo_joints_LDADD) $(LIBS) + +demo_kinematic$(EXEEXT): $(demo_kinematic_OBJECTS) $(demo_kinematic_DEPENDENCIES) $(EXTRA_demo_kinematic_DEPENDENCIES) + @rm -f demo_kinematic$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_kinematic_OBJECTS) $(demo_kinematic_LDADD) $(LIBS) + +demo_motion$(EXEEXT): $(demo_motion_OBJECTS) $(demo_motion_DEPENDENCIES) $(EXTRA_demo_motion_DEPENDENCIES) + @rm -f demo_motion$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_motion_OBJECTS) $(demo_motion_LDADD) $(LIBS) + +demo_motor$(EXEEXT): $(demo_motor_OBJECTS) $(demo_motor_DEPENDENCIES) $(EXTRA_demo_motor_DEPENDENCIES) + @rm -f demo_motor$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_motor_OBJECTS) $(demo_motor_LDADD) $(LIBS) + +demo_moving_convex$(EXEEXT): $(demo_moving_convex_OBJECTS) $(demo_moving_convex_DEPENDENCIES) $(EXTRA_demo_moving_convex_DEPENDENCIES) + @rm -f demo_moving_convex$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_moving_convex_OBJECTS) $(demo_moving_convex_LDADD) $(LIBS) + +demo_moving_trimesh$(EXEEXT): $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_DEPENDENCIES) $(EXTRA_demo_moving_trimesh_DEPENDENCIES) + @rm -f demo_moving_trimesh$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_LDADD) $(LIBS) + +demo_ode$(EXEEXT): $(demo_ode_OBJECTS) $(demo_ode_DEPENDENCIES) $(EXTRA_demo_ode_DEPENDENCIES) + @rm -f demo_ode$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_ode_OBJECTS) $(demo_ode_LDADD) $(LIBS) + +demo_piston$(EXEEXT): $(demo_piston_OBJECTS) $(demo_piston_DEPENDENCIES) $(EXTRA_demo_piston_DEPENDENCIES) + @rm -f demo_piston$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_piston_OBJECTS) $(demo_piston_LDADD) $(LIBS) + +demo_plane2d$(EXEEXT): $(demo_plane2d_OBJECTS) $(demo_plane2d_DEPENDENCIES) $(EXTRA_demo_plane2d_DEPENDENCIES) + @rm -f demo_plane2d$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_plane2d_OBJECTS) $(demo_plane2d_LDADD) $(LIBS) + +demo_rfriction$(EXEEXT): $(demo_rfriction_OBJECTS) $(demo_rfriction_DEPENDENCIES) $(EXTRA_demo_rfriction_DEPENDENCIES) + @rm -f demo_rfriction$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_rfriction_OBJECTS) $(demo_rfriction_LDADD) $(LIBS) + +demo_slider$(EXEEXT): $(demo_slider_OBJECTS) $(demo_slider_DEPENDENCIES) $(EXTRA_demo_slider_DEPENDENCIES) + @rm -f demo_slider$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_slider_OBJECTS) $(demo_slider_LDADD) $(LIBS) + +demo_space$(EXEEXT): $(demo_space_OBJECTS) $(demo_space_DEPENDENCIES) $(EXTRA_demo_space_DEPENDENCIES) + @rm -f demo_space$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_space_OBJECTS) $(demo_space_LDADD) $(LIBS) + +demo_space_stress$(EXEEXT): $(demo_space_stress_OBJECTS) $(demo_space_stress_DEPENDENCIES) $(EXTRA_demo_space_stress_DEPENDENCIES) + @rm -f demo_space_stress$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_space_stress_OBJECTS) $(demo_space_stress_LDADD) $(LIBS) + +demo_step$(EXEEXT): $(demo_step_OBJECTS) $(demo_step_DEPENDENCIES) $(EXTRA_demo_step_DEPENDENCIES) + @rm -f demo_step$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_step_OBJECTS) $(demo_step_LDADD) $(LIBS) + +demo_tracks$(EXEEXT): $(demo_tracks_OBJECTS) $(demo_tracks_DEPENDENCIES) $(EXTRA_demo_tracks_DEPENDENCIES) + @rm -f demo_tracks$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_tracks_OBJECTS) $(demo_tracks_LDADD) $(LIBS) + +demo_transmission$(EXEEXT): $(demo_transmission_OBJECTS) $(demo_transmission_DEPENDENCIES) $(EXTRA_demo_transmission_DEPENDENCIES) + @rm -f demo_transmission$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_transmission_OBJECTS) $(demo_transmission_LDADD) $(LIBS) + +demo_trimesh$(EXEEXT): $(demo_trimesh_OBJECTS) $(demo_trimesh_DEPENDENCIES) $(EXTRA_demo_trimesh_DEPENDENCIES) + @rm -f demo_trimesh$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_trimesh_OBJECTS) $(demo_trimesh_LDADD) $(LIBS) + +mostlyclean-compile: + -rm -f *.$(OBJEXT) + +distclean-compile: + -rm -f *.tab.c + +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_I.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_basket.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_boxstack.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_buggy.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cards.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_chain1.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_chain2.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_collision.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_convex.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_crash.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cyl.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cylvssphere.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_dball.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_dhinge.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_feedback.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_friction.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_gyro2.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_gyroscopic.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_heightfield.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_hinge.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_jointPR.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_jointPU.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_joints.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_kinematic.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_motion.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_motor.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_moving_convex.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_moving_trimesh.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_ode.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_piston.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_plane2d.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_rfriction.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_slider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_space.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_space_stress.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_step.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_tracks.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_transmission.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_trimesh.Po@am__quote@ + +.c.o: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(COMPILE) -c -o $@ $< + +.c.obj: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(COMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.c.lo: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(LTCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(LTCOMPILE) -c -o $@ $< + +.cpp.o: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ $< + +.cpp.obj: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.cpp.lo: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(LTCXXCOMPILE) -c -o $@ $< + +mostlyclean-libtool: + -rm -f *.lo + +clean-libtool: + -rm -rf .libs _libs + +ID: $(am__tagged_files) + $(am__define_uniq_tagged_files); mkid -fID $$unique +tags: tags-am +TAGS: tags + +tags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + set x; \ + here=`pwd`; \ + $(am__define_uniq_tagged_files); \ + shift; \ + if test -z "$(ETAGS_ARGS)$$*$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + if test $$# -gt 0; then \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + "$$@" $$unique; \ + else \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$unique; \ + fi; \ + fi +ctags: ctags-am + +CTAGS: ctags +ctags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + $(am__define_uniq_tagged_files); \ + test -z "$(CTAGS_ARGS)$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && $(am__cd) $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) "$$here" +cscopelist: cscopelist-am + +cscopelist-am: $(am__tagged_files) + list='$(am__tagged_files)'; \ + case "$(srcdir)" in \ + [\\/]* | ?:[\\/]*) sdir="$(srcdir)" ;; \ + *) sdir=$(subdir)/$(srcdir) ;; \ + esac; \ + for i in $$list; do \ + if test -f "$$i"; then \ + echo "$(subdir)/$$i"; \ + else \ + echo "$$sdir/$$i"; \ + fi; \ + done >> $(top_builddir)/cscope.files + +distclean-tags: + -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d "$(distdir)/$$file"; then \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \ + else \ + test -f "$(distdir)/$$file" \ + || cp -p $$d/$$file "$(distdir)/$$file" \ + || exit 1; \ + fi; \ + done +check-am: all-am +check: check-am +all-am: Makefile $(PROGRAMS) $(HEADERS) +installdirs: +install: install-am +install-exec: install-exec-am +install-data: install-data-am +uninstall: uninstall-am + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-am +install-strip: + if test -z '$(STRIP)'; then \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + install; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \ + fi +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-am + +clean-am: clean-generic clean-libtool clean-noinstPROGRAMS \ + mostlyclean-am + +distclean: distclean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +distclean-am: clean-am distclean-compile distclean-generic \ + distclean-tags + +dvi: dvi-am + +dvi-am: + +html: html-am + +html-am: + +info: info-am + +info-am: + +install-data-am: + +install-dvi: install-dvi-am + +install-dvi-am: + +install-exec-am: + +install-html: install-html-am + +install-html-am: + +install-info: install-info-am + +install-info-am: + +install-man: + +install-pdf: install-pdf-am + +install-pdf-am: + +install-ps: install-ps-am + +install-ps-am: + +installcheck-am: + +maintainer-clean: maintainer-clean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-am + +mostlyclean-am: mostlyclean-compile mostlyclean-generic \ + mostlyclean-libtool + +pdf: pdf-am + +pdf-am: + +ps: ps-am + +ps-am: + +uninstall-am: + +.MAKE: install-am install-strip + +.PHONY: CTAGS GTAGS TAGS all all-am check check-am clean clean-generic \ + clean-libtool clean-noinstPROGRAMS cscopelist-am ctags \ + ctags-am distclean distclean-compile distclean-generic \ + distclean-libtool distclean-tags distdir dvi dvi-am html \ + html-am info info-am install install-am install-data \ + install-data-am install-dvi install-dvi-am install-exec \ + install-exec-am install-html install-html-am install-info \ + install-info-am install-man install-pdf install-pdf-am \ + install-ps install-ps-am install-strip installcheck \ + installcheck-am installdirs maintainer-clean \ + maintainer-clean-generic mostlyclean mostlyclean-compile \ + mostlyclean-generic mostlyclean-libtool pdf pdf-am ps ps-am \ + tags tags-am uninstall uninstall-am + +.PRECIOUS: Makefile + + +@WIN32_TRUE@resources.o: $(top_srcdir)/drawstuff/src/resources.rc $(top_srcdir)/drawstuff/src/resource.h +@WIN32_TRUE@ @WINDRES@ $(top_srcdir)/drawstuff/src/resources.rc -o resources.o + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/libs/ode-0.16.1/ode/demo/basket_geom.h b/libs/ode-0.16.1/ode/demo/basket_geom.h new file mode 100644 index 0000000..ec88327 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/basket_geom.h @@ -0,0 +1,599 @@ + +static float world_normals[] = { + 0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,-0,0,1,0,0,1,0,0,1,0,0,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,-0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,1,0,-0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,0,-0.948064f,0.318080f,0,-0.989482f,0.144655f,0,-0.983494f,0.180939f,0,-0.983494f,0.180939f,0,-0.908999f,0.416798f,0,-0.948064f,0.318080f,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-1,0,-0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,-0.132460f,0.991188f,0,0.264920f,0.964270f,0,0.132460f,0.991188f,0,0.132460f,0.991188f,0,-0.264920f,0.964270f,0,-0.132460f,0.991188f,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,-0.687592f,-0.726097f,-0,-0.881727f,-0.471761f,0,-0.687592f,-0.726097f,-0,-0.881727f,-0.471761f,0,-0.881727f,-0.471761f,0,-0.687592f,-0.726097f,-0,0.687592f,-0.726097f,0,0.928375f,-0.371644f,0,0.824321f,-0.566123f,0,0.687592f,-0.726097f,0,0.824321f,-0.566123f,0,0.687592f,-0.726097f,0,-0.881727f,-0.471761f,0,-0.985594f,-0.169128f,0,-0.985594f,-0.169128f,0,-0.985594f,-0.169128f,0,-0.881727f,-0.471761f,0,-0.881727f,-0.471761f,0,0.928375f,-0.371644f,0,0.985594f,-0.169128f,0,0.985594f,-0.169128f,0,0.928375f,-0.371644f,0,0.985594f,-0.169128f,0,0.824321f,-0.566123f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,-0,-0.390313f,0.920682f,0,-0.132460f,0.991188f,0,-0.264920f,0.964270f,0,-0.264920f,0.964270f,0,-0.390313f,0.920682f,0,-0.390313f,0.920682f,0,0.390313f,0.920682f,0,0.132460f,0.991188f,0,0.264920f,0.964270f,0,0.390313f,0.920682f,0,0.264920f,0.964270f,0,0.390313f,0.920682f,-0,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0.985594f,0.169128f,0,0.824321f,0.566123f,0,0.928375f,0.371644f,0,0.928375f,0.371644f,0,0.985594f,0.169128f,0,0.985594f,0.169128f,0,0.824321f,0.566123f,0,0.687592f,0.726097f,0,0.687592f,0.726097f,0,0.687592f,0.726097f,0,0.928375f,0.371644f,0,0.824321f,0.566123f,0,0,1,0,-0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-0.687592f,0.726097f,0,-0.687592f,0.726097f,0,-0.881727f,0.471761f,0,-0.881727f,0.471761f,0,-0.881727f,0.471761f,0,-0.687592f,0.726097f,0,-0.881727f,0.471761f,0,-0.985594f,0.169128f,0,-0.985594f,0.169128f,0,-0.985594f,0.169128f,0,-0.881727f,0.471761f,0,-0.881727f,0.471761f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.390314f,-0.920682f,0,-0.132460f,-0.991188f,0,-0.264921f,-0.964270f,0,-0.264921f,-0.964270f,0,-0.390314f,-0.920682f,0,-0.390314f,-0.920682f,0,-0.132460f,-0.991188f,0,0.264921f,-0.964270f,0,0.132460f,-0.991188f,0,0.132460f,-0.991188f,0,-0.264921f,-0.964270f,0,-0.132460f,-0.991188f,0,0.264921f,-0.964270f,0,0.390314f,-0.920682f,0,0.390314f,-0.920682f,0,0.390314f,-0.920682f,0,0.132460f,-0.991188f,0,0.264921f,-0.964270f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-0.527606f,0.849489f,0,-0.793893f,0.608057f,0,-0.715135f,0.698986f,0,-0.715135f,0.698986f,0,-0.418249f,0.908332f,0,-0.527606f,0.849489f,0,-0.075284f,0.997162f,0,-0.253577f,0.967315f,0,-0.202069f,0.979371f,0,-0.202069f,0.979371f,0,-0.075284f,0.997162f,0,-0.075284f,0.997162f,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0.160137f,0.987095f,0,0.049305f,0.998784f,0,0.049305f,0.998784f,0,0.049305f,0.998784f,0,0.221401f,0.975183f,0,0.160137f,0.987095f,0,0.696124f,0.717921f,0,0.696124f,0.717921f,0,0.433340f,0.901230f,0,0.433340f,0.901230f,0,0.433340f,0.901230f,0,0.696124f,0.717921f,0,0.696124f,0.717921f,0,0.696124f,0.717921f,0,0.838308f,0.545197f,0,0.696124f,0.717921f,0,0.872167f,0.489208f,0,0.838308f,0.545197f,0,-0.994126f,0.108225f,0,-0.983494f,0.180939f,0,-0.989482f,0.144655f,0,-0.994126f,0.108225f,0,-0.989482f,0.144655f,0,-0.994126f,0.108225f,0,-0.948064f,0.318080f,0,-0.908999f,0.416798f,0,-0.793893f,0.608057f,0,-0.908999f,0.416798f,0,-0.715135f,0.698986f,0,-0.793893f,0.608057f,0,-0.527606f,0.849489f,0,-0.418249f,0.908332f,0,-0.253577f,0.967315f,0,-0.418249f,0.908332f,0,-0.202069f,0.979371f,0,-0.253577f,0.967315f,0,-0.075284f,0.997162f,0,-0.075284f,0.997162f,0,0,1,0,-0.075284f,0.997162f,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0.049305f,0.998784f,0,0,1,0,0.049305f,0.998784f,0,0.049305f,0.998784f,0,0.160137f,0.987095f,0,0.221401f,0.975183f,0,0.433340f,0.901230f,0,0.221401f,0.975183f,0,0.433340f,0.901230f,0,0.433340f,0.901230f,0,0.902172f,0.431376f,0,0.838308f,0.545197f,0,0.872167f,0.489208f,0,0.872167f,0.489208f,0,0.902172f,0.431376f,0,0.902172f,0.431376f, +}; + +static float world_vertices[] = { + -4,-4,-0.100000f, + 4,-4,-0.100000f, + 4,-4,0.100000f, + -4,-4,0.100000f, + 4,0,0.100000f, + 4,4,-0.100000f, + 4,4,0.100000f, + -4,4,-0.100000f, + 0.066000f,-2.060000f,2, + 0.066000f,-1.940000f,2, + -0.066000f,-2.060000f,2, + -0.066000f,-1.940000f,2, + -4,4,0.100000f, + -4,0,0.100000f, + 0.360000f,3.244444f,1.466974f, + 0.360000f,3.422222f,2.266974f, + -0.360000f,3.422222f,2.266974f, + -0.360000f,3.244444f,1.466974f, + 0.066000f,-2.060000f,0.100000f, + -0.066000f,-2.060000f,0.100000f, + 0.066000f,-1.940000f,0.100000f, + -0.066000f,-1.940000f,0.100000f, + 0.066000f,-1.940000f,1.950000f, + -0.052853f,-1.506390f,2, + 0.052853f,-1.506390f,2, + 0.052853f,-1.506390f,1.950000f, + -0.052853f,-1.506390f,1.950000f, + -0.066000f,-1.940000f,1.950000f, + -0.066000f,-1.840000f,1.950000f, + 0.066000f,-1.840000f,1.950000f, + -0.066000f,-1.840000f,2, + 0.066000f,-1.840000f,2, + -0.171600f,-1.740000f,2, + -0.171600f,-1.740000f,1.950000f, + 0.171600f,-1.740000f,1.950000f, + 0.171600f,-1.740000f,2, + -0.188760f,-1.640000f,2, + -0.188760f,-1.640000f,1.950000f, + 0.188760f,-1.640000f,1.950000f, + 0.188760f,-1.640000f,2, + -0.132132f,-1.540000f,2, + -0.132132f,-1.540000f,1.950000f, + 0.132132f,-1.540000f,1.950000f, + 0.132132f,-1.540000f,2, + 0.173397f,-1.642679f,1.950000f, + 0.121808f,-1.551577f,1.950000f, + 0.157950f,-1.732697f,1.950000f, + 0.060149f,-1.825311f,1.950000f, + -0.060149f,-1.825311f,1.950000f, + -0.157950f,-1.732697f,1.950000f, + -0.173397f,-1.642679f,1.950000f, + -0.121808f,-1.551577f,1.950000f, + -0.049868f,-1.521079f,1.950000f, + 0.049868f,-1.521079f,1.950000f, + -0.173397f,-1.642679f,2, + -0.121808f,-1.551577f,2, + -0.157950f,-1.732697f,2, + -0.060149f,-1.825311f,2, + 0.060149f,-1.825311f,2, + 0.157950f,-1.732697f,2, + 0.173397f,-1.642679f,2, + 0.121808f,-1.551577f,2, + 0.049868f,-1.521079f,2, + -0.049868f,-1.521079f,2, + -0.360000f,3.600000f,0.100000f, + 0.360000f,3.600000f,0.100000f, + -0.360000f,0.400000f,0.100000f, + 0.360000f,0.400000f,0.100000f, + 0.360000f,2.888889f,1.023752f, + 0.360000f,3.066667f,1.166974f, + -0.360000f,3.066667f,1.166974f, + -0.360000f,2.888889f,1.023752f, + 0.360000f,2.533333f,0.939976f, + 0.360000f,2.711111f,0.966974f, + -0.360000f,2.711111f,0.966974f, + -0.360000f,2.533333f,0.939976f, + -0.360000f,2.177778f,0.939976f, + 0.360000f,2.177778f,0.939976f, + 0.360000f,2.355556f,0.939976f, + -0.360000f,2.355556f,0.939976f, + -0.360000f,1.822222f,0.939976f, + 0.360000f,1.822222f,0.939976f, + 0.360000f,2,0.939976f, + -0.360000f,2,0.939976f, + -0.360000f,1.466667f,0.939976f, + 0.360000f,1.466667f,0.939976f, + 0.360000f,1.644444f,0.939976f, + -0.360000f,1.644444f,0.939976f, + 0.360000f,1.111111f,0.957571f, + 0.360000f,1.288889f,0.939976f, + -0.360000f,1.288889f,0.939976f, + -0.360000f,1.111111f,0.957571f, + -0.360000f,0.755556f,1.134246f, + 0.360000f,0.755556f,1.134246f, + 0.360000f,0.933333f,1.009739f, + -0.360000f,0.933333f,1.009739f, + 0.360000f,0.577778f,1.372130f, + -0.360000f,0.577778f,1.372130f, + -0.360000f,3.600000f,3.900000f, + 0.360000f,3.600000f,3.900000f, + 0.360000f,0.400000f,1.743932f, + -0.360000f,0.400000f,1.743932f, +}; + +static dTriIndex world_indices[] = { + 0, + 1, + 2, + 0, + 2, + 3, + 4, + 1, + 5, + 4, + 5, + 6, + 4, + 2, + 1, + 0, + 7, + 5, + 0, + 5, + 1, + 8, + 9, + 10, + 9, + 11, + 10, + 12, + 6, + 5, + 5, + 7, + 12, + 3, + 13, + 0, + 13, + 12, + 7, + 13, + 7, + 0, + 14, + 15, + 16, + 16, + 17, + 14, + 2, + 18, + 19, + 19, + 3, + 2, + 4, + 20, + 2, + 20, + 18, + 2, + 21, + 20, + 4, + 4, + 13, + 21, + 19, + 21, + 13, + 13, + 3, + 19, + 8, + 10, + 19, + 19, + 18, + 8, + 22, + 9, + 8, + 8, + 18, + 22, + 18, + 20, + 22, + 23, + 24, + 25, + 25, + 26, + 23, + 19, + 10, + 27, + 19, + 27, + 21, + 10, + 11, + 27, + 21, + 27, + 22, + 21, + 22, + 20, + 27, + 28, + 22, + 28, + 29, + 22, + 11, + 30, + 28, + 28, + 27, + 11, + 9, + 31, + 11, + 31, + 30, + 11, + 22, + 29, + 31, + 22, + 31, + 9, + 30, + 32, + 28, + 32, + 33, + 28, + 29, + 34, + 35, + 29, + 35, + 31, + 32, + 36, + 37, + 37, + 33, + 32, + 34, + 38, + 39, + 34, + 39, + 35, + 36, + 40, + 41, + 41, + 37, + 36, + 38, + 42, + 43, + 38, + 43, + 39, + 40, + 23, + 26, + 26, + 41, + 40, + 42, + 25, + 24, + 42, + 24, + 43, + 38, + 44, + 45, + 45, + 42, + 38, + 34, + 46, + 44, + 34, + 44, + 38, + 34, + 29, + 47, + 34, + 47, + 46, + 28, + 48, + 29, + 48, + 47, + 29, + 33, + 49, + 48, + 33, + 48, + 28, + 50, + 49, + 33, + 33, + 37, + 50, + 51, + 50, + 37, + 37, + 41, + 51, + 26, + 52, + 51, + 26, + 51, + 41, + 53, + 52, + 26, + 26, + 25, + 53, + 25, + 42, + 45, + 25, + 45, + 53, + 36, + 54, + 55, + 55, + 40, + 36, + 32, + 56, + 54, + 54, + 36, + 32, + 30, + 57, + 32, + 57, + 56, + 32, + 31, + 58, + 30, + 58, + 57, + 30, + 35, + 59, + 58, + 35, + 58, + 31, + 60, + 59, + 35, + 35, + 39, + 60, + 61, + 60, + 39, + 39, + 43, + 61, + 24, + 62, + 61, + 24, + 61, + 43, + 63, + 62, + 24, + 24, + 23, + 63, + 55, + 63, + 23, + 23, + 40, + 55, + 54, + 56, + 49, + 49, + 50, + 54, + 56, + 57, + 48, + 48, + 49, + 56, + 57, + 58, + 47, + 47, + 48, + 57, + 47, + 58, + 59, + 59, + 46, + 47, + 59, + 60, + 44, + 44, + 46, + 59, + 60, + 61, + 45, + 45, + 44, + 60, + 61, + 62, + 53, + 53, + 45, + 61, + 62, + 63, + 52, + 52, + 53, + 62, + 63, + 55, + 51, + 51, + 52, + 63, + 55, + 54, + 50, + 50, + 51, + 55, + 64, + 65, + 6, + 6, + 12, + 64, + 66, + 64, + 12, + 12, + 13, + 66, + 4, + 67, + 66, + 66, + 13, + 4, + 6, + 65, + 4, + 65, + 67, + 4, + 68, + 69, + 70, + 70, + 71, + 68, + 72, + 73, + 74, + 74, + 75, + 72, + 76, + 77, + 78, + 78, + 79, + 76, + 80, + 81, + 82, + 82, + 83, + 80, + 84, + 85, + 86, + 86, + 87, + 84, + 88, + 89, + 90, + 90, + 91, + 88, + 92, + 93, + 94, + 94, + 95, + 92, + 93, + 92, + 96, + 92, + 97, + 96, + 98, + 16, + 15, + 98, + 15, + 99, + 14, + 17, + 69, + 17, + 70, + 69, + 68, + 71, + 73, + 71, + 74, + 73, + 72, + 75, + 79, + 72, + 79, + 78, + 77, + 76, + 83, + 77, + 83, + 82, + 81, + 80, + 87, + 81, + 87, + 86, + 85, + 84, + 90, + 85, + 90, + 89, + 88, + 91, + 94, + 91, + 95, + 94, + 100, + 96, + 97, + 97, + 101, + 100, +}; + diff --git a/libs/ode-0.16.1/ode/demo/bunny_geom.h b/libs/ode-0.16.1/ode/demo/bunny_geom.h new file mode 100644 index 0000000..78f8eb0 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/bunny_geom.h @@ -0,0 +1,1366 @@ +// Bunny mesh ripped from Opcode +const int VertexCount = 453; +const int IndexCount = 902 * 3; + + +float Vertices[VertexCount * 3] = { + -0.334392f, 0.133007f, 0.062259f, + -0.350189f, 0.150354f, -0.147769f, + -0.234201f, 0.343811f, -0.174307f, + -0.200259f, 0.285207f, 0.093749f, + 0.003520f, 0.475208f, -0.159365f, + 0.001856f, 0.419203f, 0.098582f, + -0.252802f, 0.093666f, 0.237538f, + -0.162901f, 0.237984f, 0.206905f, + 0.000865f, 0.318141f, 0.235370f, + -0.414624f, 0.164083f, -0.278254f, + -0.262213f, 0.357334f, -0.293246f, + 0.004628f, 0.482694f, -0.338626f, + -0.402162f, 0.133528f, -0.443247f, + -0.243781f, 0.324275f, -0.436763f, + 0.005293f, 0.437592f, -0.458332f, + -0.339884f, -0.041150f, -0.668211f, + -0.248382f, 0.255825f, -0.627493f, + 0.006261f, 0.376103f, -0.631506f, + -0.216201f, -0.126776f, -0.886936f, + -0.171075f, 0.011544f, -0.881386f, + -0.181074f, 0.098223f, -0.814779f, + -0.119891f, 0.218786f, -0.760153f, + -0.078895f, 0.276780f, -0.739281f, + 0.006801f, 0.310959f, -0.735661f, + -0.168842f, 0.102387f, -0.920381f, + -0.104072f, 0.177278f, -0.952530f, + -0.129704f, 0.211848f, -0.836678f, + -0.099875f, 0.310931f, -0.799381f, + 0.007237f, 0.361687f, -0.794439f, + -0.077913f, 0.258753f, -0.921640f, + 0.007957f, 0.282241f, -0.931680f, + -0.252222f, -0.550401f, -0.557810f, + -0.267633f, -0.603419f, -0.655209f, + -0.446838f, -0.118517f, -0.466159f, + -0.459488f, -0.093017f, -0.311341f, + -0.370645f, -0.100108f, -0.159454f, + -0.371984f, -0.091991f, -0.011044f, + -0.328945f, -0.098269f, 0.088659f, + -0.282452f, -0.018862f, 0.311501f, + -0.352403f, -0.131341f, 0.144902f, + -0.364126f, -0.200299f, 0.202388f, + -0.283965f, -0.231869f, 0.023668f, + -0.298943f, -0.155218f, 0.369716f, + -0.293787f, -0.121856f, 0.419097f, + -0.290163f, -0.290797f, 0.107824f, + -0.264165f, -0.272849f, 0.036347f, + -0.228567f, -0.372573f, 0.290309f, + -0.190431f, -0.286997f, 0.421917f, + -0.191039f, -0.240973f, 0.507118f, + -0.287272f, -0.276431f, -0.065444f, + -0.295675f, -0.280818f, -0.174200f, + -0.399537f, -0.313131f, -0.376167f, + -0.392666f, -0.488581f, -0.427494f, + -0.331669f, -0.570185f, -0.466054f, + -0.282290f, -0.618140f, -0.589220f, + -0.374238f, -0.594882f, -0.323298f, + -0.381071f, -0.629723f, -0.350777f, + -0.382112f, -0.624060f, -0.221577f, + -0.272701f, -0.566522f, 0.259157f, + -0.256702f, -0.663406f, 0.286079f, + -0.280948f, -0.428359f, 0.055790f, + -0.184974f, -0.508894f, 0.326265f, + -0.279971f, -0.526918f, 0.395319f, + -0.282599f, -0.663393f, 0.412411f, + -0.188329f, -0.475093f, 0.417954f, + -0.263384f, -0.663396f, 0.466604f, + -0.209063f, -0.663393f, 0.509344f, + -0.002044f, -0.319624f, 0.553078f, + -0.001266f, -0.371260f, 0.413296f, + -0.219753f, -0.339762f, -0.040921f, + -0.256986f, -0.282511f, -0.006349f, + -0.271706f, -0.260881f, 0.001764f, + -0.091191f, -0.419184f, -0.045912f, + -0.114944f, -0.429752f, -0.124739f, + -0.113970f, -0.382987f, -0.188540f, + -0.243012f, -0.464942f, -0.242850f, + -0.314815f, -0.505402f, -0.324768f, + 0.002774f, -0.437526f, -0.262766f, + -0.072625f, -0.417748f, -0.221440f, + -0.160112f, -0.476932f, -0.293450f, + 0.003859f, -0.453425f, -0.443916f, + -0.120363f, -0.581567f, -0.438689f, + -0.091499f, -0.584191f, -0.294511f, + -0.116469f, -0.599861f, -0.188308f, + -0.208032f, -0.513640f, -0.134649f, + -0.235749f, -0.610017f, -0.040939f, + -0.344916f, -0.622487f, -0.085380f, + -0.336401f, -0.531864f, -0.212298f, + 0.001961f, -0.459550f, -0.135547f, + -0.058296f, -0.430536f, -0.043440f, + 0.001378f, -0.449511f, -0.037762f, + -0.130135f, -0.510222f, 0.079144f, + 0.000142f, -0.477549f, 0.157064f, + -0.114284f, -0.453206f, 0.304397f, + -0.000592f, -0.443558f, 0.285401f, + -0.056215f, -0.663402f, 0.326073f, + -0.026248f, -0.568010f, 0.273318f, + -0.049261f, -0.531064f, 0.389854f, + -0.127096f, -0.663398f, 0.479316f, + -0.058384f, -0.663401f, 0.372891f, + -0.303961f, 0.054199f, 0.625921f, + -0.268594f, 0.193403f, 0.502766f, + -0.277159f, 0.126123f, 0.443289f, + -0.287605f, -0.005722f, 0.531844f, + -0.231396f, -0.121289f, 0.587387f, + -0.253475f, -0.081797f, 0.756541f, + -0.195164f, -0.137969f, 0.728011f, + -0.167673f, -0.156573f, 0.609388f, + -0.145917f, -0.169029f, 0.697600f, + -0.077776f, -0.214247f, 0.622586f, + -0.076873f, -0.214971f, 0.696301f, + -0.002341f, -0.233135f, 0.622859f, + -0.002730f, -0.213526f, 0.691267f, + -0.003136f, -0.192628f, 0.762731f, + -0.056136f, -0.201222f, 0.763806f, + -0.114589f, -0.166192f, 0.770723f, + -0.155145f, -0.129632f, 0.791738f, + -0.183611f, -0.058705f, 0.847012f, + -0.165562f, 0.001980f, 0.833386f, + -0.220084f, 0.019914f, 0.768935f, + -0.255730f, 0.090306f, 0.670782f, + -0.255594f, 0.113833f, 0.663389f, + -0.226380f, 0.212655f, 0.617740f, + -0.003367f, -0.195342f, 0.799680f, + -0.029743f, -0.210508f, 0.827180f, + -0.003818f, -0.194783f, 0.873636f, + -0.004116f, -0.157907f, 0.931268f, + -0.031280f, -0.184555f, 0.889476f, + -0.059885f, -0.184448f, 0.841330f, + -0.135333f, -0.164332f, 0.878200f, + -0.085574f, -0.170948f, 0.925547f, + -0.163833f, -0.094170f, 0.897114f, + -0.138444f, -0.104250f, 0.945975f, + -0.083497f, -0.084934f, 0.979607f, + -0.004433f, -0.146642f, 0.985872f, + -0.150715f, 0.032650f, 0.884111f, + -0.135892f, -0.035520f, 0.945455f, + -0.070612f, 0.036849f, 0.975733f, + -0.004458f, -0.042526f, 1.015670f, + -0.004249f, 0.046042f, 1.003240f, + -0.086969f, 0.133224f, 0.947633f, + -0.003873f, 0.161605f, 0.970499f, + -0.125544f, 0.140012f, 0.917678f, + -0.125651f, 0.250246f, 0.857602f, + -0.003127f, 0.284070f, 0.878870f, + -0.159174f, 0.125726f, 0.888878f, + -0.183807f, 0.196970f, 0.844480f, + -0.159890f, 0.291736f, 0.732480f, + -0.199495f, 0.207230f, 0.779864f, + -0.206182f, 0.164608f, 0.693257f, + -0.186315f, 0.160689f, 0.817193f, + -0.192827f, 0.166706f, 0.782271f, + -0.175112f, 0.110008f, 0.860621f, + -0.161022f, 0.057420f, 0.855111f, + -0.172319f, 0.036155f, 0.816189f, + -0.190318f, 0.064083f, 0.760605f, + -0.195072f, 0.129179f, 0.731104f, + -0.203126f, 0.410287f, 0.680536f, + -0.216677f, 0.309274f, 0.642272f, + -0.241515f, 0.311485f, 0.587832f, + -0.002209f, 0.366663f, 0.749413f, + -0.088230f, 0.396265f, 0.678635f, + -0.170147f, 0.109517f, 0.840784f, + -0.160521f, 0.067766f, 0.830650f, + -0.181546f, 0.139805f, 0.812146f, + -0.180495f, 0.148568f, 0.776087f, + -0.180255f, 0.129125f, 0.744192f, + -0.186298f, 0.078308f, 0.769352f, + -0.167622f, 0.060539f, 0.806675f, + -0.189876f, 0.102760f, 0.802582f, + -0.108340f, 0.455446f, 0.657174f, + -0.241585f, 0.527592f, 0.669296f, + -0.265676f, 0.513366f, 0.634594f, + -0.203073f, 0.478550f, 0.581526f, + -0.266772f, 0.642330f, 0.602061f, + -0.216961f, 0.564846f, 0.535435f, + -0.202210f, 0.525495f, 0.475944f, + -0.193888f, 0.467925f, 0.520606f, + -0.265837f, 0.757267f, 0.500933f, + -0.240306f, 0.653440f, 0.463215f, + -0.309239f, 0.776868f, 0.304726f, + -0.271009f, 0.683094f, 0.382018f, + -0.312111f, 0.671099f, 0.286687f, + -0.268791f, 0.624342f, 0.377231f, + -0.302457f, 0.533996f, 0.360289f, + -0.263656f, 0.529310f, 0.412564f, + -0.282311f, 0.415167f, 0.447666f, + -0.239201f, 0.442096f, 0.495604f, + -0.220043f, 0.569026f, 0.445877f, + -0.001263f, 0.395631f, 0.602029f, + -0.057345f, 0.442535f, 0.572224f, + -0.088927f, 0.506333f, 0.529106f, + -0.125738f, 0.535076f, 0.612913f, + -0.126251f, 0.577170f, 0.483159f, + -0.149594f, 0.611520f, 0.557731f, + -0.163188f, 0.660791f, 0.491080f, + -0.172482f, 0.663387f, 0.415416f, + -0.160464f, 0.591710f, 0.370659f, + -0.156445f, 0.536396f, 0.378302f, + -0.136496f, 0.444358f, 0.425226f, + -0.095564f, 0.373768f, 0.473659f, + -0.104146f, 0.315912f, 0.498104f, + -0.000496f, 0.384194f, 0.473817f, + -0.000183f, 0.297770f, 0.401486f, + -0.129042f, 0.270145f, 0.434495f, + 0.000100f, 0.272963f, 0.349138f, + -0.113060f, 0.236984f, 0.385554f, + 0.007260f, 0.016311f, -0.883396f, + 0.007865f, 0.122104f, -0.956137f, + -0.032842f, 0.115282f, -0.953252f, + -0.089115f, 0.108449f, -0.950317f, + -0.047440f, 0.014729f, -0.882756f, + -0.104458f, 0.013137f, -0.882070f, + -0.086439f, -0.584866f, -0.608343f, + -0.115026f, -0.662605f, -0.436732f, + -0.071683f, -0.665372f, -0.606385f, + -0.257884f, -0.665381f, -0.658052f, + -0.272542f, -0.665381f, -0.592063f, + -0.371322f, -0.665382f, -0.353620f, + -0.372362f, -0.665381f, -0.224420f, + -0.335166f, -0.665380f, -0.078623f, + -0.225999f, -0.665375f, -0.038981f, + -0.106719f, -0.665374f, -0.186351f, + -0.081749f, -0.665372f, -0.292554f, + 0.006943f, -0.091505f, -0.858354f, + 0.006117f, -0.280985f, -0.769967f, + 0.004495f, -0.502360f, -0.559799f, + -0.198638f, -0.302135f, -0.845816f, + -0.237395f, -0.542544f, -0.587188f, + -0.270001f, -0.279489f, -0.669861f, + -0.134547f, -0.119852f, -0.959004f, + -0.052088f, -0.122463f, -0.944549f, + -0.124463f, -0.293508f, -0.899566f, + -0.047616f, -0.289643f, -0.879292f, + -0.168595f, -0.529132f, -0.654931f, + -0.099793f, -0.515719f, -0.645873f, + -0.186168f, -0.605282f, -0.724690f, + -0.112970f, -0.583097f, -0.707469f, + -0.108152f, -0.665375f, -0.700408f, + -0.183019f, -0.665378f, -0.717630f, + -0.349529f, -0.334459f, -0.511985f, + -0.141182f, -0.437705f, -0.798194f, + -0.212670f, -0.448725f, -0.737447f, + -0.261111f, -0.414945f, -0.613835f, + -0.077364f, -0.431480f, -0.778113f, + 0.005174f, -0.425277f, -0.651592f, + 0.089236f, -0.431732f, -0.777093f, + 0.271006f, -0.415749f, -0.610577f, + 0.223981f, -0.449384f, -0.734774f, + 0.153275f, -0.438150f, -0.796391f, + 0.358414f, -0.335529f, -0.507649f, + 0.193434f, -0.665946f, -0.715325f, + 0.118363f, -0.665717f, -0.699021f, + 0.123515f, -0.583454f, -0.706020f, + 0.196851f, -0.605860f, -0.722345f, + 0.109788f, -0.516035f, -0.644590f, + 0.178656f, -0.529656f, -0.652804f, + 0.061157f, -0.289807f, -0.878626f, + 0.138234f, -0.293905f, -0.897958f, + 0.066933f, -0.122643f, -0.943820f, + 0.149571f, -0.120281f, -0.957264f, + 0.280989f, -0.280321f, -0.666487f, + 0.246581f, -0.543275f, -0.584224f, + 0.211720f, -0.302754f, -0.843303f, + 0.086966f, -0.665627f, -0.291520f, + 0.110634f, -0.665702f, -0.185021f, + 0.228099f, -0.666061f, -0.036201f, + 0.337743f, -0.666396f, -0.074503f, + 0.376722f, -0.666513f, -0.219833f, + 0.377265f, -0.666513f, -0.349036f, + 0.281411f, -0.666217f, -0.588670f, + 0.267564f, -0.666174f, -0.654834f, + 0.080745f, -0.665602f, -0.605452f, + 0.122016f, -0.662963f, -0.435280f, + 0.095767f, -0.585141f, -0.607228f, + 0.118944f, 0.012799f, -0.880702f, + 0.061944f, 0.014564f, -0.882086f, + 0.104725f, 0.108156f, -0.949130f, + 0.048513f, 0.115159f, -0.952753f, + 0.112696f, 0.236643f, 0.386937f, + 0.128177f, 0.269757f, 0.436071f, + 0.102643f, 0.315600f, 0.499370f, + 0.094535f, 0.373481f, 0.474824f, + 0.136270f, 0.443946f, 0.426895f, + 0.157071f, 0.535923f, 0.380222f, + 0.161350f, 0.591224f, 0.372630f, + 0.173035f, 0.662865f, 0.417531f, + 0.162808f, 0.660299f, 0.493077f, + 0.148250f, 0.611070f, 0.559555f, + 0.125719f, 0.576790f, 0.484702f, + 0.123489f, 0.534699f, 0.614440f, + 0.087621f, 0.506066f, 0.530188f, + 0.055321f, 0.442365f, 0.572915f, + 0.219936f, 0.568361f, 0.448571f, + 0.238099f, 0.441375f, 0.498528f, + 0.281711f, 0.414315f, 0.451121f, + 0.263833f, 0.528513f, 0.415794f, + 0.303284f, 0.533081f, 0.363998f, + 0.269687f, 0.623528f, 0.380528f, + 0.314255f, 0.670153f, 0.290524f, + 0.272023f, 0.682273f, 0.385343f, + 0.311480f, 0.775931f, 0.308527f, + 0.240239f, 0.652714f, 0.466159f, + 0.265619f, 0.756464f, 0.504187f, + 0.192562f, 0.467341f, 0.522972f, + 0.201605f, 0.524885f, 0.478417f, + 0.215743f, 0.564193f, 0.538084f, + 0.264969f, 0.641527f, 0.605317f, + 0.201031f, 0.477940f, 0.584002f, + 0.263086f, 0.512567f, 0.637832f, + 0.238615f, 0.526867f, 0.672237f, + 0.105309f, 0.455123f, 0.658482f, + 0.183993f, 0.102195f, 0.804872f, + 0.161563f, 0.060042f, 0.808692f, + 0.180748f, 0.077754f, 0.771600f, + 0.175168f, 0.128588f, 0.746368f, + 0.175075f, 0.148030f, 0.778264f, + 0.175658f, 0.139265f, 0.814333f, + 0.154191f, 0.067291f, 0.832578f, + 0.163818f, 0.109013f, 0.842830f, + 0.084760f, 0.396004f, 0.679695f, + 0.238888f, 0.310760f, 0.590775f, + 0.213380f, 0.308625f, 0.644905f, + 0.199666f, 0.409678f, 0.683003f, + 0.190143f, 0.128597f, 0.733463f, + 0.184833f, 0.063516f, 0.762902f, + 0.166070f, 0.035644f, 0.818261f, + 0.154361f, 0.056943f, 0.857042f, + 0.168542f, 0.109489f, 0.862725f, + 0.187387f, 0.166131f, 0.784599f, + 0.180428f, 0.160135f, 0.819438f, + 0.201823f, 0.163991f, 0.695756f, + 0.194206f, 0.206635f, 0.782275f, + 0.155438f, 0.291260f, 0.734412f, + 0.177696f, 0.196424f, 0.846693f, + 0.152305f, 0.125256f, 0.890786f, + 0.119546f, 0.249876f, 0.859104f, + 0.118369f, 0.139643f, 0.919173f, + 0.079410f, 0.132973f, 0.948652f, + 0.062419f, 0.036648f, 0.976547f, + 0.127847f, -0.035919f, 0.947070f, + 0.143624f, 0.032206f, 0.885913f, + 0.074888f, -0.085173f, 0.980577f, + 0.130184f, -0.104656f, 0.947620f, + 0.156201f, -0.094653f, 0.899074f, + 0.077366f, -0.171194f, 0.926545f, + 0.127722f, -0.164729f, 0.879810f, + 0.052670f, -0.184618f, 0.842019f, + 0.023477f, -0.184638f, 0.889811f, + 0.022626f, -0.210587f, 0.827500f, + 0.223089f, 0.211976f, 0.620493f, + 0.251444f, 0.113067f, 0.666494f, + 0.251419f, 0.089540f, 0.673887f, + 0.214360f, 0.019258f, 0.771595f, + 0.158999f, 0.001490f, 0.835374f, + 0.176696f, -0.059249f, 0.849218f, + 0.148696f, -0.130091f, 0.793599f, + 0.108290f, -0.166528f, 0.772088f, + 0.049820f, -0.201382f, 0.764454f, + 0.071341f, -0.215195f, 0.697209f, + 0.073148f, -0.214475f, 0.623510f, + 0.140502f, -0.169461f, 0.699354f, + 0.163374f, -0.157073f, 0.611416f, + 0.189466f, -0.138550f, 0.730366f, + 0.247593f, -0.082554f, 0.759610f, + 0.227468f, -0.121982f, 0.590197f, + 0.284702f, -0.006586f, 0.535347f, + 0.275741f, 0.125287f, 0.446676f, + 0.266650f, 0.192594f, 0.506044f, + 0.300086f, 0.053287f, 0.629620f, + 0.055450f, -0.663935f, 0.375065f, + 0.122854f, -0.664138f, 0.482323f, + 0.046520f, -0.531571f, 0.391918f, + 0.024824f, -0.568450f, 0.275106f, + 0.053855f, -0.663931f, 0.328224f, + 0.112829f, -0.453549f, 0.305788f, + 0.131265f, -0.510617f, 0.080746f, + 0.061174f, -0.430716f, -0.042710f, + 0.341019f, -0.532887f, -0.208150f, + 0.347705f, -0.623533f, -0.081139f, + 0.238040f, -0.610732f, -0.038037f, + 0.211764f, -0.514274f, -0.132078f, + 0.120605f, -0.600219f, -0.186856f, + 0.096985f, -0.584476f, -0.293357f, + 0.127621f, -0.581941f, -0.437170f, + 0.165902f, -0.477425f, -0.291453f, + 0.077720f, -0.417975f, -0.220519f, + 0.320892f, -0.506363f, -0.320874f, + 0.248214f, -0.465684f, -0.239842f, + 0.118764f, -0.383338f, -0.187114f, + 0.118816f, -0.430106f, -0.123307f, + 0.094131f, -0.419464f, -0.044777f, + 0.274526f, -0.261706f, 0.005110f, + 0.259842f, -0.283292f, -0.003185f, + 0.222861f, -0.340431f, -0.038210f, + 0.204445f, -0.664380f, 0.513353f, + 0.259286f, -0.664547f, 0.471281f, + 0.185402f, -0.476020f, 0.421718f, + 0.279163f, -0.664604f, 0.417328f, + 0.277157f, -0.528122f, 0.400208f, + 0.183069f, -0.509812f, 0.329995f, + 0.282599f, -0.429210f, 0.059242f, + 0.254816f, -0.664541f, 0.290687f, + 0.271436f, -0.567707f, 0.263966f, + 0.386561f, -0.625221f, -0.216870f, + 0.387086f, -0.630883f, -0.346073f, + 0.380021f, -0.596021f, -0.318679f, + 0.291269f, -0.619007f, -0.585707f, + 0.339280f, -0.571198f, -0.461946f, + 0.400045f, -0.489778f, -0.422640f, + 0.406817f, -0.314349f, -0.371230f, + 0.300588f, -0.281718f, -0.170549f, + 0.290866f, -0.277304f, -0.061905f, + 0.187735f, -0.241545f, 0.509437f, + 0.188032f, -0.287569f, 0.424234f, + 0.227520f, -0.373262f, 0.293102f, + 0.266526f, -0.273650f, 0.039597f, + 0.291592f, -0.291676f, 0.111386f, + 0.291914f, -0.122741f, 0.422683f, + 0.297574f, -0.156119f, 0.373368f, + 0.286603f, -0.232731f, 0.027162f, + 0.364663f, -0.201399f, 0.206850f, + 0.353855f, -0.132408f, 0.149228f, + 0.282208f, -0.019715f, 0.314960f, + 0.331187f, -0.099266f, 0.092701f, + 0.375463f, -0.093120f, -0.006467f, + 0.375917f, -0.101236f, -0.154882f, + 0.466635f, -0.094416f, -0.305669f, + 0.455805f, -0.119881f, -0.460632f, + 0.277465f, -0.604242f, -0.651871f, + 0.261022f, -0.551176f, -0.554667f, + 0.093627f, 0.258494f, -0.920589f, + 0.114248f, 0.310608f, -0.798070f, + 0.144232f, 0.211434f, -0.835001f, + 0.119916f, 0.176940f, -0.951159f, + 0.184061f, 0.101854f, -0.918220f, + 0.092431f, 0.276521f, -0.738231f, + 0.133504f, 0.218403f, -0.758602f, + 0.194987f, 0.097655f, -0.812476f, + 0.185542f, 0.011005f, -0.879202f, + 0.230315f, -0.127450f, -0.884202f, + 0.260471f, 0.255056f, -0.624378f, + 0.351567f, -0.042194f, -0.663976f, + 0.253742f, 0.323524f, -0.433716f, + 0.411612f, 0.132299f, -0.438264f, + 0.270513f, 0.356530f, -0.289984f, + 0.422146f, 0.162819f, -0.273130f, + 0.164724f, 0.237490f, 0.208912f, + 0.253806f, 0.092900f, 0.240640f, + 0.203608f, 0.284597f, 0.096223f, + 0.241006f, 0.343093f, -0.171396f, + 0.356076f, 0.149288f, -0.143443f, + 0.337656f, 0.131992f, 0.066374f +}; + +dTriIndex Indices[IndexCount / 3][3] = { + {126,134,133}, + {342,138,134}, + {133,134,138}, + {126,342,134}, + {312,316,317}, + {169,163,162}, + {312,317,319}, + {312,319,318}, + {169,162,164}, + {169,168,163}, + {312,314,315}, + {169,164,165}, + {169,167,168}, + {312,315,316}, + {312,313,314}, + {169,165,166}, + {169,166,167}, + {312,318,313}, + {308,304,305}, + {308,305,306}, + {179,181,188}, + {177,173,175}, + {177,175,176}, + {302,293,300}, + {322,294,304}, + {188,176,175}, + {188,175,179}, + {158,177,187}, + {305,293,302}, + {305,302,306}, + {322,304,308}, + {188,181,183}, + {158,173,177}, + {293,298,300}, + {304,294,296}, + {304,296,305}, + {185,176,188}, + {185,188,183}, + {187,177,176}, + {187,176,185}, + {305,296,298}, + {305,298,293}, + {436,432, 28}, + {436, 28, 23}, + {434,278,431}, + { 30,208,209}, + { 30,209, 29}, + { 19, 20, 24}, + {208,207,211}, + {208,211,209}, + { 19,210,212}, + {433,434,431}, + {433,431,432}, + {433,432,436}, + {436,437,433}, + {277,275,276}, + {277,276,278}, + {209,210, 25}, + { 21, 26, 24}, + { 21, 24, 20}, + { 25, 26, 27}, + { 25, 27, 29}, + {435,439,277}, + {439,275,277}, + {432,431, 30}, + {432, 30, 28}, + {433,437,438}, + {433,438,435}, + {434,277,278}, + { 24, 25,210}, + { 24, 26, 25}, + { 29, 27, 28}, + { 29, 28, 30}, + { 19, 24,210}, + {208, 30,431}, + {208,431,278}, + {435,434,433}, + {435,277,434}, + { 25, 29,209}, + { 27, 22, 23}, + { 27, 23, 28}, + { 26, 22, 27}, + { 26, 21, 22}, + {212,210,209}, + {212,209,211}, + {207,208,278}, + {207,278,276}, + {439,435,438}, + { 12, 9, 10}, + { 12, 10, 13}, + { 2, 3, 5}, + { 2, 5, 4}, + { 16, 13, 14}, + { 16, 14, 17}, + { 22, 21, 16}, + { 13, 10, 11}, + { 13, 11, 14}, + { 1, 0, 3}, + { 1, 3, 2}, + { 15, 12, 16}, + { 19, 18, 15}, + { 19, 15, 16}, + { 19, 16, 20}, + { 9, 1, 2}, + { 9, 2, 10}, + { 3, 7, 8}, + { 3, 8, 5}, + { 16, 17, 23}, + { 16, 23, 22}, + { 21, 20, 16}, + { 10, 2, 4}, + { 10, 4, 11}, + { 0, 6, 7}, + { 0, 7, 3}, + { 12, 13, 16}, + {451,446,445}, + {451,445,450}, + {442,440,439}, + {442,439,438}, + {442,438,441}, + {421,420,422}, + {412,411,426}, + {412,426,425}, + {408,405,407}, + {413, 67, 68}, + {413, 68,414}, + {391,390,412}, + { 80,384,386}, + {404,406,378}, + {390,391,377}, + {390,377, 88}, + {400,415,375}, + {398,396,395}, + {398,395,371}, + {398,371,370}, + {112,359,358}, + {112,358,113}, + {351,352,369}, + {125,349,348}, + {345,343,342}, + {342,340,339}, + {341,335,337}, + {328,341,327}, + {331,323,333}, + {331,322,323}, + {327,318,319}, + {327,319,328}, + {315,314,324}, + {302,300,301}, + {302,301,303}, + {320,311,292}, + {285,284,289}, + {310,307,288}, + {310,288,290}, + {321,350,281}, + {321,281,282}, + {423,448,367}, + {272,273,384}, + {272,384,274}, + {264,265,382}, + {264,382,383}, + {440,442,261}, + {440,261,263}, + {252,253,254}, + {252,254,251}, + {262,256,249}, + {262,249,248}, + {228,243,242}, + {228, 31,243}, + {213,215,238}, + {213,238,237}, + { 19,212,230}, + {224,225,233}, + {224,233,231}, + {217,218, 56}, + {217, 56, 54}, + {217,216,239}, + {217,239,238}, + {217,238,215}, + {218,217,215}, + {218,215,214}, + { 6,102,206}, + {186,199,200}, + {197,182,180}, + {170,171,157}, + {201,200,189}, + {170,190,191}, + {170,191,192}, + {175,174,178}, + {175,178,179}, + {168,167,155}, + {122,149,158}, + {122,158,159}, + {135,153,154}, + {135,154,118}, + {143,140,141}, + {143,141,144}, + {132,133,136}, + {130,126,133}, + {124,125,127}, + {122,101,100}, + {122,100,121}, + {110,108,107}, + {110,107,109}, + { 98, 99, 97}, + { 98, 97, 64}, + { 98, 64, 66}, + { 87, 55, 57}, + { 83, 82, 79}, + { 83, 79, 84}, + { 78, 74, 50}, + { 49, 71, 41}, + { 49, 41, 37}, + { 49, 37, 36}, + { 58, 44, 60}, + { 60, 59, 58}, + { 51, 34, 33}, + { 39, 40, 42}, + { 39, 42, 38}, + {243,240, 33}, + {243, 33,229}, + { 39, 38, 6}, + { 44, 46, 40}, + { 55, 56, 57}, + { 64, 62, 65}, + { 64, 65, 66}, + { 41, 71, 45}, + { 75, 50, 51}, + { 81, 79, 82}, + { 77, 88, 73}, + { 93, 92, 94}, + { 68, 47, 46}, + { 96, 97, 99}, + { 96, 99, 95}, + {110,109,111}, + {111,112,110}, + {114,113,123}, + {114,123,124}, + {132,131,129}, + {133,137,136}, + {135,142,145}, + {145,152,135}, + {149,147,157}, + {157,158,149}, + {164,150,151}, + {153,163,168}, + {153,168,154}, + {185,183,182}, + {185,182,184}, + {161,189,190}, + {200,199,191}, + {200,191,190}, + {180,178,195}, + {180,195,196}, + {102,101,204}, + {102,204,206}, + { 43, 48,104}, + { 43,104,103}, + {216,217, 54}, + {216, 54, 32}, + {207,224,231}, + {230,212,211}, + {230,211,231}, + {227,232,241}, + {227,241,242}, + {235,234,241}, + {235,241,244}, + {430,248,247}, + {272,274,253}, + {272,253,252}, + {439,260,275}, + {225,224,259}, + {225,259,257}, + {269,270,407}, + {269,407,405}, + {270,269,273}, + {270,273,272}, + {273,269,268}, + {273,268,267}, + {273,267,266}, + {273,266,265}, + {273,265,264}, + {448,279,367}, + {281,350,368}, + {285,286,301}, + {290,323,310}, + {290,311,323}, + {282,281,189}, + {292,311,290}, + {292,290,291}, + {307,306,302}, + {307,302,303}, + {316,315,324}, + {316,324,329}, + {331,351,350}, + {330,334,335}, + {330,335,328}, + {341,337,338}, + {344,355,354}, + {346,345,348}, + {346,348,347}, + {364,369,352}, + {364,352,353}, + {365,363,361}, + {365,361,362}, + {376,401,402}, + {373,372,397}, + {373,397,400}, + {376, 92,377}, + {381,378,387}, + {381,387,385}, + {386, 77, 80}, + {390,389,412}, + {416,417,401}, + {403,417,415}, + {408,429,430}, + {419,423,418}, + {427,428,444}, + {427,444,446}, + {437,436,441}, + {450,445, 11}, + {450, 11, 4}, + {447,449, 5}, + {447, 5, 8}, + {441,438,437}, + {425,426,451}, + {425,451,452}, + {417,421,415}, + {408,407,429}, + {399,403,400}, + {399,400,397}, + {394,393,416}, + {389,411,412}, + {386,383,385}, + {408,387,378}, + {408,378,406}, + {377,391,376}, + { 94,375,415}, + {372,373,374}, + {372,374,370}, + {359,111,360}, + {359,112,111}, + {113,358,349}, + {113,349,123}, + {346,343,345}, + {343,340,342}, + {338,336,144}, + {338,144,141}, + {327,341,354}, + {327,354,326}, + {331,350,321}, + {331,321,322}, + {314,313,326}, + {314,326,325}, + {300,298,299}, + {300,299,301}, + {288,287,289}, + {189,292,282}, + {287,288,303}, + {284,285,297}, + {368,280,281}, + {448,447,279}, + {274,226,255}, + {267,268,404}, + {267,404,379}, + {429,262,430}, + {439,440,260}, + {257,258,249}, + {257,249,246}, + {430,262,248}, + {234,228,242}, + {234,242,241}, + {237,238,239}, + {237,239,236}, + { 15, 18,227}, + { 15,227,229}, + {222,223, 82}, + {222, 82, 83}, + {214,215,213}, + {214,213, 81}, + { 38,102, 6}, + {122,159,200}, + {122,200,201}, + {174,171,192}, + {174,192,194}, + {197,193,198}, + {190,170,161}, + {181,179,178}, + {181,178,180}, + {166,156,155}, + {163,153,152}, + {163,152,162}, + {120,156,149}, + {120,149,121}, + {152,153,135}, + {140,143,142}, + {135,131,132}, + {135,132,136}, + {130,129,128}, + {130,128,127}, + {100,105,119}, + {100,119,120}, + {106,104,107}, + {106,107,108}, + { 91, 95, 59}, + { 93, 94, 68}, + { 91, 89, 92}, + { 76, 53, 55}, + { 76, 55, 87}, + { 81, 78, 79}, + { 74, 73, 49}, + { 69, 60, 45}, + { 58, 62, 64}, + { 58, 64, 61}, + { 53, 31, 32}, + { 32, 54, 53}, + { 42, 43, 38}, + { 35, 36, 0}, + { 35, 0, 1}, + { 34, 35, 1}, + { 34, 1, 9}, + { 44, 40, 41}, + { 44, 41, 45}, + { 33,240, 51}, + { 63, 62, 58}, + { 63, 58, 59}, + { 45, 71, 70}, + { 76, 75, 51}, + { 76, 51, 52}, + { 86, 85, 84}, + { 86, 84, 87}, + { 89, 72, 73}, + { 89, 73, 88}, + { 91, 92, 96}, + { 91, 96, 95}, + { 72, 91, 60}, + { 72, 60, 69}, + {104,106,105}, + {119,105,117}, + {119,117,118}, + {124,127,128}, + {117,116,129}, + {117,129,131}, + {118,117,131}, + {135,140,142}, + {146,150,152}, + {146,152,145}, + {149,122,121}, + {166,165,151}, + {166,151,156}, + {158,172,173}, + {161,160,189}, + {199,198,193}, + {199,193,191}, + {204,201,202}, + {178,174,194}, + {200,159,186}, + {109, 48, 67}, + { 48,107,104}, + {216, 32,236}, + {216,236,239}, + {223,214, 81}, + {223, 81, 82}, + { 33, 12, 15}, + { 32,228,234}, + { 32,234,236}, + {240, 31, 52}, + {256,255,246}, + {256,246,249}, + {258,263,248}, + {258,248,249}, + {275,260,259}, + {275,259,276}, + {207,276,259}, + {270,271,429}, + {270,429,407}, + {413,418,366}, + {413,366,365}, + {368,367,279}, + {368,279,280}, + {303,301,286}, + {303,286,287}, + {283,282,292}, + {283,292,291}, + {320,292,189}, + {298,296,297}, + {298,297,299}, + {318,327,326}, + {318,326,313}, + {329,330,317}, + {336,333,320}, + {326,354,353}, + {334,332,333}, + {334,333,336}, + {342,339,139}, + {342,139,138}, + {345,342,126}, + {347,357,356}, + {369,368,351}, + {363,356,357}, + {363,357,361}, + {366,367,368}, + {366,368,369}, + {375,373,400}, + { 92, 90,377}, + {409,387,408}, + {386,385,387}, + {386,387,388}, + {412,394,391}, + {396,398,399}, + {408,406,405}, + {415,421,419}, + {415,419,414}, + {425,452,448}, + {425,448,424}, + {444,441,443}, + {448,452,449}, + {448,449,447}, + {446,444,443}, + {446,443,445}, + {250,247,261}, + {250,261,428}, + {421,422,423}, + {421,423,419}, + {427,410,250}, + {417,403,401}, + {403,402,401}, + {420,392,412}, + {420,412,425}, + {420,425,424}, + {386,411,389}, + {383,382,381}, + {383,381,385}, + {378,379,404}, + {372,371,395}, + {372,395,397}, + {371,372,370}, + {361,359,360}, + {361,360,362}, + {368,350,351}, + {349,347,348}, + {356,355,344}, + {356,344,346}, + {344,341,340}, + {344,340,343}, + {338,337,336}, + {328,335,341}, + {324,352,351}, + {324,351,331}, + {320,144,336}, + {314,325,324}, + {322,308,309}, + {310,309,307}, + {287,286,289}, + {203,280,279}, + {203,279,205}, + {297,295,283}, + {297,283,284}, + {447,205,279}, + {274,384, 80}, + {274, 80,226}, + {266,267,379}, + {266,379,380}, + {225,257,246}, + {225,246,245}, + {256,254,253}, + {256,253,255}, + {430,247,250}, + {226,235,244}, + {226,244,245}, + {232,233,244}, + {232,244,241}, + {230, 18, 19}, + { 32, 31,228}, + {219,220, 86}, + {219, 86, 57}, + {226,213,235}, + {206, 7, 6}, + {122,201,101}, + {201,204,101}, + {180,196,197}, + {170,192,171}, + {200,190,189}, + {194,193,195}, + {183,181,180}, + {183,180,182}, + {155,154,168}, + {149,156,151}, + {149,151,148}, + {155,156,120}, + {145,142,143}, + {145,143,146}, + {136,137,140}, + {133,132,130}, + {128,129,116}, + {100,120,121}, + {110,112,113}, + {110,113,114}, + { 66, 65, 63}, + { 66, 63, 99}, + { 66, 99, 98}, + { 96, 46, 61}, + { 89, 88, 90}, + { 86, 87, 57}, + { 80, 78, 81}, + { 72, 69, 49}, + { 67, 48, 47}, + { 67, 47, 68}, + { 56, 55, 53}, + { 50, 49, 36}, + { 50, 36, 35}, + { 40, 39, 41}, + {242,243,229}, + {242,229,227}, + { 6, 37, 39}, + { 42, 47, 48}, + { 42, 48, 43}, + { 61, 46, 44}, + { 45, 70, 69}, + { 69, 70, 71}, + { 69, 71, 49}, + { 74, 78, 77}, + { 83, 84, 85}, + { 73, 74, 77}, + { 93, 96, 92}, + { 68, 46, 93}, + { 95, 99, 63}, + { 95, 63, 59}, + {115,108,110}, + {115,110,114}, + {125,126,127}, + {129,130,132}, + {137,133,138}, + {137,138,139}, + {148,146,143}, + {148,143,147}, + {119,118,154}, + {161,147,143}, + {165,164,151}, + {158,157,171}, + {158,171,172}, + {159,158,187}, + {159,187,186}, + {194,192,191}, + {194,191,193}, + {189,202,201}, + {182,197,184}, + {205, 8, 7}, + { 48,109,107}, + {218,219, 57}, + {218, 57, 56}, + {207,231,211}, + {232,230,231}, + {232,231,233}, + { 53, 52, 31}, + {388,411,386}, + {409,430,250}, + {262,429,254}, + {262,254,256}, + {442,444,428}, + {273,264,383}, + {273,383,384}, + {429,271,251}, + {429,251,254}, + {413,365,362}, + { 67,413,360}, + {282,283,295}, + {285,301,299}, + {202,281,280}, + {284,283,291}, + {284,291,289}, + {320,189,160}, + {308,306,307}, + {307,309,308}, + {319,317,330}, + {319,330,328}, + {353,352,324}, + {332,331,333}, + {340,341,338}, + {354,341,344}, + {349,358,357}, + {349,357,347}, + {364,355,356}, + {364,356,363}, + {364,365,366}, + {364,366,369}, + {374,376,402}, + {375, 92,373}, + { 77,389,390}, + {382,380,381}, + {389, 77,386}, + {393,394,412}, + {393,412,392}, + {401,394,416}, + {415,400,403}, + {411,410,427}, + {411,427,426}, + {422,420,424}, + {247,248,263}, + {247,263,261}, + {445,443, 14}, + {445, 14, 11}, + {449,450, 4}, + {449, 4, 5}, + {443,441, 17}, + {443, 17, 14}, + {436, 23, 17}, + {436, 17,441}, + {424,448,422}, + {448,423,422}, + {414,419,418}, + {414,418,413}, + {406,404,405}, + {399,397,395}, + {399,395,396}, + {420,416,392}, + {388,410,411}, + {386,384,383}, + {390, 88, 77}, + {375, 94, 92}, + {415,414, 68}, + {415, 68, 94}, + {370,374,402}, + {370,402,398}, + {361,357,358}, + {361,358,359}, + {125,348,126}, + {346,344,343}, + {340,338,339}, + {337,335,334}, + {337,334,336}, + {325,353,324}, + {324,331,332}, + {324,332,329}, + {323,322,309}, + {323,309,310}, + {294,295,297}, + {294,297,296}, + {289,286,285}, + {202,280,203}, + {288,307,303}, + {282,295,321}, + { 67,360,111}, + {418,423,367}, + {418,367,366}, + {272,252,251}, + {272,251,271}, + {272,271,270}, + {255,253,274}, + {265,266,380}, + {265,380,382}, + {442,428,261}, + {440,263,258}, + {440,258,260}, + {409,250,410}, + {255,226,245}, + {255,245,246}, + { 31,240,243}, + {236,234,235}, + {236,235,237}, + {233,225,245}, + {233,245,244}, + {220,221, 85}, + {220, 85, 86}, + { 81,213,226}, + { 81,226, 80}, + { 7,206,205}, + {186,184,198}, + {186,198,199}, + {204,203,205}, + {204,205,206}, + {195,193,196}, + {171,174,172}, + {173,174,175}, + {173,172,174}, + {155,167,166}, + {160,161,143}, + {160,143,144}, + {119,154,155}, + {148,151,150}, + {148,150,146}, + {140,137,139}, + {140,139,141}, + {127,126,130}, + {114,124,128}, + {114,128,115}, + {117,105,106}, + {117,106,116}, + {104,105,100}, + {104,100,103}, + { 59, 60, 91}, + { 97, 96, 61}, + { 97, 61, 64}, + { 91, 72, 89}, + { 87, 84, 79}, + { 87, 79, 76}, + { 78, 80, 77}, + { 49, 50, 74}, + { 60, 44, 45}, + { 61, 44, 58}, + { 51, 50, 35}, + { 51, 35, 34}, + { 39, 37, 41}, + { 33, 34, 9}, + { 33, 9, 12}, + { 0, 36, 37}, + { 0, 37, 6}, + { 40, 46, 47}, + { 40, 47, 42}, + { 53, 54, 56}, + { 65, 62, 63}, + { 72, 49, 73}, + { 79, 78, 75}, + { 79, 75, 76}, + { 52, 53, 76}, + { 92, 89, 90}, + { 96, 93, 46}, + {102,103,100}, + {102,100,101}, + {116,106,108}, + {116,108,115}, + {123,125,124}, + {116,115,128}, + {118,131,135}, + {140,135,136}, + {148,147,149}, + {120,119,155}, + {164,162,152}, + {164,152,150}, + {157,147,161}, + {157,161,170}, + {186,187,185}, + {186,185,184}, + {193,197,196}, + {202,203,204}, + {194,195,178}, + {198,184,197}, + { 67,111,109}, + { 38, 43,103}, + { 38,103,102}, + {214,223,222}, + {214,222,221}, + {214,221,220}, + {214,220,219}, + {214,219,218}, + {213,237,235}, + {221,222, 83}, + {221, 83, 85}, + { 15,229, 33}, + {227, 18,230}, + {227,230,232}, + { 52, 51,240}, + { 75, 78, 50}, + {408,430,409}, + {260,258,257}, + {260,257,259}, + {224,207,259}, + {268,269,405}, + {268,405,404}, + {413,362,360}, + {447, 8,205}, + {299,297,285}, + {189,281,202}, + {290,288,289}, + {290,289,291}, + {322,321,295}, + {322,295,294}, + {333,323,311}, + {333,311,320}, + {317,316,329}, + {320,160,144}, + {353,325,326}, + {329,332,334}, + {329,334,330}, + {339,338,141}, + {339,141,139}, + {348,345,126}, + {347,356,346}, + {123,349,125}, + {364,353,354}, + {364,354,355}, + {365,364,363}, + {376,391,394}, + {376,394,401}, + { 92,376,374}, + { 92,374,373}, + {377, 90, 88}, + {380,379,378}, + {380,378,381}, + {388,387,409}, + {388,409,410}, + {416,393,392}, + {399,398,402}, + {399,402,403}, + {250,428,427}, + {421,417,416}, + {421,416,420}, + {426,427,446}, + {426,446,451}, + {444,442,441}, + {452,451,450}, + {452,450,449} +}; + diff --git a/libs/ode-0.16.1/ode/demo/convex_bunny_geom.h b/libs/ode-0.16.1/ode/demo/convex_bunny_geom.h new file mode 100644 index 0000000..09fe6de --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/convex_bunny_geom.h @@ -0,0 +1,468 @@ +const unsigned int convexBunnyPlaneCount = 176; +dReal convexBunnyPlanes[] = +{ + 0.986167, -0.0612533, -0.154021, 0.399481, + 0.982735, -0.0691036, -0.171628, 0.409884, + -0.984387, -0.0582774, -0.166089, 0.403079, + 0.985044, -0.172279, 0.00302105, 0.437531, + 0.976915, -0.184361, 0.107929, 0.465334, + 0.951478, -0.281619, 0.124019, 0.475223, + 0.798136, -0.502214, 0.332805, 0.535555, + 0.949728, -0.211128, -0.231176, 0.528156, + -0.000894561, -0.995066, -0.0992088, 0.80299, + -0.000896015, -0.995066, -0.0992131, 0.802991, + -0.0035709, -0.935822, 0.352454, 0.618504, + -0.291551, -0.828515, 0.478081, 0.681579, + -0.978715, -0.181408, 0.0959605, 0.468907, + -0.985523, -0.169302, -0.00904739, 0.441129, + -0.953768, -0.278749, 0.11236, 0.478704, + 0.372745, -0.827499, 0.419888, 0.630174, + 0.976911, -0.18316, 0.109991, 0.466086, + 0.817827, -0.387738, 0.425227, 0.569153, + -0.978732, -0.180211, 0.0980152, 0.469656, + 0.662794, -0.0277654, 0.748287, 0.803459, + 0.00359857, -0.660581, -0.750746, 0.877267, + 0.00359952, -0.660579, -0.750748, 0.877266, + -0.947456, -0.208272, -0.242797, 0.531628, + -0.980764, -0.0661375, -0.18365, 0.413468, + -0.940835, -0.0698657, -0.331585, 0.494827, + 0.983751, 0.0529367, -0.171556, 0.403533, + 0.981839, 0.0996302, -0.16145, 0.410144, + 0.977938, 0.0857834, -0.190468, 0.411823, + 0.959636, 0.106068, -0.260476, 0.44898, + 0.85334, -0.0495414, -0.518996, 0.60489, + -0.803667, -0.499793, 0.322995, 0.538478, + -0.689742, -0.615484, 0.381361, 0.574754, + -0.380353, -0.826364, 0.415277, 0.63155, + -0.39985, -0.817283, 0.414933, 0.630682, + -0.380309, -0.826285, 0.415473, 0.631677, + -0.981411, 0.0559147, -0.183592, 0.407121, + -0.979526, 0.101914, -0.173615, 0.413635, + -0.975381, 0.0881975, -0.202119, 0.415257, + 0.988445, 0.140182, 0.0576755, 0.485174, + 0.876515, 0.0408992, 0.479634, 0.620093, + 0.848907, -0.0996824, 0.519057, 0.631268, + 0.895754, -0.195088, 0.399457, 0.563346, + 0.861448, -0.256989, 0.438023, 0.574909, + 0.775672, -0.447481, 0.445076, 0.586422, + 0.683157, -0.617557, 0.389768, 0.572247, + 0.391755, -0.818722, 0.419789, 0.629264, + 0.28317, -0.829384, 0.481599, 0.680529, + 0.3727, -0.827422, 0.420079, 0.630298, + -0.824143, -0.385255, 0.415171, 0.57215, + -0.782413, -0.445128, 0.435536, 0.589267, + 0.00152876, 0.999994, -0.00308275, 0.665646, + 0.00242466, 0.999989, -0.0038994, 0.665879, + -0.979892, 0.121321, -0.158406, 0.420287, + 0.767537, -0.190214, -0.612132, 0.695479, + 0.372649, -0.42747, -0.823652, 0.869878, + 0.537245, -0.335515, -0.77382, 0.82472, + 0.0263648, -0.598975, -0.800334, 0.873623, + 0.00393345, -0.60959, -0.792707, 0.869865, + -0.0183706, -0.598907, -0.800608, 0.873704, + 0.00875728, 0.676014, -0.736836, 0.825597, + 0.852333, -0.0355955, -0.521786, 0.607886, + 0.392036, 0.534934, -0.748434, 0.818042, + 0.847696, -0.122973, -0.516033, 0.615814, + 0.884763, -0.0760716, -0.45979, 0.565893, + 0.9446, -0.0727144, -0.320069, 0.491401, + 0.904971, -0.0675211, -0.420081, 0.541673, + -0.899959, -0.0647928, -0.431134, 0.544968, + -0.955972, 0.108494, -0.272667, 0.452769, + -0.363823, -0.426358, -0.828161, 0.871222, + -0.528689, -0.333936, -0.780368, 0.826685, + 0.982068, 0.118277, -0.146811, 0.416542, + 0.98951, 0.144455, 0.00164104, 0.468616, + 0.50797, -0.0708041, 0.85846, 0.883126, + 0.748614, -0.431275, 0.503565, 0.634026, + 0.214863, -0.405791, 0.888351, 0.94048, + -0.901162, -0.192379, 0.388455, 0.566627, + -0.867521, -0.254376, 0.427435, 0.578065, + -0.226957, -0.405135, 0.885639, 0.941284, + -0.756029, -0.429007, 0.494341, 0.636765, + -0.00629553, -0.188362, 0.982079, 0.968197, + 0.0165684, 0.999846, -0.00581961, 0.670373, + 0.00313267, 0.999987, -0.00405124, 0.666103, + 0.545069, 0.472158, -0.692796, 0.780052, + 0.932011, 0.148856, -0.330451, 0.498417, + 0.844043, 0.227673, -0.485547, 0.599903, + -0.019033, 0.999801, -0.00590978, 0.672252, + -0.959662, 0.239262, -0.147656, 0.488538, + 0.00151234, 0.999999, -0.000399466, 0.665855, + -0.988649, 0.143168, 0.0455675, 0.488784, + -0.989015, 0.147444, -0.01048, 0.472227, + -0.972439, 0.232727, -0.01415, 0.518344, + 0.587681, -0.160147, -0.793085, 0.823999, + 0.640479, -0.269179, -0.719256, 0.778142, + 0.541109, -0.332896, -0.772257, 0.823226, + 0.546185, -0.14771, -0.824538, 0.84854, + 0.528519, -0.026044, -0.848522, 0.873136, + 0.447231, -0.0756684, -0.891212, 0.903953, + 0.490619, -0.0795123, -0.867739, 0.884255, + 0.279393, 0.264257, -0.923097, 0.950045, + 0.374653, 0.39486, -0.83888, 0.886593, + 0.00050174, 0.999994, -0.00331563, 0.665976, + -0.0103777, 0.999934, -0.00487936, 0.669494, + -0.927571, 0.150741, -0.341889, 0.501807, + -0.267268, 0.265084, -0.926444, 0.951043, + -0.845984, -0.0330207, -0.532184, 0.610986, + -0.518165, -0.0244575, -0.854931, 0.875047, + -0.83753, 0.228953, -0.496108, 0.603116, + -0.535666, 0.472122, -0.700116, 0.78259, + -0.381083, 0.534649, -0.754272, 0.820328, + -0.363157, 0.395975, -0.843398, 0.88794, + -0.326829, -0.256634, -0.909572, 0.922946, + 0.394875, -0.128601, -0.90969, 0.920236, + 0.337169, -0.257642, -0.905504, 0.921733, + 0.398433, -0.193767, -0.896496, 0.910015, + -0.536477, -0.146072, -0.831177, 0.850523, + -0.436512, -0.0743406, -0.896622, 0.905564, + -0.480187, -0.0780522, -0.873687, 0.886029, + -0.384093, -0.127432, -0.914458, 0.921656, + -0.388009, -0.192572, -0.901313, 0.911451, + 0.977045, 0.15796, 0.14294, 0.521934, + 0.930035, 0.231515, 0.28537, 0.600301, + -0.855499, -0.0971121, 0.508616, 0.634376, + -0.875419, 0.136849, 0.463589, 0.62897, + -0.882196, 0.0435387, 0.468864, 0.623303, + 0.0204398, -0.0238739, 0.999506, 0.958419, + -0.0062197, -0.0777937, 0.99695, 0.962769, + 0.907123, 0.250746, 0.338015, 0.623205, + 0.902358, 0.173321, 0.394601, 0.607696, + 0.870085, 0.134211, 0.474278, 0.625782, + 0.0015108, 0.999999, 6.34978e-06, 0.665945, + 0.00150567, 0.999999, 0.000568537, 0.666143, + 0.00150738, 0.999999, 0.000565012, 0.666141, + -0.963954, 0.266039, -0.00405118, 0.539571, + 0.0015136, 0.999999, -0.000393102, 0.665856, + 0.00151117, 0.999999, 4.32104e-06, 0.665944, + 0.0272711, 0.999604, -0.00696439, 0.673709, + 0.962047, 0.236383, -0.136341, 0.484917, + 0.973236, 0.229796, -0.00223071, 0.514797, + 0.964728, 0.263133, 0.00776342, 0.536054, + -0.906596, 0.176056, 0.383521, 0.610999, + -0.978237, 0.160918, 0.130987, 0.525513, + -0.910434, 0.253486, 0.326887, 0.626522, + -0.932759, 0.234321, 0.27396, 0.603697, + 0.800621, -0.0921333, -0.592045, 0.665278, + 0.679569, -0.15358, -0.717356, 0.765121, + 0.684928, -0.199829, -0.700672, 0.756959, + -0.532604, -0.33128, -0.778837, 0.82519, + -0.578395, -0.158384, -0.800234, 0.826134, + -0.671209, -0.151537, -0.725613, 0.767576, + -0.00530287, 0.323551, 0.946196, 0.94547, + -0.719766, 0.229227, 0.655281, 0.774388, + -0.604194, 0.29171, 0.741522, 0.841564, + -0.544989, 0.302925, 0.781808, 0.866398, + -0.518662, -0.0692529, 0.85217, 0.884999, + -0.671987, -0.0257512, 0.740115, 0.805898, + -0.00613626, -0.00823685, 0.999947, 0.957141, + -0.032747, -0.0237908, 0.99918, 0.958516, + -0.00530611, 0.323546, 0.946198, 0.945471, + -0.545061, 0.302657, 0.781861, 0.866377, + -0.639902, 0.23832, 0.730568, 0.823722, + 0.00149706, 0.999997, 0.00193434, 0.667038, + 0.00149731, 0.999997, 0.00193252, 0.667037, + -0.0048341, 0.44008, 0.897946, 0.936327, + -0.00483143, 0.440078, 0.897947, 0.936327, + -0.632454, -0.267241, -0.727039, 0.780455, + -0.67691, -0.197765, -0.709, 0.759436, + -0.841667, -0.120432, -0.526396, 0.618913, + -0.760706, -0.187792, -0.621337, 0.698143, + -0.87929, -0.0734062, -0.470597, 0.569116, + -0.847068, -0.0469762, -0.529405, 0.607991, + -0.793572, -0.0897399, -0.601823, 0.668201, + 0.536355, 0.301024, 0.788485, 0.864403, + 0.536284, 0.301291, 0.788431, 0.864424, + 0.595945, 0.289897, 0.748872, 0.839373, + 0.631626, 0.236397, 0.738353, 0.8214, + 0.712378, 0.227061, 0.664049, 0.771772, +}; +const unsigned int convexBunnyPointCount = 105; +dReal convexBunnyPoints[] = +{ + -0.459488, -0.093017, -0.311341, + 0.466635, -0.094416, -0.305669, + -0.309239, 0.776868, 0.304726, + -0.004458, -0.042526, 1.01567, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.007957, 0.282241, -0.93168, + 0.204445, -0.66438, 0.513353, + -0.303961, 0.054199, 0.625921, + 0.265619, 0.756464, 0.504187, + -0.402162, 0.133528, -0.443247, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.266772, 0.64233, 0.602061, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.411612, 0.132299, -0.438264, + 0.31148, 0.775931, 0.308527, + 0.300086, 0.053287, 0.62962, + -0.414624, 0.164083, -0.278254, + -0.248382, 0.255825, -0.627493, + -0.216201, -0.126776, -0.886936, + 0.267564, -0.666174, -0.654834, + -0.135892, -0.03552, 0.945455, + -0.265837, 0.757267, 0.500933, + -0.003873, 0.161605, 0.970499, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.282599, -0.663393, 0.412411, + 0.007237, 0.361687, -0.794439, + 0.093627, 0.258494, -0.920589, + 0.422146, 0.162819, -0.27313, + 0.279163, -0.664604, 0.417328, + 0.263086, 0.512567, 0.637832, + -0.099875, 0.310931, -0.799381, + -0.446838, -0.118517, -0.466159, + -0.168842, 0.102387, -0.920381, + 0.455805, -0.119881, -0.460632, + 0.337743, -0.666396, -0.074503, + -0.134547, -0.119852, -0.959004, + -0.183807, 0.19697, 0.84448, + 0.264969, 0.641527, 0.605317, + -0.209063, -0.663393, 0.509344, + -0.364126, -0.200299, 0.202388, + -0.253475, -0.081797, 0.756541, + 0.260471, 0.255056, -0.624378, + 0.114248, 0.310608, -0.79807, + 0.364663, -0.201399, 0.20685, + 0.127847, -0.035919, 0.94707, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.381071, -0.629723, -0.350777, + -0.339884, -0.04115, -0.668211, + -0.077913, 0.258753, -0.92164, + 0.184061, 0.101854, -0.91822, + -0.335166, -0.66538, -0.078623, + 0.386561, -0.625221, -0.21687, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.241585, 0.527592, 0.669296, + -0.086969, 0.133224, 0.947633, + -0.003127, 0.28407, 0.87887, + -0.004433, -0.146642, 0.985872, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.138444, -0.10425, 0.945975, + -0.265676, 0.513366, 0.634594, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.247593, -0.082554, 0.75961, + 0.07941, 0.132973, 0.948652, + 0.238615, 0.526867, 0.672237, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.382112, -0.62406, -0.221577, + -0.104072, 0.177278, -0.95253, + 0.351567, -0.042194, -0.663976, + 0.138234, -0.293905, -0.897958, + 0.119916, 0.17694, -0.951159, + -0.371322, -0.665382, -0.35362, + -0.263384, -0.663396, 0.466604, + 0.376722, -0.666513, -0.219833, + 0.387086, -0.630883, -0.346073, + -0.125544, 0.140012, 0.917678, + -0.070612, 0.036849, 0.975733, + -0.083497, -0.084934, 0.979607, + 0.259286, -0.664547, 0.471281, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.074888, -0.085173, 0.980577, + 0.152305, 0.125256, 0.890786, + 0.130184, -0.104656, 0.94762, + -0.004249, 0.046042, 1.00324, + 0.062419, 0.036648, 0.976547, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.392666, -0.488581, -0.427494, + 0.230315, -0.12745, -0.884202, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.193434, -0.665946, -0.715325, + 0.007865, 0.122104, -0.956137, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.257884, -0.665381, -0.658052, + 0.377265, -0.666513, -0.349036, + -0.372362, -0.665381, -0.22442, + 0.400045, -0.489778, -0.42264, + -0.159174, 0.125726, 0.888878, + 0.118369, 0.139643, 0.919173, + -0.124463, -0.293508, -0.899566, + 0.21172, -0.302754, -0.843303, + 0.149571, -0.120281, -0.957264, + -0.183019, -0.665378, -0.71763, + 0.177696, 0.196424, 0.846693, + -0.198638, -0.302135, -0.845816, +}; +unsigned int convexBunnyPolygons[] = +{ + 3, 7, 2, 0, + 3, 2, 7, 11, + 3, 1, 15, 16, + 3, 0, 2, 17, + 3, 17, 9, 0, + 3, 2, 9, 17, + 3, 18, 9, 2, + 3, 2, 11, 22, + 3, 22, 15, 2, + 3, 8, 15, 22, + 3, 2, 15, 26, + 3, 5, 26, 27, + 3, 1, 14, 28, + 3, 28, 15, 1, + 3, 14, 15, 28, + 3, 2, 26, 31, + 3, 0, 9, 32, + 3, 9, 18, 33, + 3, 34, 14, 1, + 3, 19, 33, 36, + 3, 8, 22, 38, + 3, 38, 22, 11, + 3, 38, 15, 8, + 3, 38, 16, 15, + 3, 38, 30, 16, + 3, 40, 7, 0, + 3, 0, 25, 40, + 3, 40, 25, 7, + 3, 7, 25, 41, + 3, 21, 37, 41, + 3, 42, 15, 14, + 3, 42, 27, 15, + 3, 43, 26, 15, + 3, 15, 27, 43, + 3, 43, 27, 26, + 3, 1, 16, 44, + 3, 44, 29, 1, + 3, 16, 29, 44, + 3, 0, 32, 47, + 3, 19, 32, 48, + 3, 48, 33, 19, + 3, 48, 32, 9, + 3, 9, 33, 48, + 3, 49, 33, 18, + 3, 49, 18, 2, + 3, 2, 31, 49, + 3, 49, 26, 5, + 3, 49, 31, 26, + 3, 50, 42, 14, + 3, 27, 42, 50, + 3, 51, 35, 6, + 3, 6, 39, 51, + 3, 1, 29, 52, + 3, 11, 37, 54, + 3, 55, 23, 11, + 3, 11, 54, 55, + 3, 11, 23, 56, + 3, 56, 38, 11, + 3, 23, 38, 56, + 3, 57, 39, 6, + 3, 21, 41, 59, + 3, 39, 57, 59, + 3, 60, 37, 11, + 3, 60, 41, 37, + 3, 60, 11, 7, + 3, 7, 41, 60, + 3, 16, 30, 62, + 3, 62, 29, 16, + 3, 63, 38, 23, + 3, 38, 63, 64, + 3, 67, 25, 0, + 3, 0, 47, 67, + 3, 68, 36, 33, + 3, 33, 49, 68, + 3, 68, 49, 5, + 3, 14, 34, 69, + 3, 69, 50, 14, + 3, 5, 27, 71, + 3, 27, 50, 71, + 3, 71, 68, 5, + 3, 25, 51, 73, + 3, 73, 51, 39, + 3, 39, 59, 73, + 3, 73, 41, 25, + 3, 73, 59, 41, + 3, 29, 35, 74, + 3, 74, 52, 29, + 3, 35, 51, 74, + 3, 75, 34, 1, + 3, 1, 52, 75, + 3, 52, 74, 75, + 3, 21, 55, 76, + 3, 76, 54, 37, + 3, 76, 55, 54, + 3, 77, 55, 21, + 3, 21, 59, 78, + 3, 3, 77, 78, + 3, 78, 77, 21, + 3, 78, 57, 3, + 3, 78, 59, 57, + 3, 6, 35, 79, + 3, 79, 35, 29, + 3, 29, 62, 79, + 3, 3, 57, 81, + 3, 83, 62, 45, + 3, 45, 81, 83, + 3, 83, 79, 62, + 3, 6, 79, 83, + 3, 83, 57, 6, + 3, 83, 81, 57, + 3, 84, 63, 23, + 3, 84, 77, 3, + 3, 23, 55, 84, + 3, 55, 77, 84, + 3, 45, 63, 85, + 3, 3, 81, 85, + 3, 85, 81, 45, + 3, 85, 84, 3, + 3, 63, 84, 85, + 3, 87, 47, 32, + 3, 87, 72, 47, + 3, 50, 69, 88, + 3, 88, 34, 20, + 3, 88, 69, 34, + 3, 36, 68, 91, + 3, 68, 71, 91, + 3, 72, 87, 93, + 3, 93, 87, 32, + 3, 93, 32, 19, + 3, 94, 74, 72, + 3, 94, 93, 20, + 3, 72, 93, 94, + 3, 94, 75, 74, + 3, 95, 74, 51, + 3, 72, 74, 95, + 3, 95, 51, 25, + 3, 25, 67, 95, + 3, 95, 67, 47, + 3, 47, 72, 95, + 3, 20, 34, 96, + 3, 34, 75, 96, + 3, 96, 94, 20, + 3, 75, 94, 96, + 3, 97, 37, 21, + 3, 21, 76, 97, + 3, 97, 76, 37, + 3, 98, 64, 63, + 3, 98, 63, 45, + 3, 45, 82, 98, + 3, 36, 70, 99, + 3, 100, 88, 20, + 3, 20, 90, 100, + 3, 100, 90, 70, + 3, 101, 71, 50, + 3, 50, 88, 101, + 3, 36, 91, 101, + 3, 101, 91, 71, + 3, 101, 70, 36, + 3, 101, 100, 70, + 3, 88, 100, 101, + 3, 102, 90, 20, + 3, 20, 93, 102, + 3, 70, 90, 102, + 3, 102, 99, 70, + 3, 64, 98, 103, + 3, 103, 98, 82, + 3, 30, 38, 103, + 3, 38, 64, 103, + 3, 103, 62, 30, + 3, 45, 62, 103, + 3, 103, 82, 45, + 3, 36, 99, 104, + 3, 99, 102, 104, + 3, 104, 102, 93, + 3, 19, 36, 104, + 3, 104, 93, 19, +}; diff --git a/libs/ode-0.16.1/ode/demo/convex_prism.h b/libs/ode-0.16.1/ode/demo/convex_prism.h new file mode 100644 index 0000000..1ee7fcb --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/convex_prism.h @@ -0,0 +1,28 @@ +unsigned int prism_pointcount = 8;
+unsigned int prism_planecount = 6;
+dReal prism_points[24]={
+ 10.0, 1.0,-1.0,
+ 10.0,-1.0,-1.0,
+-10.0,-1.0,-1.0,
+-10.0, 1.0,-1.0,
+ 10.0, 1.0, 1.0,
+ 10.0,-1.0, 1.0,
+-10.0,-1.0, 1.0,
+-10.0, 1.0, 1.0
+};
+unsigned int prism_polygons[]={
+4,0,1,2,3,
+4,4,7,6,5,
+4,0,4,5,1,
+4,1,5,6,2,
+4,2,6,7,3,
+4,4,0,3,7,
+};
+dReal prism_planes[]={
+0.0,0.0,-1.0,1.0,
+0.0,0.0,1.0,1.0,
+1.0,0.0,0.0,10.0,
+0.0,-1.0,0.0,1.0,
+-1.0,0.0,-0.0,10.0,
+0.0,1.0,0.0,1.0,
+};
diff --git a/libs/ode-0.16.1/ode/demo/demo_I.cpp b/libs/ode-0.16.1/ode/demo/demo_I.cpp new file mode 100644 index 0000000..156a4ad --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_I.cpp @@ -0,0 +1,253 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +test that the rotational physics is correct. + +an "anchor body" has a number of other randomly positioned bodies +("particles") attached to it by ball-and-socket joints, giving it some +random effective inertia tensor. the effective inertia matrix is calculated, +and then this inertia is assigned to another "test" body. a random torque is +applied to both bodies and the difference in angular velocity and orientation +is observed after a number of iterations. + +typical errors for each test cycle are about 1e-5 ... 1e-4. + +*/ + + +#include <time.h> +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10 // number of particles +#define SIDE 0.1 // visual size of the particles + + +// dynamics objects an globals + +static dWorldID world=0; +static dBodyID anchor_body,particle[NUM],test_body; +static dJointID particle_joint[NUM]; +static dReal torque[3]; +static int iteration; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.5572f,-1.8886f,1.5700f}; + static float hpr[3] = {118.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// compute the mass parameters of a particle set. q = particle positions, +// pm = particle masses + +#define _I(i,j) I[(i)*4+(j)] + +void computeMassParams (dMass *m, dReal q[NUM][3], dReal pm[NUM]) +{ + int i,j; + dMassSetZero (m); + for (i=0; i<NUM; i++) { + m->mass += pm[i]; + for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j]; + m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]); + m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]); + m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]); + m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]); + m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]); + m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]); + } + for (j=0; j<3; j++) m->c[j] /= m->mass; + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); +} + + +void reset_test() +{ + int i; + dMass m,anchor_m; + dReal q[NUM][3], pm[NUM]; // particle positions and masses + dReal pos1[3] = {1,0,1}; // point of reference (POR) + dReal pos2[3] = {-1,0,1}; // point of reference (POR) + + // make random particle positions (relative to POR) and masses + for (i=0; i<NUM; i++) { + pm[i] = dRandReal()+0.1; + q[i][0] = dRandReal()-0.5; + q[i][1] = dRandReal()-0.5; + q[i][2] = dRandReal()-0.5; + } + + // adjust particle positions so centor of mass = POR + computeMassParams (&m,q,pm); + for (i=0; i<NUM; i++) { + q[i][0] -= m.c[0]; + q[i][1] -= m.c[1]; + q[i][2] -= m.c[2]; + } + + if (world) dWorldDestroy (world); + world = dWorldCreate(); + + anchor_body = dBodyCreate (world); + dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]); + dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE); + dMassAdjust (&anchor_m,0.1); + dBodySetMass (anchor_body,&anchor_m); + + for (i=0; i<NUM; i++) { + particle[i] = dBodyCreate (world); + dBodySetPosition (particle[i], + pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]); + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,pm[i]); + dBodySetMass (particle[i],&m); + } + + for (i=0; i < NUM; i++) { + particle_joint[i] = dJointCreateBall (world,0); + dJointAttach (particle_joint[i],anchor_body,particle[i]); + const dReal *p = dBodyGetPosition (particle[i]); + dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]); + } + + // make test_body with the same mass and inertia of the anchor_body plus + // all the particles + + test_body = dBodyCreate (world); + dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]); + computeMassParams (&m,q,pm); + m.mass += anchor_m.mass; + for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i]; + dBodySetMass (test_body,&m); + + // rotate the test and anchor bodies by a random amount + dQuaternion qrot; + for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5; + dNormalize4 (qrot); + dBodySetQuaternion (anchor_body,qrot); + dBodySetQuaternion (test_body,qrot); + dMatrix3 R; + dQtoR (qrot,R); + for (i=0; i<NUM; i++) { + dVector3 v; + dMultiply0 (v,R,&q[i][0],3,3,1); + dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]); + } + + // set random torque + for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1; + + + iteration=0; +} + + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]); + dBodyAddTorque (test_body,torque[0],torque[1],torque[2]); + dWorldStep (world,0.03); + + iteration++; + if (iteration >= 100) { + // measure the difference between the anchor and test bodies + const dReal *w1 = dBodyGetAngularVel (anchor_body); + const dReal *w2 = dBodyGetAngularVel (test_body); + const dReal *q1 = dBodyGetQuaternion (anchor_body); + const dReal *q2 = dBodyGetQuaternion (test_body); + dReal maxdiff = dMaxDifference (w1,w2,1,3); + printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n", + maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]); + maxdiff = dMaxDifference (q1,q2,1,4); + printf ("q-error = %.4e\n",maxdiff); + reset_test(); + } + } + + dReal sides[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE}; + dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE}; + dsSetColor (1,1,1); + dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body), + sides3); + dsSetColor (1,0,0); + dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2); + dsSetColor (1,1,0); + for (int i=0; i<NUM; i++) + dsDrawBox (dBodyGetPosition (particle[i]), + dBodyGetRotation (particle[i]), sides); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + dRandSetSeed (time(0)); + reset_test(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_basket.cpp b/libs/ode-0.16.1/ode/demo/demo_basket.cpp new file mode 100644 index 0000000..ab6a5c8 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_basket.cpp @@ -0,0 +1,276 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Basket ball demo. +// Serves as a test for the sphere vs trimesh collider +// By Bram Stolk. +// Press the spacebar to reset the position of the ball. + +#include <assert.h> +#ifdef HAVE_UNISTD_H +#include <unistd.h> +#endif +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" +#include "basket_geom.h" // this is our world mesh + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// some constants + +#define RADIUS 0.14 + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; + +static dBodyID sphbody; +static dGeomID sphgeom; + +static dJointGroupID contactgroup; +static dGeomID world_mesh; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + +// fprintf(stderr,"testing geoms %p %p\n", o1, o2); + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i<n; i++) + { + // Paranoia <-- not working for some people, temporarily removed for 0.6 + //dIASSERT(dVALIDVEC3(contact[i].geom.pos)); + //dIASSERT(dVALIDVEC3(contact[i].geom.normal)); + //dIASSERT(!dIsNan(contact[i].geom.depth)); + contact[i].surface.slip1 = 0.7; + contact[i].surface.slip2 = 0.7; + contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2; + contact[i].surface.mu = 50.0; // was: dInfinity + contact[i].surface.soft_erp = 0.96; + contact[i].surface.soft_cfm = 0.04; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c, + dGeomGetBody(contact[i].geom.g1), + dGeomGetBody(contact[i].geom.g2)); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {-8,0,5}; + static float hpr[3] = {0.0f,-29.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + + +static void reset_ball(void) +{ + float sx=0.0f, sy=3.40f, sz=7.15; + + dQuaternion q; + dQSetIdentity(q); + dBodySetPosition (sphbody, sx, sy, sz); + dBodySetQuaternion(sphbody, q); + dBodySetLinearVel (sphbody, 0,0,0); + dBodySetAngularVel (sphbody, 0,0,0); +} + + +// called when a key pressed + +static void command (int cmd) +{ + switch (cmd) + { + case ' ': + reset_ball(); + break; + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + double simstep = 0.001; // 1ms simulation steps + double dt = dsElapsedTime(); + + int nrofsteps = (int) ceilf(dt/simstep); +// fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps); + + for (int i=0; i<nrofsteps && !pause; i++) + { + dSpaceCollide (space,0,&nearCallback); + dWorldQuickStep (world, simstep); + dJointGroupEmpty (contactgroup); + } + + dsSetColor (1,1,1); + const dReal *SPos = dBodyGetPosition(sphbody); + const dReal *SRot = dBodyGetRotation(sphbody); + float spos[3] = {SPos[0], SPos[1], SPos[2]}; + float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] }; + dsDrawSphere + ( + spos, + srot, + RADIUS + ); + + // draw world trimesh + dsSetColor(0.4,0.7,0.9); + dsSetTexture (DS_NONE); + + const dReal* Pos = dGeomGetPosition(world_mesh); + //dIASSERT(dVALIDVEC3(Pos)); + float pos[3] = { Pos[0], Pos[1], Pos[2] }; + + const dReal* Rot = dGeomGetRotation(world_mesh); + //dIASSERT(dVALIDMAT3(Rot)); + float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] }; + + int numi = sizeof(world_indices) / sizeof(dTriIndex); + + for (int i=0; i<numi/3; i++) + { + int i0 = world_indices[i*3+0]; + int i1 = world_indices[i*3+1]; + int i2 = world_indices[i*3+2]; + float *v0 = world_vertices+i0*3; + float *v1 = world_vertices+i1*3; + float *v2 = world_vertices+i2*3; + dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw + } +} + + +int main (int argc, char **argv) +{ + dMass m; + dMatrix3 R; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-9.8); + dWorldSetQuickStepNumIterations (world, 64); + + // Create a static world using a triangle mesh that we can collide with. + int numv = sizeof(world_vertices)/(3*sizeof(float)); + int numi = sizeof(world_indices)/ sizeof(dTriIndex); + printf("numv=%d, numi=%d\n", numv, numi); + dTriMeshDataID Data = dGeomTriMeshDataCreate(); + +// fprintf(stderr,"Building Single Precision Mesh\n"); + + dGeomTriMeshDataBuildSingle + ( + Data, + world_vertices, + 3 * sizeof(float), + numv, + world_indices, + numi, + 3 * sizeof(dTriIndex) + ); + + world_mesh = dCreateTriMesh(space, Data, 0, 0, 0); + dGeomTriMeshEnableTC(world_mesh, dSphereClass, false); + dGeomTriMeshEnableTC(world_mesh, dBoxClass, false); + dGeomSetPosition(world_mesh, 0, 0, 0.5); + dRSetIdentity(R); + //dIASSERT(dVALIDMAT3(R)); + + dGeomSetRotation (world_mesh, R); + + //float sx=0.0, sy=3.40, sz=6.80; + (void)world_normals; // get rid of compiler warning + sphbody = dBodyCreate (world); + dMassSetSphere (&m,1,RADIUS); + dBodySetMass (sphbody,&m); + sphgeom = dCreateSphere(0, RADIUS); + dGeomSetBody (sphgeom,sphbody); + reset_ball(); + dSpaceAdd (space, sphgeom); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + // Causes segm violation? Why? + // (because dWorldDestroy() destroys body connected to geom; must call first!) + dGeomDestroy(sphgeom); + dGeomDestroy (world_mesh); + + dJointGroupEmpty (contactgroup); + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + + + diff --git a/libs/ode-0.16.1/ode/demo/demo_boxstack.cpp b/libs/ode-0.16.1/ode/demo/demo_boxstack.cpp new file mode 100644 index 0000000..ac46fcf --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_boxstack.cpp @@ -0,0 +1,619 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#include "icosahedron_geom.h" + + +//<---- Convex Object +dReal planes[]= // planes for a cube, these should coincide with the face array + { + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ + }; +const unsigned int planecount=6; + +dReal points[]= // points for a cube + { + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 + }; +const unsigned int pointcount=8; +unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 8 // maximum number of contact points per body +#define MAX_FEEDBACKNUM 20 +#define GRAVITY REAL(0.5) + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int write_world = 0; +static int show_body = 0; + +struct MyFeedback { + dJointFeedback fb; + bool first; +}; +static int doFeedback=0; +static MyFeedback feedbacks[MAX_FEEDBACKNUM]; +static int fbnum=0; + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) + return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i<MAX_CONTACTS; i++) { + contact[i].surface.mode = dContactBounce | dContactSoftCFM; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.1; + contact[i].surface.bounce_vel = 0.1; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact))) { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.02,0.02,0.02}; + for (i=0; i<numc; i++) { + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + if (show_contacts) { + dsSetColor(0,0,1); + dsDrawBox(contact[i].geom.pos,RI,ss); + } + if (doFeedback && (b1==obj[selected].body || b2==obj[selected].body)) { + if (fbnum<MAX_FEEDBACKNUM) { + feedbacks[fbnum].first = b1==obj[selected].body; + dJointSetFeedback(c,&feedbacks[fbnum++].fb); + } + else fbnum++; + } + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("To drop another object, press:\n"); + printf (" b for box.\n"); + printf (" s for sphere.\n"); + printf (" c for capsule.\n"); + printf (" y for cylinder.\n"); + printf (" v for a convex object.\n"); + printf (" x for a composite object.\n"); + printf ("To select an object, press space.\n"); + printf ("To disable the selected object, press d.\n"); + printf ("To enable the selected object, press e.\n"); + printf ("To dump transformation data for the selected object, press p.\n"); + printf ("To toggle showing the geom AABBs, press a.\n"); + printf ("To toggle showing the contact points, press t.\n"); + printf ("To toggle dropping from random position/orientation, press r.\n"); + printf ("To save the current state to 'state.dif', press 1.\n"); + printf ("To show joint feedbacks of selected object, press f.\n"); +} + + +static char locase(char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command(int cmd) +{ + dsizeint i; + int j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase(cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') { + if (num < NUM) { + // new object to be created + i = num; + num++; + } else { + // recycle existing object + i = nextobj++; + nextobj %= num; // wrap-around if needed + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + obj[i].body = 0; + + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) { + dGeomDestroy(obj[i].geom[k]); + obj[i].geom[k] = 0; + } + } + + obj[i].body = dBodyCreate(world); + + for (k=0; k<3; k++) + sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition(obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); + dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } else { + // higher than highest body position + dReal maxheight = 0; + for (k=0; k<num; k++) { + const dReal *pos = dBodyGetPosition(obj[k].body); + if (pos[2] > maxheight) + maxheight = pos[2]; + } + dBodySetPosition(obj[i].body, 0,0,maxheight+1); + dRSetIdentity(R); + //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); + } + + dBodySetRotation(obj[i].body,R); + + if (cmd == 'b') { + + dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]); + + } else if (cmd == 'c') { + + sides[0] *= 0.5; + dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + + } else if (cmd == 'v') { + + dMassSetBox(&m,DENSITY,0.25,0.25,0.25); +#if 0 + obj[i].geom[0] = dCreateConvex(space, + planes, + planecount, + points, + pointcount, + polygons); +#else + obj[i].geom[0] = dCreateConvex(space, + Sphere_planes, + Sphere_planecount, + Sphere_points, + Sphere_pointcount, + Sphere_polygons); +#endif + + } else if (cmd == 'y') { + + dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]); + + } else if (cmd == 's') { + + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + + } else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j<GPB; j++) + for (k=0; k<3; k++) + dpos[j][k] = dRandReal()*0.3-0.15; + + for (k=0; k<GPB; k++) { + if (k==0) { + dReal radius = dRandReal()*0.25+0.05; + obj[i].geom[k] = dCreateSphere (space,radius); + dMassSetSphere (&m2,DENSITY,radius); + } else if (k==1) { + obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); + dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); + } else { + dReal radius = dRandReal()*0.1+0.05; + dReal length = dRandReal()*1.0+0.1; + obj[i].geom[k] = dCreateCapsule(space,radius,length); + dMassSetCapsule(&m2,DENSITY,3,radius,length); + } + + dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dMassRotate(&m2,drot[k]); + + dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); + + // add to the total mass + dMassAdd(&m,&m2); + + } + for (k=0; k<GPB; k++) { + dGeomSetBody(obj[i].geom[k],obj[i].body); + dGeomSetOffsetPosition(obj[i].geom[k], + dpos[k][0]-m.c[0], + dpos[k][1]-m.c[1], + dpos[k][2]-m.c[2]); + dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); + } + dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); + dBodySetMass(obj[i].body,&m); + + } + + if (!setBody) { // avoid calling for composite geometries + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) + dGeomSetBody(obj[i].geom[k],obj[i].body); + + dBodySetMass(obj[i].body,&m); + } + } + + if (cmd == ' ') { + + selected++; + if (selected >= num) + selected = 0; + if (selected == -1) + selected = 0; + + } else if (cmd == 'd' && selected >= 0 && selected < num) { + + dBodyDisable(obj[selected].body); + + } else if (cmd == 'e' && selected >= 0 && selected < num) { + + dBodyEnable(obj[selected].body); + + } else if (cmd == 'a') { + + show_aabb = !show_aabb; + + } else if (cmd == 't') { + + show_contacts = !show_contacts; + + } else if (cmd == 'r') { + + random_pos = !random_pos; + } else if (cmd == '1') { + + write_world = 1; + + } else if (cmd == 'p'&& selected >= 0) { + + const dReal* pos = dGeomGetPosition(obj[selected].geom[0]); + const dReal* rot = dGeomGetRotation(obj[selected].geom[0]); + printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]); + printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n", + rot[0],rot[1],rot[2],rot[3], + rot[4],rot[5],rot[6],rot[7], + rot[8],rot[9],rot[10],rot[11]); + + } else if (cmd == 'f' && selected >= 0 && selected < num) { + + if (dBodyIsEnabled(obj[selected].body)) + doFeedback = 1; + + } +} + + +// draw a geom + +void drawGeom(dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + int i; + + if (!g) + return; + if (!pos) + pos = dGeomGetPosition(g); + if (!R) + R = dGeomGetRotation(g); + + int type = dGeomGetClass(g); + if (type == dBoxClass) { + + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox(pos,R,sides); + + } else if (type == dSphereClass) { + + dsDrawSphere(pos,R,dGeomSphereGetRadius(g)); + + } else if (type == dCapsuleClass) { + + dReal radius,length; + dGeomCapsuleGetParams(g,&radius,&length); + dsDrawCapsule(pos,R,length,radius); + + } else if (type == dConvexClass) { + +#if 0 + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); +#else + dsDrawConvex(pos,R, + Sphere_planes, + Sphere_planecount, + Sphere_points, + Sphere_pointcount, + Sphere_polygons); +#endif + + } else if (type == dCylinderClass) { + + dReal radius,length; + dGeomCylinderGetParams(g,&radius,&length); + dsDrawCylinder(pos,R,length,radius); + + } + + if (show_body) { + dBodyID body = dGeomGetBody(g); + if (body) { + const dReal *bodypos = dBodyGetPosition(body); + const dReal *bodyr = dBodyGetRotation(body); + dReal bodySides[3] = { 0.1, 0.1, 0.1 }; + dsSetColorAlpha(0,1,0,1); + dsDrawBox(bodypos,bodyr,bodySides); + } + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB(g,aabb); + dVector3 bbpos; + for (i=0; i<3; i++) + bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (i=0; i<3; i++) + bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha(1,0,0,0.5); + dsDrawBox(bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void simLoop(int pause) +{ + dSpaceCollide(space, 0, &nearCallback); + + if (!pause) + dWorldQuickStep(world, 0.02); + + if (write_world) { + FILE *f = fopen("state.dif","wt"); + if (f) { + dWorldExportDIF(world,f,"X"); + fclose (f); + } + write_world = 0; + } + + + if (doFeedback) { + if (fbnum>MAX_FEEDBACKNUM) + printf("joint feedback buffer overflow!\n"); + else { + dVector3 sum = {0, 0, 0}; + printf("\n"); + for (int i=0; i<fbnum; i++) { + dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2; + printf("%f %f %f\n", f[0], f[1], f[2]); + sum[0] += f[0]; + sum[1] += f[1]; + sum[2] += f[2]; + } + printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]); + dMass m; + dBodyGetMass(obj[selected].body, &m); + printf("Object G=%f\n", GRAVITY*m.mass); + } + doFeedback = 0; + fbnum = 0; + } + + // remove all contact joints + dJointGroupEmpty(contactgroup); + + dsSetTexture(DS_WOOD); + for (int i=0; i<num; i++) { + for (int j=0; j < GPB; j++) { + if (i==selected) { + dsSetColor(0,0.7,1); + } else if (!dBodyIsEnabled(obj[i].body)) { + dsSetColor(1,0.8,0); + } else { + dsSetColor(1,1,0); + } + drawGeom(obj[i].geom[j],0,0,show_aabb); + } + } +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate(0); + contactgroup = dJointGroupCreate(0); + dWorldSetGravity(world,0,0,-GRAVITY); + dWorldSetCFM(world,1e-5); + dWorldSetAutoDisableFlag(world,1); + +#if 1 + + dWorldSetAutoDisableAverageSamplesCount( world, 10 ); + +#endif + + dWorldSetLinearDamping(world, 0.00001); + dWorldSetAngularDamping(world, 0.005); + dWorldSetMaxAngularSpeed(world, 200); + + dWorldSetContactMaxCorrectingVel(world,0.1); + dWorldSetContactSurfaceLayer(world,0.001); + dCreatePlane(space,0,0,1,0); + memset(obj,0,sizeof(obj)); + + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); + dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); + + // run simulation + dsSimulationLoop(argc,argv,640,480,&fn); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dWorldSetStepThreadingImplementation(world, NULL, NULL); + dThreadingFreeImplementation(threading); + + dJointGroupDestroy(contactgroup); + dSpaceDestroy(space); + dWorldDestroy(world); + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_buggy.cpp b/libs/ode-0.16.1/ode/demo/demo_buggy.cpp new file mode 100644 index 0000000..b96d93f --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_buggy.cpp @@ -0,0 +1,308 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +buggy with suspension. +this also shows you how to use geom groups. + +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define LENGTH 0.7 // chassis length +#define WIDTH 0.5 // chassis width +#define HEIGHT 0.2 // chassis height +#define RADIUS 0.18 // wheel radius +#define STARTZ 0.5 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 0.2 // wheel mass + +static const dVector3 yunit = { 0, 1, 0 }, zunit = { 0, 0, 1 }; + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +static dBodyID body[4]; +static dJointID joint[3]; // joint[0] is the front wheel +static dJointGroupID contactgroup; +static dGeomID ground; +static dSpaceID car_space; +static dGeomID box[1]; +static dGeomID sphere[3]; +static dGeomID ground_box; + + +// things that the user controls + +static dReal speed=0,steer=0; // user commands + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + // only collide things with the ground + int g1 = (o1 == ground || o1 == ground_box); + int g2 = (o2 == ground || o2 == ground_box); + if (!(g1 ^ g2)) return; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i<n; i++) { + contact[i].surface.mode = dContactSlip1 | dContactSlip2 | + dContactSoftERP | dContactSoftCFM | dContactApprox1; + contact[i].surface.mu = dInfinity; + contact[i].surface.slip1 = 0.1; + contact[i].surface.slip2 = 0.1; + contact[i].surface.soft_erp = 0.5; + contact[i].surface.soft_cfm = 0.3; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c, + dGeomGetBody(contact[i].geom.g1), + dGeomGetBody(contact[i].geom.g2)); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {0.8317f,-0.9817f,0.8000f}; + static float hpr[3] = {121.0000f,-27.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("Press:\t'a' to increase speed.\n" + "\t'z' to decrease speed.\n" + "\t',' to steer left.\n" + "\t'.' to steer right.\n" + "\t' ' to reset speed and steering.\n" + "\t'1' to save the current state to 'state.dif'.\n"); +} + + +// called when a key pressed + +static void command (int cmd) +{ + switch (cmd) { + case 'a': case 'A': + speed += 0.3; + break; + case 'z': case 'Z': + speed -= 0.3; + break; + case ',': + steer -= 0.5; + break; + case '.': + steer += 0.5; + break; + case ' ': + speed = 0; + steer = 0; + break; + case '1': { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF (world,f,""); + fclose (f); + } + } + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + int i; + if (!pause) { + // motor + dJointSetHinge2Param (joint[0],dParamVel2,-speed); + dJointSetHinge2Param (joint[0],dParamFMax2,0.1); + + // steering + dReal v = steer - dJointGetHinge2Angle1 (joint[0]); + if (v > 0.1) v = 0.1; + if (v < -0.1) v = -0.1; + v *= 10.0; + dJointSetHinge2Param (joint[0],dParamVel,v); + dJointSetHinge2Param (joint[0],dParamFMax,0.2); + dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); + dJointSetHinge2Param (joint[0],dParamHiStop,0.75); + dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); + + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + } + + dsSetColor (0,1,1); + dsSetTexture (DS_WOOD); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); + dsSetColor (1,1,1); + for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]),0.02f,RADIUS); + + dVector3 ss; + dGeomBoxGetLengths (ground_box,ss); + dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); + +} + + +int main (int argc, char **argv) +{ + int i; + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + ground = dCreatePlane (space,0,0,1,0); + + // chassis body + body[0] = dBodyCreate (world); + dBodySetPosition (body[0],0,0,STARTZ); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m,CMASS); + dBodySetMass (body[0],&m); + box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[0],body[0]); + + // wheel bodies + for (i=1; i<=3; i++) { + body[i] = dBodyCreate (world); + dQuaternion q; + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); + dBodySetQuaternion (body[i],q); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m,WMASS); + dBodySetMass (body[i],&m); + sphere[i-1] = dCreateSphere (0,RADIUS); + dGeomSetBody (sphere[i-1],body[i]); + } + dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5); + + // front and back wheel hinges + for (i=0; i<3; i++) { + joint[i] = dJointCreateHinge2 (world,0); + dJointAttach (joint[i],body[0],body[i+1]); + const dReal *a = dBodyGetPosition (body[i+1]); + dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]); + dJointSetHinge2Axes (joint[i], zunit, yunit); + } + + // set joint suspension + for (i=0; i<3; i++) { + dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4); + dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8); + } + + // lock back wheels along the steering axis + for (i=1; i<3; i++) { + // set stops to make sure wheels always stay in alignment + dJointSetHinge2Param (joint[i],dParamLoStop,0); + dJointSetHinge2Param (joint[i],dParamHiStop,0); + // the following alternative method is no good as the wheels may get out + // of alignment: + // dJointSetHinge2Param (joint[i],dParamVel,0); + // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity); + } + + // create car space and add it to the top level space + car_space = dSimpleSpaceCreate (space); + dSpaceSetCleanup (car_space,0); + dSpaceAdd (car_space,box[0]); + dSpaceAdd (car_space,sphere[0]); + dSpaceAdd (car_space,sphere[1]); + dSpaceAdd (car_space,sphere[2]); + + // environment + ground_box = dCreateBox (space,2,1.5,1); + dMatrix3 R; + dRFromAxisAndAngle (R,0,1,0,-0.15); + dGeomSetPosition (ground_box,2,0,-0.34); + dGeomSetRotation (ground_box,R); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dGeomDestroy (box[0]); + dGeomDestroy (sphere[0]); + dGeomDestroy (sphere[1]); + dGeomDestroy (sphere[2]); + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_cards.cpp b/libs/ode-0.16.1/ode/demo/demo_cards.cpp new file mode 100644 index 0000000..17284ba --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_cards.cpp @@ -0,0 +1,237 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <vector> +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + +static int levels = 5; +static int ncards = 0; + +static dSpaceID space; +static dWorldID world; +static dJointGroupID contactgroup; + +struct Card { + dBodyID body; + dGeomID geom; + static const dReal sides[3]; + + Card() + { + body = dBodyCreate(world); + geom = dCreateBox(space, sides[0], sides[1], sides[2]); + dGeomSetBody(geom, body); + dGeomSetData(geom, this); + dMass mass; + mass.setBox(1, sides[0], sides[1], sides[2]); + dBodySetMass(body, &mass); + } + + ~Card() + { + dBodyDestroy(body); + dGeomDestroy(geom); + } + + void draw() const + { + dsDrawBox(dBodyGetPosition(body), + dBodyGetRotation(body), sides); + } +}; +static const dReal cwidth=.5, cthikness=.02, clength=1; +const dReal Card::sides[3] = { cwidth, cthikness, clength }; + + +std::vector<Card*> cards; + +int getncards(int levels) +{ + return (3*levels*levels + levels) / 2; +} + +void place_cards() +{ + ncards = getncards(levels); + // destroy removed cards (if any) + int oldcards = cards.size(); + for (int i=ncards; i<oldcards; ++i) + delete cards[i]; + cards.resize(ncards); + // construct new cards (if any) + for (int i=oldcards; i<ncards; ++i) + cards[i] = new Card; + + // for each level + int c = 0; + dMatrix3 right, left, hrot; + dReal angle = 20*M_PI/180.; + dRFromAxisAndAngle(right, 1, 0, 0, -angle); + dRFromAxisAndAngle(left, 1, 0, 0, angle); + + dRFromAxisAndAngle(hrot, 1, 0, 0, 91*M_PI/180.); + + dReal eps = 0.05; + dReal vstep = cos(angle)*clength + eps; + dReal hstep = sin(angle)*clength + eps; + + for (int lvl=0; lvl<levels; ++lvl) { + // there are 3*(levels-lvl)-1 cards in each level, except last + int n = (levels-lvl); + dReal height = (lvl)*vstep + vstep/2; + // inclined cards + for (int i=0; i<2*n; ++i, ++c) { + dBodySetPosition(cards[c]->body, + 0, + -n*hstep + hstep*i, + height + ); + if (i%2) + dBodySetRotation(cards[c]->body, left); + else + dBodySetRotation(cards[c]->body, right); + } + + if (n==1) // top of the house + break; + + // horizontal cards + for (int i=0; i<n-1; ++i, ++c) { + dBodySetPosition(cards[c]->body, + 0, + -(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i, + height + vstep/2); + dBodySetRotation(cards[c]->body, hrot); + } + } + +} + + +void start() +{ + puts("Controls:"); + puts(" SPACE - reposition cards"); + puts(" - - one less level"); + puts(" = - one more level"); +} + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + const int MAX_CONTACTS = 8; + dContact contact[MAX_CONTACTS]; + + int numc = dCollide (o1, o2, MAX_CONTACTS, + &contact[0].geom, + sizeof(dContact)); + + for (int i=0; i<numc; i++) { + contact[i].surface.mode = dContactApprox1; + contact[i].surface.mu = 5; + dJointID c = dJointCreateContact (world, contactgroup, contact+i); + dJointAttach (c, b1, b2); + } +} + + +void simLoop(int pause) +{ + if (!pause) { + dSpaceCollide (space, 0, &nearCallback); + dWorldQuickStep(world, 0.01); + dJointGroupEmpty(contactgroup); + } + + dsSetColor (1,1,0); + for (int i=0; i<ncards; ++i) { + dsSetColor (1, dReal(i)/ncards, 0); + cards[i]->draw(); + } + +} + +void command(int c) +{ + switch (c) { + case '=': + levels++; + place_cards(); + break; + case '-': + levels--; + if (levels <= 0) + levels++; + place_cards(); + break; + case ' ': + place_cards(); + break; + } +} + +int main(int argc, char **argv) +{ + dInitODE(); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + + world = dWorldCreate(); + dWorldSetGravity(world, 0, 0, -0.5); + dWorldSetQuickStepNumIterations(world, 50); // <-- increase for more stability + + space = dSimpleSpaceCreate(0); + contactgroup = dJointGroupCreate(0); + dGeomID ground = dCreatePlane(space, 0, 0, 1, 0); + + place_cards(); + + // run simulation + dsSimulationLoop (argc, argv, 640, 480, &fn); + + levels = 0; + place_cards(); + + dJointGroupDestroy(contactgroup); + dWorldDestroy(world); + dGeomDestroy(ground); + dSpaceDestroy(space); + + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_chain1.c b/libs/ode-0.16.1/ode/demo/demo_chain1.c new file mode 100644 index 0000000..a6d7b38 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_chain1.c @@ -0,0 +1,171 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* exercise the C interface */ + +#include <stdio.h> +#include "ode/ode.h" +#include "drawstuff/drawstuff.h" +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) /* for VC++, no precision loss complaints */ +#endif + +/* select correct drawing functions */ + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +/* some constants */ + +#define NUM 10 /* number of boxes */ +#define SIDE (0.2) /* side length of a box */ +#define MASS (1.0) /* mass of a box */ +#define RADIUS (0.1732f) /* sphere radius */ + + +/* dynamics and collision objects */ + +static dWorldID world; +static dSpaceID space; +static dBodyID body[NUM]; +static dJointID joint[NUM-1]; +static dJointGroupID contactgroup; +static dGeomID sphere[NUM]; + + +/* this is called by dSpaceCollide when two objects in space are + * potentially colliding. + */ + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + /* exit without doing anything if the two bodies are connected by a joint */ + dBodyID b1,b2; + dContact contact; + (void)data; + + b1 = dGeomGetBody(o1); + b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnected (b1,b2)) return; + + contact.surface.mode = 0; + contact.surface.mu = 0.1; + contact.surface.mu2 = 0; + if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) { + dJointID c = dJointCreateContact (world,contactgroup,&contact); + dJointAttach (c,b1,b2); + } +} + + +/* start simulation - set viewpoint */ + +static void start() +{ + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + + dAllocateODEDataForThread(dAllocateMaskAll); + dsSetViewpoint (xyz,hpr); +} + + +/* simulation loop */ + +static void simLoop (int pause) +{ + int i; + if (!pause) { + static double angle = 0; + angle += 0.05; + dBodyAddForce (body[NUM-1],0,0,1.5*(sin(angle)+1.0)); + + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + + /* remove all contact joints */ + dJointGroupEmpty (contactgroup); + } + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (i=0; i<NUM; i++) dsDrawSphere (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]),RADIUS); +} + + +int main (int argc, char **argv) +{ + int i; + dReal k; + dMass m; + + /* setup pointers to drawstuff callback functions */ + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + /* create world */ + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (1000000); + dWorldSetGravity (world,0,0,-0.5); + dCreatePlane (space,0,0,1,0); + + for (i=0; i<NUM; i++) { + body[i] = dBodyCreate (world); + k = i*SIDE; + dBodySetPosition (body[i],k,k,k+0.4); + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + dBodySetMass (body[i],&m); + sphere[i] = dCreateSphere (space,RADIUS); + dGeomSetBody (sphere[i],body[i]); + } + for (i=0; i<(NUM-1); i++) { + joint[i] = dJointCreateBall (world,0); + dJointAttach (joint[i],body[i],body[i+1]); + k = (i+0.5)*SIDE; + dJointSetBallAnchor (joint[i],k,k,k+0.4); + } + + /* run simulation */ + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_chain2.cpp b/libs/ode-0.16.1/ode/demo/demo_chain2.cpp new file mode 100644 index 0000000..3dec131 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_chain2.cpp @@ -0,0 +1,165 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* exercise the C++ interface */ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10 // number of boxes +#define SIDE (0.2) // side length of a box +#define MASS (1.0) // mass of a box +#define RADIUS (0.1732f) // sphere radius + +//using namespace ode; + +// dynamics and collision objects + +static dWorld world; +static dSimpleSpace space (0); +static dBody body[NUM]; +static dBallJoint joint[NUM-1]; +static dJointGroup contactgroup; +static dBox box[NUM]; + + +// this is called by space.collide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnected (b1,b2)) return; + + // @@@ it's still more convenient to use the C interface here. + + dContact contact; + contact.surface.mode = 0; + contact.surface.mu = dInfinity; + if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) { + dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact); + dJointAttach (c,b1,b2); + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + static double angle = 0; + angle += 0.05; + body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0)); + + space.collide (0,&nearCallback); + world.step (0.05); + + // remove all contact joints + contactgroup.empty(); + } + + dReal sides[3] = {SIDE,SIDE,SIDE}; + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i<NUM; i++) + dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + + int i; + contactgroup.create (); + world.setGravity (0,0,-0.5); + dWorldSetCFM (world.id(),1e-5); + dPlane plane (space,0,0,1,0); + + for (i=0; i<NUM; i++) { + body[i].create (world); + dReal k = i*SIDE; + body[i].setPosition (k,k,k+0.4); + dMass m; + m.setBox (1,SIDE,SIDE,SIDE); + m.adjust (MASS); + body[i].setMass (&m); + body[i].setData ((void*)(dsizeint)i); + + box[i].create (space,SIDE,SIDE,SIDE); + box[i].setBody (body[i]); + } + for (i=0; i<(NUM-1); i++) { + joint[i].create (world); + joint[i].attach (body[i],body[i+1]); + dReal k = (i+0.5)*SIDE; + joint[i].setAnchor (k,k,k+0.4); + } + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_collision.cpp b/libs/ode-0.16.1/ode/demo/demo_collision.cpp new file mode 100644 index 0000000..e45d106 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_collision.cpp @@ -0,0 +1,1463 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +collision tests. if this program is run without any arguments it will +perform all the tests multiple times, with different random data for each +test. if this program is given a test number it will run that test +graphically/interactively, in which case the space bar can be used to +change the random test conditions. + +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawCylinder dsDrawCylinderD +#endif + +//**************************************************************************** +// test infrastructure, including constants and macros + +#define TEST_REPS1 1000 // run each test this many times (first batch) +#define TEST_REPS2 10000 // run each test this many times (second batch) +const dReal tol = 1e-8; // tolerance used for numerical checks +#define MAX_TESTS 1000 // maximum number of test slots +#define Z_OFFSET 2 // z offset for drawing (to get above ground) + +//using namespace ode; + +// test function. returns 1 if the test passed or 0 if it failed +typedef int test_function_t(); + +struct TestSlot { + int number; // number of test + const char *name; // name of test + int failcount; + test_function_t *test_fn; + int last_failed_line; +}; +TestSlot testslot[MAX_TESTS]; + + +// globals used by the test functions +int graphical_test=0; // show graphical results of this test, 0=none +int current_test; // currently execiting test +int draw_all_objects_called; + + +#define MAKE_TEST(number,function) \ + if (testslot[number].name) dDebug (0,"test number already used"); \ + if (number <= 0 || number >= MAX_TESTS) dDebug (0,"bad test number"); \ + testslot[number].name = # function; \ + testslot[number].test_fn = function; + +#define FAILED() { if (graphical_test==0) { \ + testslot[current_test].last_failed_line=__LINE__; return 0; } } +#define PASSED() { return 1; } + +//**************************************************************************** +// globals + +/* int dBoxBox (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2, + dVector3 normal, dReal *depth, int *code, + int maxc, dContactGeom *contact, int skip); */ + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta); + +//**************************************************************************** +// draw all objects in a space, and draw all the collision contact points + +void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,j,n; + const int N = 100; + dContactGeom contact[N]; + + if (dGeomGetClass (o2) == dRayClass) { + n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom)); + } + else { + n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom)); + } + if (n > 0) { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.01,0.01,0.01}; + for (i=0; i<n; i++) { + contact[i].pos[2] += Z_OFFSET; + dsDrawBox (contact[i].pos,RI,ss); + dVector3 n; + for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j]; + dsDrawLine (contact[i].pos,n); + } + } +} + + +void draw_all_objects (dSpaceID space) +{ + int i, j; + + draw_all_objects_called = 1; + if (!graphical_test) return; + int n = dSpaceGetNumGeoms (space); + + // draw all contact points + dsSetColor (0,1,1); + dSpaceCollide (space,0,&nearCallback); + + // draw all rays + for (i=0; i<n; i++) { + dGeomID g = dSpaceGetGeom (space,i); + if (dGeomGetClass (g) == dRayClass) { + dsSetColor (1,1,1); + dVector3 origin,dir; + dGeomRayGet (g,origin,dir); + origin[2] += Z_OFFSET; + dReal length = dGeomRayGetLength (g); + for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j]; + dsDrawLine (origin,dir); + dsSetColor (0,0,1); + dsDrawSphere (origin,dGeomGetRotation(g),0.01); + } + } + + // draw all other objects + for (i=0; i<n; i++) { + dGeomID g = dSpaceGetGeom (space,i); + dVector3 pos; + if (dGeomGetClass (g) != dPlaneClass) { + memcpy (pos,dGeomGetPosition(g),sizeof(pos)); + pos[2] += Z_OFFSET; + } + + switch (dGeomGetClass (g)) { + + case dSphereClass: { + dsSetColorAlpha (1,0,0,0.8); + dReal radius = dGeomSphereGetRadius (g); + dsDrawSphere (pos,dGeomGetRotation(g),radius); + break; + } + + case dBoxClass: { + dsSetColorAlpha (1,1,0,0.8); + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,dGeomGetRotation(g),sides); + break; + } + + case dCapsuleClass: { + dsSetColorAlpha (0,1,0,0.8); + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,dGeomGetRotation(g),length,radius); + break; + } + case dCylinderClass: { + dsSetColorAlpha (0,1,0,0.8); + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,dGeomGetRotation(g),length,radius); + break; + } + + case dPlaneClass: { + dVector4 n; + dMatrix3 R,sides; + dVector3 pos2; + dGeomPlaneGetParams (g,n); + dRFromZAxis (R,n[0],n[1],n[2]); + for (j=0; j<3; j++) pos[j] = n[j]*n[3]; + pos[2] += Z_OFFSET; + sides[0] = 2; + sides[1] = 2; + sides[2] = 0.001; + dsSetColor (1,0,1); + for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j]; + dsDrawLine (pos,pos2); + dsSetColorAlpha (1,0,1,0.8); + dsDrawBox (pos,R,sides); + break; + } + + } + } +} + +//**************************************************************************** +// point depth tests + +int test_sphere_point_depth() +{ + int j; + dVector3 p,q; + dMatrix3 R; + dReal r,d; + + dSimpleSpace space(0); + dGeomID sphere = dCreateSphere (0,1); + dSpaceAdd (space,sphere); + + // ********** make a random sphere of radius r at position p + + r = dRandReal()+0.1; + dGeomSphereSetRadius (sphere,r); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (sphere,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (sphere,R); + + // ********** test center point has depth r + + if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED(); + + // ********** test point on surface has depth 0 + + for (j=0; j<3; j++) q[j] = dRandReal()-0.5; + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = q[j]*r + p[j]; + if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED(); + + // ********** test point at random depth + + d = (dRandReal()*2-1) * r; + for (j=0; j<3; j++) q[j] = dRandReal()-0.5; + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j]; + if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED(); + + PASSED(); +} + + +int test_box_point_depth() +{ + int i,j; + dVector3 s,p,q,q2; // s = box sides + dMatrix3 R; + dReal ss,d; // ss = smallest side + + dSimpleSpace space(0); + dGeomID box = dCreateBox (0,1,1,1); + dSpaceAdd (space,box); + + // ********** make a random box + + for (j=0; j<3; j++) s[j] = dRandReal() + 0.1; + dGeomBoxSetLengths (box,s[0],s[1],s[2]); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (box,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (box,R); + + // ********** test center point has depth of smallest side + + ss = 1e9; + for (j=0; j<3; j++) if (s[j] < ss) ss = s[j]; + if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol) + FAILED(); + + // ********** test point on surface has depth 0 + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED(); + + // ********** test points outside box have -ve depth + + for (j=0; j<3; j++) { + q[j] = 0.5*s[j] + dRandReal() + 0.01; + if (dRandReal() > 0.5) q[j] = -q[j]; + } + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED(); + + // ********** test points inside box have +ve depth + + for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5); + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED(); + + // ********** test random depth of point aligned along axis (up to ss deep) + + i = dRandInt (3); + for (j=0; j<3; j++) q[j] = 0; + d = (dRandReal()*(ss*0.5+1)-1); + q[i] = s[i]*0.5 - d; + if (dRandReal() > 0.5) q[i] = -q[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED(); + + PASSED(); +} + + +int test_ccylinder_point_depth() +{ + int j; + dVector3 p,a; + dMatrix3 R; + dReal r,l,beta,x,y,d; + + dSimpleSpace space(0); + dGeomID ccyl = dCreateCapsule (0,1,1); + dSpaceAdd (space,ccyl); + + // ********** make a random ccyl + + r = dRandReal()*0.5 + 0.01; + l = dRandReal()*1 + 0.01; + dGeomCapsuleSetParams (ccyl,r,l); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (ccyl,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ccyl,R); + + // ********** test point on axis has depth of 'radius' + + beta = dRandReal()-0.5; + for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2]; + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol) + FAILED(); + + // ********** test point on surface (excluding caps) has depth 0 + + beta = dRandReal()*2*M_PI; + x = r*sin(beta); + y = r*cos(beta); + beta = dRandReal()-0.5; + for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2]; + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED(); + + // ********** test point on surface of caps has depth 0 + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + if (dCalcVectorDot3_14(a,R+2) > 0) { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2]; + } + else { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2]; + } + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED(); + + // ********** test point inside ccyl has positive depth + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + beta = dRandReal()-0.5; + for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2]; + if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED(); + + // ********** test point depth (1) + + d = (dRandReal()*2-1) * r; + beta = dRandReal()*2*M_PI; + x = (r-d)*sin(beta); + y = (r-d)*cos(beta); + beta = dRandReal()-0.5; + for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2]; + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol) + FAILED(); + + // ********** test point depth (2) + + d = (dRandReal()*2-1) * r; + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + if (dCalcVectorDot3_14(a,R+2) > 0) { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2]; + } + else { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2]; + } + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol) + FAILED(); + + PASSED(); +} + + +int test_plane_point_depth() +{ + int j; + dVector3 n,p,q,a,b; // n = plane normal + dReal d; + + dSimpleSpace space(0); + dGeomID plane = dCreatePlane (0,0,0,1,0); + dSpaceAdd (space,plane); + + // ********** make a random plane + + for (j=0; j<3; j++) n[j] = dRandReal() - 0.5; + dNormalize3 (n); + d = dRandReal() - 0.5; + dGeomPlaneSetParams (plane,n[0],n[1],n[2],d); + dPlaneSpace (n,p,q); + + // ********** test point on plane has depth 0 + + a[0] = dRandReal() - 0.5; + a[1] = dRandReal() - 0.5; + a[2] = 0; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED(); + + // ********** test arbitrary depth point + + a[0] = dRandReal() - 0.5; + a[1] = dRandReal() - 0.5; + a[2] = dRandReal() - 0.5; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol) + FAILED(); + + // ********** test depth-1 point + + a[0] = dRandReal() - 0.5; + a[1] = dRandReal() - 0.5; + a[2] = -1; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED(); + + PASSED(); +} + +//**************************************************************************** +// ray tests + +int test_ray_and_sphere() +{ + int j; + dContactGeom contact; + dVector3 p,q,q2,n,v1; + dMatrix3 R; + dReal r,k; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID sphere = dCreateSphere (0,1); + dSpaceAdd (space,ray); + dSpaceAdd (space,sphere); + + // ********** make a random sphere of radius r at position p + + r = dRandReal()+0.1; + dGeomSphereSetRadius (sphere,r); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (sphere,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (sphere,R); + + // ********** test zero length ray just inside sphere + + dGeomRaySetLength (ray,0); + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j]; + dGeomSetPosition (ray,q[0],q[1],q[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test zero length ray just outside that sphere + + dGeomRaySetLength (ray,0); + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j]; + dGeomSetPosition (ray,q[0],q[1],q[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally contained inside the sphere + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + k = dRandReal(); + for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j]; + dMakeRandomVector (q2,3,1.0); + dNormalize3 (q2); + k = dRandReal(); + for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j]; + for (j=0; j<3; j++) n[j] = q2[j] - q[j]; + dNormalize3 (n); + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,dCalcPointsDistance3(q,q2)); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally outside the sphere + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + do { + dMakeRandomVector (n,3,1.0); + dNormalize3 (n); + } + while (dCalcVectorDot3(n,q) < 0); // make sure normal goes away from sphere + for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j]; + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,100); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just above surface + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + for (j=0; j<3; j++) n[j] = -q[j]; + for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j]; + dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,0.99*r); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just below surface + + dGeomRaySetLength (ray,1.01*r); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + for (j=0; j<3; j++) q2[j] = r * q[j] + p[j]; + if (dCalcPointsDistance3 (contact.pos,q2) > tol) FAILED(); + + // ********** test contact point distance for random rays + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + k = dRandReal()+0.5; + for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j]; + dMakeRandomVector (n,3,1.0); + dNormalize3 (n); + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,100); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) { + k = dCalcPointsDistance3 (contact.pos,dGeomGetPosition(sphere)); + if (dFabs(k - r) > tol) FAILED(); + // also check normal signs + if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED(); + // also check depth of contact point + if (dFabs (dGeomSpherePointDepth + (sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + draw_all_objects (space); + } + + // ********** test tangential grazing - miss + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + dPlaneSpace (q,n,v1); + for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j]; + for (j=0; j<3; j++) q[j] -= n[j]; + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,2); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test tangential grazing - hit + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + dPlaneSpace (q,n,v1); + for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j]; + for (j=0; j<3; j++) q[j] -= n[j]; + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,2); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + PASSED(); +} + + +int test_ray_and_box() +{ + int i,j; + dContactGeom contact; + dVector3 s,p,q,n,q2,q3,q4; // s = box sides + dMatrix3 R; + dReal k; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID box = dCreateBox (0,1,1,1); + dSpaceAdd (space,ray); + dSpaceAdd (space,box); + + // ********** make a random box + + for (j=0; j<3; j++) s[j] = dRandReal() + 0.1; + dGeomBoxSetLengths (box,s[0],s[1],s[2]); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (box,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (box,R); + + // ********** test zero length ray just inside box + + dGeomRaySetLength (ray,0); + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + dGeomSetPosition (ray,q2[0],q2[1],q2[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test zero length ray just outside box + + dGeomRaySetLength (ray,0); + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + dGeomSetPosition (ray,q2[0],q2[1],q2[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally contained inside the box + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j]; + dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1); + for (j=0; j<3; j++) q4[j] += p[j]; + for (j=0; j<3; j++) n[j] = q4[j] - q2[j]; + dNormalize3 (n); + dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4)); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally outside the box + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q3[j] = q2[j] + p[j]; + dNormalize3 (q2); + dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just above surface + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j]; + k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]); + for (j=0; j<3; j++) q2[j] = -q2[j]; + dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]); + dGeomRaySetLength (ray,k*0.99); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just below surface + + dGeomRaySetLength (ray,k*1.01); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + // ********** test contact point position for random rays + + for (j=0; j<3; j++) q[j] = dRandReal()*s[j]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + for (j=0; j<3; j++) q3[j] = dRandReal()-0.5; + dNormalize3 (q3); + dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) { + // check depth of contact point + if (dFabs (dGeomBoxPointDepth + (box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + // check position of contact point + for (j=0; j<3; j++) contact.pos[j] -= p[j]; + dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1); + if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol && + dFabs(dFabs (q[1]) - 0.5*s[1]) > tol && + dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) { + FAILED(); + } + // also check normal signs + if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED(); + + draw_all_objects (space); + } + + PASSED(); +} + + +int test_ray_and_ccylinder() +{ + int j; + dContactGeom contact; + dVector3 p,a,b,n; + dMatrix3 R; + dReal r,l,k,x,y; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID ccyl = dCreateCapsule (0,1,1); + dSpaceAdd (space,ray); + dSpaceAdd (space,ccyl); + + // ********** make a random capped cylinder + + r = dRandReal()*0.5 + 0.01; + l = dRandReal()*1 + 0.01; + dGeomCapsuleSetParams (ccyl,r,l); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (ccyl,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ccyl,R); + + // ********** test ray completely within ccyl + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + k = (dRandReal()-0.5)*l; + for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2]; + for (j=0; j<3; j++) b[j] = dRandReal()-0.5; + dNormalize3 (b); + k = (dRandReal()-0.5)*l; + for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2]; + dGeomRaySetLength (ray,dCalcPointsDistance3(a,b)); + for (j=0; j<3; j++) b[j] -= a[j]; + dNormalize3 (b); + dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray outside ccyl that just misses (between caps) + + k = dRandReal()*2*M_PI; + x = sin(k); + y = cos(k); + for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1]; + k = (dRandReal()-0.5)*l; + for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j]; + dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]); + dGeomRaySetLength (ray,r*0.99); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray outside ccyl that just hits (between caps) + + dGeomRaySetLength (ray,r*1.01); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + // check depth of contact point + if (dFabs (dGeomCapsulePointDepth + (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + // ********** test ray outside ccyl that just misses (caps) + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + if (dCalcVectorDot3_14(a,R+2) < 0) { + for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2]; + } + else { + for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2]; + } + dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]); + dGeomRaySetLength (ray,r*0.99); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray outside ccyl that just hits (caps) + + dGeomRaySetLength (ray,r*1.01); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + // check depth of contact point + if (dFabs (dGeomCapsulePointDepth + (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + // ********** test random rays + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + for (j=0; j<3; j++) n[j] = dRandReal()-0.5; + dNormalize3 (n); + dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,10); + + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) { + // check depth of contact point + if (dFabs (dGeomCapsulePointDepth + (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + // check normal signs + if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED(); + + draw_all_objects (space); + } + + PASSED(); +} + +/* + Test rays within the cylinder + -completely inside + -exiting through side + -exiting through cap + -exiting through corner + Test rays outside the cylinder +*/ +int test_ray_and_cylinder() +{ + dVector3 a,b; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay(space,4); + + // The first thing that happens is the ray is + // rotated into cylinder coordinates. We'll trust that's + // done right. The major axis is in the z-dir. + + + // Random tests + /*b[0]=4*dRandReal()-2; + b[1]=4*dRandReal()-2; + b[2]=4*dRandReal()-2; + a[0]=2*dRandReal()-1; + a[1]=2*dRandReal()-1; + a[2]=2*dRandReal()-1;*/ + + // Inside out + b[0]=dRandReal()-0.5; + b[1]=dRandReal()-0.5; + b[2]=dRandReal()-0.5; + a[0]=2*dRandReal()-1; + a[1]=2*dRandReal()-1; + a[2]=2*dRandReal()-1; + + // Outside in + /*b[0]=4*dRandReal()-2; + b[1]=4*dRandReal()-2; + b[2]=4*dRandReal()-2; + a[0]=-b[0]; + a[1]=-b[1]; + a[2]=-b[2];*/ + + + dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]); + // This is just for visual inspection right now. + //if (dCollide (ray,cyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + draw_all_objects (space); + + PASSED(); +} + + +int test_ray_and_plane() +{ + int j; + dContactGeom contact; + dVector3 n,p,q,a,b,g,h; // n,d = plane parameters + dMatrix3 R; + dReal d; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID plane = dCreatePlane (0,0,0,1,0); + dSpaceAdd (space,ray); + dSpaceAdd (space,plane); + + // ********** make a random plane + + for (j=0; j<3; j++) n[j] = dRandReal() - 0.5; + dNormalize3 (n); + d = dRandReal() - 0.5; + dGeomPlaneSetParams (plane,n[0],n[1],n[2],d); + dPlaneSpace (n,p,q); + + // ********** test finite length ray below plane + + dGeomRaySetLength (ray,0.09); + a[0] = dRandReal()-0.5; + a[1] = dRandReal()-0.5; + a[2] = -dRandReal()*0.5 - 0.1; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + dGeomSetPosition (ray,b[0],b[1],b[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray above plane + + a[0] = dRandReal()-0.5; + a[1] = dRandReal()-0.5; + a[2] = dRandReal()*0.5 + 0.01; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + g[0] = dRandReal()-0.5; + g[1] = dRandReal()-0.5; + g[2] = dRandReal() + 0.01; + for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j]; + dNormalize3 (h); + dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray that intersects plane + + a[0] = dRandReal()-0.5; + a[1] = dRandReal()-0.5; + a[2] = dRandReal()-0.5; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + g[0] = dRandReal()-0.5; + g[1] = dRandReal()-0.5; + g[2] = dRandReal()-0.5; + for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j]; + dNormalize3 (h); + dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) { + // test that contact is on plane surface + if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED(); + // also check normal signs + if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED(); + // also check contact point depth + if (dFabs (dGeomPlanePointDepth + (plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + draw_all_objects (space); + } + + // ********** test ray that just misses + + for (j=0; j<3; j++) b[j] = (1+d)*n[j]; + for (j=0; j<3; j++) h[j] = -n[j]; + dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); + dGeomRaySetLength (ray,0.99); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray that just hits + + dGeomRaySetLength (ray,1.01); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + // ********** test polarity with typical ground plane + + dGeomPlaneSetParams (plane,0,0,1,0); + for (j=0; j<3; j++) a[j] = 0.1; + for (j=0; j<3; j++) b[j] = 0; + a[2] = 1; + b[2] = -1; + dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); + dGeomRaySetLength (ray,2); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + if (dFabs (contact.depth - 1) > tol) FAILED(); + a[2] = -1; + b[2] = 1; + dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + if (dFabs (contact.depth - 1) > tol) FAILED(); + + PASSED(); +} + +//**************************************************************************** +// a really inefficient, but hopefully correct implementation of +// dBoxTouchesBox(), that does 144 edge-face tests. + +// return 1 if edge v1 -> v2 hits the rectangle described by p1,p2,p3 + +static int edgeIntersectsRect (dVector3 v1, dVector3 v2, + dVector3 p1, dVector3 p2, dVector3 p3) +{ + int k; + dVector3 u1, u2, n, tmp; + + for (k=0; k < 3; k++) u1[k] = p3[k] - p1[k]; + for (k=0; k < 3; k++) u2[k] = p2[k] - p1[k]; + + dReal d1 = dSqrt(dCalcVectorDot3(u1, u1)); + dReal d2 = dSqrt(dCalcVectorDot3(u2, u2)); + dNormalize3(u1); + dNormalize3(u2); + + dReal error; +#ifdef dSINGLE + const dReal uEpsilon = 1e-5, pEpsilon = 1e-6, tmpEpsilon = 1.5e-4; +#else + const dReal uEpsilon = 1e-6, pEpsilon = 1e-8, tmpEpsilon = 1e-6; +#endif + + error = dFabs(dCalcVectorDot3(u1, u2)); + if (error > uEpsilon) dDebug(0, "bad u1/u2"); + + dCalcVectorCross3(n, u1, u2); + + for (k=0; k < 3; k++) tmp[k] = v2[k] - v1[k]; + + dReal d = -dCalcVectorDot3(n, p1); + + error = dFabs(dCalcVectorDot3(n, p1) + d); + if (error > pEpsilon) dDebug(0, "bad n wrt p1"); + + error = dFabs(dCalcVectorDot3(n, p2) + d); + if (error > pEpsilon) dDebug(0, "bad n wrt p2"); + + error = dFabs(dCalcVectorDot3(n, p3) + d); + if (error > pEpsilon) dDebug(0, "bad n wrt p3"); + + dReal alpha = -(d + dCalcVectorDot3(n, v1)) / dCalcVectorDot3(n, tmp); + for (k=0; k < 3; k++) tmp[k] = v1[k] + alpha * (v2[k] - v1[k]); + + error = dFabs(dCalcVectorDot3(n, tmp) + d); + if (error > tmpEpsilon) dDebug(0, "bad tmp"); + + if (alpha < 0) return 0; + if (alpha > 1) return 0; + + for (k=0; k < 3; k++) tmp[k] -= p1[k]; + dReal a1 = dCalcVectorDot3(u1, tmp); + dReal a2 = dCalcVectorDot3(u2, tmp); + if (a1 < 0 || a2 < 0 || a1 > d1 || a2 > d2) return 0; + + return 1; +} + + +// return 1 if box 1 is completely inside box 2 + +static int box1inside2 (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2) +{ + for (int i=-1; i<=1; i+=2) { + for (int j=-1; j<=1; j+=2) { + for (int k=-1; k<=1; k+=2) { + dVector3 v,vv; + v[0] = i*0.5*side1[0]; + v[1] = j*0.5*side1[1]; + v[2] = k*0.5*side1[2]; + dMultiply0_331 (vv,R1,v); + vv[0] += p1[0] - p2[0]; + vv[1] += p1[1] - p2[1]; + vv[2] += p1[2] - p2[2]; + for (int axis=0; axis < 3; axis++) { + dReal z = dCalcVectorDot3_14(vv,R2+axis); + if (z < (-side2[axis]*0.5) || z > (side2[axis]*0.5)) return 0; + } + } + } + } + return 1; +} + + +// test if any edge from box 1 hits a face from box 2 + +static int testBoxesTouch2 (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2) +{ + int j,k,j1,j2; + + // for 6 faces from box 2 + for (int fd=0; fd<3; fd++) { // direction for face + + for (int fo=0; fo<2; fo++) { // offset of face + // get four points on the face. first get 2 indexes that are not fd + int k1=0,k2=0; + if (fd==0) { k1 = 1; k2 = 2; } + if (fd==1) { k1 = 0; k2 = 2; } + if (fd==2) { k1 = 0; k2 = 1; } + dVector3 fp[4],tmp; + k=0; + for (j1=-1; j1<=1; j1+=2) { + for (j2=-1; j2<=1; j2+=2) { + fp[k][k1] = j1; + fp[k][k2] = j2; + fp[k][fd] = fo*2-1; + k++; + } + } + for (j=0; j<4; j++) { + for (k=0; k<3; k++) fp[j][k] *= 0.5*side2[k]; + dMultiply0_331 (tmp,R2,fp[j]); + for (k=0; k<3; k++) fp[j][k] = tmp[k] + p2[k]; + } + + // for 8 vertices + dReal v1[3]; + for (v1[0]=-1; v1[0] <= 1; v1[0] += 2) { + for (v1[1]=-1; v1[1] <= 1; v1[1] += 2) { + for (v1[2]=-1; v1[2] <= 1; v1[2] += 2) { + // for all possible +ve leading edges from those vertices + for (int ei=0; ei < 3; ei ++) { + if (v1[ei] < 0) { + // get vertex1 -> vertex2 = an edge from box 1 + dVector3 vv1,vv2; + for (k=0; k<3; k++) vv1[k] = v1[k] * 0.5*side1[k]; + for (k=0; k<3; k++) vv2[k] = (v1[k] + (k==ei)*2)*0.5*side1[k]; + dVector3 vertex1,vertex2; + dMultiply0_331 (vertex1,R1,vv1); + dMultiply0_331 (vertex2,R1,vv2); + for (k=0; k<3; k++) vertex1[k] += p1[k]; + for (k=0; k<3; k++) vertex2[k] += p1[k]; + + // see if vertex1 -> vertex2 interesects face + if (edgeIntersectsRect (vertex1,vertex2,fp[0],fp[1],fp[2])) + return 1; + } + } + } + } + } + } + } + + if (box1inside2 (p1,R1,side1,p2,R2,side2)) return 1; + if (box1inside2 (p2,R2,side2,p1,R1,side1)) return 1; + + return 0; +} + +//**************************************************************************** +// dBoxTouchesBox() test + +int test_dBoxTouchesBox() +{ + int k,bt1,bt2; + dVector3 p1,p2,side1,side2; + dMatrix3 R1,R2; + + dSimpleSpace space(0); + dGeomID box1 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box1); + dGeomID box2 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box2); + + dMakeRandomVector (p1,3,0.5); + dMakeRandomVector (p2,3,0.5); + for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01; + for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01; + dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + + dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]); + dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]); + dGeomSetPosition (box1,p1[0],p1[1],p1[2]); + dGeomSetRotation (box1,R1); + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + dGeomSetRotation (box2,R2); + draw_all_objects (space); + + int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2); + int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1); + bt1 = t1 || t2; + bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2); + + if (bt1 != bt2) FAILED(); + + /* + // some more debugging info if necessary + if (bt1 && bt2) printf ("agree - boxes touch\n"); + if (!bt1 && !bt2) printf ("agree - boxes don't touch\n"); + if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox " + "says no\n"); + if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox " + "says yes\n"); + */ + + PASSED(); +} + +//**************************************************************************** +// test box-box collision + +int test_dBoxBox() +{ + int k,bt; + dVector3 p1,p2,side1,side2,normal,normal2; + dMatrix3 R1,R2; + dReal depth,depth2; + int code; + dContactGeom contact[48]; + + dSimpleSpace space(0); + dGeomID box1 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box1); + dGeomID box2 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box2); + + dMakeRandomVector (p1,3,0.5); + dMakeRandomVector (p2,3,0.5); + for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01; + for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01; + + dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + + // dRSetIdentity (R1); // we can also try this + // dRSetIdentity (R2); + + dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]); + dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]); + dGeomSetPosition (box1,p1[0],p1[1],p1[2]); + dGeomSetRotation (box1,R1); + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + dGeomSetRotation (box2,R2); + + code = 0; + depth = 0; + bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact, + sizeof(dContactGeom)); + if (bt==1) { + p2[0] += normal[0] * 0.96 * depth; + p2[1] += normal[1] * 0.96 * depth; + p2[2] += normal[2] * 0.96 * depth; + bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact, + sizeof(dContactGeom)); + + /* + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + draw_all_objects (space); + */ + + if (bt != 1) { + FAILED(); + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + draw_all_objects (space); + } + + p2[0] += normal[0] * 0.08 * depth; + p2[1] += normal[1] * 0.08 * depth; + p2[2] += normal[2] * 0.08 * depth; + bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact, + sizeof(dContactGeom)); + if (bt != 0) FAILED(); + + // dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + // draw_all_objects (space); + } + + // printf ("code=%2d depth=%.4f ",code,depth); + + PASSED(); +} + +//**************************************************************************** +// graphics + +int space_pressed = 0; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.4807,-1.8023,2.7600}; + static float hpr[3] = {141.5000,-18.5000,0.0000}; + dsSetViewpoint (xyz,hpr); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == ' ') space_pressed = 1; +} + + +// simulation loop + +static void simLoop (int) +{ + do { + draw_all_objects_called = 0; + unsigned long seed = dRandGetSeed(); + testslot[graphical_test].test_fn(); + if (draw_all_objects_called) { + if (space_pressed) space_pressed = 0; else dRandSetSeed (seed); + } + } + while (!draw_all_objects_called); +} + +//**************************************************************************** +// do all the tests + +void do_tests (int argc, char **argv) +{ + int i,j; + + // process command line arguments + if (argc >= 2) { + graphical_test = atoi (argv[1]); + } + + if (graphical_test) { + // do one test gaphically and interactively + + if (graphical_test < 1 || graphical_test >= MAX_TESTS || + !testslot[graphical_test].name) { + dError (0,"invalid test number"); + } + + printf ("performing test: %s\n",testslot[graphical_test].name); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dsSetSphereQuality (3); + dsSetCapsuleQuality (8); + dsSimulationLoop (argc,argv,1280,900,&fn); + } + else { + // do all tests noninteractively + + for (i=0; i<MAX_TESTS; i++) testslot[i].number = i; + + // first put the active tests into a separate array + int n=0; + for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) n++; + TestSlot **ts = (TestSlot**) malloc (n * sizeof(TestSlot*)); + j = 0; + for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) ts[j++] = testslot+i; + if (j != n) dDebug (0,"internal"); + + // do two test batches. the first test batch has far fewer reps and will + // catch problems quickly. if all tests in the first batch passes, the + // second batch is run. + + for (i=0; i<n; i++) ts[i]->failcount = 0; + int total_reps=0; + for (int batch=0; batch<2; batch++) { + int reps = (batch==0) ? TEST_REPS1 : TEST_REPS2; + total_reps += reps; + printf ("testing batch %d (%d reps)...\n",batch+1,reps); + + // run tests + for (j=0; j<reps; j++) { + for (i=0; i<n; i++) { + current_test = ts[i]->number; + if (ts[i]->test_fn() != 1) ts[i]->failcount++; + } + } + + // check for failures + int total_fail_count=0; + for (i=0; i<n; i++) total_fail_count += ts[i]->failcount; + if (total_fail_count) break; + } + + // print results + for (i=0; i<n; i++) { + printf ("%3d: %-30s: ",ts[i]->number,ts[i]->name); + if (ts[i]->failcount) { + printf ("FAILED (%.2f%%) at line %d\n", + double(ts[i]->failcount)/double(total_reps)*100.0, + ts[i]->last_failed_line); + } + else { + printf ("ok\n"); + } + } + } +} + +//**************************************************************************** + +int main (int argc, char **argv) +{ + // setup all tests + + memset (testslot,0,sizeof(testslot)); + dInitODE2(0); + + MAKE_TEST(1,test_sphere_point_depth); + MAKE_TEST(2,test_box_point_depth); + MAKE_TEST(3,test_ccylinder_point_depth); + MAKE_TEST(4,test_plane_point_depth); + + MAKE_TEST(10,test_ray_and_sphere); + MAKE_TEST(11,test_ray_and_box); + MAKE_TEST(12,test_ray_and_ccylinder); + MAKE_TEST(13,test_ray_and_plane); + MAKE_TEST(14,test_ray_and_cylinder); + + MAKE_TEST(100,test_dBoxTouchesBox); + MAKE_TEST(101,test_dBoxBox); + + do_tests (argc,argv); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_convex.cpp b/libs/ode-0.16.1/ode/demo/demo_convex.cpp new file mode 100644 index 0000000..eea5c6e --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_convex.cpp @@ -0,0 +1,307 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Convex demo. +// Serves as a test for the convex geometry. +// By Bram Stolk. + +#include <assert.h> +#ifdef HAVE_UNISTD_H +# include <unistd.h> +#endif +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#include "halton235_geom.h" + +#ifdef dDOUBLE +# define dsDrawConvex dsDrawConvexD +# define dsDrawLine dsDrawLineD +#endif + + +#ifdef _MSC_VER +# pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +// Height at which we drop the composite block. +const dReal H=4.20; + +static dWorldID world; +static dSpaceID space; + +static dBodyID mbody; + +static dBodyID hbody[ halton_numc ]; +static dGeomID hgeom[ halton_numc ]; + +static dJointGroupID contactgroup; + +static bool drawpos=false; +static bool solidkernel=false; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback(void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i<n; i++) + { + contact[i].surface.slip1 = 0.7; + contact[i].surface.slip2 = 0.7; + contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2; + contact[i].surface.mu = 500.0; // was: dInfinity + contact[i].surface.soft_erp = 0.50; + contact[i].surface.soft_cfm = 0.03; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach + ( + c, + dGeomGetBody(contact[i].geom.g1), + dGeomGetBody(contact[i].geom.g2) + ); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + static float xyz[3] = {-8,0,5}; + static float hpr[3] = {0.0f,-29.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + fprintf(stderr,"Press SPACE to reset the simulation.\n"); +} + + +static void reset() +{ + dQuaternion q; + dQSetIdentity(q); + dBodySetPosition(mbody,0,0,0+H); + dBodySetQuaternion(mbody, q); + dBodySetLinearVel(mbody, 0,0,0); + dBodySetAngularVel(mbody, 0,0,0); + dBodyEnable(mbody); + for ( int i=0; i<halton_numc; ++i ) + { + dBodyID body = hbody[i]; + if ( !body ) continue; + dBodySetPosition(body, halton_pos[i][0], halton_pos[i][1], halton_pos[i][2]+H); + dBodySetQuaternion(body, q); + dBodySetLinearVel(body, 0,0,0); + dBodySetAngularVel(body, 0,0,0); + dBodyEnable(body); + } +} + + +// called when a key pressed + +static void command(int cmd) +{ + switch (cmd) + { + case ' ': + reset(); + break; + default: + break; + } +} + + + +static void simLoop(int pause) +{ + double simstep = 1/240.0; + double dt = dsElapsedTime(); + + int nrofsteps = (int) ceilf(dt/simstep); + nrofsteps = nrofsteps > 8 ? 8 : nrofsteps; + + for (int i=0; i<nrofsteps && !pause; i++) + { + dSpaceCollide (space,0,&nearCallback); + dWorldQuickStep (world, simstep); + dJointGroupEmpty (contactgroup); + } + + dsSetColor (1,1,1); + // Draw the convex objects. + for ( int i=0; i<halton_numc; ++i ) + { + dGeomID geom = hgeom[i]; + dBodyID body = dGeomGetBody(geom); + //const dReal *pos = dBodyGetPosition(body); + //const dReal *rot = dBodyGetRotation(body); + const dReal *pos = dGeomGetPosition(geom); + const dReal *rot = dGeomGetRotation(geom); + dsDrawConvex + ( + pos, rot, + halton_planes[i], + halton_numf[i], + halton_verts[i], + halton_numv[i], + halton_faces[i] + ); + } + + if (drawpos) + { + dsSetColor(1,0,0.2); + dsSetTexture(DS_NONE); + const dReal l = 0.35; + for ( int i=0; i<halton_numc; ++i ) + { + dBodyID body = hbody[i]; + const dReal *pos = dBodyGetPosition(body); + dReal x0[3] = { pos[0]-l, pos[1], pos[2] }; + dReal x1[3] = { pos[0]+l, pos[1], pos[2] }; + dReal y0[3] = { pos[0], pos[1]-l, pos[2] }; + dReal y1[3] = { pos[0], pos[1]+l, pos[2] }; + dReal z0[3] = { pos[0], pos[1], pos[2]-l }; + dReal z1[3] = { pos[0], pos[1], pos[2]+l }; + dsDrawLine(x0,x1); + dsDrawLine(y0,y1); + dsDrawLine(z0,z1); + } + } +} + + +int main (int argc, char **argv) +{ + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + dHashSpaceSetLevels(space, -3, 5); + dCreatePlane(space,0,0,1,0); // Add a ground plane. + + contactgroup = dJointGroupCreate (0); + dWorldSetGravity(world,0,0,-9.8); + dWorldSetQuickStepNumIterations(world, 32); + dWorldSetContactMaxCorrectingVel(world, 40); + dWorldSetMaxAngularSpeed(world, 62.8); + dWorldSetERP(world, 0.7); + dWorldSetQuickStepW(world, 0.75); // For increased stability. + + dWorldSetAutoDisableFlag( world, true ); + dWorldSetAutoDisableLinearThreshold( world, 0.01 ); + dWorldSetAutoDisableAngularThreshold( world, 0.03 ); + dWorldSetAutoDisableTime( world, 0.15f ); + + const float kernelrad = 0.7; + + mbody = dBodyCreate(world); + dBodySetPosition(mbody, 0,0,0+H); + dMassSetSphere( &m, 5, kernelrad ); + dBodySetMass( mbody, &m ); + + for (int i=0; i<halton_numc; ++i ) + { + dGeomID geom = dCreateConvex + ( + space, + halton_planes[i], + halton_numf[i], + halton_verts[i], + halton_numv[i], + halton_faces[i] + ); + hgeom[i] = geom; + const dReal x = halton_pos[i][0]; + const dReal y = halton_pos[i][1]; + const dReal z = halton_pos[i][2]; + const dReal dsqr = x*x + y*y + z*z; + + if ( dsqr < kernelrad*kernelrad && solidkernel ) + { + dGeomSetBody(geom, mbody); + dGeomSetOffsetPosition(geom, x,y,z); + } + else + { + dBodyID body = dBodyCreate(world); + hbody[i] = body; + dBodySetPosition(body, x,y,z+H); + dReal volu = halton_volu[i]; + dReal rad = pow( volu * 3 / (4*M_PI), (1/3.0) ); + dMassSetSphere( &m,5,rad ); + dBodySetMass( body,&m ); +#if 1 + dBodySetLinearDamping (body, 0.0005); + dBodySetAngularDamping(body, 0.0300); +#endif + dGeomSetBody(geom,body); + } + } + + // run simulation + const int w=1280; + const int h=720; + dsSimulationLoop (argc,argv,w,h,&fn); + + dJointGroupEmpty (contactgroup); + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + + diff --git a/libs/ode-0.16.1/ode/demo/demo_crash.cpp b/libs/ode-0.16.1/ode/demo/demo_crash.cpp new file mode 100644 index 0000000..38ec1ad --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_crash.cpp @@ -0,0 +1,652 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// This is a demo of the QuickStep and StepFast methods, +// originally by David Whittaker. + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define LENGTH 3.5 // chassis length +#define WIDTH 2.5 // chassis width +#define HEIGHT 1.0 // chassis height +#define RADIUS 0.5 // wheel radius +#define STARTZ 1.0 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 1 // wheel mass +#define COMOFFSET -5 // center of mass offset +#define WALLMASS 1 // wall box mass +#define BALLMASS 1 // ball mass +#define FMAX 25 // car engine fmax +#define ROWS 1 // rows of cars +#define COLS 1 // columns of cars +#define ITERS 20 // number of iterations +#define WBOXSIZE 1.0 // size of wall boxes +#define WALLWIDTH 12 // width of wall +#define WALLHEIGHT 10 // height of wall +#define DISABLE_THRESHOLD 0.008 // maximum velocity (squared) a body can have and be disabled +#define DISABLE_STEPS 10 // number of steps a box has to have been disable-able before it will be disabled +#define CANNON_X -10 // x position of cannon +#define CANNON_Y 5 // y position of cannon +#define CANNON_BALL_MASS 10 // mass of the cannon ball +#define CANNON_BALL_RADIUS 0.5 + +static const dVector3 xunit = { 1, 0, 0 }, yunit = { 0, 1, 0 }, zpunit = { 0, 0, 1 }, zmunit = { 0, 0, -1 }; + +//#define BOX +#define CARS +#define WALL +//#define BALLS +//#define BALLSTACK +//#define ONEBALL +//#define CENTIPEDE +#define CANNON + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +static dThreadingImplementationID threading; +static dThreadingThreadPoolID pool; +static dBodyID body[10000]; +static int bodies; +static dJointID joint[100000]; +static int joints; +static dJointGroupID contactgroup; +static dGeomID ground; +static dGeomID box[10000]; +static int boxes; +static dGeomID sphere[10000]; +static int spheres; +static dGeomID wall_boxes[10000]; +static dBodyID wall_bodies[10000]; +static dGeomID cannon_ball_geom; +static dBodyID cannon_ball_body; +static int wb_stepsdis[10000]; +static int wb; +static bool doFast; +static dBodyID b; +static dMass m; + + +// things that the user controls + +static dReal turn = 0, speed = 0; // user commands +static dReal cannon_angle=0,cannon_elevation=-1.2; + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnected(b1, b2)) + return; + + const int N = 4; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i<n; i++) { + contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; + if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass) + contact[i].surface.mu = 20; + else + contact[i].surface.mu = 0.5; + contact[i].surface.slip1 = 0.0; + contact[i].surface.slip2 = 0.0; + contact[i].surface.soft_erp = 0.8; + contact[i].surface.soft_cfm = 0.01; + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2)); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {3.8548f,9.0843f,7.5900f}; + static float hpr[3] = {-145.5f,-22.5f,0.25f}; + dsSetViewpoint (xyz,hpr); + printf ("Press:\t'a' to increase speed.\n" + "\t'z' to decrease speed.\n" + "\t',' to steer left.\n" + "\t'.' to steer right.\n" + "\t' ' to reset speed and steering.\n" + "\t'[' to turn the cannon left.\n" + "\t']' to turn the cannon right.\n" + "\t'1' to raise the cannon.\n" + "\t'2' to lower the cannon.\n" + "\t'x' to shoot from the cannon.\n" + "\t'f' to toggle fast step mode.\n" + "\t'r' to reset simulation.\n"); +} + + +void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI) +{ + int i; + dMass m; + + // chassis body + body[bodyI] = dBodyCreate (world); + dBodySetPosition (body[bodyI],x,y,STARTZ); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m,CMASS/2.0); + dBodySetMass (body[bodyI],&m); + box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[boxI],body[bodyI]); + + // wheel bodies + for (i=1; i<=4; i++) { + body[bodyI+i] = dBodyCreate (world); + dQuaternion q; + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); + dBodySetQuaternion (body[bodyI+i],q); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m,WMASS); + dBodySetMass (body[bodyI+i],&m); + sphere[sphereI+i-1] = dCreateSphere (space,RADIUS); + dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]); + } + dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); + + // front and back wheel hinges + for (i=0; i<4; i++) { + joint[jointI+i] = dJointCreateHinge2 (world,0); + dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]); + const dReal *a = dBodyGetPosition (body[bodyI+i+1]); + dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]); + dJointSetHinge2Axes (joint[jointI+i], (i<2 ? zpunit : zmunit), yunit); + dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8); + dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5); + dJointSetHinge2Param (joint[jointI+i],dParamVel2,0); + dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX); + } + + //center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint) + dBodyID b = dBodyCreate (world); + dBodySetPosition (b,x,y,STARTZ+COMOFFSET); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m,CMASS/2.0); + dBodySetMass (b,&m); + dJointID j = dJointCreateFixed(world, 0); + dJointAttach(j, body[bodyI], b); + dJointSetFixed(j); + //box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT); + //dGeomSetBody (box[boxI+1],b); + + bodyI += 5; + jointI += 4; + boxI += 1; + sphereI += 4; +} + +static +void shutdownSimulation() +{ + // destroy world if it exists + if (bodies) + { + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dWorldSetStepThreadingImplementation(world, NULL, NULL); + dThreadingFreeImplementation(threading); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + + bodies = 0; + } +} + +static +void setupSimulation() +{ + int i; + for (i = 0; i < 1000; i++) + wb_stepsdis[i] = 0; + + // recreate world + + world = dWorldCreate(); + +// space = dHashSpaceCreate( 0 ); +// space = dSimpleSpaceCreate( 0 ); + space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ ); + + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-1.5); + dWorldSetCFM (world, 1e-5); + dWorldSetERP (world, 0.8); + dWorldSetQuickStepNumIterations (world,ITERS); + + threading = dThreadingAllocateMultiThreadedImplementation(); + pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); + dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); + + + ground = dCreatePlane (space,0,0,1,0); + + bodies = 0; + joints = 0; + boxes = 0; + spheres = 0; + wb = 0; + +#ifdef CARS + for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS) + for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2) + makeCar(x, y, bodies, joints, boxes, spheres); +#endif +#ifdef WALL + bool offset = false; + for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE) + { + offset = !offset; + for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE) + { + wall_bodies[wb] = dBodyCreate (world); + dBodySetPosition (wall_bodies[wb],-20,y,z); + dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE); + dMassAdjust (&m, WALLMASS); + dBodySetMass (wall_bodies[wb],&m); + wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE); + dGeomSetBody (wall_boxes[wb],wall_bodies[wb]); + //dBodyDisable(wall_bodies[wb++]); + wb++; + } + } + dMessage(0,"wall boxes: %i", wb); +#endif +#ifdef BALLS + for (dReal x = -7; x <= -4; x+=1) + for (dReal y = -1.5; y <= 1.5; y+=1) + for (dReal z = 1; z <= 4; z+=1) + { + b = dBodyCreate (world); + dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m, BALLMASS); + dBodySetMass (b,&m); + sphere[spheres] = dCreateSphere (space,RADIUS); + dGeomSetBody (sphere[spheres++],b); + } +#endif +#ifdef ONEBALL + b = dBodyCreate (world); + dBodySetPosition (b,0,0,2); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m, 1); + dBodySetMass (b,&m); + sphere[spheres] = dCreateSphere (space,RADIUS); + dGeomSetBody (sphere[spheres++],b); +#endif +#ifdef BALLSTACK + for (dReal z = 1; z <= 6; z+=1) + { + b = dBodyCreate (world); + dBodySetPosition (b,0,0,z*RADIUS*2); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m, 0.1); + dBodySetMass (b,&m); + sphere[spheres] = dCreateSphere (space,RADIUS); + dGeomSetBody (sphere[spheres++],b); + } +#endif +#ifdef CENTIPEDE + dBodyID lastb = 0; + for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1) + { + // chassis body + + b = body[bodies] = dBodyCreate (world); + dBodySetPosition (body[bodies],-15,y,STARTZ); + dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT); + dMassAdjust (&m,CMASS); + dBodySetMass (body[bodies],&m); + box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT); + dGeomSetBody (box[boxes++],body[bodies++]); + + for (dReal x = -17; x > -20; x-=RADIUS*2) + { + body[bodies] = dBodyCreate (world); + dBodySetPosition(body[bodies], x, y, STARTZ); + dMassSetSphere(&m, 1, RADIUS); + dMassAdjust(&m, WMASS); + dBodySetMass(body[bodies], &m); + sphere[spheres] = dCreateSphere (space, RADIUS); + dGeomSetBody (sphere[spheres++], body[bodies]); + + joint[joints] = dJointCreateHinge2 (world,0); + if (x == -17) + dJointAttach (joint[joints],b,body[bodies]); + else + dJointAttach (joint[joints],body[bodies-2],body[bodies]); + const dReal *a = dBodyGetPosition (body[bodies++]); + dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]); + dJointSetHinge2Axes (joint[joints], zpunit, xunit); + dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); + dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); + dJointSetHinge2Param (joint[joints],dParamLoStop,0); + dJointSetHinge2Param (joint[joints],dParamHiStop,0); + dJointSetHinge2Param (joint[joints],dParamVel2,-10.0); + dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); + + body[bodies] = dBodyCreate (world); + dBodySetPosition(body[bodies], -30 - x, y, STARTZ); + dMassSetSphere(&m, 1, RADIUS); + dMassAdjust(&m, WMASS); + dBodySetMass(body[bodies], &m); + sphere[spheres] = dCreateSphere (space, RADIUS); + dGeomSetBody (sphere[spheres++], body[bodies]); + + joint[joints] = dJointCreateHinge2 (world,0); + if (x == -17) + dJointAttach (joint[joints],b,body[bodies]); + else + dJointAttach (joint[joints],body[bodies-2],body[bodies]); + const dReal *b = dBodyGetPosition (body[bodies++]); + dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]); + dJointSetHinge2Axes (joint[joints], zpunit, xunit); + dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); + dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); + dJointSetHinge2Param (joint[joints],dParamLoStop,0); + dJointSetHinge2Param (joint[joints],dParamHiStop,0); + dJointSetHinge2Param (joint[joints],dParamVel2,10.0); + dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); + } + if (lastb) + { + dJointID j = dJointCreateFixed(world,0); + dJointAttach (j, b, lastb); + dJointSetFixed(j); + } + lastb = b; + } +#endif +#ifdef BOX + body[bodies] = dBodyCreate (world); + dBodySetPosition (body[bodies],0,0,HEIGHT/2); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m, 1); + dBodySetMass (body[bodies],&m); + box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[boxes++],body[bodies++]); +#endif +#ifdef CANNON + cannon_ball_body = dBodyCreate (world); + cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS); + dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS); + dBodySetMass (cannon_ball_body,&m); + dGeomSetBody (cannon_ball_geom,cannon_ball_body); + dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS); +#endif +} + +// called when a key pressed + +static void command (int cmd) +{ + switch (cmd) { + case 'a': case 'A': + speed += 0.3; + break; + case 'z': case 'Z': + speed -= 0.3; + break; + case ',': + turn += 0.1; + if (turn > 0.3) + turn = 0.3; + break; + case '.': + turn -= 0.1; + if (turn < -0.3) + turn = -0.3; + break; + case ' ': + speed = 0; + turn = 0; + break; + case 'f': case 'F': + doFast = !doFast; + break; + case 'r': case 'R': + shutdownSimulation(); + setupSimulation(); + break; + case '[': + cannon_angle += 0.1; + break; + case ']': + cannon_angle -= 0.1; + break; + case '1': + cannon_elevation += 0.1; + break; + case '2': + cannon_elevation -= 0.1; + break; + case 'x': case 'X': { + dMatrix3 R2,R3,R4; + dRFromAxisAndAngle (R2,0,0,1,cannon_angle); + dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); + dMultiply0 (R4,R2,R3,3,3,3); + dReal cpos[3] = {CANNON_X,CANNON_Y,1}; + for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2]; + dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]); + dReal force = 10; + dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]); + dBodySetAngularVel (cannon_ball_body,0,0,0); + break; + } + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + int i, j; + + dsSetTexture (DS_WOOD); + + if (!pause) { +#ifdef BOX + dBodyAddForce(body[bodies-1],lspeed,0,0); +#endif + for (j = 0; j < joints; j++) + { + dReal curturn = dJointGetHinge2Angle1 (joint[j]); + //dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0); + dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0); + dJointSetHinge2Param(joint[j],dParamFMax,dInfinity); + dJointSetHinge2Param(joint[j],dParamVel2,speed); + dJointSetHinge2Param(joint[j],dParamFMax2,FMAX); + dBodyEnable(dJointGetBody(joint[j],0)); + dBodyEnable(dJointGetBody(joint[j],1)); + } + if (doFast) + { + dSpaceCollide (space,0,&nearCallback); + dWorldQuickStep (world,0.05); + dJointGroupEmpty (contactgroup); + } + else + { + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + dJointGroupEmpty (contactgroup); + } + + for (i = 0; i < wb; i++) + { + b = dGeomGetBody(wall_boxes[i]); + if (dBodyIsEnabled(b)) + { + bool disable = true; + const dReal *lvel = dBodyGetLinearVel(b); + dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2]; + if (lspeed > DISABLE_THRESHOLD) + disable = false; + const dReal *avel = dBodyGetAngularVel(b); + dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2]; + if (aspeed > DISABLE_THRESHOLD) + disable = false; + + if (disable) + wb_stepsdis[i]++; + else + wb_stepsdis[i] = 0; + + if (wb_stepsdis[i] > DISABLE_STEPS) + { + dBodyDisable(b); + dsSetColor(0.5,0.5,1); + } + else + dsSetColor(1,1,1); + + } + else + dsSetColor(0.4,0.4,0.4); + dVector3 ss; + dGeomBoxGetLengths (wall_boxes[i], ss); + dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); + } + } + else + { + for (i = 0; i < wb; i++) + { + b = dGeomGetBody(wall_boxes[i]); + if (dBodyIsEnabled(b)) + dsSetColor(1,1,1); + else + dsSetColor(0.4,0.4,0.4); + dVector3 ss; + dGeomBoxGetLengths (wall_boxes[i], ss); + dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); + } + } + + dsSetColor (0,1,1); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + for (i = 0; i < boxes; i++) + dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides); + dsSetColor (1,1,1); + for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]), + dGeomGetRotation(sphere[i]),RADIUS); + + // draw the cannon + dsSetColor (1,1,0); + dMatrix3 R2,R3,R4; + dRFromAxisAndAngle (R2,0,0,1,cannon_angle); + dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); + dMultiply0 (R4,R2,R3,3,3,3); + dReal cpos[3] = {CANNON_X,CANNON_Y,1}; + dReal csides[3] = {2,2,2}; + dsDrawBox (cpos,R2,csides); + for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2]; + dsDrawCylinder (cpos,R4,3,0.5); + + // draw the cannon ball + dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body), + CANNON_BALL_RADIUS); +} + +int main (int argc, char **argv) +{ + doFast = true; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + + bodies = 0; + joints = 0; + boxes = 0; + spheres = 0; + + setupSimulation(); + + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(8, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); + dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dWorldSetStepThreadingImplementation(world, NULL, NULL); + dThreadingFreeImplementation(threading); + + shutdownSimulation(); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_cyl.cpp b/libs/ode-0.16.1/ode/demo/demo_cyl.cpp new file mode 100644 index 0000000..368a0c1 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_cyl.cpp @@ -0,0 +1,321 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Test for non-capped cylinder, by Bram Stolk +#include <ode/odeconfig.h> +#include <assert.h> +#ifdef HAVE_UNISTD_H +#include <unistd.h> +#endif +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#include "world_geom3.h" // this is our world mesh + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +#define BOX +#define CYL + +// some constants + +#define RADIUS 0.22 // wheel radius +#define WMASS 0.2 // wheel mass +#define WHEELW 0.2 // wheel width +#define BOXSZ 0.4 // box size +//#define CYL_GEOM_OFFSET // rotate cylinder using geom offset + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +#ifdef BOX +static dBodyID boxbody; +static dGeomID boxgeom; +#endif +#ifdef CYL +static dBodyID cylbody; +static dGeomID cylgeom; +#endif +static dJointGroupID contactgroup; +static dGeomID world_mesh; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + +// fprintf(stderr,"testing geoms %p %p\n", o1, o2); + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i<n; i++) + { + contact[i].surface.slip1 = 0.7; + contact[i].surface.slip2 = 0.7; + contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2; + contact[i].surface.mu = 50.0; // was: dInfinity + contact[i].surface.soft_erp = 0.96; + contact[i].surface.soft_cfm = 0.04; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c, + dGeomGetBody(contact[i].geom.g1), + dGeomGetBody(contact[i].geom.g2)); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {-8,-9,3}; + static float hpr[3] = {45.0000f,-27.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + + +static void reset_state(void) +{ + float sx=-4, sy=-4, sz=2; + dQuaternion q; + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); +#ifdef BOX + dBodySetPosition (boxbody, sx, sy+1, sz); + dBodySetLinearVel (boxbody, 0,0,0); + dBodySetAngularVel (boxbody, 0,0,0); + dBodySetQuaternion (boxbody, q); +#endif +#ifdef CYL + dBodySetPosition (cylbody, sx, sy, sz); + dBodySetLinearVel (cylbody, 0,0,0); + dBodySetAngularVel (cylbody, 0,0,0); + dBodySetQuaternion (cylbody, q); +#endif +} + + +// called when a key pressed + +static void command (int cmd) +{ + switch (cmd) + { + case ' ': + reset_state(); + break; + } +} + + + +// simulation loop + +static void simLoop (int pause) +{ + double simstep = 0.005; // 5ms simulation steps + double dt = dsElapsedTime(); + int nrofsteps = (int) ceilf(dt/simstep); + for (int i=0; i<nrofsteps && !pause; i++) + { + dSpaceCollide (space,0,&nearCallback); + dWorldQuickStep (world, simstep); + dJointGroupEmpty (contactgroup); + } + + dsSetColor (1,1,1); +#ifdef BOX + const dReal *BPos = dBodyGetPosition(boxbody); + const dReal *BRot = dBodyGetRotation(boxbody); + float bpos[3] = {BPos[0], BPos[1], BPos[2]}; + float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] }; + float sides[3] = {BOXSZ, BOXSZ, BOXSZ}; + dsDrawBox + ( + bpos, + brot, + sides + ); // single precision +#endif +#ifdef CYL + const dReal *CPos = dGeomGetPosition(cylgeom); + const dReal *CRot = dGeomGetRotation(cylgeom); + float cpos[3] = {CPos[0], CPos[1], CPos[2]}; + float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] }; + dsDrawCylinder + ( +// dBodyGetPosition(cylbody), +// dBodyGetRotation(cylbody), + cpos, + crot, + WHEELW, + RADIUS + ); // single precision +#endif + + // draw world trimesh + dsSetColor(0.7,0.7,0.4); + dsSetTexture (DS_NONE); + + const dReal* Pos = dGeomGetPosition(world_mesh); + float pos[3] = { Pos[0], Pos[1], Pos[2] }; + + const dReal* Rot = dGeomGetRotation(world_mesh); + float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] }; + + int numi = sizeof(world_indices) / sizeof(dTriIndex); + + for (int i=0; i<numi/3; i++) + { + int i0 = world_indices[i*3+0]; + int i1 = world_indices[i*3+1]; + int i2 = world_indices[i*3+2]; + float *v0 = world_vertices+i0*3; + float *v1 = world_vertices+i1*3; + float *v2 = world_vertices+i2*3; + dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw + } +} + + +int main (int argc, char **argv) +{ + dMass m; + dMatrix3 R; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-9.8); + dWorldSetQuickStepNumIterations (world, 12); + + + // Create a static world using a triangle mesh that we can collide with. + int numv = sizeof(world_vertices)/(3*sizeof(float)); + int numi = sizeof(world_indices)/ sizeof(dTriIndex); + printf("numv=%d, numi=%d\n", numv, numi); + dTriMeshDataID Data = dGeomTriMeshDataCreate(); + + dGeomTriMeshDataBuildSingle + ( + Data, + world_vertices, + 3 * sizeof(float), + numv, + world_indices, + numi, + 3 * sizeof(dTriIndex) + ); + + world_mesh = dCreateTriMesh(space, Data, 0, 0, 0); + dGeomSetPosition(world_mesh, 0, 0, 0.5); + dRFromAxisAndAngle (R, 0,1,0, 0.0); + dGeomSetRotation (world_mesh, R); + + +#ifdef BOX + boxbody = dBodyCreate (world); + dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ); + dMassAdjust (&m, 1); + dBodySetMass (boxbody,&m); + boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ); + dGeomSetBody (boxgeom,boxbody); + dSpaceAdd (space, boxgeom); +#endif +#ifdef CYL + cylbody = dBodyCreate (world); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m,WMASS); + dBodySetMass (cylbody,&m); + cylgeom = dCreateCylinder(0, RADIUS, WHEELW); + dGeomSetBody (cylgeom,cylbody); + + #if defined(CYL_GEOM_OFFSET) + dMatrix3 mat; + dRFromAxisAndAngle(mat,1.0f,0.0f,0.0f,M_PI/2.0); + dGeomSetOffsetRotation(cylgeom,mat); + #endif + + dSpaceAdd (space, cylgeom); +#endif + reset_state(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupEmpty (contactgroup); + dJointGroupDestroy (contactgroup); + + // First destroy geoms, then space, then the world. +#ifdef CYL + dGeomDestroy (cylgeom); +#endif +#ifdef BOX + dGeomDestroy (boxgeom); +#endif + dGeomDestroy (world_mesh); + + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; + (void)world_normals; // get rid of compiler warnings +} + + + diff --git a/libs/ode-0.16.1/ode/demo/demo_cylvssphere.cpp b/libs/ode-0.16.1/ode/demo/demo_cylvssphere.cpp new file mode 100644 index 0000000..a2dc750 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_cylvssphere.cpp @@ -0,0 +1,240 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Test for cylinder vs sphere, by Bram Stolk + +#include <ode/odeconfig.h> +#include <assert.h> +#ifdef HAVE_UNISTD_H +#include <unistd.h> +#endif +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; + +static dBodyID cylbody; +static dGeomID cylgeom; + +static dBodyID sphbody; +static dGeomID sphgeom; + +static dJointGroupID contactgroup; + +static bool show_contacts = true; + +#define CYLRADIUS 0.6 +#define CYLLENGTH 2.0 +#define SPHERERADIUS 0.5 + + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#endif + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i<n; i++) + { + contact[i].surface.mode = 0; + contact[i].surface.mu = 50.0; // was: dInfinity + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); + if (show_contacts) + { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.12,0.12,0.12}; + dsSetColorAlpha (0,0,1,0.5); + dsDrawBox (contact[i].geom.pos,RI,ss); + dReal *pos = contact[i].geom.pos; + dReal depth = contact[i].geom.depth; + dReal *norm = contact[i].geom.normal; + dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]}; + dsSetColorAlpha (1,1,1,1); + dsDrawLine (contact[i].geom.pos, endp); + } + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {-8,-9,3}; + static float hpr[3] = {45.0000f,-27.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// called when a key pressed + +static void command (int cmd) +{ + switch (cmd) + { + case ' ': + break; + } +} + + + +// simulation loop + +static void simLoop (int pause) +{ + dSpaceCollide (space,0,&nearCallback); + if (!pause) + { + dWorldQuickStep (world, 0.01); // 100 Hz + } + dJointGroupEmpty (contactgroup); + + dsSetColorAlpha (1,1,0,0.5); + + const dReal *CPos = dBodyGetPosition(cylbody); + const dReal *CRot = dBodyGetRotation(cylbody); + float cpos[3] = {CPos[0], CPos[1], CPos[2]}; + float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] }; + dsDrawCylinder + ( + cpos, + crot, + CYLLENGTH, + CYLRADIUS + ); // single precision + + const dReal *SPos = dBodyGetPosition(sphbody); + const dReal *SRot = dBodyGetRotation(sphbody); + float spos[3] = {SPos[0], SPos[1], SPos[2]}; + float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] }; + dsDrawSphere + ( + spos, + srot, + SPHERERADIUS + ); // single precision +} + + +int main (int argc, char **argv) +{ + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-9.8); + dWorldSetQuickStepNumIterations (world, 32); + + dCreatePlane (space,0,0,1, 0.0); + + cylbody = dBodyCreate (world); + dQuaternion q; +#if 0 + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); +#else +// dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0); + dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77); +#endif + dBodySetQuaternion (cylbody,q); + dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH); + dBodySetMass (cylbody,&m); + cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH); + dGeomSetBody (cylgeom,cylbody); + dBodySetPosition (cylbody, 0, 0, 3); + dSpaceAdd (space, cylgeom); + + sphbody = dBodyCreate (world); + dMassSetSphere (&m,1,SPHERERADIUS); + dBodySetMass (sphbody,&m); + sphgeom = dCreateSphere(0, SPHERERADIUS); + dGeomSetBody (sphgeom,sphbody); + dBodySetPosition (sphbody, 0, 0, 5.5); + dSpaceAdd (space, sphgeom); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupEmpty (contactgroup); + dJointGroupDestroy (contactgroup); + + dGeomDestroy(sphgeom); + dGeomDestroy (cylgeom); + + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + + + diff --git a/libs/ode-0.16.1/ode/demo/demo_dball.cpp b/libs/ode-0.16.1/ode/demo/demo_dball.cpp new file mode 100644 index 0000000..d264cd7 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_dball.cpp @@ -0,0 +1,194 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#endif + + +dWorldID world; +dSpaceID space; +dBodyID body1; +dBodyID body2; +dJointID joint1, joint2; + +void start() +{ + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + dWorldSetDamping(world, 1e-4, 1e-5); +// dWorldSetERP(world, 1); + + space = dSimpleSpaceCreate (0); + + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetPosition(body1, 0, 0, 3); + dBodySetPosition(body2, 0, 0, 1); + + + dGeomID g; + dMass mass; + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body1); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body1, &mass); + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body2); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body2, &mass); + + joint1 = dJointCreateDBall(world, 0); + dJointAttach(joint1, body1, 0); + dJointSetDBallAnchor1(joint1, 0, 0, 3.5); + dJointSetDBallAnchor2(joint1, 0, 0, 4.5); + + joint2 = dJointCreateDBall(world, 0); + dJointAttach(joint2, body1, body2); + dJointSetDBallAnchor1(joint2, 0, 0, 2.5); + dJointSetDBallAnchor2(joint2, 0, 0, 1.5); + + + // initial camera position + static float xyz[3] = {3.8966, -2.0614, 4.0300}; + static float hpr[3] = {153.5, -16.5, 0}; + dsSetViewpoint (xyz,hpr); +} + +void stop() +{ + dSpaceDestroy(space); + + dWorldDestroy(world); +} + + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dSphereClass: + dsSetColorAlpha(0, 0.75, 0.5, 1); + dsSetTexture (DS_CHECKERED); + dsDrawSphere(pos, rot, dGeomSphereGetRadius(g)); + break; + case dBoxClass: + { + dVector3 lengths; + dsSetColorAlpha(1, 1, 0, 1); + dsSetTexture (DS_WOOD); + dGeomBoxGetLengths(g, lengths); + dsDrawBox(pos, rot, lengths); + break; + } + + default: + {} + } +} + +void simLoop(int pause) +{ + if (!pause) { + + static dReal t = 0; + + const dReal step = 0.005; + const unsigned nsteps = 4; + + for (unsigned i=0; i<nsteps; ++i) { + + dReal f = sin(t*1.2)*0.8; + dBodyAddForceAtRelPos(body1, + f, 0, 0, + 0, 0, -0.5); // at the lower end + + dReal g = sin(t*0.7)*0.8; + dBodyAddForceAtRelPos(body2, + 0, g, 0, + 0, 0, -0.5); // at the lower end + t += step; + + dWorldQuickStep(world, step); + } + } + + // now we draw everything + unsigned ngeoms = dSpaceGetNumGeoms(space); + for (unsigned i=0; i<ngeoms; ++i) { + dGeomID g = dSpaceGetGeom(space, i); + + drawGeom(g); + } + + dVector3 a11, a12; + dJointGetDBallAnchor1(joint1, a11); + dJointGetDBallAnchor2(joint1, a12); + dsSetColor(1, 0, 0); + dsDrawLine(a11, a12); + + //printf("Error 1: %f\n", fabs(dJointGetDBallDistance(joint1) - dCalcPointsDistance3(a11, a12))); + + dVector3 a21, a22; + dJointGetDBallAnchor1(joint2, a21); + dJointGetDBallAnchor2(joint2, a22); + dsSetColor(0, 1, 0); + dsDrawLine(a21, a22); + + //printf("Error 2: %f\n", fabs(dJointGetDBallDistance(joint2) - dCalcPointsDistance3(a21, a22))); +} + + + +int main(int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = stop; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE(); + + // run demo + dsSimulationLoop (argc, argv, 800, 600, &fn); + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_dhinge.cpp b/libs/ode-0.16.1/ode/demo/demo_dhinge.cpp new file mode 100644 index 0000000..c27f29e --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_dhinge.cpp @@ -0,0 +1,217 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#endif + + +dWorldID world; +dSpaceID space; +dBodyID body1; +dBodyID body2; +dJointID joint1, joint2; +bool applyForce = false; + +void start() +{ + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + dWorldSetDamping(world, 1e-4, 1e-5); +// dWorldSetERP(world, 1); + + space = dSimpleSpaceCreate (0); + + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetPosition(body1, 0, 0, 3); + dBodySetPosition(body2, 0, 0, 1); + + + dGeomID g; + dMass mass; + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body1); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body1, &mass); + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body2); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body2, &mass); + +#if 1 + joint1 = dJointCreateDHinge(world, 0); + dJointAttach(joint1, body1, 0); + dJointSetDHingeAxis(joint1, 0, 1, 0); + dJointSetDHingeAnchor1(joint1, 0, 0, 3.5); + dJointSetDHingeAnchor2(joint1, 0, 0, 4.5); +#endif + +#if 1 + joint2 = dJointCreateDHinge(world, 0); + dJointAttach(joint2, body1, body2); + dJointSetDHingeAxis(joint2, 1, 0, 0); + dJointSetDHingeAnchor1(joint2, 0, 0, 2.5); + dJointSetDHingeAnchor2(joint2, 0, 0, 1.5); +#else + joint2 = dJointCreateDBall(world, 0); + dJointAttach(joint2, body1, body2); + dJointSetDBallAnchor1(joint2, 0, 0, 2.5); + dJointSetDBallAnchor2(joint2, 0, 0, 1.5); +#endif + + //dBodyAddForce(body1, 20, 0, 0); + + + // initial camera position + static float xyz[3] = {3.8966, -2.0614, 4.0300}; + static float hpr[3] = {153.5, -16.5, 0}; + dsSetViewpoint (xyz,hpr); +} + +void stop() +{ + dSpaceDestroy(space); + + dWorldDestroy(world); +} + + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dBoxClass: + { + dVector3 lengths; + if (applyForce) + dsSetColor(1, .5, 0); + else + dsSetColor(1, 1, 0); + dsSetTexture (DS_WOOD); + dGeomBoxGetLengths(g, lengths); + dsDrawBox(pos, rot, lengths); + break; + } + + default: + {} + } +} + + +void simLoop(int pause) +{ + if (!pause) { + + static dReal t = 0; + + const dReal step = 0.005; + const unsigned nsteps = 2; + + for (unsigned i=0; i<nsteps; ++i) { + + applyForce = fmodf(t, 3.) > 2.; + + if (applyForce) { + dReal f = 0.3 * sin(t*1.2); + dBodyAddForceAtRelPos(body1, + f, 0, 0, + 0, 0, -0.5); // at the lower end + + dReal g = 0.3 * sin(t*0.7); + dBodyAddForceAtRelPos(body2, + 0, g, 0, + 0, 0, -0.5); // at the lower end + } + + t += step; + if (t > 20.) + t = 0.; + + dWorldQuickStep(world, step); + } + } + + // now we draw everything + unsigned ngeoms = dSpaceGetNumGeoms(space); + for (unsigned i=0; i<ngeoms; ++i) { + dGeomID g = dSpaceGetGeom(space, i); + + drawGeom(g); + } + +#if 1 + dVector3 a11, a12; + dJointGetDHingeAnchor1(joint1, a11); + dJointGetDHingeAnchor2(joint1, a12); + dsSetColor(1, 0, 0); + dsDrawLine(a11, a12); + //printf("Error 1: %f\n", fabs(dJointGetDHingeDistance(joint1) - dCalcPointsDistance3(a11, a12))); +#endif + +#if 1 + dVector3 a21, a22; + dJointGetDHingeAnchor1(joint2, a21); + dJointGetDHingeAnchor2(joint2, a22); + dsSetColor(0, 1, 0); + dsDrawLine(a21, a22); + + //printf("Error 2: %f\n", fabs(dJointGetDHingeDistance(joint2) - dCalcPointsDistance3(a21, a22))); +#endif +} + + + +int main(int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = stop; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE(); + + // run demo + dsSimulationLoop (argc, argv, 800, 600, &fn); + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_feedback.cpp b/libs/ode-0.16.1/ode/demo/demo_feedback.cpp new file mode 100644 index 0000000..ebcd129 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_feedback.cpp @@ -0,0 +1,312 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Test for breaking joints, by Bram Stolk + +#include <ode/odeconfig.h> +#include <assert.h> +#ifdef HAVE_UNISTD_H +#include <unistd.h> +#endif +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#endif + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; + +static const int STACKCNT=10; // nr of weights on bridge +static const int SEGMCNT=16; // nr of segments in bridge +static const float SEGMDIM[3] = { 0.9, 4, 0.1 }; + +static dGeomID groundgeom; +static dBodyID segbodies[SEGMCNT]; +static dGeomID seggeoms[SEGMCNT]; +static dBodyID stackbodies[STACKCNT]; +static dGeomID stackgeoms[STACKCNT]; +static dJointID hinges[SEGMCNT-1]; +static dJointID sliders[2]; +static dJointFeedback jfeedbacks[SEGMCNT-1]; +static dReal colours[SEGMCNT]; +static int stress[SEGMCNT-1]; + +static dJointGroupID contactgroup; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i<n; i++) + { + contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1; + contact[i].surface.mu = 100.0; + contact[i].surface.soft_erp = 0.96; + contact[i].surface.soft_cfm = 0.02; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c, + dGeomGetBody(contact[i].geom.g1), + dGeomGetBody(contact[i].geom.g2)); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = { -6, 8, 6}; + static float hpr[3] = { -65.0f, -27.0f, 0.0f}; + dsSetViewpoint (xyz,hpr); +} + + +// called when a key pressed + +static void command (int) +{} + + +void drawGeom (dGeomID g) +{ + const dReal *pos = dGeomGetPosition(g); + const dReal *R = dGeomGetRotation(g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) + { + dVector3 sides; + dGeomBoxGetLengths (g, sides); + dsDrawBox (pos,R,sides); + } + if (type == dCylinderClass) + { + dReal r,l; + dGeomCylinderGetParams(g, &r, &l); + dsDrawCylinder (pos, R, l, r); + } +} + + +static void inspectJoints(void) +{ + const dReal forcelimit = 4000.0; + int i; + for (i=0; i<SEGMCNT-1; i++) + { + if (dJointGetBody(hinges[i], 0)) + { + // This joint has not snapped already... inspect it. + dReal l0 = dCalcVectorLength3(jfeedbacks[i].f1); + dReal l1 = dCalcVectorLength3(jfeedbacks[i].f2); + colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit; + colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit; + if (l0 > forcelimit || l1 > forcelimit) + stress[i]++; + else + stress[i]=0; + if (stress[i]>4) + { + // Low-pass filter the noisy feedback data. + // Only after 4 consecutive timesteps with excessive load, snap. + fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i); + dJointAttach (hinges[i], 0, 0); + } + } + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + int i; + + double simstep = 0.002; // 2ms simulation steps + double dt = dsElapsedTime(); + int nrofsteps = (int) ceilf(dt/simstep); + for (i=0; i<nrofsteps && !pause; i++) + { + dSpaceCollide (space,0,&nearCallback); + dWorldQuickStep (world, simstep); + dJointGroupEmpty (contactgroup); + inspectJoints(); + } + + for (i=0; i<SEGMCNT; i++) + { + float r=0,g=0,b=0.2; + float v = colours[i]; + if (v>1.0) v=1.0; + if (v<0.5) + { + r=2*v; + g=1.0; + } + else + { + r=1.0; + g=2*(1.0-v); + } + dsSetColor (r,g,b); + drawGeom(seggeoms[i]); + } + dsSetColor (1,1,1); + for (i=0; i<STACKCNT; i++) + drawGeom(stackgeoms[i]); +} + + + +int main (int argc, char **argv) +{ + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-9.8); + dWorldSetQuickStepNumIterations (world, 20); + + int i; + for (i=0; i<SEGMCNT; i++) + { + segbodies[i] = dBodyCreate (world); + dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5); + dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]); + dBodySetMass (segbodies[i], &m); + seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]); + dGeomSetBody (seggeoms[i], segbodies[i]); + dSpaceAdd (space, seggeoms[i]); + } + + for (i=0; i<SEGMCNT-1; i++) + { + hinges[i] = dJointCreateHinge (world,0); + dJointAttach (hinges[i], segbodies[i],segbodies[i+1]); + dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5); + dJointSetHingeAxis (hinges[i], 0,1,0); + dJointSetHingeParam (hinges[i],dParamFMax, 8000.0); + // NOTE: + // Here we tell ODE where to put the feedback on the forces for this hinge + dJointSetFeedback (hinges[i], jfeedbacks+i); + stress[i]=0; + } + + for (i=0; i<STACKCNT; i++) + { + stackbodies[i] = dBodyCreate(world); + dMassSetBox (&m, 2.0, 2, 2, 0.6); + dBodySetMass(stackbodies[i],&m); + + stackgeoms[i] = dCreateBox(0, 2, 2, 0.6); + dGeomSetBody(stackgeoms[i], stackbodies[i]); + dBodySetPosition(stackbodies[i], 0,0,8+2*i); + dSpaceAdd(space, stackgeoms[i]); + } + + sliders[0] = dJointCreateSlider (world,0); + dJointAttach(sliders[0], segbodies[0], 0); + dJointSetSliderAxis (sliders[0], 1,0,0); + dJointSetSliderParam (sliders[0],dParamFMax, 4000.0); + dJointSetSliderParam (sliders[0],dParamLoStop, 0.0); + dJointSetSliderParam (sliders[0],dParamHiStop, 0.2); + + sliders[1] = dJointCreateSlider (world,0); + dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0); + dJointSetSliderAxis (sliders[1], 1,0,0); + dJointSetSliderParam (sliders[1],dParamFMax, 4000.0); + dJointSetSliderParam (sliders[1],dParamLoStop, 0.0); + dJointSetSliderParam (sliders[1],dParamHiStop, -0.2); + + groundgeom = dCreatePlane(space, 0,0,1,0); + + for (i=0; i<SEGMCNT; i++) + colours[i]=0.0; + + // run simulation + dsSimulationLoop (argc,argv,1280,720,&fn); + + dJointGroupEmpty(contactgroup); + dJointGroupDestroy (contactgroup); + + // First destroy seggeoms, then space, then the world. + for (i=0; i<SEGMCNT; i++) + dGeomDestroy (seggeoms[i]); + for (i=0; i<STACKCNT; i++) + dGeomDestroy (stackgeoms[i]); + + dSpaceDestroy(space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + + + diff --git a/libs/ode-0.16.1/ode/demo/demo_friction.cpp b/libs/ode-0.16.1/ode/demo/demo_friction.cpp new file mode 100644 index 0000000..f6260bb --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_friction.cpp @@ -0,0 +1,205 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +test the Coulomb friction approximation. + +a 10x10 array of boxes is made, each of which rests on the ground. +a horizantal force is applied to each box to try and get it to slide. +box[i][j] has a mass (i+1)*MASS and a force (j+1)*FORCE. by the Coloumb +friction model, the box should only slide if the force is greater than MU +times the contact normal force, i.e. + + f > MU * body_mass * GRAVITY + (j+1)*FORCE > MU * (i+1)*MASS * GRAVITY + (j+1) > (i+1) * (MU*MASS*GRAVITY/FORCE) + (j+1) > (i+1) * k + +this should be independent of the number of contact points, as N contact +points will each have 1/N'th the normal force but the pushing force will +have to overcome N contacts. the constants are chosen so that k=1. +thus you should see a triangle made of half the bodies in the array start to +slide. + +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define LENGTH 0.2 // box length & width +#define HEIGHT 0.05 // box height +#define MASS 0.2 // mass of box[i][j] = (i+1) * MASS +#define FORCE 0.05 // force applied to box[i][j] = (j+1) * FORCE +#define MU 0.5 // the global mu to use +#define GRAVITY 0.5 // the global gravity to use +#define N1 10 // number of different forces to try +#define N2 10 // number of different masses to try + + +// dynamics and collision objects + +static dWorldID world; +static dSpaceID space; +static dBodyID body[N1][N2]; +static dJointGroupID contactgroup; +static dGeomID ground; +static dGeomID box[N1][N2]; + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + + // only collide things with the ground + int g1 = (o1 == ground); + int g2 = (o2 == ground); + if (!(g1 ^ g2)) return; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + dContact contact[3]; // up to 3 contacts per box + for (i=0; i<3; i++) { + contact[i].surface.mode = dContactSoftCFM | dContactApprox1; + contact[i].surface.mu = MU; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) { + for (i=0; i<numc; i++) { + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.7772,-0.7924,2.7600}; + static float hpr[3] = {90.0000,-54.0000,0.0000}; + dsSetViewpoint (xyz,hpr); +} + + +// simulation loop + +static void simLoop (int pause) +{ + int i; + if (!pause) { + // apply forces to all bodies + for (i=0; i<N1; i++) { + for (int j=0; j<N2; j++) { + dBodyAddForce (body[i][j],FORCE*(i+1),0,0); + } + } + + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + } + + dsSetColor (1,0,1); + dReal sides[3] = {LENGTH,LENGTH,HEIGHT}; + for (i=0; i<N1; i++) { + for (int j=0; j<N2; j++) { + dsDrawBox (dGeomGetPosition(box[i][j]),dGeomGetRotation(box[i][j]), + sides); + } + } +} + + +int main (int argc, char **argv) +{ + int i,j; + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-GRAVITY); + ground = dCreatePlane (space,0,0,1,0); + + // bodies + for (i=0; i<N1; i++) { + for (j=0; j<N2; j++) { + body[i][j] = dBodyCreate (world); + dMassSetBox (&m,1,LENGTH,LENGTH,HEIGHT); + dMassAdjust (&m,MASS*(j+1)); + dBodySetMass (body[i][j],&m); + dBodySetPosition (body[i][j],i*2*LENGTH,j*2*LENGTH,HEIGHT*0.5); + + box[i][j] = dCreateBox (space,LENGTH,LENGTH,HEIGHT); + dGeomSetBody (box[i][j],body[i][j]); + } + } + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_gyro2.cpp b/libs/ode-0.16.1/ode/demo/demo_gyro2.cpp new file mode 100644 index 0000000..454b6b3 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_gyro2.cpp @@ -0,0 +1,210 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* +Angular friction demo: + +A bunch of ramps of different pitch. +A bunch of spheres with rolling friction. +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + +// dynamics and collision objects +static dWorldID world = 0; + +static const dReal dt = 1/REAL(60.0); // 60 fps +// Water density if units are meters and kg +static const dReal density = 1000; + +// A long skinny thing +static dVector3 sides = {2,.5,.25}; +// Initial angular velocity +static dVector3 omega = {5,1,2}; +static dVector3 torque = {0,10,0}; +static dBodyID noGyroBody; +static dBodyID expGyroBody; +static dBodyID impGyroBody; + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {0,-4.0f,3.0f}; + static float hpr[3] = {90.0000,-15.0000,0.0000}; + dsSetViewpoint (xyz,hpr); + printf ("Press:\n" + "\t'a' to apply a torque\n" + "\t'r' to reset simulation.\n"); +} + +/** + Delete the bodies, etc. +*/ +static void clear() +{ + if (world) dWorldDestroy (world); + world = 0; +} + + + +/** + Cleanup if necessary and rebuild the + world. +*/ +static void reset() +{ + clear(); + + // create world + world = dWorldCreate(); + + // Calculate mass for a box; + dMass boxMass; + dMassSetBox(&boxMass,density,sides[0],sides[1],sides[2]); + + noGyroBody = dBodyCreate(world);// Conservation of ang-velocity + expGyroBody = dBodyCreate(world);// Explicit conservation of ang-momentum + impGyroBody = dBodyCreate(world);// Implicit conservation of ang-momentum + + dBodySetMass( noGyroBody , &boxMass ); + dBodySetMass( expGyroBody, &boxMass ); + dBodySetMass( impGyroBody, &boxMass ); + + // Try to avoid collisions. + dReal sep = dCalcVectorLength3(sides); + dBodySetPosition( noGyroBody , -sep, 0, sep); + dBodySetPosition( expGyroBody, 0, 0, sep); + dBodySetPosition( impGyroBody, sep, 0, sep); + + // Set the initial angular velocity + dBodySetAngularVel( noGyroBody , omega[0], omega[1], omega[2]); + dBodySetAngularVel( expGyroBody, omega[0], omega[1], omega[2]); + dBodySetAngularVel( impGyroBody, omega[0], omega[1], omega[2]); + + dBodySetGyroscopicMode( noGyroBody, 0); + // We compute this ourselves using the math + // that was in the old stepper. + dBodySetGyroscopicMode(expGyroBody, 0); + // Keep things from crashing by limiting + // the angular speed of the explicit body. + // Note that this isn't necessary for + // the other two bodies. + dBodySetMaxAngularSpeed( expGyroBody, 40 ); +} + +static void command (int cmd) +{ + switch (cmd) { + case 'a': case 'A': + dBodyAddTorque( noGyroBody, torque[0], torque[1], torque[2]); + dBodyAddTorque(expGyroBody, torque[0], torque[1], torque[2]); + dBodyAddTorque(impGyroBody, torque[0], torque[1], torque[2]); + break; + case 'r': case 'R': + reset(); + break; + } + +} + +/** + This is the explicit computation of + gyroscopic forces. +*/ +static void expStep(dBodyID body) +{ + // Explicit computation + dMatrix3 I,tmp; + dMass m; + dBodyGetMass(body,&m); + const dReal* R = dBodyGetRotation(body); + // compute inertia tensor in global frame + dMultiply2_333 (tmp,m.I,R); + dMultiply0_333 (I,R,tmp); + // compute explicit rotational force + // we treat 'tmp'like a vector, but that's okay. + const dReal* w = dBodyGetAngularVel(body); + dMultiply0_331 (tmp,I,w); + dVector3 tau; + dCalcVectorCross3(tau,tmp,w); + dBodyAddTorque(body,tau[0],tau[1],tau[2]); +} + + +// simulation loop +static void simLoop (int pause) +{ + if (!pause) { + expStep(expGyroBody); + dWorldStep (world,dt); + } + + dsSetTexture (DS_WOOD); + dsSetColor(1,0,0); + dsDrawBox(dBodyGetPosition(noGyroBody ),dBodyGetRotation(noGyroBody ),sides); + dsSetColor(1,1,0); + dsDrawBox(dBodyGetPosition(expGyroBody),dBodyGetRotation(expGyroBody),sides); + dsSetColor(0,1,0); + dsDrawBox(dBodyGetPosition(impGyroBody),dBodyGetRotation(impGyroBody),sides); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + reset(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + clear(); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp b/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp new file mode 100644 index 0000000..5b6a532 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp @@ -0,0 +1,258 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + +bool write_world = false; +bool show_contacts = false; +dWorld * world; +dBody *top1, *top2; +dSpace *space; +dJointGroup contactgroup; + +const dReal pinradius = 0.05f; +const dReal pinlength = 1.5f; +const dReal topradius = 1.0f; +const dReal toplength = 0.25f; +const dReal topmass = 1.0f; + +#define MAX_CONTACTS 4 + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // for drawing the contact points + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.02,0.02,0.02}; + + int i; + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + dContact contact[MAX_CONTACTS]; + int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact)); + + for (i=0; i<numc; i++) { + contact[i].surface.mode = dContactApprox1; + contact[i].surface.mu = 2; + + dJointID c = dJointCreateContact (*world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + if (show_contacts) + dsDrawBox (contact[i].geom.pos, RI, ss); + + } +} + + +// start simulation - set viewpoint + +static void start() +{ + static float xyz[3] = {4.777f, -2.084f, 2.18f}; + static float hpr[3] = {153.0f, -14.5f, 0.0f}; + dsSetViewpoint (xyz,hpr); + printf ("Orange top approximates conservation of angular momentum\n"); + printf ("Green top uses conservation of angular velocity\n"); + printf ("---\n"); + printf ("SPACE to reset\n"); + printf ("A to tilt the tops.\n"); + printf ("T to toggle showing the contact points.\n"); + printf ("1 to save the current state to 'state.dif'.\n"); +} + + +char locase (char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed +static void reset(); +static void tilt(); + +static void command (int cmd) +{ + cmd = locase (cmd); + if (cmd == ' ') + { + reset(); + } + else if (cmd == 'a') { + tilt(); + } + else if (cmd == 't') { + show_contacts = !show_contacts; + } + else if (cmd == '1') { + write_world = true; + } +} + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + space->collide(0,&nearCallback); + if (!pause) + //world->quickStep(0.02); + world->step(0.02); + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF (*world,f,"X"); + fclose (f); + } + write_world = false; + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetTexture (DS_WOOD); + + dsSetColor (1,0.5f,0); + dsDrawCylinder(top1->getPosition(), + top1->getRotation(), + toplength, topradius); + dsDrawCapsule(top1->getPosition(), + top1->getRotation(), + pinlength, pinradius); + + dsSetColor (0.5f,1,0); + dsDrawCylinder(top2->getPosition(), + top2->getRotation(), + toplength, topradius); + dsDrawCapsule(top2->getPosition(), + top2->getRotation(), + pinlength, pinradius); + +} + + +static void reset() +{ + dMatrix3 R; + dRSetIdentity(R); + + top1->setRotation(R); + top2->setRotation(R); + + top1->setPosition(0.8f, -2, 2); + top2->setPosition(0.8f, 2, 2); + + top1->setAngularVel(1,0,7); + top2->setAngularVel(1,0,7); + + top1->setLinearVel(0,0.2f,0); + top2->setLinearVel(0,0.2f,0); +} + +static void tilt() +{ + top1->addTorque(0, 10, 0); + top2->addTorque(0, 10, 0); +} + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + + // create world + dInitODE(); + world = new dWorld(); + world->setGravity(0,0,-0.5f); + world->setCFM(1e-5f); + world->setLinearDamping(0.00001f); + world->setAngularDamping(0.0001f); + + space = new dSimpleSpace(0); + + dPlane *floor = new dPlane(*space, 0,0,1,0); + + top1 = new dBody(*world); + top2 = new dBody(*world); + + dMass m; + m.setCylinderTotal(1, 3, topradius, toplength); + top1->setMass(m); + top2->setMass(m); + + dGeom *g1, *g2, *pin1, *pin2; + g1 = new dCylinder(*space, topradius, toplength); + g1->setBody(*top1); + g2 = new dCylinder(*space, topradius, toplength); + g2->setBody(*top2); + + pin1 = new dCapsule(*space, pinradius, pinlength); + pin1->setBody(*top1); + pin2 = new dCapsule(*space, pinradius, pinlength); + pin2->setBody(*top2); + + top2->setGyroscopicMode(false); + + reset(); + + // run simulation + dsSimulationLoop (argc,argv,512,384,&fn); + + delete g1; + delete g2; + delete pin1; + delete pin2; + delete floor; + contactgroup.empty(); + delete top1; + delete top2; + delete space; + delete world; + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_heightfield.cpp b/libs/ode-0.16.1/ode/demo/demo_heightfield.cpp new file mode 100644 index 0000000..d68cb6a --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_heightfield.cpp @@ -0,0 +1,714 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" +#include "bunny_geom.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +#define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians + +int g_allow_trimesh; + +// Our heightfield geom +dGeomID gheight; + + + +// Heightfield dimensions + +#define HFIELD_WSTEP 15 // Vertex count along edge >= 2 +#define HFIELD_DSTEP 31 + +#define HFIELD_WIDTH REAL( 4.0 ) +#define HFIELD_DEPTH REAL( 8.0 ) + +#define HFIELD_WSAMP ( HFIELD_WIDTH / ( HFIELD_WSTEP-1 ) ) +#define HFIELD_DSAMP ( HFIELD_DEPTH / ( HFIELD_DSTEP-1 ) ) + + + +//<---- Convex Object +dReal planes[]= // planes for a cube + { + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ + }; +const unsigned int planecount=6; + +dReal points[]= // points for a cube + { + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 + }; +const unsigned int pointcount=8; +unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#define dsDrawTriangle dsDrawTriangleD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 64 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body + + // Trimesh only - double buffered matrices for 'last transform' setup + dReal matrix_dblbuff[ 16 * 2 ]; + int last_matrix_index; +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int write_world = 0; + + + + +//============================ + +dGeomID TriMesh1; +dGeomID TriMesh2; +//static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data + +//============================ + + +dReal heightfield_callback( void*, int x, int z ) +{ + dReal fx = ( ((dReal)x) - ( HFIELD_WSTEP-1 )/2 ) / (dReal)( HFIELD_WSTEP-1 ); + dReal fz = ( ((dReal)z) - ( HFIELD_DSTEP-1 )/2 ) / (dReal)( HFIELD_DSTEP-1 ); + + // Create an interesting 'hump' shape + dReal h = REAL( 1.0 ) + ( REAL( -16.0 ) * ( fx*fx*fx + fz*fz*fz ) ); + + return h; +} + + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) + return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i<MAX_CONTACTS; i++) { + contact[i].surface.mode = dContactBounce | dContactSoftCFM; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.1; + contact[i].surface.bounce_vel = 0.1; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact))) { + dMatrix3 RI; + dRSetIdentity(RI); + const dReal ss[3] = {0.02,0.02,0.02}; + for (i=0; i<numc; i++) { + dJointID c = dJointCreateContact(world,contactgroup,contact+i); + dJointAttach(c,b1,b2); + if (show_contacts) { + dsSetColor(0,0,1); + dsDrawBox(contact[i].geom.pos,RI,ss); + } + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf("To drop another object, press:\n"); + printf(" b for box.\n"); + printf(" s for sphere.\n"); + printf(" c for capsule.\n"); + printf(" y for cylinder.\n"); + printf(" v for a convex object.\n"); + printf(" x for a composite object.\n"); + if ( g_allow_trimesh ) + printf(" m for a trimesh.\n"); + printf("To select an object, press space.\n"); + printf("To disable the selected object, press d.\n"); + printf("To enable the selected object, press e.\n"); + printf("To toggle showing the geom AABBs, press a.\n"); + printf("To toggle showing the contact points, press t.\n"); + printf("To toggle dropping from random position/orientation, press r.\n"); + printf("To save the current state to 'state.dif', press 1.\n"); +} + + +char locase(char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command(int cmd) +{ + dsizeint i; + int j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase (cmd); + + + // + // Geom Creation + // + + if ( cmd == 'b' || cmd == 's' || cmd == 'c' || ( cmd == 'm' && g_allow_trimesh ) || + cmd == 'x' || cmd == 'y' || cmd == 'v' ) { + + if ( num < NUM ) { + i = num; + num++; + } else { + i = nextobj++; + nextobj %= num; + + // destroy the body and geoms for slot i + dBodyDestroy(obj[i].body); + obj[i].body = 0; + + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) { + dGeomDestroy(obj[i].geom[k]); + obj[i].geom[k] = 0; + } + } + + obj[i].body = dBodyCreate(world); + for (k=0; k<3; k++) + sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition(obj[i].body, + (dRandReal()-0.5)*HFIELD_WIDTH*0.75, + (dRandReal()-0.5)*HFIELD_DEPTH*0.75, + dRandReal() + 2 ); + dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } else { + dReal maxheight = 0; + for (k=0; k<num; k++) { + const dReal *pos = dBodyGetPosition(obj[k].body); + if (pos[2] > maxheight) + maxheight = pos[2]; + } + dBodySetPosition(obj[i].body, 0,maxheight+1,0); + dRFromAxisAndAngle(R,0,0,1,dRandReal()*10.0-5.0); + } + + dBodySetRotation(obj[i].body,R); + + if (cmd == 'b') { + + dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]); + + } else if (cmd == 'c') { + + sides[0] *= 0.5; + dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule(space,sides[0],sides[1]); + + } else if (cmd == 'v') { + + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex(space, + planes, + planecount, + points, + pointcount, + polygons); + + } else if (cmd == 'y') { + + dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]); + + } else if (cmd == 's') { + + sides[0] *= 0.5; + dMassSetSphere(&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere(space,sides[0]); + + } else if (cmd == 'm' && g_allow_trimesh) { + + dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, + &Indices[0], IndexCount, 3 * sizeof(dTriIndex)); + dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); + + obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); + + dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); + printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); + dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); + dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); + + } else if (cmd == 'x') { + + setBody = 1; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j<GPB; j++) + for (k=0; k<3; k++) + dpos[j][k] = dRandReal()*0.3-0.15; + + for (k=0; k<GPB; k++) { + if (k==0) { + dReal radius = dRandReal()*0.25+0.05; + obj[i].geom[k] = dCreateSphere (space,radius); + dMassSetSphere (&m2,DENSITY,radius); + } + else if (k==1) { + obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); + dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); + } else { + dReal radius = dRandReal()*0.1+0.05; + dReal length = dRandReal()*1.0+0.1; + obj[i].geom[k] = dCreateCapsule(space,radius,length); + dMassSetCapsule(&m2,DENSITY,3,radius,length); + } + + dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dMassRotate(&m2,drot[k]); + + dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); + + // add to the total mass + dMassAdd(&m,&m2); + + } + for (k=0; k<GPB; k++) { + dGeomSetBody(obj[i].geom[k],obj[i].body); + dGeomSetOffsetPosition(obj[i].geom[k], + dpos[k][0]-m.c[0], + dpos[k][1]-m.c[1], + dpos[k][2]-m.c[2]); + dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); + } + dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); + dBodySetMass(obj[i].body,&m); + + } + + if (!setBody) { // avoid calling for composite geometries + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) + dGeomSetBody(obj[i].geom[k],obj[i].body); + + dBodySetMass(obj[i].body,&m); + } + } + + + // + // Control Commands + // + + if (cmd == ' ') { + + selected++; + if (selected >= num) + selected = 0; + if (selected < -1) + selected = 0; + + } else if (cmd == 'd' && selected >= 0 && selected < num) { + + dBodyDisable(obj[selected].body); + + } else if (cmd == 'e' && selected >= 0 && selected < num) { + + dBodyEnable(obj[selected].body); + + } else if (cmd == 'a') { + + show_aabb = !show_aabb; + + } else if (cmd == 't') { + + show_contacts = !show_contacts; + + } else if (cmd == 'r') { + + random_pos = !random_pos; + + } else if (cmd == '1') { + + write_world = 1; + + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!g) + return; + if (!pos) + pos = dGeomGetPosition(g); + if (!R) + R = dGeomGetRotation(g); + + int type = dGeomGetClass(g); + if (type == dBoxClass) { + + dVector3 sides; + dGeomBoxGetLengths(g,sides); + dsDrawBox(pos,R,sides); + + } else if (type == dSphereClass) { + + dsDrawSphere(pos,R,dGeomSphereGetRadius(g)); + + } else if (type == dCapsuleClass) { + + dReal radius,length; + dGeomCapsuleGetParams(g,&radius,&length); + dsDrawCapsule(pos,R,length,radius); + + } else if (type == dConvexClass) { + + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + + } else if (type == dCylinderClass) { + + dReal radius,length; + dGeomCylinderGetParams(g,&radius,&length); + dsDrawCylinder(pos,R,length,radius); + + } else if (type == dTriMeshClass) { + + dTriIndex* Indices = (dTriIndex*)::Indices; + + // assume all trimeshes are drawn as bunnies + for (int ii = 0; ii < IndexCount / 3; ii++) { + const dReal v[9] = { // explicit conversion from float to dReal + Vertices[Indices[ii * 3 + 0] * 3 + 0], + Vertices[Indices[ii * 3 + 0] * 3 + 1], + Vertices[Indices[ii * 3 + 0] * 3 + 2], + Vertices[Indices[ii * 3 + 1] * 3 + 0], + Vertices[Indices[ii * 3 + 1] * 3 + 1], + Vertices[Indices[ii * 3 + 1] * 3 + 2], + Vertices[Indices[ii * 3 + 2] * 3 + 0], + Vertices[Indices[ii * 3 + 2] * 3 + 1], + Vertices[Indices[ii * 3 + 2] * 3 + 2] + }; + dsDrawTriangle(pos, R, &v[0], &v[3], &v[6], 1); + } + + } else if (type == dHeightfieldClass) { + + // Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode. + int ox = (int) ( -HFIELD_WIDTH/2 ); + int oz = (int) ( -HFIELD_DEPTH/2 ); + + // for ( int tx = -1; tx < 2; ++tx ) + // for ( int tz = -1; tz < 2; ++tz ) + dsSetColorAlpha (0.5,1,0.5,0.5); + dsSetTexture( DS_WOOD ); + + for ( int i = 0; i < HFIELD_WSTEP - 1; ++i ) + for ( int j = 0; j < HFIELD_DSTEP - 1; ++j ) { + dReal a[3], b[3], c[3], d[3]; + + a[ 0 ] = ox + ( i ) * HFIELD_WSAMP; + a[ 1 ] = heightfield_callback( NULL, i, j ); + a[ 2 ] = oz + ( j ) * HFIELD_DSAMP; + + b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP; + b[ 1 ] = heightfield_callback( NULL, i + 1, j ); + b[ 2 ] = oz + ( j ) * HFIELD_DSAMP; + + c[ 0 ] = ox + ( i ) * HFIELD_WSAMP; + c[ 1 ] = heightfield_callback( NULL, i, j + 1 ); + c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP; + + d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP; + d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 ); + d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP; + + dsDrawTriangle( pos, R, a, c, b, 1 ); + dsDrawTriangle( pos, R, b, c, d, 1 ); + } + + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB(g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) + bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int i=0; i<3; i++) + bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity(RI); + dsSetColorAlpha(1,0,0,0.5); + dsDrawBox(bbpos,RI,bbsides); + } + +} + +// simulation loop + +static void simLoop (int pause) +{ + int i,j; + + dSpaceCollide(space,0,&nearCallback); + + if (!pause) + dWorldQuickStep(world,0.05); + + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF(world,f,"X"); + fclose (f); + } + write_world = 0; + } + + // remove all contact joints + dJointGroupEmpty(contactgroup); + + + + // + // Draw Heightfield + // + + drawGeom(gheight, 0, 0, 0); + + + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (i=0; i<num; i++) { + for (j=0; j < GPB; j++) { + if (i==selected) { + dsSetColor (0,0.7,1); + } else if (! dBodyIsEnabled (obj[i].body)) { + dsSetColor (1,0.8,0); + } else { + dsSetColor (1,1,0); + } + + drawGeom (obj[i].geom[j],0,0,show_aabb); + } + } + +} + + +int main (int argc, char **argv) +{ + printf("ODE configuration: %s\n", dGetConfiguration()); + + // Is trimesh support built into this ODE? + g_allow_trimesh = dCheckConfiguration( "ODE_EXT_trimesh" ); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity(world,0,0,-0.05); + dWorldSetCFM(world,1e-5); + dWorldSetAutoDisableFlag(world,1); + dWorldSetContactMaxCorrectingVel(world,0.1); + dWorldSetContactSurfaceLayer(world,0.001); + memset(obj,0,sizeof(obj)); + + dWorldSetAutoDisableAverageSamplesCount( world, 1 ); + + // base plane to catch overspill + dCreatePlane( space, 0, 0, 1, 0 ); + + + // our heightfield floor + + dHeightfieldDataID heightid = dGeomHeightfieldDataCreate(); + + // Create an finite heightfield. + dGeomHeightfieldDataBuildCallback( heightid, NULL, heightfield_callback, + HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP, + REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 ); + + // Give some very bounds which, while conservative, + // makes AABB computation more accurate than +/-INF. + dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) ); + + gheight = dCreateHeightfield( space, heightid, 1 ); + + dVector3 pos; + pos[ 0 ] = 0; + pos[ 1 ] = 0; + pos[ 2 ] = 0; + + // Rotate so Z is up, not Y (which is the default orientation) + dMatrix3 R; + dRSetIdentity( R ); + dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 ); + + // Place it. + dGeomSetRotation( gheight, R ); + dGeomSetPosition( gheight, pos[0], pos[1], pos[2] ); + + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); + dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dWorldSetStepThreadingImplementation(world, NULL, NULL); + dThreadingFreeImplementation(threading); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + + // destroy heightfield data, because _we_ own it not ODE + dGeomHeightfieldDataDestroy( heightid ); + + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_hinge.cpp b/libs/ode-0.16.1/ode/demo/demo_hinge.cpp new file mode 100644 index 0000000..926da21 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_hinge.cpp @@ -0,0 +1,165 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define SIDE (0.5f) // side length of a box +#define MASS (1.0) // mass of a box + + +// dynamics and collision objects +static dWorldID world; +static dBodyID body[2]; +static dJointID hinge; + + +// state set by keyboard commands +static int occasional_error = 0; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("Press 'e' to start/stop occasional error.\n"); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == 'e' || cmd == 'E') { + occasional_error ^= 1; + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + const dReal kd = -0.3; // angular damping constant + if (!pause) { + // add an oscillating torque to body 0, and also damp its rotational motion + static dReal a=0; + const dReal *w = dBodyGetAngularVel (body[0]); + dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a)); + dWorldStep (world,0.05); + a += 0.01; + + // occasionally re-orient one of the bodies to create a deliberate error. + if (occasional_error) { + static int count = 0; + if ((count % 20)==0) { + // randomly adjust orientation of body[0] + const dReal *R1; + dMatrix3 R2,R3; + R1 = dBodyGetRotation (body[0]); + dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, + dRandReal()-0.5,dRandReal()-0.5); + dMultiply0 (R3,R1,R2,3,3,3); + dBodySetRotation (body[0],R3); + + // randomly adjust position of body[0] + const dReal *pos = dBodyGetPosition (body[0]); + dBodySetPosition (body[0], + pos[0]+0.2*(dRandReal()-0.5), + pos[1]+0.2*(dRandReal()-0.5), + pos[2]+0.2*(dRandReal()-0.5)); + } + count++; + } + } + + dReal sides1[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {SIDE,SIDE,SIDE*0.8f}; + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + dQuaternion q; + dQFromAxisAndAngle (q,1,1,0,0.25*M_PI); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1); + dBodySetQuaternion (body[0],q); + + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1); + dBodySetQuaternion (body[1],q); + + hinge = dJointCreateHinge (world,0); + dJointAttach (hinge,body[0],body[1]); + dJointSetHingeAnchor (hinge,0,0,1); + dJointSetHingeAxis (hinge,1,-1,1.41421356); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp b/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp new file mode 100644 index 0000000..b760af1 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp @@ -0,0 +1,434 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +This file try to demonstrate how the PR joint is working. + +The axisP is draw in red and the axisR is in green + +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include <iostream> +#include <math.h> +#include "texturepath.h" + + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// physics parameters +#define BOX1_LENGTH 2 // Size along the X axis +#define BOX1_WIDTH 1 // Size along the Y axis +#define BOX1_HEIGHT 0.4 // Size along the Z axis (up) since gravity is (0,0,-10) +#define BOX2_LENGTH 0.2 +#define BOX2_WIDTH 0.1 +#define BOX2_HEIGHT 0.4 +#define Mass1 10 +#define Mass2 0.1 + + +#define PRISMATIC_ONLY 1 +#define ROTOIDE_ONLY 2 +int flag = 0; + + +//camera view +static float xyz[3] = {2.0f,-3.5f,2.0000f}; +static float hpr[3] = {90.000f,-25.5000f,0.0000f}; +//world,space,body & geom +static dWorldID world; +static dSpaceID space; +static dSpaceID box1_space; +static dBodyID box1_body[1]; +static dBodyID box2_body[1]; +static dJointID joint[1]; +static dJointGroupID contactgroup; +static dGeomID ground; +static dGeomID box1[1]; +static dGeomID box2[1]; + + +//collision detection +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) + { + for (i=0; i<n; i++) + { + contact[i].surface.mode = dContactSlip1 | dContactSlip2 | + dContactSoftERP | dContactSoftCFM | dContactApprox1; + contact[i].surface.mu = 0.1; + contact[i].surface.slip1 = 0.02; + contact[i].surface.slip2 = 0.02; + contact[i].surface.soft_erp = 0.1; + contact[i].surface.soft_cfm = 0.0001; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2)); + } + } +} + + +// start simulation - set viewpoint +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + dsSetViewpoint (xyz,hpr); + printf ("Press 'd' to add force along positive x direction.\nPress 'a' to add force along negative x direction.\n"); + printf ("Press 'w' to add force along positive y direction.\nPress 's' to add force along negative y direction.\n"); + printf ("Press 'e' to add torque around positive z direction.\nPress 'q' to add torque around negative z direction.\n"); + printf ("Press 'o' to add force around positive x direction \n"); + + printf("Press 'v' to give a defined velocity and add a FMax to the rotoide axis\n"); + printf("Press 'c' to set the velocity to zero and remove the FMax\n"); + + printf("Press 'l' to add limits (-0.5 to 0.5rad) on the rotoide axis\n"); + printf("Press 'k' to remove the limits on the rotoide axis\n"); + + printf("Press 'i' to get joint info\n"); +} + +// function to update camera position at each step. +void update() +{ +// const dReal *a =(dBodyGetPosition (box1_body[0])); +// float dx=a[0]; +// float dy=a[1]; +// float dz=a[2]; +// xyz[0]=dx; +// xyz[1]=dy-5; +// xyz[2]=dz+2; +// hpr[1]=-22.5000f; +// dsSetViewpoint (xyz,hpr); +} + + +// called when a key pressed +static void command (int cmd) +{ + switch (cmd) + { + case 'w': + case 'W': + dBodyAddForce(box2_body[0],0,500,0); + std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n'; + break; + case 's': + case 'S': + dBodyAddForce(box2_body[0],0,-500,0); + std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n'; + break; + case 'd': + case 'D': + dBodyAddForce(box2_body[0],500,0,0); + std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n'; + break; + case 'a': + case 'A': + dBodyAddForce(box2_body[0],-500,0,0); + std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n'; + break; + case 'e': + case 'E': + dBodyAddRelTorque(box2_body[0],0,0,200); + break; + case 'q': + case 'Q': + dBodyAddRelTorque(box2_body[0],0,0,-200); + break; + case 'o': + case 'O': + dBodyAddForce(box1_body[0],10000,0,0); + break; + + case 'v': + case 'V': + dJointSetPRParam(joint[0], dParamVel2, 2); + dJointSetPRParam(joint[0], dParamFMax2, 500); + break; + + case 'c': + case 'C': + dJointSetPRParam(joint[0], dParamVel2, 0); + dJointSetPRParam(joint[0], dParamFMax2, 0); + break; + + case 'l': + case 'L': + dJointSetPRParam(joint[0], dParamLoStop2, -0.5); + dJointSetPRParam(joint[0], dParamHiStop2, 0.5); + break; + + case 'k': + case 'K': + dJointSetPRParam(joint[0], dParamLoStop2, -dInfinity); + dJointSetPRParam(joint[0], dParamHiStop2, dInfinity); + break; + + case 'i': + case 'I': + dVector3 anchor; + dJointGetPRAnchor(joint[0], anchor); + dReal angle = dJointGetPRAngle(joint[0]); + dReal w = dJointGetPRAngleRate(joint[0]); + + dReal l = dJointGetPRPosition(joint[0]); + dReal v = dJointGetPRPositionRate(joint[0]); + + printf("Anchor: [%6.4f, %6.4f, %6.4f]\n", anchor[0], anchor[1], anchor[2]); + printf("Position: %7.4f, Rate: %7.4f\n", l, v); + printf("Angle: %7.4f, Rate: %7.4f\n", angle, w); + break; + } +} + + +// simulation loop +static void simLoop (int pause) +{ + if (!pause) + { + //draw 2 boxes + dVector3 ss; + dsSetTexture (DS_WOOD); + + const dReal *posBox2 = dGeomGetPosition(box2[0]); + const dReal *rotBox2 = dGeomGetRotation(box2[0]); + dsSetColor (1,1,0); + dGeomBoxGetLengths (box2[0],ss); + dsDrawBox (posBox2, rotBox2, ss); + + const dReal *posBox1 = dGeomGetPosition(box1[0]); + const dReal *rotBox1 = dGeomGetRotation(box1[0]); + dsSetColor (1,1,2); + dGeomBoxGetLengths (box1[0], ss); + dsDrawBox (posBox1, rotBox1, ss); + + dVector3 anchorPos; + dJointGetPRAnchor (joint[0], anchorPos); + + // Draw the axisP + if (ROTOIDE_ONLY != flag ) + { + dsSetColor (1,0,0); + dVector3 sizeP = {0, 0.1, 0.1}; + for (int i=0; i<3; ++i) + sizeP[0] += (anchorPos[i] - posBox1[i])*(anchorPos[i] - posBox1[i]); + sizeP[0] = sqrt(sizeP[0]); + dVector3 posAxisP; + for (int i=0; i<3; ++i) + posAxisP[i] = posBox1[i] + (anchorPos[i] - posBox1[i])/2.0; + dsDrawBox (posAxisP, rotBox1, sizeP); + } + + + // Draw the axisR + if (PRISMATIC_ONLY != flag ) + { + dsSetColor (0,1,0); + dVector3 sizeR = {0, 0.1, 0.1}; + for (int i=0; i<3; ++i) + sizeR[0] += (anchorPos[i] - posBox2[i])*(anchorPos[i] - posBox2[i]); + sizeR[0] = sqrt(sizeR[0]); + dVector3 posAxisR; + for (int i=0; i<3; ++i) + posAxisR[i] = posBox2[i] + (anchorPos[i] - posBox2[i])/2.0; + dsDrawBox (posAxisR, rotBox2, sizeR); + } + + dSpaceCollide (space,0,&nearCallback); + dWorldQuickStep (world,0.0001); + update(); + dJointGroupEmpty (contactgroup); + } +} + + +void Help(char **argv) +{ + printf("%s ", argv[0]); + printf(" -h | --help : print this help\n"); + printf(" -b | --both : Display how the complete joint works\n"); + printf(" Default behavior\n"); + printf(" -p | --prismatic-only : Display how the prismatic part works\n"); + printf(" The anchor pts is set at the center of body 2\n"); + printf(" -r | --rotoide-only : Display how the rotoide part works\n"); + printf(" The anchor pts is set at the center of body 1\n"); + printf(" -t | --texture-path path : Path to the texture.\n"); + printf(" Default = %s\n", DRAWSTUFF_TEXTURE_PATH); + printf("--------------------------------------------------\n"); + printf("Hit any key to continue:"); + getchar(); + + exit(0); +} + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + if (argc >= 2 ) + { + for (int i=1; i < argc; ++i) + { + if ( 0 == strcmp("-h", argv[i]) || 0 == strcmp("--help", argv[i]) ) + Help(argv); + + if (!flag && (0 == strcmp("-p", argv[i]) ||0 == strcmp("--prismatic-only", argv[i])) ) + flag = PRISMATIC_ONLY; + + if (!flag && (0 == strcmp("-r", argv[i]) || 0 == strcmp("--rotoide-only", argv[i])) ) + flag = ROTOIDE_ONLY; + + if (0 == strcmp("-t", argv[i]) || 0 == strcmp("--texture-path", argv[i])) + { + int j = i+1; + if ( j >= argc || // Check if we have enough arguments + argv[j][0] == '\0' || // We should have a path here + argv[j][0] == '-' ) // We should have a path not a command line + Help(argv); + else + fn.path_to_textures = argv[++i]; // Increase i since we use this argument + } + } + } + + dInitODE2(0); + + // create world + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-10); + ground = dCreatePlane (space,0,0,1,0); + + //create two boxes + dMass m; + box1_body[0] = dBodyCreate (world); + dMassSetBox (&m,1,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT); + dMassAdjust (&m,Mass1); + dBodySetMass (box1_body[0],&m); + box1[0] = dCreateBox (0,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT); + dGeomSetBody (box1[0],box1_body[0]); + + box2_body[0] = dBodyCreate (world); + dMassSetBox (&m,10,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT); + dMassAdjust (&m,Mass2); + dBodySetMass (box2_body[0],&m); + box2[0] = dCreateBox (0,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT); + dGeomSetBody (box2[0],box2_body[0]); + + //set the initial positions of body1 and body2 + dMatrix3 R; + dRSetIdentity(R); + dBodySetPosition (box1_body[0],0,0,BOX1_HEIGHT/2.0); + dBodySetRotation (box1_body[0], R); + + dBodySetPosition (box2_body[0], + 2.1, + 0.0, + BOX2_HEIGHT/2.0); + dBodySetRotation (box2_body[0], R); + + + //set PR joint + joint[0] = dJointCreatePR(world,0); + dJointAttach (joint[0],box1_body[0],box2_body[0]); + switch (flag) + { + case PRISMATIC_ONLY: + dJointSetPRAnchor (joint[0], + 2.1, + 0.0, + BOX2_HEIGHT/2.0); + dJointSetPRParam (joint[0],dParamLoStop, -0.5); + dJointSetPRParam (joint[0],dParamHiStop, 1.5); + break; + + case ROTOIDE_ONLY: + dJointSetPRAnchor (joint[0], + 0.0, + 0.0, + BOX2_HEIGHT/2.0); + dJointSetPRParam (joint[0],dParamLoStop, 0.0); + dJointSetPRParam (joint[0],dParamHiStop, 0.0); + break; + + default: + dJointSetPRAnchor (joint[0], + 1.1, + 0.0, + BOX2_HEIGHT/2.0); + dJointSetPRParam (joint[0],dParamLoStop, -0.5); + dJointSetPRParam (joint[0],dParamHiStop, 1.5); + break; + } + + dJointSetPRAxis1(joint[0],1,0,0); + dJointSetPRAxis2(joint[0],0,0,1); +// We position the 2 body +// The position of the rotoide joint is on the second body so it can rotate on itself +// and move along the X axis. +// With this anchor +// - A force in X will move only the body 2 inside the low and hi limit +// of the prismatic +// - A force in Y will make the 2 bodies to rotate around on the plane + + box1_space = dSimpleSpaceCreate (space); + dSpaceSetCleanup (box1_space,0); + dSpaceAdd(box1_space,box1[0]); + + // run simulation + dsSimulationLoop (argc,argv,400,300,&fn); + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + diff --git a/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp new file mode 100644 index 0000000..6ec3093 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp @@ -0,0 +1,735 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + This program demonstrates how the PU joint works. + A PU joint is a combination of a Universal joint and a Slider joint. + It is a universal joint with a slider between the anchor point and + body 1. + + + The upper yellow body is fixed to the world + The lower yellow body is attached to the upper body by a PU joint + The green object is one aprt of the slider. + The purple object is the second part of the slider. + The red object represent the axis1 of the universal part. + The blue object represent the axis2 of the universal part. + The gray object represent the anchor2 of the PU joint. +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include <iostream> +#include <math.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + +enum IDX_CYL_DIM +{ + RADIUS, + LENGTH, + NUM_CYL_DIM +}; + + +const dVector3 boxDim = {1,1,1}; +const dVector3 extDim = {0.2,0.2,1.2}; +const dVector3 ancDim = {0.2,0.2,0.5}; +const dReal axDim[NUM_CYL_DIM] = {0.1,1.0}; + + +int type = dJointTypePU; + + +const dReal VEL_INC = 0.01; // Velocity increment + +// physics parameters +const dReal PI = 3.14159265358979323846264338327950288419716939937510; + + +const dReal INT_EXT_RATIO = 0.8; + +#define X 0 +#define Y 1 +#define Z 2 + +enum INDEX +{ + W = 0, + D, + EXT, + INT, + AXIS1, + AXIS2, + ANCHOR, + GROUND, + NUM_PARTS, + ALL = NUM_PARTS, + // INDEX for catBits + JOINT, + LAST_INDEX_CNT +}; + +const int catBits[LAST_INDEX_CNT] = + { + 0x0001, ///< W Cylinder category + 0x0002, ///< D Cylinder category + 0x0004, ///< EXT sliderr category + 0x0008, ///< INT slider category + 0x0010, ///< AXIS1 universal category + 0x0020, ///< AXIS2 universal category + 0x0040, ///< ANCHOR category + 0x0080, ///< Ground category + ~0L, ///< All categories + 0x0004 | 0x0008 | 0x0010 | 0x0020 ///< JOINT category + }; + +#define Mass1 10 +#define Mass2 8 + + +//camera view +static float xyz[3] = {6.0f,0.0f,6.0000f}; +static float hpr[3] = {-180.000f,-25.5000f,0.0000f}; + + +//world,space,body & geom +static dWorldID world; +static dSpaceID space; +static dJointGroupID contactgroup; +static dBodyID body[NUM_PARTS]; +static dGeomID geom[NUM_PARTS]; + +static dJoint *joint; + + + +const dReal BOX_SIDES[3] = {1.0,1.0,1.0}; +const dReal OBS_SIDES[3] = {0.4,0.4,0.4}; +const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2}; + + +//collision detection +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) ); + if (n > 0) { + for (i=0; i<n; i++) { + contact[i].surface.mode = (dContactSlip1 | dContactSlip2 | + dContactSoftERP | dContactSoftCFM | + dContactApprox1); + contact[i].surface.mu = 0.1; + contact[i].surface.slip1 = 0.02; + contact[i].surface.slip2 = 0.02; + contact[i].surface.soft_erp = 0.1; + contact[i].surface.soft_cfm = 0.0001; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) ); + } + } +} + +static void printKeyBoardShortCut() +{ + printf ("Press 'h' for this help.\n"); + printf ("Press 'q' to add force on BLUE body along positive x direction.\n"); + printf ("Press 'w' to add force on BLUE body along negative x direction.\n"); + + printf ("Press 'a' to add force on BLUE body along positive y direction.\n"); + printf ("Press 's' to add force on BLUE body along negative y direction.\n"); + + printf ("Press 'z' to add force on BLUE body along positive z direction.\n"); + printf ("Press 'x' to add force on BLUE body along negative z direction.\n"); + + printf ("Press 'e' to add torque on BLUE body around positive x direction \n"); + printf ("Press 'r' to add torque on BLUE body around negative x direction \n"); + + printf ("Press 'd' to add torque on BLUE body around positive y direction \n"); + printf ("Press 'f' to add torque on BLUE body around negative y direction \n"); + + printf ("Press 'c' to add torque on BLUE body around positive z direction \n"); + printf ("Press 'v' to add torque on BLUE body around negative z direction \n"); + + printf ("Press '.' to increase joint velocity along the prismatic direction.\n"); + printf ("Press ',' to decrease joint velocity along the prismatic direction.\n"); + + printf ("Press 'l' Toggle ON/OFF the limits on all the axis\n"); + printf ("Press 'g' Toggle ON/OFF the gravity\n"); + + + printf ("Press 'p' to print the position, angle and rates of the joint.\n"); +} + + +// start simulation - set viewpoint +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + dsSetViewpoint (xyz,hpr); + printf ("This program demonstrates how the PU joint works.\n"); + printf ("A PU joint is a combination of a Universal joint and a Slider joint.\n"); + printf ("It is a universal joint with a slider between the anchor point and \n"); + printf ("body 1.\n\n"); + printf ("The upper yellow body is fixed to the world\n"); + printf ("The lower yellow body is attached to the upper body by a PU joint\n"); + printf ("The green object is one aprt of the slider.\n"); + printf ("The purple object is the second part of the slider.\n"); + printf ("The red object represent the axis1 of the universal part. \n"); + printf ("The blue object represent the axis2 of the universal part. \n"); + printf ("The gray object represent the anchor2 of the PU joint. \n"); + printKeyBoardShortCut(); +} + +// function to update camera position at each step. +void update() +{ +// static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w"); + +// static int cnt = 0; +// char str[24]; +// sprintf(str, "%06d",cnt++); + +// dWorldExportDIF(world, file, str); +} + + +// called when a key pressed +static void command (int cmd) +{ + switch (cmd) { +case 'h' : case 'H' : case '?' : + printKeyBoardShortCut(); + break; + + // Force + case 'q' : case 'Q' : + dBodyAddForce(body[D],40,0,0); + break; + case 'w' : case 'W' : + dBodyAddForce(body[D],-40,0,0); + break; + + case 'a' : case 'A' : + dBodyAddForce(body[D],0,40,0); + break; + case 's' : case 'S' : + dBodyAddForce(body[D],0,-40,0); + break; + + case 'z' : case 'Z' : + dBodyAddForce(body[D],0,0,40); + break; + case 'x' : case 'X' : + dBodyAddForce(body[D],0,0,-40); + break; + + // Torque + case 'e': case 'E': + dBodyAddTorque(body[D],0.1,0,0); + break; + case 'r': case 'R': + dBodyAddTorque(body[D],-0.1,0,0); + break; + + case 'd': case 'D': + dBodyAddTorque(body[D],0, 0.1,0); + break; + case 'f': case 'F': + dBodyAddTorque(body[D],0,-0.1,0); + break; + + case 'c': case 'C': + dBodyAddTorque(body[D],0,0,0.1); + break; + case 'v': case 'V': + dBodyAddTorque(body[D],0,0,0.1); + break; + + // Velocity of joint + case ',': case '<' : { + dReal vel = joint->getParam (dParamVel3) - VEL_INC; + joint->setParam (dParamVel3, vel); + joint->setParam (dParamFMax3, 2); + std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n'; + } + break; + + case '.': case '>' : { + dReal vel = joint->getParam (dParamVel3) + VEL_INC; + joint->setParam (dParamVel3, vel); + joint->setParam (dParamFMax3, 2); + std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n'; + } + break; + + case 'l': case 'L' : { + dReal aLimit, lLimit, fmax; + if ( joint->getParam (dParamFMax) ) { + aLimit = dInfinity; + lLimit = dInfinity; + fmax = 0; + } + else { + aLimit = 0.25*PI; + lLimit = 0.5*axDim[LENGTH]; + fmax = 0.02; + } + + joint->setParam (dParamFMax1, fmax); + joint->setParam (dParamFMax2, fmax); + joint->setParam (dParamFMax3, fmax); + + switch (joint->getType() ) { + case dJointTypePR : { + dPRJoint *pr = reinterpret_cast<dPRJoint *> (joint); + pr->setParam (dParamLoStop, -lLimit); + pr->setParam (dParamHiStop, -lLimit); + pr->setParam (dParamLoStop2, aLimit); + pr->setParam (dParamHiStop2, -aLimit); + } + break; + case dJointTypePU : { + dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint); + pu->setParam (dParamLoStop1, -aLimit); + pu->setParam (dParamHiStop1, aLimit); + pu->setParam (dParamLoStop2, -aLimit); + pu->setParam (dParamHiStop2, aLimit); + pu->setParam (dParamLoStop3, -lLimit); + pu->setParam (dParamHiStop3, lLimit); + } + break; + default: {} // keep the compiler happy + } + } + + break; + + case 'g': case 'G' : { + dVector3 g; + dWorldGetGravity(world, g); + if ( g[2]< -0.1 ) + dWorldSetGravity(world, 0, 0, 0); + else + dWorldSetGravity(world, 0, 0, -0.5); + + } + +case 'p' :case 'P' : { + switch (joint->getType() ) { + case dJointTypeSlider : { + dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint); + std::cout<<"Position ="<<sj->getPosition() <<"\n"; + } + break; + case dJointTypePU : { + dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint); + std::cout<<"Position ="<<pu->getPosition() <<"\n"; + std::cout<<"Position Rate="<<pu->getPositionRate() <<"\n"; + std::cout<<"Angle1 ="<<pu->getAngle1() <<"\n"; + std::cout<<"Angle1 Rate="<<pu->getAngle1Rate() <<"\n"; + std::cout<<"Angle2 ="<<pu->getAngle2() <<"\n"; + std::cout<<"Angle2 Rate="<<pu->getAngle2Rate() <<"\n"; + } + break; + default: {} // keep the compiler happy + } + } + break; + } +} + +static void drawBox (dGeomID id, int R, int G, int B) +{ + if (!id) + return; + + const dReal *pos = dGeomGetPosition (id); + const dReal *rot = dGeomGetRotation (id); + dsSetColor (R,G,B); + + dVector3 l; + dGeomBoxGetLengths (id, l); + dsDrawBox (pos, rot, l); +} + + +// simulation loop +static void simLoop (int pause) +{ + static bool todo = false; + if ( todo ) { // DEBUG + static int cnt = 0; + ++cnt; + + if (cnt == 5) + command ( 'q' ); + if (cnt == 10) + dsStop(); + } + + + + + if (!pause) { + double simstep = 0.01; // 10ms simulation steps + double dt = dsElapsedTime(); + + int nrofsteps = (int) ceilf (dt/simstep); + if (!nrofsteps) + nrofsteps = 1; + + for (int i=0; i<nrofsteps && !pause; i++) { + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world, simstep); + + dJointGroupEmpty (contactgroup); + } + + update(); + + + dReal radius, length; + + dsSetTexture (DS_WOOD); + + drawBox (geom[W], 1,1,0); + + + drawBox (geom[EXT], 0,1,0); + + dVector3 anchorPos; + + + + dReal ang1 = 0; + dReal ang2 = 0; + dVector3 axisP, axisR1, axisR2; + + if ( dJointTypePU == type ) { + dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); + ang1 = pu->getAngle1(); + ang2 = pu->getAngle2(); + pu->getAxis1 (axisR1); + pu->getAxis2 (axisR2); + pu->getAxisP (axisP); + + dJointGetPUAnchor (pu->id(), anchorPos); + } + else if ( dJointTypePR == type ) { + dPRJoint *pr = dynamic_cast<dPRJoint *> (joint); + pr->getAxis1 (axisP); + pr->getAxis2 (axisR1); + + dJointGetPRAnchor (pr->id(), anchorPos); + } + + + // Draw the axisR + if ( geom[INT] ) { + dsSetColor (1,0,1); + dVector3 l; + dGeomBoxGetLengths (geom[INT], l); + + const dReal *rotBox = dGeomGetRotation (geom[W]); + + dVector3 pos; + for (int i=0; i<3; ++i) + pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i]; + dsDrawBox (pos, rotBox, l); + } + + dsSetTexture (DS_CHECKERED); + if ( geom[AXIS1] ) { + dQuaternion q, qAng; + dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); + dGeomGetQuaternion (geom[AXIS1], q); + + dQuaternion qq; + dQMultiply1 (qq, qAng, q); + dMatrix3 R; + dQtoR (qq,R); + + + dGeomCylinderGetParams (geom[AXIS1], &radius, &length); + dsSetColor (1,0,0); + dsDrawCylinder (anchorPos, R, length, radius); + } + + if ( dJointTypePU == type && geom[AXIS2] ) { + //dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); + + dQuaternion q, qAng, qq, qq1; + dGeomGetQuaternion (geom[AXIS2], q); + + dQFromAxisAndAngle (qAng, 0, 1, 0, ang2); + dQMultiply1 (qq, qAng, q); + + + dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); + + dQMultiply1 (qq1, qAng, qq); + + + dMatrix3 R; + dQtoR (qq1,R); + + + dGeomCylinderGetParams (geom[AXIS2], &radius, &length); + dsSetColor (0,0,1); + dsDrawCylinder (anchorPos, R, length, radius); + } + + dsSetTexture (DS_WOOD); + + // Draw the anchor + if ( geom[ANCHOR] ) { + dsSetColor (1,1,1); + dVector3 l; + dGeomBoxGetLengths (geom[ANCHOR], l); + + const dReal *rotBox = dGeomGetRotation (geom[D]); + const dReal *posBox = dGeomGetPosition (geom[D]); + + dVector3 e; + for (int i=0; i<3; ++i) + e[i] = posBox[i] - anchorPos[i]; + dNormalize3 (e); + + dVector3 pos; + for (int i=0; i<3; ++i) + pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i]; + dsDrawBox (pos, rotBox, l); + } + + drawBox (geom[D], 1,1,0); + } +} + + +void Help (char **argv) +{ + printf ("%s ", argv[0]); + printf (" -h | --help : print this help\n"); + printf (" -p | --PRJoint : Use a PR joint instead of PU joint\n"); + printf (" -t | --texture-path path : Path to the texture.\n"); + printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH); + printf ("--------------------------------------------------\n"); + printf ("Hit any key to continue:"); + getchar(); + + exit (0); +} + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + if (argc >= 2 ) { + for (int i=1; i < argc; ++i) { + if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) ) + Help (argv); + + if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) ) + type = dJointTypePR; + + if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) { + int j = i+1; + if ( j >= argc || // Check if we have enough arguments + argv[j][0] == '\0' || // We should have a path here + argv[j][0] == '-' ) // We should have a path not a command line + Help (argv); + else + fn.path_to_textures = argv[++i]; // Increase i since we use this argument + } + } + } + + dInitODE2(0); + + world = dWorldCreate(); + dWorldSetERP (world, 0.8); + + space = dSimpleSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + geom[GROUND] = dCreatePlane (space, 0,0,1,0); + dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]); + dGeomSetCollideBits (geom[GROUND], catBits[ALL]); + + dMass m; + + // Create the body attached to the World + body[W] = dBodyCreate (world); + // Main axis of cylinder is along X=1 + m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]); + m.adjust (Mass1); + geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]); + dGeomSetBody (geom[W], body[W]); + dGeomSetCategoryBits (geom[W], catBits[W]); + dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) ); + dBodySetMass (body[W], &m); + + + + + + // Create the dandling body + body[D] = dBodyCreate (world); + // Main axis of capsule is along X=1 + m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]); + m.adjust (Mass1); + geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]); + dGeomSetBody (geom[D], body[D]); + dGeomSetCategoryBits (geom[D], catBits[D]); + dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) ); + dBodySetMass (body[D], &m); + + + // Create the external part of the slider joint + geom[EXT] = dCreateBox (0, extDim[X], extDim[Y], extDim[Z]); + dGeomSetCategoryBits (geom[EXT], catBits[EXT]); + dGeomSetCollideBits (geom[EXT], + catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); + + // Create the internal part of the slider joint + geom[INT] = dCreateBox (0, INT_EXT_RATIO*extDim[X], + INT_EXT_RATIO*extDim[Y], + INT_EXT_RATIO*extDim[Z]); + dGeomSetCategoryBits (geom[INT], catBits[INT]); + dGeomSetCollideBits (geom[INT], + catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); + + + dMatrix3 R; + // Create the first axis of the universal joi9nt + //Rotation of 90deg around y + geom[AXIS1] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]); + dRFromAxisAndAngle(R, 0,1,0, 0.5*PI); + dGeomSetRotation(geom[AXIS1], R); + dGeomSetCategoryBits(geom[AXIS1], catBits[AXIS1]); + dGeomSetCollideBits(geom[AXIS1], + catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]); + + + // Create the second axis of the universal joint + geom[AXIS2] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]); + //Rotation of 90deg around y + dRFromAxisAndAngle(R, 1,0,0, 0.5*PI); + dGeomSetRotation(geom[AXIS2], R); + dGeomSetCategoryBits(geom[AXIS2], catBits[AXIS2]); + dGeomSetCollideBits(geom[AXIS2], + catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]); + + // Create the anchor + geom[ANCHOR] = dCreateBox (0, ancDim[X], ancDim[Y], ancDim[Z]); + dGeomSetCategoryBits(geom[ANCHOR], catBits[ANCHOR]); + dGeomSetCollideBits(geom[ANCHOR], + catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); + + + + if (body[W]) { + dBodySetPosition(body[W], 0, 0, 5); + } + + + if (geom[EXT]) { + dGeomSetPosition(geom[EXT], 0,0,3.8); + } + if (geom[INT]) { + dGeomSetPosition(geom[INT], 0,0,2.6); + } + if (geom[AXIS1]) { + dGeomSetPosition(geom[AXIS1], 0,0,2.5); + } + if (geom[AXIS2]) { + dGeomSetPosition(geom[AXIS2], 0,0,2.5); + } + + if (geom[ANCHOR]) { + dGeomSetPosition(geom[ANCHOR], 0,0,2.25); + } + + if (body[D]) { + dBodySetPosition(body[D], 0,0,1.5); + } + + + + // Attache the upper box to the world + dJointID fixed = dJointCreateFixed (world,0); + dJointAttach (fixed , NULL, body[W]); + dJointSetFixed (fixed ); + + if (type == dJointTypePR) { + dPRJoint *pr = new dPRJoint (world, 0); + pr->attach (body[W], body[D]); + pr->setAxis1 (0, 0, -1); + pr->setAxis2 (1, 0, 0); + joint = pr; + + dJointSetPRAnchor (pr->id(), 0, 0, 2.5); + } + else { + dPUJoint *pu = new dPUJoint (world, 0); + pu->attach (body[W], body[D]); + pu->setAxis1 (1, 0, 0); + pu->setAxis2 (0, 1, 0); + pu->setAxisP (0, 0, -1); + joint = pu; + + dJointSetPUAnchor (pu->id(), 0, 0, 2.5); + } + + + // run simulation + dsSimulationLoop (argc,argv,400,300,&fn); + + delete joint; + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + diff --git a/libs/ode-0.16.1/ode/demo/demo_joints.cpp b/libs/ode-0.16.1/ode/demo/demo_joints.cpp new file mode 100644 index 0000000..d545369 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_joints.cpp @@ -0,0 +1,1090 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +perform tests on all the joint types. +this should be done using the double precision version of the library. + +usage: + test_joints [-nXXX] [-g] [-i] [-e] [path_to_textures] + +if a test number is given then that specific test is performed, otherwise +all the tests are performed. the tests are numbered `xxyy', where xx +corresponds to the joint type and yy is the sub-test number. not every +number maps to an actual test. + +flags: + i: the test is interactive. + g: turn off graphical display (can't use this with `i'). + e: turn on occasional error perturbations + n: performe test XXX +some tests compute and display error values. these values are scaled so +<1 is good and >1 is bad. other tests just show graphical results which +you must verify visually. + +*/ + +#include <ctype.h> +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define NUM_JOINTS 10 // number of joints to test (the `xx' value) +#define SIDE (0.5f) // side length of a box - don't change this +#define MASS (1.0) // mass of a box +#define STEPSIZE 0.05 + +static const dVector3 xunit = { 1, 0, 0 }, zunit = { 0, 0, 1 }; + + +// dynamics objects +static dWorldID world; +static dBodyID body[2]; +static dJointID joint; + + +// data from the command line arguments +static int cmd_test_num = -1; +static int cmd_interactive = 0; +static int cmd_graphics = 1; +static char *cmd_path_to_textures = NULL; +static int cmd_occasional_error = 0; // perturb occasionally + + +// info about the current test +struct TestInfo; +static int test_num = 0; // number of the current test +static int iteration = 0; +static int max_iterations = 0; +static dReal max_error = 0; + +//**************************************************************************** +// utility stuff + +static dReal length (dVector3 a) +{ + return dSqrt (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]); +} + + +// get the max difference between a 3x3 matrix and the identity + +dReal cmpIdentity (const dMatrix3 A) +{ + dMatrix3 I; + dSetZero (I,12); + I[0] = 1; + I[5] = 1; + I[10] = 1; + return dMaxDifference (A,I,3,3); +} + +//**************************************************************************** +// test world construction and utilities + +void constructWorldForTest (dReal gravity, int bodycount, + /* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z, + /* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z, + /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z, + /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z, + /* rotation angles */ dReal a1, dReal a2) +{ + // create world + world = dWorldCreate(); + dWorldSetERP (world,0.2); + dWorldSetCFM (world,1e-6); + dWorldSetGravity (world,0,0,gravity); + + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0], pos1x, pos1y, pos1z); + dQuaternion q; + dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1); + dBodySetQuaternion (body[0],q); + + if (bodycount==2) { + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dBodySetPosition (body[1], pos2x, pos2y, pos2z); + dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2); + dBodySetQuaternion (body[1],q); + } + else body[1] = 0; +} + + +// add an oscillating torque to body 0 + +void addOscillatingTorque (dReal tscale) +{ + static dReal a=0; + dBodyAddTorque (body[0],tscale*cos(2*a),tscale*cos(2.7183*a), + tscale*cos(1.5708*a)); + a += 0.01; +} + + +void addOscillatingTorqueAbout(dReal tscale, dReal x, dReal y, dReal z) +{ + static dReal a=0; + dBodyAddTorque (body[0], tscale*cos(a) * x, tscale*cos(a) * y, + tscale * cos(a) * z); + a += 0.02; +} + + +// damp the rotational motion of body 0 a bit + +void dampRotationalMotion (dReal kd) +{ + const dReal *w = dBodyGetAngularVel (body[0]); + dBodyAddTorque (body[0],-kd*w[0],-kd*w[1],-kd*w[2]); +} + + +// add a spring force to keep the bodies together, otherwise they may fly +// apart with some joints. + +void addSpringForce (dReal ks) +{ + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),ks*(p2[2]-p1[2])); + dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),ks*(p1[2]-p2[2])); +} + + +// add an oscillating Force to body 0 + +void addOscillatingForce (dReal fscale) +{ + static dReal a=0; + dBodyAddForce (body[0],fscale*cos(2*a),fscale*cos(2.7183*a), + fscale*cos(1.5708*a)); + a += 0.01; +} + +//**************************************************************************** +// stuff specific to the tests +// +// 0xx : fixed +// 1xx : ball and socket +// 2xx : hinge +// 3xx : slider +// 4xx : hinge 2 +// 5xx : contact +// 6xx : amotor +// 7xx : universal joint +// 8xx : PR joint (Prismatic and Rotoide) + +// setup for the given test. return 0 if there is no such test + +int setupTest (int n) +{ + switch (n) { + + // ********** fixed joint + + case 0: { // 2 body + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, + 0.25*M_PI,0.25*M_PI); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetFixed (joint); + return 1; + } + + case 1: { // 1 body to static env + constructWorldForTest (0,1, + 0.5*SIDE,0.5*SIDE,1, 0,0,0, + 1,0,0, 1,0,0, + 0,0); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],0); + dJointSetFixed (joint); + return 1; + } + + case 2: { // 2 body with relative rotation + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, + 0.25*M_PI,-0.25*M_PI); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetFixed (joint); + return 1; + } + + case 3: { // 1 body to static env with relative rotation + constructWorldForTest (0,1, + 0.5*SIDE,0.5*SIDE,1, 0,0,0, + 1,0,0, 1,0,0, + 0.25*M_PI,0); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],0); + dJointSetFixed (joint); + return 1; + } + + // ********** hinge joint + + case 200: // 2 body + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,1,-1,1.41421356); + return 1; + + case 220: // hinge angle polarity test + case 221: // hinge angle rate test + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,0,0,1); + max_iterations = 50; + return 1; + + case 230: // hinge motor rate (and polarity) test + case 231: // ...with stops + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,0,0,1); + dJointSetHingeParam (joint,dParamFMax,1); + if (n==231) { + dJointSetHingeParam (joint,dParamLoStop,-0.5); + dJointSetHingeParam (joint,dParamHiStop,0.5); + } + return 1; + + case 250: // limit bounce test (gravity down) + case 251: { // ...gravity up + constructWorldForTest ((n==251) ? 0.1 : -0.1, 2, + 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,0,1,0); + dJointSetHingeParam (joint,dParamLoStop,-0.9); + dJointSetHingeParam (joint,dParamHiStop,0.7854); + dJointSetHingeParam (joint,dParamBounce,0.5); + // anchor 2nd body with a fixed joint + dJointID j = dJointCreateFixed (world,0); + dJointAttach (j,body[1],0); + dJointSetFixed (j); + return 1; + } + + // ********** slider + + case 300: // 2 body + constructWorldForTest (0,2, + 0,0,1, 0.2,0.2,1.2, + 0,0,1, -1,1,0, 0,0.25*M_PI); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,1,1,1); + return 1; + + case 320: // slider angle polarity test + case 321: // slider angle rate test + constructWorldForTest (0,2, + 0,0,1, 0,0,1.2, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,0,0,1); + max_iterations = 50; + return 1; + + case 330: // slider motor rate (and polarity) test + case 331: // ...with stops + constructWorldForTest (0, 2, + 0,0,1, 0,0,1.2, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,0,0,1); + dJointSetSliderParam (joint,dParamFMax,100); + if (n==331) { + dJointSetSliderParam (joint,dParamLoStop,-0.4); + dJointSetSliderParam (joint,dParamHiStop,0.4); + } + return 1; + + case 350: // limit bounce tests + case 351: { + constructWorldForTest ((n==351) ? 0.1 : -0.1, 2, + 0,0,1, 0,0,1.2, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,0,0,1); + dJointSetSliderParam (joint,dParamLoStop,-0.5); + dJointSetSliderParam (joint,dParamHiStop,0.5); + dJointSetSliderParam (joint,dParamBounce,0.5); + // anchor 2nd body with a fixed joint + dJointID j = dJointCreateFixed (world,0); + dJointAttach (j,body[1],0); + dJointSetFixed (j); + return 1; + } + + // ********** hinge-2 joint + + case 420: // hinge-2 steering angle polarity test + case 421: // hinge-2 steering angle rate test + constructWorldForTest (0,2, + 0.5*SIDE,0,1, -0.5*SIDE,0,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge2 (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); + dJointSetHinge2Axes (joint, zunit, xunit); + max_iterations = 50; + return 1; + + case 430: // hinge 2 steering motor rate (+polarity) test + case 431: // ...with stops + case 432: // hinge 2 wheel motor rate (+polarity) test + constructWorldForTest (0,2, + 0.5*SIDE,0,1, -0.5*SIDE,0,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge2 (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); + dJointSetHinge2Axes (joint, zunit, xunit); + dJointSetHinge2Param (joint,dParamFMax,1); + dJointSetHinge2Param (joint,dParamFMax2,1); + if (n==431) { + dJointSetHinge2Param (joint,dParamLoStop,-0.5); + dJointSetHinge2Param (joint,dParamHiStop,0.5); + } + return 1; + + // ********** angular motor joint + + case 600: // test euler angle calculations + constructWorldForTest (0,2, + -SIDE*0.5,0,1, SIDE*0.5,0,1, + 0,0,1, 0,0,1, 0,0); + joint = dJointCreateAMotor (world,0); + dJointAttach (joint,body[0],body[1]); + + dJointSetAMotorNumAxes (joint,3); + dJointSetAMotorAxis (joint,0,1, 0,0,1); + dJointSetAMotorAxis (joint,2,2, 1,0,0); + dJointSetAMotorMode (joint,dAMotorEuler); + max_iterations = 200; + return 1; + + // ********** universal joint + + case 700: // 2 body + case 701: + case 702: + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); + joint = dJointCreateUniversal (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetUniversalAnchor (joint,0,0,1); + dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356); + dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356); + return 1; + + case 720: // universal transmit torque test + case 721: + case 722: + case 730: // universal torque about axis 1 + case 731: + case 732: + case 740: // universal torque about axis 2 + case 741: + case 742: + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateUniversal (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetUniversalAnchor (joint,0,0,1); + dJointSetUniversalAxis1 (joint,0,0,1); + dJointSetUniversalAxis2 (joint, 1, -1,0); + max_iterations = 100; + return 1; + + // Joint PR (Prismatic and Rotoide) + case 800: // 2 body + case 801: // 2 bodies with spring force and prismatic fixed + case 802: // 2 bodies with torque on body1 and prismatic fixed + constructWorldForTest (0, 2, + -1.0, 0.0, 1.0, + 1.0, 0.0, 1.0, + 1,0,0, 1,0,0, + 0, 0); + joint = dJointCreatePR (world, 0); + dJointAttach (joint, body[0], body[1]); + dJointSetPRAnchor (joint,-0.5, 0.0, 1.0); + dJointSetPRAxis1 (joint, 0, 1, 0); + dJointSetPRAxis2 (joint, 1, 0, 0); + dJointSetPRParam (joint,dParamLoStop,-0.5); + dJointSetPRParam (joint,dParamHiStop,0.5); + dJointSetPRParam (joint,dParamLoStop2,0); + dJointSetPRParam (joint,dParamHiStop2,0); + return 1; + case 803: // 2 bodies with spring force and prismatic NOT fixed + case 804: // 2 bodies with torque force and prismatic NOT fixed + case 805: // 2 bodies with force only on first body + constructWorldForTest (0, 2, + -1.0, 0.0, 1.0, + 1.0, 0.0, 1.0, + 1,0,0, 1,0,0, + 0, 0); + joint = dJointCreatePR (world, 0); + dJointAttach (joint, body[0], body[1]); + dJointSetPRAnchor (joint,-0.5, 0.0, 1.0); + dJointSetPRAxis1 (joint, 0, 1, 0); + dJointSetPRAxis2 (joint, 1, 0, 0); + dJointSetPRParam (joint,dParamLoStop,-0.5); + dJointSetPRParam (joint,dParamHiStop,0.5); + dJointSetPRParam (joint,dParamLoStop2,-0.5); + dJointSetPRParam (joint,dParamHiStop2,0.5); + return 1; + } + return 0; +} + + +// do stuff specific to this test each iteration. you can check some +// invariants for the test -- the return value is some scaled error measurement +// that must be less than 1. +// return a dInfinity if error is not measured for this n. + +dReal doStuffAndGetError (int n) +{ + switch (n) { + + // ********** fixed joint + + case 0: { // 2 body + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + // check the orientations are the same + const dReal *R1 = dBodyGetRotation (body[0]); + const dReal *R2 = dBodyGetRotation (body[1]); + dReal err1 = dMaxDifference (R1,R2,3,3); + // check the body offset is correct + dVector3 p,pp; + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; + dMultiply1_331 (pp,R1,p); + pp[0] += 0.5; + pp[1] += 0.5; + return (err1 + length (pp)) * 300; + } + + case 1: { // 1 body to static env + addOscillatingTorque (0.1); + + // check the orientation is the identity + dReal err1 = cmpIdentity (dBodyGetRotation (body[0])); + + // check the body offset is correct + dVector3 p; + const dReal *p1 = dBodyGetPosition (body[0]); + for (int i=0; i<3; i++) p[i] = p1[i]; + p[0] -= 0.25; + p[1] -= 0.25; + p[2] -= 1; + return (err1 + length (p)) * 1e6; + } + + case 2: { // 2 body + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + // check the body offset is correct + // Should really check body rotation too. Oh well. + const dReal *R1 = dBodyGetRotation (body[0]); + dVector3 p,pp; + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; + dMultiply1_331 (pp,R1,p); + pp[0] += 0.5; + pp[1] += 0.5; + return length(pp) * 300; + } + + case 3: { // 1 body to static env with relative rotation + addOscillatingTorque (0.1); + + // check the body offset is correct + dVector3 p; + const dReal *p1 = dBodyGetPosition (body[0]); + for (int i=0; i<3; i++) p[i] = p1[i]; + p[0] -= 0.25; + p[1] -= 0.25; + p[2] -= 1; + return length (p) * 1e6; + } + + + // ********** hinge joint + + case 200: // 2 body + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + return dInfinity; + + case 220: // hinge angle polarity test + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + if (iteration == 40) { + dReal a = dJointGetHingeAngle (joint); + if (a > 0.5 && a < 1) return 0; else return 10; + } + return 0; + + case 221: { // hinge angle rate test + static dReal last_angle = 0; + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + dReal a = dJointGetHingeAngle (joint); + dReal r = dJointGetHingeAngleRate (joint); + dReal er = (a-last_angle)/STEPSIZE; // estimated rate + last_angle = a; + return fabs(r-er) * 4e4; + } + + case 230: // hinge motor rate (and polarity) test + case 231: { // ...with stops + static dReal a = 0; + dReal r = dJointGetHingeAngleRate (joint); + dReal err = fabs (cos(a) - r); + if (a==0) err = 0; + a += 0.03; + dJointSetHingeParam (joint,dParamVel,cos(a)); + if (n==231) return dInfinity; + return err * 1e6; + } + + // ********** slider joint + + case 300: // 2 body + addOscillatingTorque (0.05); + dampRotationalMotion (0.1); + addSpringForce (0.5); + return dInfinity; + + case 320: // slider angle polarity test + dBodyAddForce (body[0],0,0,0.1); + dBodyAddForce (body[1],0,0,-0.1); + if (iteration == 40) { + dReal a = dJointGetSliderPosition (joint); + if (a > 0.2 && a < 0.5) + return 0; + else + return 10; // Failed + } + return 0; + + case 321: { // slider angle rate test + static dReal last_pos = 0; + dBodyAddForce (body[0],0,0,0.1); + dBodyAddForce (body[1],0,0,-0.1); + dReal p = dJointGetSliderPosition (joint); + dReal r = dJointGetSliderPositionRate (joint); + dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact) + last_pos = p; + return fabs(r-er) * 1e9; + } + + case 330: // slider motor rate (and polarity) test + case 331: { // ...with stops + static dReal a = 0; + dReal r = dJointGetSliderPositionRate (joint); + dReal err = fabs (0.7*cos(a) - r); + if (a < 0.04) err = 0; + a += 0.03; + dJointSetSliderParam (joint,dParamVel,0.7*cos(a)); + if (n==331) return dInfinity; + return err * 1e6; + } + + // ********** hinge-2 joint + + case 420: // hinge-2 steering angle polarity test + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + if (iteration == 40) { + dReal a = dJointGetHinge2Angle1 (joint); + if (a > 0.5 && a < 0.6) return 0; else return 10; + } + return 0; + + case 421: { // hinge-2 steering angle rate test + static dReal last_angle = 0; + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + dReal a = dJointGetHinge2Angle1 (joint); + dReal r = dJointGetHinge2Angle1Rate (joint); + dReal er = (a-last_angle)/STEPSIZE; // estimated rate + last_angle = a; + return fabs(r-er)*2e4; + } + + case 430: // hinge 2 steering motor rate (+polarity) test + case 431: { // ...with stops + static dReal a = 0; + dReal r = dJointGetHinge2Angle1Rate (joint); + dReal err = fabs (cos(a) - r); + if (a==0) err = 0; + a += 0.03; + dJointSetHinge2Param (joint,dParamVel,cos(a)); + if (n==431) return dInfinity; + return err * 1e6; + } + + case 432: { // hinge 2 wheel motor rate (+polarity) test + static dReal a = 0; + dReal r = dJointGetHinge2Angle2Rate (joint); + dReal err = fabs (cos(a) - r); + if (a==0) err = 0; + a += 0.03; + dJointSetHinge2Param (joint,dParamVel2,cos(a)); + return err * 1e6; + } + + // ********** angular motor joint + + case 600: { // test euler angle calculations + // desired euler angles from last iteration + static dReal a1,a2,a3; + + // find actual euler angles + dReal aa1 = dJointGetAMotorAngle (joint,0); + dReal aa2 = dJointGetAMotorAngle (joint,1); + dReal aa3 = dJointGetAMotorAngle (joint,2); + // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3); + + dReal err = dInfinity; + if (iteration > 0) { + err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3); + err *= 1e10; + } + + // get random base rotation for both bodies + dMatrix3 Rbase; + dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), + 3*(dRandReal()-0.5), 3*(dRandReal()-0.5)); + dBodySetRotation (body[0],Rbase); + + // rotate body 2 by random euler angles w.r.t. body 1 + a1 = 3.14 * 2 * (dRandReal()-0.5); + a2 = 1.57 * 2 * (dRandReal()-0.5); + a3 = 3.14 * 2 * (dRandReal()-0.5); + dMatrix3 R1,R2,R3,Rtmp1,Rtmp2; + dRFromAxisAndAngle (R1,0,0,1,-a1); + dRFromAxisAndAngle (R2,0,1,0,a2); + dRFromAxisAndAngle (R3,1,0,0,-a3); + dMultiply0 (Rtmp1,R2,R3,3,3,3); + dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3); + dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3); + dBodySetRotation (body[1],Rtmp1); + // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3); + + return err; + } + + // ********** universal joint + + case 700: { // 2 body: joint constraint + dVector3 ax1, ax2; + + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 701: { // 2 body: angle 1 rate + static dReal last_angle = 0; + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + // I'm not sure why the error is so large here. + return fabs(r - er) * 1e1; + } + + case 702: { // 2 body: angle 2 rate + static dReal last_angle = 0; + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + // I'm not sure why the error is so large here. + return fabs(r - er) * 1e1; + } + + case 720: { // universal transmit torque test: constraint error + dVector3 ax1, ax2; + addOscillatingTorqueAbout (0.1, 1, 1, 0); + dampRotationalMotion (0.1); + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 721: { // universal transmit torque test: angle1 rate + static dReal last_angle = 0; + addOscillatingTorqueAbout (0.1, 1, 1, 0); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 722: { // universal transmit torque test: angle2 rate + static dReal last_angle = 0; + addOscillatingTorqueAbout (0.1, 1, 1, 0); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 730:{ + dVector3 ax1, ax2; + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); + dampRotationalMotion (0.1); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 731:{ + dVector3 ax1; + static dReal last_angle = 0; + dJointGetUniversalAxis1(joint, ax1); + addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 2e3; + } + + case 732:{ + dVector3 ax1; + static dReal last_angle = 0; + dJointGetUniversalAxis1(joint, ax1); + addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 740:{ + dVector3 ax1, ax2; + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); + dampRotationalMotion (0.1); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 741:{ + dVector3 ax2; + static dReal last_angle = 0; + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 742:{ + dVector3 ax2; + static dReal last_angle = 0; + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e4; + } + + // ********** slider joint + case 801: + case 803: + addSpringForce (0.25); + return dInfinity; + + case 802: + case 804: { + static dReal a = 0; + dBodyAddTorque (body[0], 0, 0.01*cos(1.5708*a), 0); + a += 0.01; + return dInfinity; + } + + case 805: + addOscillatingForce (0.1); + return dInfinity; + } + + + return dInfinity; +} + +//**************************************************************************** +// simulation stuff common to all the tests + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// simulation loop + +static void simLoop (int pause) +{ + // stop after a given number of iterations, as long as we are not in + // interactive mode + if (cmd_graphics && !cmd_interactive && + (iteration >= max_iterations)) { + dsStop(); + return; + } + iteration++; + + if (!pause) { + // do stuff for this test and check to see if the joint is behaving well + dReal error = doStuffAndGetError (test_num); + if (error > max_error) max_error = error; + if (cmd_interactive && error < dInfinity) { + printf ("scaled error = %.4e\n",error); + } + + // take a step + dWorldStep (world,STEPSIZE); + + // occasionally re-orient the first body to create a deliberate error. + if (cmd_occasional_error) { + static int count = 0; + if ((count % 20)==0) { + // randomly adjust orientation of body[0] + const dReal *R1; + dMatrix3 R2,R3; + R1 = dBodyGetRotation (body[0]); + dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, + dRandReal()-0.5,dRandReal()-0.5); + dMultiply0 (R3,R1,R2,3,3,3); + dBodySetRotation (body[0],R3); + + // randomly adjust position of body[0] + const dReal *pos = dBodyGetPosition (body[0]); + dBodySetPosition (body[0], + pos[0]+0.2*(dRandReal()-0.5), + pos[1]+0.2*(dRandReal()-0.5), + pos[2]+0.2*(dRandReal()-0.5)); + } + count++; + } + } + + if (cmd_graphics) { + dReal sides1[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f}; + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + if (body[1]) { + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); + } + } +} + +//**************************************************************************** +// conduct a specific test, and report the results + +void doTest (int argc, char **argv, int n, int fatal_if_bad_n) +{ + test_num = n; + iteration = 0; + max_iterations = 300; + max_error = 0; + + if (! setupTest (n)) { + if (fatal_if_bad_n) dError (0,"bad test number"); + return; + } + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = 0; + if (cmd_path_to_textures) + fn.path_to_textures = cmd_path_to_textures; + else + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // run simulation + if (cmd_graphics) { + dsSimulationLoop (argc,argv,352,288,&fn); + } + else { + for (int i=0; i < max_iterations; i++) simLoop (0); + } + dWorldDestroy (world); + body[0] = 0; + body[1] = 0; + joint = 0; + + // print results + printf ("test %d: ",n); + if (max_error == dInfinity) printf ("error not computed\n"); + else { + printf ("max scaled error = %.4e",max_error); + if (max_error < 1) printf (" - passed\n"); + else printf (" - FAILED\n"); + } +} + +//**************************************************************************** +// main + +int main (int argc, char **argv) +{ + int i; + dInitODE2(0); + + // process the command line args. anything that starts with `-' is assumed + // to be a drawstuff argument. + for (i=1; i<argc; i++) { + if ( argv[i][0]=='-' && argv[i][1]=='i' && argv[i][2]==0) cmd_interactive = 1; + else if ( argv[i][0]=='-' && argv[i][1]=='g' && argv[i][2]==0) cmd_graphics = 0; + else if ( argv[i][0]=='-' && argv[i][1]=='e' && argv[i][2]==0) cmd_graphics = 0; + else if ( argv[i][0]=='-' && argv[i][1]=='n' && isdigit(argv[i][2]) ) { + char *endptr; + long int n = strtol (&(argv[i][2]),&endptr,10); + if (*endptr == 0) cmd_test_num = n; + } + else + cmd_path_to_textures = argv[i]; + } + + // do the tests + if (cmd_test_num == -1) { + for (i=0; i<NUM_JOINTS*100; i++) doTest (argc,argv,i,0); + } + else { + doTest (argc,argv,cmd_test_num,1); + } + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_kinematic.cpp b/libs/ode-0.16.1/ode/demo/demo_kinematic.cpp new file mode 100644 index 0000000..5f2c613 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_kinematic.cpp @@ -0,0 +1,239 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <iostream> +#include <set> +#include <algorithm> +#include <functional> +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#endif + + +using namespace std; + +dWorld *world; +dSpace *space; +dPlane *ground; +dBody *kbody; +dBox *kbox; +dJointGroup joints; +dCylinder *kpole; +dBody *matraca; +dBox *matraca_geom; +dHingeJoint *hinge; + +struct Box { + dBody body; + dBox geom; + Box() : + body(*world), + geom(*space, 0.2, 0.2, 0.2) + { + dMass mass; + mass.setBox(10, 0.2, 0.2, 0.2); + body.setMass(mass); + geom.setData(this); + geom.setBody(body); + } + void draw() const + { + dVector3 lengths; + geom.getLengths(lengths); + dsSetTexture(DS_WOOD); + dsSetColor(0,1,0); + dsDrawBox(geom.getPosition(), geom.getRotation(), lengths); + } +}; + +set<Box*> boxes; +set<Box*> to_remove; + +void dropBox() +{ + Box *box = new Box(); + + dReal px = (rand() / float(RAND_MAX)) * 2 - 1; + dReal py = (rand() / float(RAND_MAX)) * 2 - 1; + dReal pz = 2.5; + box->body.setPosition(px, py, pz); + + boxes.insert(box); +} + +void queueRemoval(dGeomID g) +{ + Box *b = (Box*)dGeomGetData(g); + to_remove.insert(b); +} + +void removeQueued() +{ + while (!to_remove.empty()) { + Box *b = *to_remove.begin(); + to_remove.erase(b); + boxes.erase(b); + delete b; + } +} + + +void nearCallback(void *, dGeomID g1, dGeomID g2) +{ + if (g1 == ground->id()) { + queueRemoval(g2); + return; + } + if (g2 == ground->id()) { + queueRemoval(g1); + return; + } + + dBodyID b1 = dGeomGetBody(g1); + dBodyID b2 = dGeomGetBody(g2); + + if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) + return; + + const int MAX_CONTACTS = 10; + dContact contact[MAX_CONTACTS]; + int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); + for (int i=0; i<n; ++i) { + contact[i].surface.mode = 0; + contact[i].surface.mu = 1; + dJointID j = dJointCreateContact (*world, joints.id(), contact+i); + dJointAttach(j, b1, b2); + } +} + + +void +simLoop(int pause) +{ + if (!pause) { + const dReal timestep = 0.04; + + // this does a hard-coded circular motion animation + static float t=0; + t += timestep/4; + if (t > 2*M_PI) + t = 0; + dVector3 next_pos = { dCos(t), dSin(t), REAL(0.5)}; + dVector3 vel; + // vel = (next_pos - cur_pos) / timestep + dSubtractVectors3(vel, next_pos, kbody->getPosition()); + dScaleVector3(vel, 1/timestep); + kbody->setLinearVel(vel); + // end of hard-coded animation + + space->collide(0, nearCallback); + removeQueued(); + + world->quickStep(timestep); + joints.clear(); + } + + dVector3 lengths; + + // the moving platform + kbox->getLengths(lengths); + dsSetTexture(DS_WOOD); + dsSetColor(.3, .3, 1); + dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths); + dReal length, radius; + kpole->getParams(&radius, &length); + dsSetTexture(DS_CHECKERED); + dsSetColor(1, 1, 0); + dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius); + + // the matraca + matraca_geom->getLengths(lengths); + dsSetColor(1,0,0); + dsSetTexture(DS_WOOD); + dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths); + + // and the boxes + for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw)); +} + +void command(int c) +{ + switch (c) { + case ' ': + dropBox(); + break; + } +} + +int main(int argc, char **argv) +{ + dInitODE(); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = 0; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + cout << endl << "*** Press SPACE to drop boxes **" << endl; + + space = new dSimpleSpace(); + ground = new dPlane(*space, 0, 0, 1, 0); + + world = new dWorld; + world->setGravity(0, 0, -.5); + + kbody = new dBody(*world); + kbody->setKinematic(); + const dReal kx = 1, ky = 0, kz = .5; + kbody->setPosition(kx, ky, kz); + kbox = new dBox(*space, 3, 3, .5); + kbox->setBody(*kbody); + kpole = new dCylinder(*space, .125, 1.5); + kpole->setBody(*kbody); + dGeomSetOffsetPosition(kpole->id(), 0, 0, 0.8); + + matraca = new dBody(*world); + matraca->setPosition(kx+0, ky+1, kz+1); + matraca_geom = new dBox(*space, 0.5, 2, 0.75); + matraca_geom->setBody(*matraca); + dMass mass; + mass.setBox(1, 0.5, 2, 0.75); + matraca->setMass(mass); + + hinge = new dHingeJoint(*world); + hinge->attach(*kbody, *matraca); + hinge->setAnchor(kx, ky, kz+1); + hinge->setAxis(0, 0, 1); + + dsSimulationLoop (argc, argv, 640, 480, &fn); + + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_motion.cpp b/libs/ode-0.16.1/ode/demo/demo_motion.cpp new file mode 100644 index 0000000..a83887d --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_motion.cpp @@ -0,0 +1,527 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + This demo shows how to use dContactMotionN in a lifting platform. +*/ +//#include <unistd.h> // for usleep() +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 8 // maximum number of contact points per body +#define USE_GEOM_OFFSET 1 + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int write_world = 0; +static int show_body = 0; + +static dGeomID platform, ground; + +dVector3 platpos = {0, 0, 0}; +int mov_type = 2; +dReal mov_time = 0; + + +const dReal mov1_speed = 0.2; + +dVector3 mov2_vel = { 0.2, 0.1, 0.25}; + + + + +/**************************************************************** + * Movement 1: move platform up, reset every 80 units of time. * + * This is the simplest case * + ****************************************************************/ +static void moveplat_1(dReal stepsize) +{ + mov_time += stepsize; + if (mov_time > 80) + mov_time = 0; + + platpos[0] = platpos[1] = 0; + // the platform moves up (Z) at constant speed: mov1_speed + platpos[2] = mov1_speed * mov_time; +} + +// Generate contact info for movement 1 +static void contactplat_1(dContact &contact) +{ + contact.surface.mode |= dContactMotionN; + contact.surface.motionN = mov1_speed; +} + + + +/**************************************************************** + * Movement 2: move platform along direction mov2_vel, reset * + * every 80 units of time. * + * This is the most general case: the geom moves along * + * an arbitrary direction. * + ****************************************************************/ +static void moveplat_2(dReal stepsize) +{ + mov_time += stepsize; + if (mov_time > 80) + mov_time = 0; + + // the platform moves at constant speed: mov2_speed + platpos[0] = mov2_vel[0] * mov_time; + platpos[1] = mov2_vel[1] * mov_time; + platpos[2] = mov2_vel[2] * mov_time; +} + +// Generate contact info for movement 1 +static void contactplat_2(dContact &contact) +{ + /* + For arbitrary contact directions we need to project the moving + geom's velocity against the contact normal and fdir1, fdir2 + (obtained with dPlaneSpace()). Assuming moving geom=g2 + (so the contact joint is in the moving geom's reference frame): + motion1 = dCalcVectorDot3(fdir1, vel); + motion2 = dCalcVectorDot3(fdir2, vel); + motionN = dCalcVectorDot3(normal, vel); + + For geom=g1 just negate motionN and motion2. fdir1 is an arbitrary + vector, so there's no need to negate motion1. + + */ + contact.surface.mode |= + dContactMotionN | // velocity along normal + dContactMotion1 | dContactMotion2 | // and along the contact plane + dContactFDir1; // don't forget to set the direction 1 + + + // This is a convenience function: given a vector, it finds other 2 perpendicular vectors + dVector3 motiondir1, motiondir2; + dPlaneSpace(contact.geom.normal, motiondir1, motiondir2); + for (int i=0; i<3; ++i) + contact.fdir1[i] = motiondir1[i]; + + + dReal inv = 1; + if (contact.geom.g1 == platform) + inv = -1; + + contact.surface.motion1 = dCalcVectorDot3(mov2_vel, motiondir1); + contact.surface.motion2 = inv * dCalcVectorDot3(mov2_vel, motiondir2); + contact.surface.motionN = inv * dCalcVectorDot3(mov2_vel, contact.geom.normal); + +} + + + + + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + dMatrix3 RI; + static const dReal ss[3] = {0.02,0.02,0.02}; + + dContact contact[MAX_CONTACTS]; + int numc = dCollide (o1, o2, MAX_CONTACTS, + &contact[0].geom, sizeof(dContact)); + + if (numc) + dRSetIdentity(RI); + + bool isplatform = (o1 == platform) || (o2 == platform); + + for (int i=0; i< numc; i++) { + contact[i].surface.mode = dContactBounce; + contact[i].surface.mu = 1; + contact[i].surface.bounce = 0.25; + contact[i].surface.bounce_vel = 0.01; + + if (isplatform) { + switch (mov_type) { + case 1: + contactplat_1(contact[i]); + break; + case 2: + contactplat_2(contact[i]); + break; + } + } + + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c, dGeomGetBody(o1), dGeomGetBody(o2)); + if (show_contacts) + dsDrawBox (contact[i].geom.pos, RI, ss); + } +} + + +// start simulation - set viewpoint + +static float xyz[3] = {2.1106f,-1.3007,2.f}; +static float hpr[3] = {150.f,-13.5000f,0.0000f}; + +static void start() +{ + //dAllocateODEDataForThread(dAllocateMaskAll); + dsSetViewpoint (xyz,hpr); + printf ("To drop another object, press:\n"); + printf (" b for box.\n"); + printf (" s for sphere.\n"); + printf (" c for capsule.\n"); + printf (" y for cylinder.\n"); + printf ("Press m to change the movement type\n"); + printf ("Press space to reset the platform\n"); + printf ("To toggle showing the geom AABBs, press a.\n"); + printf ("To toggle showing the contact points, press t.\n"); + printf ("To toggle dropping from random position/orientation, press r.\n"); + printf ("To save the current state to 'state.dif', press 1.\n"); +} + + +char locase (char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + dsizeint i; + int k; + dReal sides[3]; + dMass m; + int setBody; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'y') + { + setBody = 0; + if (num < NUM) { + i = num; + num++; + } + else { + i = nextobj; + nextobj++; + if (nextobj >= num) nextobj = 0; + + // destroy the body and geoms for slot i + if (obj[i].body) { + dBodyDestroy (obj[i].body); + } + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) { + dGeomDestroy (obj[i].geom[k]); + } + } + memset (&obj[i],0,sizeof(obj[i])); + } + + obj[i].body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) + { + dBodySetPosition (obj[i].body, + dRandReal()*2-1 + platpos[0], + dRandReal()*2-1 + platpos[1], + dRandReal()+2 + platpos[2]); + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } + else + { + dBodySetPosition (obj[i].body, + platpos[0], + platpos[1], + platpos[2]+2); + dRSetIdentity (R); + } + dBodySetRotation (obj[i].body,R); + dBodySetData (obj[i].body,(void*) i); + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } + else if (cmd == 'y') { + dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + } + + if (!setBody) + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) { + dGeomSetBody (obj[i].geom[k],obj[i].body); + } + } + + dBodySetMass (obj[i].body,&m); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } + else if (cmd == '1') { + write_world = 1; + } + else if (cmd == ' ') { + mov_time = 0; + } + else if (cmd == 'm') { + mov_type = mov_type==1 ? 2 : 1; + mov_time = 0; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + int i; + + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } + + if (show_body) { + dBodyID body = dGeomGetBody(g); + if (body) { + const dReal *bodypos = dBodyGetPosition (body); + const dReal *bodyr = dBodyGetRotation (body); + dReal bodySides[3] = { 0.1, 0.1, 0.1 }; + dsSetColorAlpha(0,1,0,1); + dsDrawBox(bodypos,bodyr,bodySides); + } + } + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (g,aabb); + dVector3 bbpos; + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void updatecam() +{ + xyz[0] = platpos[0] + 3.3; + xyz[1] = platpos[1] - 1.8; + xyz[2] = platpos[2] + 2; + dsSetViewpoint (xyz, hpr); +} + +static void simLoop (int pause) +{ + const dReal stepsize = 0.02; + + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + if (!pause) { + + if (mov_type == 1) + moveplat_1(stepsize); + else + moveplat_2(stepsize); + + dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]); + updatecam(); + dWorldQuickStep (world,stepsize); + //dWorldStep (world,stepsize); + } + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF (world,f,"X"); + fclose (f); + } + write_world = 0; + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i<num; i++) { + for (int j=0; j < GPB; j++) { + if (! dBodyIsEnabled (obj[i].body)) { + dsSetColor (1,0.8,0); + } + else { + dsSetColor (1,1,0); + } + drawGeom (obj[i].geom[j],0,0,show_aabb); + } + } + dsSetColor (1,0,0); + drawGeom (platform,0,0,show_aabb); + //usleep(5000); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE(); + world = dWorldCreate(); + +#if 1 + space = dHashSpaceCreate (0); +#elif 0 + dVector3 center = {0,0,0}, extents = { 100, 100, 100}; + space = dQuadTreeSpaceCreate(0, center, extents, 5); +#elif 0 + space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ); +#else + space = dSimpleSpaceCreate(0); +#endif + + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + dWorldSetCFM (world,1e-5); + + dWorldSetLinearDamping(world, 0.00001); + dWorldSetAngularDamping(world, 0.005); + dWorldSetMaxAngularSpeed(world, 200); + + dWorldSetContactSurfaceLayer (world,0.001); + ground = dCreatePlane (space,0,0,1,0); + + memset (obj,0,sizeof(obj)); + + // create lift platform + platform = dCreateBox(space, 4, 4, 1); + + dGeomSetCategoryBits(ground, 1ul); + dGeomSetCategoryBits(platform, 2ul); + dGeomSetCollideBits(ground, ~2ul); + dGeomSetCollideBits(platform, ~1ul); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + + +// Local Variables: +// c-basic-offset:4 +// End: diff --git a/libs/ode-0.16.1/ode/demo/demo_motor.cpp b/libs/ode-0.16.1/ode/demo/demo_motor.cpp new file mode 100644 index 0000000..9e0dede --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_motor.cpp @@ -0,0 +1,209 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define SIDE (0.5f) // side length of a box +#define MASS (1.0) // mass of a box + + +// dynamics and collision objects +static dWorldID world; +static dBodyID body[2]; +static dGeomID geom[2]; +static dJointID lmotor[2]; +static dJointID amotor[2]; +static dSpaceID space; +static dJointGroupID contactgroup; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("Press 'q,a,z' to control one axis of lmotor connectiong two bodies. (q is +,a is 0, z is -)\n"); + printf ("Press 'w,e,r' to control one axis of lmotor connectiong first body with world. (w is +,e is 0, r is -)\n"); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == 'q' || cmd == 'Q') { + dJointSetLMotorParam(lmotor[0],dParamVel,0); + dJointSetLMotorParam(lmotor[0],dParamVel2,0); + dJointSetLMotorParam(lmotor[0],dParamVel3,0.1); + } else if (cmd == 'a' || cmd == 'A') { + dJointSetLMotorParam(lmotor[0],dParamVel,0); + dJointSetLMotorParam(lmotor[0],dParamVel2,0); + dJointSetLMotorParam(lmotor[0],dParamVel3,0); + } else if (cmd == 'z' || cmd == 'Z') { + dJointSetLMotorParam(lmotor[0],dParamVel,0); + dJointSetLMotorParam(lmotor[0],dParamVel2,0); + dJointSetLMotorParam(lmotor[0],dParamVel3,-0.1); + } else if (cmd == 'w' || cmd == 'W') { + dJointSetLMotorParam(lmotor[1],dParamVel,0.1); + dJointSetLMotorParam(lmotor[1],dParamVel2,0); + dJointSetLMotorParam(lmotor[1],dParamVel3,0); + } else if (cmd == 'e' || cmd == 'E') { + dJointSetLMotorParam(lmotor[1],dParamVel,0); + dJointSetLMotorParam(lmotor[1],dParamVel2,0); + dJointSetLMotorParam(lmotor[1],dParamVel3,0); + } else if (cmd == 'r' || cmd == 'R') { + dJointSetLMotorParam(lmotor[1],dParamVel,-0.1); + dJointSetLMotorParam(lmotor[1],dParamVel2,0); + dJointSetLMotorParam(lmotor[1],dParamVel3,0); + } + +} + + + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + dContact contact; + contact.surface.mode = 0; + contact.surface.mu = dInfinity; + if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) { + dJointID c = dJointCreateContact (world,contactgroup,&contact); + dJointAttach (c,b1,b2); + } +} + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + dSpaceCollide(space,0,&nearCallback); + dWorldQuickStep (world,0.05); + dJointGroupEmpty(contactgroup); + } + + dReal sides1[3]; + dGeomBoxGetLengths(geom[0], sides1); + dReal sides2[3]; + dGeomBoxGetLengths(geom[1], sides2); + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + contactgroup = dJointGroupCreate(0); + world = dWorldCreate(); + space = dSimpleSpaceCreate(0); + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0],0,0,1); + geom[0] = dCreateBox(space,SIDE,SIDE,SIDE); + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dBodySetPosition (body[1],0,0,2); + geom[1] = dCreateBox(space,SIDE,SIDE,SIDE); + + dGeomSetBody(geom[0],body[0]); + dGeomSetBody(geom[1],body[1]); + + lmotor[0] = dJointCreateLMotor (world,0); + dJointAttach (lmotor[0],body[0],body[1]); + lmotor[1] = dJointCreateLMotor (world,0); + dJointAttach (lmotor[1],body[0],0); + amotor[0] = dJointCreateAMotor(world,0); + dJointAttach(amotor[0], body[0],body[1]); + amotor[1] = dJointCreateAMotor(world,0); + dJointAttach(amotor[1], body[0], 0); + + for (int i=0; i<2; i++) { + dJointSetAMotorNumAxes(amotor[i], 3); + dJointSetAMotorAxis(amotor[i],0,1,1,0,0); + dJointSetAMotorAxis(amotor[i],1,1,0,1,0); + dJointSetAMotorAxis(amotor[i],2,1,0,0,1); + dJointSetAMotorParam(amotor[i],dParamFMax,0.00001); + dJointSetAMotorParam(amotor[i],dParamFMax2,0.00001); + dJointSetAMotorParam(amotor[i],dParamFMax3,0.00001); + + dJointSetAMotorParam(amotor[i],dParamVel,0); + dJointSetAMotorParam(amotor[i],dParamVel2,0); + dJointSetAMotorParam(amotor[i],dParamVel3,0); + + dJointSetLMotorNumAxes(lmotor[i],3); + dJointSetLMotorAxis(lmotor[i],0,1,1,0,0); + dJointSetLMotorAxis(lmotor[i],1,1,0,1,0); + dJointSetLMotorAxis(lmotor[i],2,1,0,0,1); + + dJointSetLMotorParam(lmotor[i],dParamFMax,0.0001); + dJointSetLMotorParam(lmotor[i],dParamFMax2,0.0001); + dJointSetLMotorParam(lmotor[i],dParamFMax3,0.0001); + } + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy(contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp b/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp new file mode 100644 index 0000000..2f03900 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp @@ -0,0 +1,415 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" +#include "bunny_geom.h" +#include "convex_bunny_geom.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawLine dsDrawLineD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 200 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 64 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject +{ + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? + +typedef dReal dVector3R[3]; + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback( void *, dGeomID o1, dGeomID o2 ) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody( o1 ); + dBodyID b2 = dGeomGetBody( o2 ); + if ( b1 && b2 && dAreConnectedExcluding( b1,b2,dJointTypeContact ) ) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for ( i=0; i<MAX_CONTACTS; i++ ) + { + contact[i].surface.mode = dContactBounce | dContactSoftCFM; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.1; + contact[i].surface.bounce_vel = 0.1; + contact[i].surface.soft_cfm = 0.01; + } + if ( int numc = dCollide( o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof( dContact ) ) ) + { + dMatrix3 RI; + dRSetIdentity( RI ); + const dReal ss[3] = {0.02,0.02,0.02}; + for ( i=0; i<numc; i++ ) + { + dJointID c = dJointCreateContact( world,contactgroup,contact+i ); + dJointAttach( c,b1,b2 ); + if ( show_contacts ) dsDrawBox( contact[i].geom.pos,RI,ss ); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread( dAllocateMaskAll ); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint( xyz,hpr ); + printf( "To drop another object, press:\n" ); + printf( " b for box.\n" ); + printf( " s for sphere.\n" ); + printf( " c for capsule.\n" ); + printf( " v for a convex.\n" ); + printf( "To select an object, press space.\n" ); + printf( "To disable the selected object, press d.\n" ); + printf( "To enable the selected object, press e.\n" ); + printf( "To toggle showing the geom AABBs, press a.\n" ); + printf( "To toggle showing the contact points, press t.\n" ); + printf( "To toggle dropping from random position/orientation, press r.\n" ); +} + + +char locase( char c ) +{ + if ( c >= 'A' && c <= 'Z' ) return c - ( 'a'-'A' ); + else return c; +} + + +// called when a key pressed +static void command( int cmd ) +{ + int i,k; + dReal sides[3]; + dMass m; + + cmd = locase( cmd ); + if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' || cmd == 'y') + { + if ( num < NUM ) + { + i = num; + num++; + } + else + { + i = nextobj; + nextobj++; + if ( nextobj >= num ) nextobj = 0; + + // destroy the body and geoms for slot i + dBodyDestroy( obj[i].body ); + for ( k=0; k < GPB; k++ ) + { + if ( obj[i].geom[k] ) dGeomDestroy( obj[i].geom[k] ); + } + memset( &obj[i],0,sizeof( obj[i] ) ); + } + + obj[i].body = dBodyCreate( world ); + for ( k=0; k<3; k++ ) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if ( random_pos ) + { + dBodySetPosition( obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 ); + dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0 ); + } + else + { + dReal maxheight = 0; + for ( k=0; k<num; k++ ) + { + const dReal *pos = dBodyGetPosition( obj[k].body ); + if ( pos[2] > maxheight ) maxheight = pos[2]; + } + dBodySetPosition( obj[i].body, 0,0,maxheight+1 ); + dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 ); + } + dBodySetRotation( obj[i].body,R ); + dBodySetData( obj[i].body,( void* )( dsizeint )i ); + + if ( cmd == 'b' ) + { + dMassSetBox( &m,DENSITY,sides[0],sides[1],sides[2] ); + obj[i].geom[0] = dCreateBox( space,sides[0],sides[1],sides[2] ); + } + else if ( cmd == 'c' ) + { + sides[0] *= 0.5; + dMassSetCapsule( &m,DENSITY,3,sides[0],sides[1] ); + obj[i].geom[0] = dCreateCapsule( space,sides[0],sides[1] ); + } + else if (cmd == 'y') { + dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } + else if ( cmd == 's' ) + { + sides[0] *= 0.5; + dMassSetSphere( &m,DENSITY,sides[0] ); + obj[i].geom[0] = dCreateSphere( space,sides[0] ); + } + else if ( cmd == 'v' ) + { + obj[i].geom[0] = dCreateConvex( space, + convexBunnyPlanes, + convexBunnyPlaneCount, + convexBunnyPoints, + convexBunnyPointCount, + convexBunnyPolygons ); + + /// Use equivalent TriMesh to set mass + dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount, + ( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) ); + dGeomTriMeshDataPreprocess2( new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL ); + + dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 ); + + dMassSetTrimesh( &m, DENSITY, triMesh ); + + dGeomDestroy( triMesh ); + dGeomTriMeshDataDestroy( new_tmdata ); + + printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] ); + dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2] ); + dMassTranslate( &m, -m.c[0], -m.c[1], -m.c[2] ); + } + + for ( k=0; k < GPB; k++ ) + { + if ( obj[i].geom[k] ) dGeomSetBody( obj[i].geom[k],obj[i].body ); + } + + dBodySetMass( obj[i].body,&m ); + } + + if ( cmd == ' ' ) + { + selected++; + if ( selected >= num ) selected = 0; + if ( selected < 0 ) selected = 0; + } + else if ( cmd == 'd' && selected >= 0 && selected < num ) + { + dBodyDisable( obj[selected].body ); + } + else if ( cmd == 'e' && selected >= 0 && selected < num ) + { + dBodyEnable( obj[selected].body ); + } + else if ( cmd == 'a' ) + { + show_aabb ^= 1; + } + else if ( cmd == 't' ) + { + show_contacts ^= 1; + } + else if ( cmd == 'r' ) + { + random_pos ^= 1; + } +} + +// draw a geom +void drawGeom( dGeomID g, const dReal *pos, const dReal *R, int show_aabb ) +{ + if ( !g ) return; + if ( !pos ) pos = dGeomGetPosition( g ); + if ( !R ) R = dGeomGetRotation( g ); + + int type = dGeomGetClass( g ); + if ( type == dBoxClass ) + { + dVector3 sides; + dGeomBoxGetLengths( g,sides ); + dsDrawBox( pos,R,sides ); + } + else if ( type == dSphereClass ) + { + dsDrawSphere( pos,R,dGeomSphereGetRadius( g ) ); + } + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } + else if ( type == dCapsuleClass ) + { + dReal radius,length; + dGeomCapsuleGetParams( g,&radius,&length ); + dsDrawCapsule( pos,R,length,radius ); + } + else if ( type == dConvexClass ) + { + dsDrawConvex( pos,R, + convexBunnyPlanes, + convexBunnyPlaneCount, + convexBunnyPoints, + convexBunnyPointCount, + convexBunnyPolygons ); + } + + if ( show_aabb ) + { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB( g,aabb ); + dVector3 bbpos; + for ( int i=0; i<3; i++ ) bbpos[i] = 0.5*( aabb[i*2] + aabb[i*2+1] ); + dVector3 bbsides; + for ( int j=0; j<3; j++ ) bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity( RI ); + dsSetColorAlpha( 1,0,0,0.5 ); + dsDrawBox( bbpos,RI,bbsides ); + } +} + +// simulation loop + +static void simLoop( int pause ) +{ + dsSetColor( 0,0,2 ); + dSpaceCollide( space,0,&nearCallback ); + + if ( !pause ) dWorldQuickStep( world,0.05 ); + + for ( int j = 0; j < dSpaceGetNumGeoms( space ); j++ ) + { + dSpaceGetGeom( space, j ); + } + + // remove all contact joints + dJointGroupEmpty( contactgroup ); + + dsSetColor( 1,1,0 ); + dsSetTexture( DS_WOOD ); + for ( int i=0; i<num; i++ ) + { + for ( int j=0; j < GPB; j++ ) + { + if ( obj[i].geom[j] ) + { + if ( i==selected ) + { + dsSetColor( 0,0.7,1 ); + } + else if ( ! dBodyIsEnabled( obj[i].body ) ) + { + dsSetColor( 1,0,0 ); + } + else + { + dsSetColor( 1,1,0 ); + } + + drawGeom( obj[i].geom[j],0,0,show_aabb ); + } + } + } +} + + +int main( int argc, char **argv ) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2( 0 ); + world = dWorldCreate(); + + space = dSimpleSpaceCreate( 0 ); + contactgroup = dJointGroupCreate( 0 ); + dWorldSetGravity( world,0,0,-0.5 ); + dWorldSetCFM( world,1e-5 ); + dCreatePlane( space,0,0,1,0 ); + memset( obj,0,sizeof( obj ) ); + + // run simulation + dsSimulationLoop( argc,argv,352,288,&fn ); + + dJointGroupDestroy( contactgroup ); + dSpaceDestroy( space ); + dWorldDestroy( world ); + dCloseODE(); + return 0; +} + + diff --git a/libs/ode-0.16.1/ode/demo/demo_moving_trimesh.cpp b/libs/ode-0.16.1/ode/demo/demo_moving_trimesh.cpp new file mode 100644 index 0000000..26034ae --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_moving_trimesh.cpp @@ -0,0 +1,677 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" +#include "bunny_geom.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +//<---- Convex Object +static const dReal planes[] = // planes for a cube +{ + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ +}; +static const unsigned int planecount=6; + +static const dReal points[] = // points for a cube +{ + 0.25f,0.25f,0.25f, + -0.25f,0.25f,0.25f, + + 0.25f,-0.25f,0.25f, + -0.25f,-0.25f,0.25f, + + 0.25f,0.25f,-0.25f, + -0.25f,0.25f,-0.25f, + + 0.25f,-0.25f,-0.25f, + -0.25f,-0.25f,-0.25f, +}; +static const unsigned int pointcount=8; + +static const unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +int tmTriangles[] = +{ + 0,2,6, + 0,6,4, + 1,0,4, + 1,4,5, + 0,1,3, + 0,3,2, + 3,1,5, + 3,5,7, + 2,3,7, + 2,7,6, + 5,4,6, + 5,6,7 +}; + +float tmVertices[] = +{ + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 +}; + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawLine dsDrawLineD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 200 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 64 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body + + // Trimesh only - double buffered matrices for 'last transform' setup + dReal matrix_dblbuff[ 16 * 2 ]; + int last_matrix_index; +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? + +typedef dReal dVector3R[3]; + +dGeomID TriMesh1; +dGeomID TriMesh2; +static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i<MAX_CONTACTS; i++) { + contact[i].surface.mode = dContactBounce | dContactSoftCFM; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.1; + contact[i].surface.bounce_vel = 0.1; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact))) { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.02,0.02,0.02}; + for (i=0; i<numc; i++) { + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("To drop another object, press:\n"); + printf (" b for box.\n"); + printf (" s for sphere.\n"); + printf (" y for cylinder.\n"); + printf (" c for capsule.\n"); + printf (" x for a composite object.\n"); + printf (" v for a convex object.\n"); + printf (" m for a trimesh.\n"); + printf ("To select an object, press space.\n"); + printf ("To disable the selected object, press d.\n"); + printf ("To enable the selected object, press e.\n"); + printf ("To toggle showing the geom AABBs, press a.\n"); + printf ("To toggle showing the contact points, press t.\n"); + printf ("To toggle dropping from random position/orientation, press r.\n"); +} + + +char locase (char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + int i,j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') { + if (num < NUM) { + i = num; + num++; + } + else { + i = nextobj; + nextobj++; + if (nextobj >= num) nextobj = 0; + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + } + memset (&obj[i],0,sizeof(obj[i])); + } + + obj[i].body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition (obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3); + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } + else { + dReal maxheight = 0; + for (k=0; k<num; k++) { + const dReal *pos = dBodyGetPosition (obj[k].body); + if (pos[2] > maxheight) maxheight = pos[2]; + } + dBodySetPosition (obj[i].body, 0,0,maxheight+1); + dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); + } + dBodySetRotation (obj[i].body,R); + dBodySetData (obj[i].body,(void*)(dsizeint)i); + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } else if (cmd == 'v') { + + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex(space, + planes, + planecount, + points, + pointcount, + polygons); + } + else if (cmd == 'y') { + sides[1] *= 0.5; + dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + } + else if (cmd == 'm') { + dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, + (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); + dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); + + + obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); + + // remember the mesh's dTriMeshDataID on its userdata for convenience. + dGeomSetData(obj[i].geom[0], new_tmdata); + + dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); + printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); + dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); + dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); + } + else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j<GPB; j++) + for (k=0; k<3; k++) + dpos[j][k] = dRandReal()*0.3-0.15; + + for (k=0; k<GPB; k++) { + if (k==0) { + dReal radius = dRandReal()*0.25+0.05; + obj[i].geom[k] = dCreateSphere (space,radius); + dMassSetSphere (&m2,DENSITY,radius); + } else if (k==1) { + obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); + dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); + } else { + dReal radius = dRandReal()*0.1+0.05; + dReal length = dRandReal()*1.0+0.1; + obj[i].geom[k] = dCreateCapsule(space,radius,length); + dMassSetCapsule(&m2,DENSITY,3,radius,length); + } + + dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dMassRotate(&m2,drot[k]); + + dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); + + // add to the total mass + dMassAdd(&m,&m2); + + } + for (k=0; k<GPB; k++) { + dGeomSetBody(obj[i].geom[k],obj[i].body); + dGeomSetOffsetPosition(obj[i].geom[k], + dpos[k][0]-m.c[0], + dpos[k][1]-m.c[1], + dpos[k][2]-m.c[2]); + dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); + } + dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); + dBodySetMass(obj[i].body,&m); + + } + + if (!setBody) { // avoid calling for composite geometries + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) + dGeomSetBody(obj[i].geom[k],obj[i].body); + + dBodySetMass(obj[i].body,&m); + } + } + + if (cmd == ' ') { + selected++; + if (selected >= num) selected = 0; + if (selected < 0) selected = 0; + } + else if (cmd == 'd' && selected >= 0 && selected < num) { + dBodyDisable (obj[selected].body); + } + else if (cmd == 'e' && selected >= 0 && selected < num) { + dBodyEnable (obj[selected].body); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } else if (type == dConvexClass) { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } +} + + +// set previous transformation matrix for trimesh +void setCurrentTransform(dGeomID geom) +{ + const dReal* Pos = dGeomGetPosition(geom); + const dReal* Rot = dGeomGetRotation(geom); + + const dReal Transform[16] = + { + Rot[0], Rot[4], Rot[8], 0, + Rot[1], Rot[5], Rot[9], 0, + Rot[2], Rot[6], Rot[10], 0, + Pos[0], Pos[1], Pos[2], 1 + }; + + dGeomTriMeshSetLastTransform( geom, *(dMatrix4*)(&Transform) ); + +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + + +#if 1 + // What is this for??? - Bram + if (!pause) + { + for (int i=0; i<num; i++) + for (int j=0; j < GPB; j++) + if (obj[i].geom[j]) + if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) + setCurrentTransform(obj[i].geom[j]); + + setCurrentTransform(TriMesh1); + setCurrentTransform(TriMesh2); + } +#endif + + //if (!pause) dWorldStep (world,0.05); + if (!pause) dWorldQuickStep (world,0.05); + + for (int j = 0; j < dSpaceGetNumGeoms(space); j++){ + dSpaceGetGeom(space, j); + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i<num; i++) { + for (int j=0; j < GPB; j++) { + if (obj[i].geom[j]) { + if (i==selected) { + dsSetColor (0,0.7,1); + } + else if (! dBodyIsEnabled (obj[i].body)) { + dsSetColor (1,0,0); + } + else { + dsSetColor (1,1,0); + } + + if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) { + dTriIndex* Indices = (dTriIndex*)::Indices; + + // assume all trimeshes are drawn as bunnies + const dReal* Pos = dGeomGetPosition(obj[i].geom[j]); + const dReal* Rot = dGeomGetRotation(obj[i].geom[j]); + + for (int ii = 0; ii < IndexCount / 3; ii++) { + const dReal v[9] = { // explicit conversion from float to dReal + Vertices[Indices[ii * 3 + 0] * 3 + 0], + Vertices[Indices[ii * 3 + 0] * 3 + 1], + Vertices[Indices[ii * 3 + 0] * 3 + 2], + Vertices[Indices[ii * 3 + 1] * 3 + 0], + Vertices[Indices[ii * 3 + 1] * 3 + 1], + Vertices[Indices[ii * 3 + 1] * 3 + 2], + Vertices[Indices[ii * 3 + 2] * 3 + 0], + Vertices[Indices[ii * 3 + 2] * 3 + 1], + Vertices[Indices[ii * 3 + 2] * 3 + 2] + }; + dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1); + } + + // tell the tri-tri collider the current transform of the trimesh -- + // this is fairly important for good results. + + // Fill in the (4x4) matrix. + dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ); + + p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0; + p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0; + p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0; + p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1; + + // Flip to other matrix. + obj[i].last_matrix_index = !obj[i].last_matrix_index; + + dGeomTriMeshSetLastTransform( obj[i].geom[j], + *(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) ); + + } else { + drawGeom (obj[i].geom[j],0,0,show_aabb); + } + } + } + } + + dTriIndex* Indices = (dTriIndex*)::Indices; + + {const dReal* Pos = dGeomGetPosition(TriMesh1); + const dReal* Rot = dGeomGetRotation(TriMesh1); + + {for (int i = 0; i < IndexCount / 3; i++){ + const dReal v[9] = { // explicit conversion from float to dReal + Vertices[Indices[i * 3 + 0] * 3 + 0], + Vertices[Indices[i * 3 + 0] * 3 + 1], + Vertices[Indices[i * 3 + 0] * 3 + 2], + Vertices[Indices[i * 3 + 1] * 3 + 0], + Vertices[Indices[i * 3 + 1] * 3 + 1], + Vertices[Indices[i * 3 + 1] * 3 + 2], + Vertices[Indices[i * 3 + 2] * 3 + 0], + Vertices[Indices[i * 3 + 2] * 3 + 1], + Vertices[Indices[i * 3 + 2] * 3 + 2] + }; + dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0); + }}} + + {const dReal* Pos = dGeomGetPosition(TriMesh2); + const dReal* Rot = dGeomGetRotation(TriMesh2); + + {for (int i = 0; i < IndexCount / 3; i++){ + const dReal v[9] = { // explicit conversion from float to dReal + Vertices[Indices[i * 3 + 0] * 3 + 0], + Vertices[Indices[i * 3 + 0] * 3 + 1], + Vertices[Indices[i * 3 + 0] * 3 + 2], + Vertices[Indices[i * 3 + 1] * 3 + 0], + Vertices[Indices[i * 3 + 1] * 3 + 1], + Vertices[Indices[i * 3 + 1] * 3 + 2], + Vertices[Indices[i * 3 + 2] * 3 + 0], + Vertices[Indices[i * 3 + 2] * 3 + 1], + Vertices[Indices[i * 3 + 2] * 3 + 2] + }; + dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1); + }}} +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + + space = dSimpleSpaceCreate(0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + dWorldSetCFM (world,1e-5); + dCreatePlane (space,0,0,1,0); + memset (obj,0,sizeof(obj)); + + // note: can't share tridata if intending to trimesh-trimesh collide + const unsigned preprocessFlags = (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES) | (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES); + TriData1 = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); + dGeomTriMeshDataPreprocess2(TriData1, preprocessFlags, NULL); + TriData2 = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); + dGeomTriMeshDataPreprocess2(TriData2, preprocessFlags, NULL); + + TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0); + TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0); + dGeomSetData(TriMesh1, TriData1); + dGeomSetData(TriMesh2, TriData2); + + {dGeomSetPosition(TriMesh1, 0, 0, 0.9); + dMatrix3 Rotation; + dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2); + dGeomSetRotation(TriMesh1, Rotation);} + + {dGeomSetPosition(TriMesh2, 1, 0, 0.9); + dMatrix3 Rotation; + dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2); + dGeomSetRotation(TriMesh2, Rotation);} + + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); + dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dWorldSetStepThreadingImplementation(world, NULL, NULL); + dThreadingFreeImplementation(threading); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_ode.cpp b/libs/ode-0.16.1/ode/demo/demo_ode.cpp new file mode 100644 index 0000000..ead9338 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_ode.cpp @@ -0,0 +1,1380 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <setjmp.h> +#include <ode/ode.h> + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +//**************************************************************************** +// matrix sizes + +#define dALIGN_SIZE(buf_size, alignment) (((buf_size) + (alignment - 1)) & (int)(~(alignment - 1))) // Casting the mask to int ensures sign-extension to larger integer sizes +#define dALIGN_PTR(buf_ptr, alignment) ((void *)(((duintptr)(buf_ptr) + ((alignment) - 1)) & (int)(~(alignment - 1)))) // Casting the mask to int ensures sign-extension to larger integer sizes + +#define MSIZE 21 +#define MSIZE4 dALIGN_SIZE(MSIZE, 4) // MSIZE rounded up to 4 + + + +//**************************************************************************** +// matrix accessors + +#define _A(i,j) A[(i)*4+(j)] +#define _I(i,j) I[(i)*4+(j)] +#define _R(i,j) R[(i)*4+(j)] + +//**************************************************************************** +// tolerances + +#ifdef dDOUBLE +const double tol = 1e-10; +#endif + +#ifdef dSINGLE +const double tol = 1e-5; +#endif + +//**************************************************************************** +// misc messages and error handling + +#ifdef __GNUC__ +#define HEADER printf ("%s()\n", __FUNCTION__); +#else +#define HEADER printf ("%s:%d\n",__FILE__,__LINE__); +#endif + +static jmp_buf jump_buffer; + + +void myMessageFunction (int num, const char *msg, va_list ap) +{ + printf ("(Message %d: ",num); + vprintf (msg,ap); + printf (")"); + dSetMessageHandler (0); + longjmp (jump_buffer,1); +} + + +#define TRAP_MESSAGE(do,ifnomsg,ifmsg) \ + dSetMessageHandler (&myMessageFunction); \ + if (setjmp (jump_buffer)) { \ + dSetMessageHandler (0); \ + ifmsg ; \ + } \ + else { \ + dSetMessageHandler (&myMessageFunction); \ + do ; \ + ifnomsg ; \ + } \ + dSetMessageHandler (0); + +//**************************************************************************** +// utility stuff + +// compare two numbers, within a threshhold, return 1 if approx equal + +int cmp (dReal a, dReal b) +{ + return (fabs(a-b) < tol); +} + +//**************************************************************************** +// matrix utility stuff + +// compare a 3x3 matrix with the identity + +int cmpIdentityMat3 (dMatrix3 A) +{ + return + (cmp(_A(0,0),1.0) && cmp(_A(0,1),0.0) && cmp(_A(0,2),0.0) && + cmp(_A(1,0),0.0) && cmp(_A(1,1),1.0) && cmp(_A(1,2),0.0) && + cmp(_A(2,0),0.0) && cmp(_A(2,1),0.0) && cmp(_A(2,2),1.0)); +} + + +// transpose a 3x3 matrix in-line + +void transpose3x3 (dMatrix3 A) +{ + dReal tmp; + tmp=A[4]; A[4]=A[1]; A[1]=tmp; + tmp=A[8]; A[8]=A[2]; A[2]=tmp; + tmp=A[9]; A[9]=A[6]; A[6]=tmp; +} + +//**************************************************************************** +// test miscellaneous math functions + +void testRandomNumberGenerator() +{ + HEADER; + if (dTestRand()) printf ("\tpassed\n"); + else printf ("\tFAILED\n"); +} + + +void testInfinity() +{ + HEADER; + if (1e10 < dInfinity && -1e10 > -dInfinity && -dInfinity < dInfinity) + printf ("\tpassed\n"); + else printf ("\tFAILED\n"); +} + + +void testPad() +{ + HEADER; + char s[100]; + s[0]=0; + for (int i=0; i<=16; i++) sprintf (s+strlen(s),"%d ",dPAD(i)); + printf ("\t%s\n", strcmp(s,"0 1 4 4 4 8 8 8 8 12 12 12 12 16 16 16 16 ") ? + "FAILED" : "passed"); +} + + +void testCrossProduct() +{ + HEADER; + + dVector3 a1,a2,b,c; + dMatrix3 B; + dMakeRandomVector (b,3,1.0); + dMakeRandomVector (c,3,1.0); + + dCalcVectorCross3(a1,b,c); + + dSetZero (B,12); + dSetCrossMatrixPlus(B,b,4); + dMultiply0 (a2,B,c,3,3,1); + + dReal diff = dMaxDifference(a1,a2,3,1); + printf ("\t%s\n", diff > tol ? "FAILED" : "passed"); +} + + +void testSetZero() +{ + HEADER; + dReal a[100]; + dMakeRandomVector (a,100,1.0); + dSetZero (a,100); + for (int i=0; i<100; i++) if (a[i] != 0.0) { + printf ("\tFAILED\n"); + return; + } + printf ("\tpassed\n"); +} + + +void testNormalize3() +{ + HEADER; + int i,j,bad=0; + dVector3 n1,n2; + for (i=0; i<1000; i++) { + dMakeRandomVector (n1,3,1.0); + for (j=0; j<3; j++) n2[j]=n1[j]; + dNormalize3 (n2); + if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1; + if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2; + if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4; + if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8; + if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16; + if (bad) { + printf ("\tFAILED (code=%x)\n",bad); + return; + } + } + printf ("\tpassed\n"); +} + + +/* +void testReorthonormalize() +{ +HEADER; +dMatrix3 R,I; +dMakeRandomMatrix (R,3,3,1.0); +for (int i=0; i<30; i++) dReorthonormalize (R); +dMultiply2 (I,R,R,3,3,3); +printf ("\t%s\n",cmpIdentityMat3 (I) ? "passed" : "FAILED"); +} +*/ + + +void testPlaneSpace() +{ + HEADER; + dVector3 n,p,q; + int bad = 0; + for (int i=0; i<1000; i++) { + dMakeRandomVector (n,3,1.0); + dNormalize3 (n); + dPlaneSpace (n,p,q); + if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1; + if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1; + if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1; + if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1; + if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1; + } + printf ("\t%s\n", bad ? "FAILED" : "passed"); +} + +//**************************************************************************** +// test matrix functions + +void testMatrixMultiply() +{ + // A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4 + dReal A[8],B[12],A2[12],B2[16],C[8]; + int i; + + HEADER; + dSetZero (A,8); + for (i=0; i<3; i++) A[i] = i+2; + for (i=0; i<3; i++) A[i+4] = i+3+2; + for (i=0; i<12; i++) B[i] = i+8; + dSetZero (A2,12); + for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3]; + dSetZero (B2,16); + for (i=0; i<12; i++) B2[i+i/3] = B[i]; + + dMultiply0 (C,A,B,2,3,4); + if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 || + C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278) + printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n"); + + dMultiply1 (C,A2,B,2,3,4); + if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 || + C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241) + printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n"); + + dMultiply2 (C,A,B2,2,3,4); + if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 || + C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326) + printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n"); +} + + +void testSmallMatrixMultiply() +{ + dMatrix3 A,B,C,A2; + dVector3 a,a2,x; + + HEADER; + dMakeRandomMatrix (A,3,3,1.0); + dMakeRandomMatrix (B,3,3,1.0); + dMakeRandomMatrix (C,3,3,1.0); + dMakeRandomMatrix (x,3,1,1.0); + + // dMultiply0_331() + dMultiply0_331 (a,B,x); + dMultiply0 (a2,B,x,3,3,1); + printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" : + "passed"); + + // dMultiply1_331() + dMultiply1_331 (a,B,x); + dMultiply1 (a2,B,x,3,3,1); + printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" : + "passed"); + + // dMultiply0_133 + dMultiply0_133 (a,x,B); + dMultiply0 (a2,x,B,1,3,3); + printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" : + "passed"); + + // dMultiply0_333() + dMultiply0_333 (A,B,C); + dMultiply0 (A2,B,C,3,3,3); + printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : + "passed"); + + // dMultiply1_333() + dMultiply1_333 (A,B,C); + dMultiply1 (A2,B,C,3,3,3); + printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : + "passed"); + + // dMultiply2_333() + dMultiply2_333 (A,B,C); + dMultiply2 (A2,B,C,3,3,3); + printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : + "passed"); +} + + +void testCholeskyFactorization() +{ + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *B = (dReal *)dAlloc(matrixSize), *C = (dReal *)dAlloc(matrixSize), diff; + + HEADER; + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,B,MSIZE4*MSIZE*sizeof(dReal)); + if (dFactorCholesky (B,MSIZE)) printf ("\tpassed (1)\n"); + else printf ("\tFAILED (1)\n"); + dClearUpperTriangle (B,MSIZE); + dMultiply2 (C,B,B,MSIZE,MSIZE,MSIZE); + diff = dMaxDifference(A,C,MSIZE,MSIZE); + printf ("\tmaximum difference = %.6e - %s (2)\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(C, matrixSize); + dFree(B, matrixSize); + dFree(A, matrixSize); +} + + +void testCholeskySolve() +{ + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize); + dReal *b = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff; + + HEADER; + + // get A,L = PD matrix + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + + // get b,x = right hand side + dMakeRandomMatrix (b,MSIZE,1,1.0); + memcpy (x,b,MSIZE*sizeof(dReal)); + + // factor L + if (dFactorCholesky (L,MSIZE)) printf ("\tpassed (1)\n"); + else printf ("\tFAILED (1)\n"); + dClearUpperTriangle (L,MSIZE); + + // solve A*x = b + dSolveCholesky (L,x,MSIZE); + + // compute A*x and compare it with b + dMultiply2 (btest,A,x,MSIZE,MSIZE,1); + diff = dMaxDifference(b,btest,MSIZE,1); + printf ("\tmaximum difference = %.6e - %s (2)\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(btest, vectorSize); + dFree(x, vectorSize); + dFree(b, vectorSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testInvertPDMatrix() +{ + int i,j,ok; + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *Ainv = (dReal *)dAlloc(matrixSize), *I = (dReal *)dAlloc(matrixSize); + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal)); + dSetZero (Ainv,MSIZE4*MSIZE); + + if (dInvertPDMatrix (A,Ainv,MSIZE)) + printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n"); + dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE); + + // compare with identity + ok = 1; + for (i=0; i<MSIZE; i++) { + for (j=0; j<MSIZE; j++) { + if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0; + } + } + for (i=0; i<MSIZE; i++) { + if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0; + } + if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n"); + + dFree(I, matrixSize); + dFree(Ainv, matrixSize); + dFree(A, matrixSize); +} + + +void testIsPositiveDefinite() +{ + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *B = (dReal *)dAlloc(matrixSize); + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE); + printf ("\t%s\n",dIsPositiveDefinite(A,MSIZE) ? "FAILED (1)":"passed (1)"); + printf ("\t%s\n",dIsPositiveDefinite(B,MSIZE) ? "passed (2)":"FAILED (2)"); + + dFree(B, matrixSize); + dFree(A, matrixSize); +} + + +void testFastLDLTFactorization() +{ + int i,j; + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), *DL = (dReal *)dAlloc(matrixSize), + *ATEST = (dReal *)dAlloc(matrixSize), *d = (dReal *)dAlloc(vectorSize), diff; + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + + dFactorLDLT (L,d,MSIZE,MSIZE4); + + dClearUpperTriangle (L,MSIZE); + for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0; + + dSetZero (DL,MSIZE4*MSIZE); + for (i=0; i<MSIZE; i++) { + for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j]; + } + + dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE); + diff = dMaxDifference(A,ATEST,MSIZE,MSIZE); + printf ("\tmaximum difference = %.6e - %s\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(d, vectorSize); + dFree(ATEST, matrixSize); + dFree(DL, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testCoopLDLTFactorization() +{ + int i,j; + + const dsizeint COOP_MSIZE = MSIZE * 51, COOP_MSIZE4 = dALIGN_SIZE(COOP_MSIZE, 4); + + dsizeint matrixSize = sizeof(dReal) * COOP_MSIZE4 * COOP_MSIZE, vectorSize = sizeof(dReal) * COOP_MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), *DL = (dReal *)dAlloc(matrixSize), + *ATEST = (dReal *)dAlloc(matrixSize), *d = (dReal *)dAlloc(vectorSize), diff; + + const unsigned threadCountMaximum = 8; + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dCooperativeID cooperative = dCooperativeCreate(dThreadingImplementationGetFunctions(threading), threading); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(threadCountMaximum, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + + dResourceRequirementsID requirements = dResourceRequirementsCreate(cooperative); + dEstimateCooperativelyFactorLDLTResourceRequirements(requirements, threadCountMaximum, COOP_MSIZE); + dResourceContainerID resources = dResourceContainerAcquire(requirements); + + HEADER; + + for (int pass = 0; pass != 4; ++pass) + { + dTimerStart ("Factoring LDLT"); + + const unsigned allowedThreads = 4; + const unsigned PASS_MSIZE = COOP_MSIZE - pass, PASS_MSIZE4 = dALIGN_SIZE(PASS_MSIZE, 4); + + dTimerNow ("Preparing data"); + dMakeRandomMatrix (L, PASS_MSIZE, PASS_MSIZE, 1.0); + dMultiply2 (A, L, L, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE); + memcpy (L, A, sizeof(dReal) * PASS_MSIZE4 * PASS_MSIZE); + + dTimerNow ("Factoring multi threaded"); + dCooperativelyFactorLDLT (resources, allowedThreads, L, d, PASS_MSIZE, PASS_MSIZE4); + + dTimerNow ("Verifying"); + dClearUpperTriangle (L, PASS_MSIZE); + for (i = 0; i < PASS_MSIZE; i++) L[i * PASS_MSIZE4 + i] = 1.0; + + dSetZero (DL, PASS_MSIZE4 * PASS_MSIZE); + for (i = 0; i < PASS_MSIZE; i++) { + for (j = 0; j < PASS_MSIZE; j++) DL[i * PASS_MSIZE4 + j] = L[i * PASS_MSIZE4 + j] / d[j]; + } + + dMultiply2 (ATEST, L, DL, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE); + diff = dMaxDifference(A, ATEST, PASS_MSIZE, PASS_MSIZE); + printf ("\tN=%u: maximum difference = %.6e - %s\n", PASS_MSIZE, diff, diff > 1e2 * tol ? "FAILED" : "passed"); + + dTimerEnd(); + dTimerReport(stdout, 0); + } + + dResourceContainerDestroy(resources); + dResourceRequirementsDestroy(requirements); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dCooperativeDestroy(cooperative); + dThreadingFreeImplementation(threading); + + dFree(d, vectorSize); + dFree(ATEST, matrixSize); + dFree(DL, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testSolveLDLT() +{ + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), + *b = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff; + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + + dMakeRandomMatrix (b,MSIZE,1,1.0); + memcpy (x,b,MSIZE*sizeof(dReal)); + + dFactorLDLT (L,d,MSIZE,MSIZE4); + dSolveLDLT (L,d,x,MSIZE,MSIZE4); + + dMultiply2 (btest,A,x,MSIZE,MSIZE,1); + diff = dMaxDifference(b,btest,MSIZE,1); + printf ("\tmaximum difference = %.6e - %s\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(btest, vectorSize); + dFree(b, vectorSize); + dFree(x, vectorSize); + dFree(d, vectorSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + +void testCoopSolveLDLT() +{ + const dsizeint COOP_MSIZE = MSIZE * 51, COOP_MSIZE4 = dALIGN_SIZE(COOP_MSIZE, 4); + + dsizeint matrixSize = sizeof(dReal) * COOP_MSIZE4 * COOP_MSIZE, vectorSize = sizeof(dReal) * COOP_MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), + *b = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff; + + const unsigned threadCountMaximum = 8; + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dCooperativeID cooperative = dCooperativeCreate(dThreadingImplementationGetFunctions(threading), threading); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(threadCountMaximum, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + + dResourceRequirementsID requirements = dResourceRequirementsCreate(cooperative); + dEstimateCooperativelySolveLDLTResourceRequirements(requirements, threadCountMaximum, COOP_MSIZE); + dResourceContainerID resources = dResourceContainerAcquire(requirements); + + HEADER; + + for (int pass = 0; pass != 4; ++pass) + { + dTimerStart ("Solving LDLT"); + + const unsigned allowedThreads = 4; + const unsigned PASS_MSIZE = COOP_MSIZE - pass, PASS_MSIZE4 = dALIGN_SIZE(PASS_MSIZE, 4); + + dTimerNow ("Preparing data"); + dMakeRandomMatrix (b, PASS_MSIZE, 1, 1.0); + + dMakeRandomMatrix (L, PASS_MSIZE, PASS_MSIZE, 1.0); + dMultiply2 (A, L, L, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE); + + memcpy (x, b, PASS_MSIZE * sizeof(dReal)); + memcpy (L, A, sizeof(dReal) * PASS_MSIZE4 * PASS_MSIZE); + + dTimerNow ("Factoring"); + dFactorLDLT (L, d, PASS_MSIZE, PASS_MSIZE4); + + dTimerNow ("Solving multi-threaded"); + dCooperativelySolveLDLT(resources, allowedThreads, L, d, x, PASS_MSIZE, PASS_MSIZE4); + + dTimerNow ("Verifying solution"); + dMultiply2 (btest, A, x, PASS_MSIZE, PASS_MSIZE, 1); + diff = dMaxDifference(b, btest, PASS_MSIZE, 1); + printf ("\tN=%u: maximum difference = %.6e - %s\n", PASS_MSIZE, diff, diff > 1e2 * tol ? "FAILED" : "passed"); + + dTimerEnd(); + dTimerReport(stdout, 0); + } + + dResourceContainerDestroy(resources); + dResourceRequirementsDestroy(requirements); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dCooperativeDestroy(cooperative); + dThreadingFreeImplementation(threading); + + dFree(btest, vectorSize); + dFree(b, vectorSize); + dFree(x, vectorSize); + dFree(d, vectorSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testLDLTAddTL() +{ + int i,j; + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *DL = (dReal *)dAlloc(matrixSize), *ATEST = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *a = (dReal *)dAlloc(vectorSize), diff; + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + dFactorLDLT (L,d,MSIZE,MSIZE4); + + // delete first row and column of factorization + for (i=0; i<MSIZE; i++) a[i] = -A[i*MSIZE4]; + a[0] += 1; + dLDLTAddTL (L,d,a,MSIZE,MSIZE4); + for (i=1; i<MSIZE; i++) L[i*MSIZE4] = 0; + d[0] = 1; + + // get modified L*D*L' + dClearUpperTriangle (L,MSIZE); + for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0; + dSetZero (DL,MSIZE4*MSIZE); + for (i=0; i<MSIZE; i++) { + for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j]; + } + dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE); + + // compare it to A with its first row/column removed + for (i=1; i<MSIZE; i++) A[i*MSIZE4] = A[i] = 0; + A[0] = 1; + diff = dMaxDifference(A,ATEST,MSIZE,MSIZE); + printf ("\tmaximum difference = %.6e - %s\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(a, vectorSize); + dFree(d, vectorSize); + dFree(ATEST, matrixSize); + dFree(DL, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testLDLTRemove() +{ + int i,j,r; + dsizeint intVectorSize = sizeof(int) * MSIZE, matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + int *p = (int *)dAlloc(intVectorSize); + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *L2 = (dReal *)dAlloc(matrixSize), *DL2 = (dReal *)dAlloc(matrixSize), + *Atest1 = (dReal *)dAlloc(matrixSize), *Atest2 = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *d2 = (dReal *)dAlloc(vectorSize), diff, maxdiff; + + HEADER; + + // make array of A row pointers + dReal *Arows[MSIZE]; + for (i=0; i<MSIZE; i++) Arows[i] = A+i*MSIZE4; + + // fill permutation vector + for (i=0; i<MSIZE; i++) p[i]=i; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + dFactorLDLT (L,d,MSIZE,MSIZE4); + + maxdiff = 1e10; + for (r=0; r<MSIZE; r++) { + // get Atest1 = A with row/column r removed + memcpy (Atest1,A,MSIZE4*MSIZE*sizeof(dReal)); + dRemoveRowCol (Atest1,MSIZE,MSIZE4,r); + + // test that the row/column removal worked + int bad = 0; + for (i=0; i<MSIZE; i++) { + for (j=0; j<MSIZE; j++) { + if (i != r && j != r) { + int ii = i; + int jj = j; + if (ii >= r) ii--; + if (jj >= r) jj--; + if (A[i*MSIZE4+j] != Atest1[ii*MSIZE4+jj]) bad = 1; + } + } + } + if (bad) printf ("\trow/col removal FAILED for row %d\n",r); + + // zero out last row/column of Atest1 + for (i=0; i<MSIZE; i++) { + Atest1[(MSIZE-1)*MSIZE4+i] = 0; + Atest1[i*MSIZE4+MSIZE-1] = 0; + } + + // get L2*D2*L2' = adjusted factorization to remove that row + memcpy (L2,L,MSIZE4*MSIZE*sizeof(dReal)); + memcpy (d2,d,MSIZE*sizeof(dReal)); + dLDLTRemove (/*A*/ Arows,p,L2,d2,MSIZE,MSIZE,r,MSIZE4); + + // get Atest2 = L2*D2*L2' + dClearUpperTriangle (L2,MSIZE); + for (i=0; i<(MSIZE-1); i++) L2[i*MSIZE4+i] = 1.0; + for (i=0; i<MSIZE; i++) L2[(MSIZE-1)*MSIZE4+i] = 0; + d2[MSIZE-1] = 1; + dSetZero (DL2,MSIZE4*MSIZE); + for (i=0; i<(MSIZE-1); i++) { + for (j=0; j<MSIZE-1; j++) DL2[i*MSIZE4+j] = L2[i*MSIZE4+j] / d2[j]; + } + + dMultiply2 (Atest2,L2,DL2,MSIZE,MSIZE,MSIZE); + + diff = dMaxDifference(Atest1,Atest2,MSIZE,MSIZE); + if (diff < maxdiff) maxdiff = diff; + + /* + dPrintMatrix (Atest1,MSIZE,MSIZE); + printf ("\n"); + dPrintMatrix (Atest2,MSIZE,MSIZE); + printf ("\n"); + */ + } + printf ("\tmaximum difference = %.6e - %s\n",maxdiff, + maxdiff > tol ? "FAILED" : "passed"); + + dFree(d2, vectorSize); + dFree(d, vectorSize); + dFree(Atest2, matrixSize); + dFree(Atest1, matrixSize); + dFree(DL2, matrixSize); + dFree(L2, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + +//**************************************************************************** +// test mass stuff + +#define NUMP 10 // number of particles + + +void printMassParams (dMass *m) +{ + printf ("mass = %.4f\n",m->mass); + printf ("com = (%.4f,%.4f,%.4f)\n",m->c[0],m->c[1],m->c[2]); + printf ("I = [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n", + m->_I(0,0),m->_I(0,1),m->_I(0,2), + m->_I(1,0),m->_I(1,1),m->_I(1,2), + m->_I(2,0),m->_I(2,1),m->_I(2,2)); +} + + +void compareMassParams (dMass *m1, dMass *m2, const char *msg) +{ + int i,j,ok = 1; + if (!(cmp(m1->mass,m2->mass) && cmp(m1->c[0],m2->c[0]) && + cmp(m1->c[1],m2->c[1]) && cmp(m1->c[2],m2->c[2]))) + ok = 0; + for (i=0; i<3; i++) for (j=0; j<3; j++) + if (cmp (m1->_I(i,j),m2->_I(i,j))==0) ok = 0; + if (ok) printf ("\tpassed (%s)\n",msg); else printf ("\tFAILED (%s)\n",msg); +} + + +// compute the mass parameters of a particle set + +void computeMassParams (dMass *m, dReal q[NUMP][3], dReal pm[NUMP]) +{ + int i,j; + dMassSetZero (m); + for (i=0; i<NUMP; i++) { + m->mass += pm[i]; + for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j]; + m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]); + m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]); + m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]); + m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]); + m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]); + m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]); + } + for (j=0; j<3; j++) m->c[j] /= m->mass; + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); +} + + +void testMassFunctions() +{ + dMass m; + int i,j; + dReal q[NUMP][3]; // particle positions + dReal pm[NUMP]; // particle masses + dMass m1,m2; + dMatrix3 R; + + HEADER; + + printf ("\t"); + dMassSetZero (&m); + TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6), + printf (" FAILED (1)\n"), printf (" passed (1)\n")); + + printf ("\t"); + dMassSetZero (&m); + TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4), + printf ("passed (2)\n") , printf (" FAILED (2)\n")); + if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) && + m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 && + m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 && + m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4) + printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n"); + + dMassSetZero (&m); + dMassSetSphere (&m,1.4, 0.86); + if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && + cmp(m._I(0,0),1.10349124669826) && + cmp(m._I(1,1),1.10349124669826) && + cmp(m._I(2,2),1.10349124669826) && + m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && + m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) + printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n"); + + dMassSetZero (&m); + dMassSetCapsule (&m,1.3,1,0.76,1.53); + if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && + cmp(m._I(0,0),1.59461986077384) && + cmp(m._I(1,1),4.21878433864904) && + cmp(m._I(2,2),4.21878433864904) && + m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && + m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) + printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n"); + + dMassSetZero (&m); + dMassSetBox (&m,0.27,3,4,5); + if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && + cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) && + m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && + m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) + printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n"); + + // test dMassAdjust? + + // make random particles and compute the mass, COM and inertia, then + // translate and repeat. + for (i=0; i<NUMP; i++) { + pm[i] = dRandReal()+0.5; + for (j=0; j<3; j++) { + q[i][j] = 2.0*(dRandReal()-0.5); + } + } + computeMassParams (&m1,q,pm); + memcpy (&m2,&m1,sizeof(dMass)); + dMassTranslate (&m2,1,2,-3); + for (i=0; i<NUMP; i++) { + q[i][0] += 1; + q[i][1] += 2; + q[i][2] -= 3; + } + computeMassParams (&m1,q,pm); + compareMassParams (&m1,&m2,"7"); + + // rotate the masses + _R(0,0) = -0.87919618797635; + _R(0,1) = 0.15278881840384; + _R(0,2) = -0.45129772879842; + _R(1,0) = -0.47307856232664; + _R(1,1) = -0.39258064912909; + _R(1,2) = 0.78871864932708; + _R(2,0) = -0.05666336483842; + _R(2,1) = 0.90693771059546; + _R(2,2) = 0.41743652473765; + dMassRotate (&m2,R); + for (i=0; i<NUMP; i++) { + dReal a[3]; + dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1); + q[i][0] = a[0]; + q[i][1] = a[1]; + q[i][2] = a[2]; + } + computeMassParams (&m1,q,pm); + compareMassParams (&m1,&m2,"8"); +} + +//**************************************************************************** +// test rotation stuff + +void makeRandomRotation (dMatrix3 R) +{ + dReal *u1 = R, *u2=R+4, *u3=R+8; + dMakeRandomVector (u1,3,1.0); + dNormalize3 (u1); + dMakeRandomVector (u2,3,1.0); + dReal d = dCalcVectorDot3(u1,u2); + u2[0] -= d*u1[0]; + u2[1] -= d*u1[1]; + u2[2] -= d*u1[2]; + dNormalize3(u2); + dCalcVectorCross3(u3,u1,u2); +} + + +void testRtoQandQtoR() +{ + HEADER; + dMatrix3 R,I,R2; + dQuaternion q; + int i; + + // test makeRandomRotation() + makeRandomRotation (R); + dMultiply2 (I,R,R,3,3,3); + printf ("\tmakeRandomRotation() - %s (1)\n", + cmpIdentityMat3(I) ? "passed" : "FAILED"); + + // test QtoR() on random normalized quaternions + int ok = 1; + for (i=0; i<100; i++) { + dMakeRandomVector (q,4,1.0); + dNormalize4 (q); + dQtoR (q,R); + dMultiply2 (I,R,R,3,3,3); + if (cmpIdentityMat3(I)==0) ok = 0; + } + printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED"); + + // test R -> Q -> R works + dReal maxdiff=0; + for (i=0; i<100; i++) { + makeRandomRotation (R); + dRtoQ (R,q); + dQtoR (q,R2); + dReal diff = dMaxDifference (R,R2,3,3); + if (diff > maxdiff) maxdiff = diff; + } + printf ("\tmaximum difference = %e - %s (3)\n",maxdiff, + (maxdiff > tol) ? "FAILED" : "passed"); +} + + +void testQuaternionMultiply() +{ + HEADER; + dMatrix3 RA,RB,RC,Rtest; + dQuaternion qa,qb,qc; + dReal diff,maxdiff=0; + + for (int i=0; i<100; i++) { + makeRandomRotation (RB); + makeRandomRotation (RC); + dRtoQ (RB,qb); + dRtoQ (RC,qc); + + dMultiply0 (RA,RB,RC,3,3,3); + dQMultiply0 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + + dMultiply1 (RA,RB,RC,3,3,3); + dQMultiply1 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + + dMultiply2 (RA,RB,RC,3,3,3); + dQMultiply2 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + + dMultiply0 (RA,RC,RB,3,3,3); + transpose3x3 (RA); + dQMultiply3 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + } + printf ("\tmaximum difference = %e - %s\n",maxdiff, + (maxdiff > tol) ? "FAILED" : "passed"); +} + + +void testRotationFunctions() +{ + dMatrix3 R1; + HEADER; + + printf ("\tdRSetIdentity - "); + dMakeRandomMatrix (R1,3,3,1.0); + dRSetIdentity (R1); + if (cmpIdentityMat3(R1)) printf ("passed\n"); else printf ("FAILED\n"); + + printf ("\tdRFromAxisAndAngle - "); + + printf ("\n"); + printf ("\tdRFromEulerAngles - "); + + printf ("\n"); + printf ("\tdRFrom2Axes - "); + + printf ("\n"); +} + +//**************************************************************************** + +#include <assert.h> + +template<class T> +class simplevector +{ +private: + int n; + int max; + T* data; + +public: + simplevector() { initialize(); } + ~simplevector() { finalize(); } + T& operator[](int i) { assert(i>=0 && i<n); return data[i]; } + const T& operator[](int i) const { assert(i>=0 && i<n); return data[i]; } + void push_back(const T& elem) + { + if (n == max) + { + max *= 2; + T* newdata = new T[max]; + memcpy(newdata, data, sizeof(T)*n); + delete[] data; + data = newdata; + } + data[n++] = elem; + } + int size() const { return n; } + void clear() { finalize(); initialize(); } + +private: + void finalize() { delete[] data; } + void initialize() { data = new T[32]; max = 32; n = 0; } +}; + +// matrix header on the stack + +class dMatrixComparison { + struct dMatInfo; + simplevector<dMatInfo*> mat; + int afterfirst,index; + +public: + dMatrixComparison(); + ~dMatrixComparison(); + + dReal nextMatrix (dReal *A, int n, int m, int lower_tri, const char *name, ...); + // add a new n*m matrix A to the sequence. the name of the matrix is given + // by the printf-style arguments (name,...). if this is the first sequence + // then this object will simply record the matrices and return 0. + // if this the second or subsequent sequence then this object will compare + // the matrices with the first sequence, and report any differences. + // the matrix error will be returned. if `lower_tri' is 1 then only the + // lower triangle of the matrix (including the diagonal) will be compared + // (the matrix must be square). + + void end(); + // end a sequence. + + void reset(); + // restarts the object, so the next sequence will be the first sequence. + + void dump(); + // print out info about all the matrices in the sequence +}; + +struct dMatrixComparison::dMatInfo { + int n,m; // size of matrix + char name[128]; // name of the matrix + dReal *data; // matrix data + int size; // size of `data' +}; + + + +dMatrixComparison::dMatrixComparison() +{ + afterfirst = 0; + index = 0; +} + + +dMatrixComparison::~dMatrixComparison() +{ + reset(); +} + + +dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri, + const char *name, ...) +{ + if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix"); + int num = n*dPAD(m); + + if (afterfirst==0) { + dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo)); + mi->n = n; + mi->m = m; + mi->size = num * sizeof(dReal); + mi->data = (dReal*) dAlloc (mi->size); + memcpy (mi->data,A,mi->size); + + va_list ap; + va_start (ap,name); + vsprintf (mi->name,name,ap); + va_end (ap); + if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long"); + + mat.push_back(mi); + return 0; + } + else { + if (lower_tri && n != m) + dDebug (0,"dMatrixComparison, lower triangular matrix must be square"); + if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices"); + dMatInfo *mp = mat[index]; + index++; + + dMatInfo mi; + va_list ap; + va_start (ap,name); + vsprintf (mi.name,name,ap); + va_end (ap); + if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long"); + + if (strcmp(mp->name,mi.name) != 0) + dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")", + mp->name,mi.name); + if (mp->n != n || mp->m != m) + dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)", + mp->n,mp->m,n,m); + + dReal maxdiff; + if (lower_tri) { + maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n); + } + else { + maxdiff = dMaxDifference (A,mp->data,n,m); + } + if (maxdiff > tol) + dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", " + "error=%.4e)",n,m,mi.name,maxdiff); + return maxdiff; + } +} + + +void dMatrixComparison::end() +{ + if (mat.size() <= 0) dDebug (0,"no matrices in sequence"); + afterfirst = 1; + index = 0; +} + + +void dMatrixComparison::reset() +{ + for (int i=0; i<mat.size(); i++) { + dFree (mat[i]->data,mat[i]->size); + dFree (mat[i],sizeof(dMatInfo)); + } + mat.clear(); + afterfirst = 0; + index = 0; +} + + +void dMatrixComparison::dump() +{ + for (int i=0; i<mat.size(); i++) + printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m); +} + +//**************************************************************************** +// unit test + +#include <setjmp.h> + +// static jmp_buf jump_buffer; + +static void myDebug (int /*num*/, const char* /*msg*/, va_list /*ap*/) +{ + // printf ("(Error %d: ",num); + // vprintf (msg,ap); + // printf (")\n"); + longjmp (jump_buffer,1); +} + + +extern "C" void dTestMatrixComparison() +{ + volatile int i; + printf ("dTestMatrixComparison()\n"); + dMessageFunction *orig_debug = dGetDebugHandler(); + + dMatrixComparison mc; + dReal A[50*50]; + + // make first sequence + unsigned long seed = dRandGetSeed(); + for (i=1; i<49; i++) { + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + } + mc.end(); + + //mc.dump(); + + // test identical sequence + dSetDebugHandler (&myDebug); + dRandSetSeed (seed); + if (setjmp (jump_buffer)) { + printf ("\tFAILED (1)\n"); + } + else { + for (i=1; i<49; i++) { + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + } + mc.end(); + printf ("\tpassed (1)\n"); + } + dSetDebugHandler (orig_debug); + + // test broken sequences (with matrix error) + dRandSetSeed (seed); + volatile int passcount = 0; + for (i=1; i<49; i++) { + if (setjmp (jump_buffer)) { + passcount++; + } + else { + dSetDebugHandler (&myDebug); + dMakeRandomMatrix (A,i,i+1,1.0); + A[(i-1)*dPAD(i+1)+i] += REAL(0.01); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + dSetDebugHandler (orig_debug); + } + } + mc.end(); + printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED"); + + // test broken sequences (with name error) + dRandSetSeed (seed); + passcount = 0; + for (i=1; i<49; i++) { + if (setjmp (jump_buffer)) { + passcount++; + } + else { + dSetDebugHandler (&myDebug); + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"B%d",i); + dSetDebugHandler (orig_debug); + } + } + mc.end(); + printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED"); + + // test identical sequence again + dSetDebugHandler (&myDebug); + dRandSetSeed (seed); + if (setjmp (jump_buffer)) { + printf ("\tFAILED (4)\n"); + } + else { + for (i=1; i<49; i++) { + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + } + mc.end(); + printf ("\tpassed (4)\n"); + } + dSetDebugHandler (orig_debug); +} + +//**************************************************************************** + +// internal unit tests +extern "C" void dTestDataStructures(); +extern "C" void dTestMatrixComparison(); +extern "C" int dTestSolveLCP(); + + +int main() +{ + dInitODE(); + testRandomNumberGenerator(); + testInfinity(); + testPad(); + testCrossProduct(); + testSetZero(); + testNormalize3(); + //testReorthonormalize(); ... not any more + testPlaneSpace(); + testMatrixMultiply(); + testSmallMatrixMultiply(); + testCholeskyFactorization(); + testCholeskySolve(); + testInvertPDMatrix(); + testIsPositiveDefinite(); + testFastLDLTFactorization(); + testCoopLDLTFactorization(); + testSolveLDLT(); + testCoopSolveLDLT(); + testLDLTAddTL(); + testLDLTRemove(); + testMassFunctions(); + testRtoQandQtoR(); + testQuaternionMultiply(); + testRotationFunctions(); + dTestMatrixComparison(); + dTestSolveLCP(); + // dTestDataStructures(); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_piston.cpp b/libs/ode-0.16.1/ode/demo/demo_piston.cpp new file mode 100644 index 0000000..8a0453a --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_piston.cpp @@ -0,0 +1,813 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + * Created by: Remi Ricard * + * (remi.ricard@simlog.com or papaDoc@videotron.ca) * + * Creation date: 2007/05/04 * + *************************************************************************/ + +/* + This program demonstrates how the Piston joint works. + + A Piston joint enables the sliding of a body with respect to another body + and the 2 bodies are free to rotate about the sliding axis. + + - The yellow body is fixed to the world. + - The yellow body and the blue body are attached by a Piston joint with + the axis along the x direction. + - The purple object is a geometry obstacle. + - The red line is the representation of the prismatic axis + - The orange line is the representation of the rotoide axis + - The light blue ball is the anchor position + + N.B. Many command options are available type -h to print them. +*/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include <iostream> +#include <math.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawSphere dsDrawSphereD +#endif + + +const dReal VEL_INC = 0.01; // Velocity increment + +// physics parameters +const dReal PI = 3.14159265358979323846264338327950288419716939937510; +const dReal BODY1_LENGTH = 1.5; // Size along the X axis + +const dReal RADIUS = 0.2; +const dReal AXIS_RADIUS = 0.01; + + +#define X 0 +#define Y 1 +#define Z 2 + +enum INDEX +{ + BODY1 = 0, + BODY2, + RECT, + BOX, + OBS, + GROUND, + NUM_PARTS, + ALL = NUM_PARTS +}; + +const int catBits[NUM_PARTS+1] = +{ + 0x0001, ///< Ext Cylinder category + 0x0002, ///< Int Cylinder category + 0x0004, ///< Int_Rect Cylinder category + 0x0008, ///< Box category + 0x0010, ///< Obstacle category + 0x0020, ///< Ground category + ~0L ///< All categories +}; + +#define Mass1 10 +#define Mass2 8 + + +//camera view +static float xyz[3] = {2.0f,-3.5f,2.0000f}; +static float hpr[3] = {90.000f,-25.5000f,0.0000f}; + + +//world,space,body & geom +static dWorldID world; +static dSpaceID space; +static dJointGroupID contactgroup; +static dBodyID body[NUM_PARTS]; +static dGeomID geom[NUM_PARTS]; + +// Default Positions and anchor of the 2 bodies +dVector3 pos1; +dVector3 pos2; +dVector3 anchor; + +static dJoint *joint; + + +const dReal BODY2_SIDES[3] = {0.4, 0.4, 0.4}; +const dReal OBS_SIDES[3] = {1,1,1}; +const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2}; + + +int type = dJointTypePiston; + +//#pragma message("tc to be changed to 0") + +int tc = 0; // The test case choice; + + +//collision detection +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + dBodyID b1 = dGeomGetBody (o1); + dBodyID b2 = dGeomGetBody (o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact) ) return; + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) ); + if (n > 0) + { + for (i=0; i<n; i++) + { + contact[i].surface.mode = (dContactSlip1 | dContactSlip2 | + dContactSoftERP | dContactSoftCFM | + dContactApprox1); + contact[i].surface.mu = 0.1; + contact[i].surface.slip1 = 0.02; + contact[i].surface.slip2 = 0.02; + contact[i].surface.soft_erp = 0.1; + contact[i].surface.soft_cfm = 0.0001; + dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); + dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) ); + } + } +} + +static void printKeyBoardShortCut() +{ + printf ("Press 'h' for this help.\n"); + printf ("Press 'q' to add force on BLUE body along positive x direction.\n"); + printf ("Press 'w' to add force on BLUE body along negative x direction.\n"); + + printf ("Press 'a' to add force on BLUE body along positive y direction.\n"); + printf ("Press 's' to add force on BLUE body along negative y direction.\n"); + + printf ("Press 'z' to add force on BLUE body along positive z direction.\n"); + printf ("Press 'x' to add force on BLUE body along negative z direction.\n"); + + printf ("Press 'e' to add torque on BLUE body around positive x direction \n"); + printf ("Press 'r' to add torque on BLUE body around negative x direction \n"); + + printf ("Press 'd' to add torque on BLUE body around positive y direction \n"); + printf ("Press 'f' to add torque on BLUE body around negative y direction \n"); + + printf ("Press 'c' to add torque on BLUE body around positive z direction \n"); + printf ("Press 'v' to add torque on BLUE body around negative z direction \n"); + + printf ("Press 't' to add force on prismatic joint in the positive axis direction\n"); + printf ("Press 'y' to add force on prismatic joint in the negative axis direction\n"); + + printf ("Press 'i' to add limits on the prismatic joint (0 to 0) \n"); + printf ("Press 'o' to add limits on the rotoide joint (0 to 0)\n"); + printf ("Press 'k' to add limits on the rotoide joint (-45 to 45deg) \n"); + printf ("Press 'l' to remove limits on the rotoide joint \n"); + + + printf ("Press '.' to increase joint velocity along the prismatic direction.\n"); + printf ("Press ',' to decrease joint velocity along the prismatic direction.\n"); + + printf ("Press 'p' to print the Position of the joint.\n"); + + printf ("Press '+' Go to the next test case.\n"); + printf ("Press '-' Go to the previous test case.\n"); + + printf ("Press '8' To remove one of the body. The blue body and the world will be\n"); + printf (" attached to the joint (blue body at position 1)\n"); + printf ("Press '9' To remove one of the body. The blue body and the world will be\n"); + printf (" attached to the joint (body body at position 2)\n"); + + +} + + +// start simulation - set viewpoint +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + dsSetViewpoint (xyz,hpr); + printf ("This program demonstrates how the Piston joint works.\n"); + printf ("A Piston joint enables the sliding of a body with respect to another body\n"); + printf ("and the 2 bodies are free to rotate about the sliding axis.\n\n"); + printf ("The yellow body is fixed to the world\n"); + printf ("The yellow body and the blue body are attached by a Piston joint with\n"); + printf ("the axis along the x direction.\n"); + printf ("The purple object is a geometry obstacle.\n"); + + printKeyBoardShortCut(); +} + + +void setPositionBodies (int val) +{ + const dVector3 POS1 = {0,0,1.5,0}; + const dVector3 POS2 = {0,0,1.5,0}; + const dVector3 ANCHOR = {0,0,1.5,0}; + + for (int i=0; i<3; ++i) + { + pos1[i] = POS1[i]; + pos2[i] = POS2[i]; + anchor[i] = ANCHOR[i]; + } + + if (body[BODY1]) + { + dBodySetLinearVel (body[BODY1], 0,0,0); + dBodySetAngularVel (body[BODY1], 0,0,0); + } + + if (body[BODY2]) + { + dBodySetLinearVel (body[BODY2], 0,0,0); + dBodySetAngularVel (body[BODY2], 0,0,0); + } + + switch (val) + { + case 3: + pos1[Z] += -0.5; + anchor[Z] -= 0.25; + break; + case 2: + pos1[Z] -= 0.5; + anchor[Z] -= 0.5; + break; + case 1: + pos1[Z] += -0.5; + break; + default: // This is also case 0 + // Nothing to be done + break; + } + + const dMatrix3 R = + { + 1,0,0,0, + 0,1,0,0, + 0,0,1,0 + }; + + if (body[BODY1]) + { + dBodySetPosition (body[BODY1], pos1[X], pos1[Y], pos1[Z]); + dBodySetRotation (body[BODY1], R); + } + + if (body[BODY2]) + { + dBodySetPosition (body[BODY2], pos2[X], pos2[Y], pos2[Z]); + dBodySetRotation (body[BODY2], R); + } + + + + if (joint) + { + joint->attach (body[BODY1], body[BODY2]); + if (joint->getType() == dJointTypePiston) + dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]); + } + +} + + +// function to update camera position at each step. +void update() +{ +// static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w"); + +// static int cnt = 0; +// char str[24]; +// sprintf(str, "%06d",cnt++); + +// dWorldExportDIF(world, file, str); +} + + +// called when a key pressed +static void command (int cmd) +{ + switch (cmd) + { + case 'h' : + case 'H' : + case '?' : + printKeyBoardShortCut(); + break; + + // Force + case 'q' : + case 'Q' : + dBodyAddForce (body[BODY1],4,0,0); + break; + case 'w' : + case 'W' : + dBodyAddForce (body[BODY1],-4,0,0); + break; + + case 'a' : + case 'A' : + dBodyAddForce (body[BODY1],0,40,0); + break; + case 's' : + case 'S' : + dBodyAddForce (body[BODY1],0,-40,0); + break; + + case 'z' : + case 'Z' : + dBodyAddForce (body[BODY1],0,0,4); + break; + case 'x' : + case 'X' : + dBodyAddForce (body[BODY1],0,0,-4); + break; + + // Torque + case 'e': + case 'E': + dBodyAddTorque (body[BODY1],0.1,0,0); + break; + case 'r': + case 'R': + dBodyAddTorque (body[BODY1],-0.1,0,0); + break; + + case 'd': + case 'D': + dBodyAddTorque (body[BODY1],0, 0.1,0); + break; + case 'f': + case 'F': + dBodyAddTorque (body[BODY1],0,-0.1,0); + break; + + case 'c': + case 'C': + dBodyAddTorque (body[BODY1],0.1,0,0); + break; + case 'v': + case 'V': + dBodyAddTorque (body[BODY1],-0.1,0,0); + break; + + case 't': + case 'T': + if (joint->getType() == dJointTypePiston) + dJointAddPistonForce (joint->id(),1); + else + dJointAddSliderForce (joint->id(),1); + break; + case 'y': + case 'Y': + if (joint->getType() == dJointTypePiston) + dJointAddPistonForce (joint->id(),-1); + else + dJointAddSliderForce (joint->id(),-1); + break; + + + case '8' : + dJointAttach(joint->id(), body[0], 0); + break; + case '9' : + dJointAttach(joint->id(), 0, body[0]); + break; + + case 'i': + case 'I' : + joint->setParam (dParamLoStop, 0); + joint->setParam (dParamHiStop, 0); + break; + + case 'o': + case 'O' : + joint->setParam (dParamLoStop2, 0); + joint->setParam (dParamHiStop2, 0); + break; + + case 'k': + case 'K': + joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0); + joint->setParam (dParamHiStop2, 45.0*3.14159267/180.0); + break; + case 'l': + case 'L': + joint->setParam (dParamLoStop2, -dInfinity); + joint->setParam (dParamHiStop2, dInfinity); + break; + + // Velocity of joint + case ',': + case '<' : + { + dReal vel = joint->getParam (dParamVel) - VEL_INC; + joint->setParam (dParamVel, vel); + std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n'; + } + break; + + case '.': + case '>' : + { + dReal vel = joint->getParam (dParamVel) + VEL_INC; + joint->setParam (dParamVel, vel); + std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n'; + } + break; + + case 'p' : + case 'P' : + { + switch (joint->getType() ) + { + case dJointTypeSlider : + { + dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint); + std::cout<<"Position ="<<sj->getPosition() <<"\n"; + } + break; + case dJointTypePiston : + { + dPistonJoint *rj = reinterpret_cast<dPistonJoint *> (joint); + std::cout<<"Position ="<<rj->getPosition() <<"\n"; + } + break; + default: + {} // keep the compiler happy + } + } + break; + + case '+' : + (++tc) %= 4; + setPositionBodies (tc); + break; + case '-' : + (--tc) %= 4; + setPositionBodies (tc); + break; + + + } +} + +static void drawBox (dGeomID id, int R, int G, int B) +{ + if (!id) + return; + + const dReal *pos = dGeomGetPosition (id); + const dReal *rot = dGeomGetRotation (id); + dsSetColor (R,G,B); + + dVector3 l; + dGeomBoxGetLengths (id, l); + dsDrawBox (pos, rot, l); +} + + +// simulation loop +static void simLoop (int pause) +{ + const dReal *rot; + dVector3 ax; + dReal l=0; + + switch (joint->getType() ) + { + case dJointTypeSlider : + ( (dSliderJoint *) joint)->getAxis (ax); + l = ( (dSliderJoint *) joint)->getPosition(); + break; + case dJointTypePiston : + ( (dPistonJoint *) joint)->getAxis (ax); + l = ( (dPistonJoint *) joint)->getPosition(); + break; + default: + {} // keep the compiler happy + } + + + if (!pause) + { + double simstep = 0.01; // 1ms simulation steps + double dt = dsElapsedTime(); + + int nrofsteps = (int) ceilf (dt/simstep); + if (!nrofsteps) + nrofsteps = 1; + + for (int i=0; i<nrofsteps && !pause; i++) + { + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world, simstep); + + dJointGroupEmpty (contactgroup); + } + + update(); + + + dReal radius, length; + + dsSetTexture (DS_WOOD); + + drawBox (geom[BODY2], 1,1,0); + + drawBox (geom[RECT], 0,0,1); + + if ( geom[BODY1] ) + { + const dReal *pos = dGeomGetPosition (geom[BODY1]); + rot = dGeomGetRotation (geom[BODY1]); + dsSetColor (0,0,1); + + dGeomCapsuleGetParams (geom[BODY1], &radius, &length); + dsDrawCapsule (pos, rot, length, radius); + } + + + drawBox (geom[OBS], 1,0,1); + + + // Draw the prismatic axis + if ( geom[BODY1] ) + { + const dReal *pos = dGeomGetPosition (geom[BODY1]); + rot = dGeomGetRotation (geom[BODY2]); + dVector3 p; + p[X] = pos[X] - l*ax[X]; + p[Y] = pos[Y] - l*ax[Y]; + p[Z] = pos[Z] - l*ax[Z]; + dsSetColor (1,0,0); + dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS); + } + + + if (joint->getType() == dJointTypePiston ) + { + dVector3 anchor; + dJointGetPistonAnchor(joint->id(), anchor); + + // Draw the rotoide axis + rot = dGeomGetRotation (geom[BODY2]); + dsSetColor (1,0.5,0); + dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS); + + + dsSetColor (0,1,1); + rot = dGeomGetRotation (geom[BODY1]); + dsDrawSphere (anchor, rot, 1.5*RADIUS); + } + + } +} + + +void Help (char **argv) +{ + printf ("%s ", argv[0]); + printf (" -h | --help : print this help\n"); + printf (" -s | --slider : Set the joint as a slider\n"); + printf (" -p | --piston : Set the joint as a Piston. (Default joint)\n"); + printf (" -1 | --offset1 : Create an offset between the 2 bodies\n"); + printf (" Offset one of the body by z=-0.5 and keep the anchor\n"); + printf (" point in the middle of the fixed body\n"); + printf (" -2 | --offset2 : Create an offset between the 2 bodies\n"); + printf (" Offset one of the body by z=-0.5 and set the anchor\n"); + printf (" point in the middle of the movable body\n"); + printf (" -3 | --offset3 : Create an offset between the 2 bodies\n"); + printf (" Offset one of the body by z=-0.5 and set the anchor\n"); + printf (" point in the middle of the 2 bodies\n"); + printf (" -t | --texture-path path : Path to the texture.\n"); + printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH); + printf (" -n | --notFixed : In free space with no gravity mode"); + printf ("-notex : Don't use texture\n"); + printf ("-noshadow : No shadow\n"); + printf ("-noshadows : No shadows\n"); + printf ("-pause : Initial pause\n"); + printf ("--------------------------------------------------\n"); + printf ("Hit any key to continue:"); + getchar(); + + exit (0); +} + +int main (int argc, char **argv) +{ + dInitODE2(0); + bool fixed = true; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dVector3 offset; + dSetZero (offset, 4); + + // Default test case + + if (argc >= 2 ) + { + for (int i=1; i < argc; ++i) + { + //static int tata = 0; + + if (1) + { + if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) ) + Help (argv); + + if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) ) + type = dJointTypeSlider; + + if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) + { + int j = i+1; + if ( j >= argc || // Check if we have enough arguments + argv[j][0] == '\0' || // We should have a path here + argv[j][0] == '-' ) // We should have a path not a command line + Help (argv); + else + fn.path_to_textures = argv[++i]; // Increase i since we use this argument + } + } + + + if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) ) + tc = 1; + + if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) ) + tc = 2; + + if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) ) + tc = 3; + + if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) ) + fixed = false; + } + } + + world = dWorldCreate(); + dWorldSetERP (world, 0.8); + + space = dSimpleSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + geom[GROUND] = dCreatePlane (space, 0,0,1,0); + dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]); + dGeomSetCollideBits (geom[GROUND], catBits[ALL]); + + dMass m; + dMatrix3 R; + + + // Create the Obstacle + geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]); + dGeomSetCategoryBits (geom[OBS], catBits[OBS]); + dGeomSetCollideBits (geom[OBS], catBits[ALL]); + //Rotation of 45deg around y + dRFromAxisAndAngle (R, 1,1,0, -0.25*PI); + dGeomSetRotation (geom[OBS], R); + dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5); + + + //Rotation of 90deg around y + // Will orient the Z axis along X + dRFromAxisAndAngle (R, 0,1,0, -0.5*PI); + + + // Create Body2 (Wiil be attached to the world) + body[BODY2] = dBodyCreate (world); + // Main axis of cylinder is along X=1 + dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); + dMassAdjust (&m, Mass1); + geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); + dGeomSetBody (geom[BODY2], body[BODY2]); + dGeomSetOffsetRotation (geom[BODY2], R); + dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]); + dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) ); + dBodySetMass (body[BODY2], &m); + + + // Create Body 1 (Slider on the prismatic axis) + body[BODY1] = dBodyCreate (world); + // Main axis of capsule is along X=1 + dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH); + dMassAdjust (&m, Mass1); + geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH); + dGeomSetBody (geom[BODY1], body[BODY1]); + dGeomSetOffsetRotation (geom[BODY1], R); + dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]); + dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]); + + dMass mRect; + dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); + dMassAdd (&m, &mRect); + // TODO: translate m? + geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); + dGeomSetBody (geom[RECT], body[BODY1]); + dGeomSetOffsetPosition (geom[RECT], + (BODY1_LENGTH-RECT_SIDES[0]) /2.0, + 0.0, + -RADIUS -RECT_SIDES[2]/2.0); + dGeomSetCategoryBits (geom[RECT], catBits[RECT]); + dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) ); + + dBodySetMass (body[BODY1], &m); + + + + setPositionBodies (tc); + + + if ( fixed ) + { + // Attache external cylinder to the world + dJointID fixed = dJointCreateFixed (world,0); + dJointAttach (fixed , NULL, body[BODY2]); + dJointSetFixed (fixed ); + dWorldSetGravity (world,0,0,-0.8); + } + else + { + dWorldSetGravity (world,0,0,0); + } + + + + + // The static is here only to help debugging + switch (type) + { + case dJointTypeSlider : + { + dSliderJoint *sj = new dSliderJoint (world, 0); + sj->attach (body[BODY1], body[BODY2]); + sj->setAxis (1, 0, 0); + joint = sj; + } + break; + + case dJointTypePiston : // fall through default + default: + { + dPistonJoint *pj = new dPistonJoint (world, 0); + pj->attach (body[BODY1], body[BODY2]); + pj->setAxis (1, 0, 0); + + dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]); + + joint = pj; + } + break; + }; + + + // run simulation + dsSimulationLoop (argc,argv,400,300,&fn); + + delete joint; + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + + + + diff --git a/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp b/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp new file mode 100644 index 0000000..559f9ae --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp @@ -0,0 +1,304 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Test my Plane2D constraint. +// Uses ode-0.35 collision API. + +# include <stdio.h> +# include <stdlib.h> +# include <math.h> +# include <ode/ode.h> +# include <drawstuff/drawstuff.h> +#include "texturepath.h" + + +# define drand48() ((double) (((double) rand()) / ((double) RAND_MAX))) + +# define N_BODIES 40 +# define STAGE_SIZE 8.0 // in m + +# define TIME_STEP 0.01 +# define K_SPRING 10.0 +# define K_DAMP 10.0 + +//using namespace ode; + +struct GlobalVars +{ + dWorld dyn_world; + dBody dyn_bodies[N_BODIES]; + dReal bodies_sides[N_BODIES][3]; + + dSpaceID coll_space_id; + dJointID plane2d_joint_ids[N_BODIES]; + dJointGroup coll_contacts; +}; + +static GlobalVars *g_globals_ptr = NULL; + + + +static void cb_start () +/*************************/ +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = { 0.5f*STAGE_SIZE, 0.5f*STAGE_SIZE, 0.65f*STAGE_SIZE}; + static float hpr[3] = { 90.0f, -90.0f, 0 }; + + dsSetViewpoint (xyz, hpr); +} + + + +static void cb_near_collision (void *, dGeomID o1, dGeomID o2) +/********************************************************************/ +{ + dBodyID b1 = dGeomGetBody (o1); + dBodyID b2 = dGeomGetBody (o2); + dContact contact; + + + // exit without doing anything if the two bodies are static + if (b1 == 0 && b2 == 0) + return; + + // exit without doing anything if the two bodies are connected by a joint + if (b1 && b2 && dAreConnected (b1, b2)) + { + /* MTRAP; */ + return; + } + + contact.surface.mode = 0; + contact.surface.mu = 0; // frictionless + + if (dCollide (o1, o2, 1, &contact.geom, sizeof (dContactGeom))) + { + dJointID c = dJointCreateContact (g_globals_ptr->dyn_world.id(), + g_globals_ptr->coll_contacts.id (), &contact); + dJointAttach (c, b1, b2); + } +} + + +static void track_to_pos (dBody &body, dJointID joint_id, + dReal target_x, dReal target_y) +/************************************************************************/ +{ + dReal curr_x = body.getPosition()[0]; + dReal curr_y = body.getPosition()[1]; + + dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x)); + dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y)); +} + + + +static void cb_sim_step (int pause) +/*************************************/ +{ + if (! pause) + { + static dReal angle = 0; + + angle += REAL( 0.01 ); + + track_to_pos (g_globals_ptr->dyn_bodies[0], g_globals_ptr->plane2d_joint_ids[0], + dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle) ), + dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle) )); + + /* double f0 = 0.001; */ + /* for (int b = 0; b < N_BODIES; b ++) */ + /* { */ + /* double p = 1 + b / (double) N_BODIES; */ + /* double q = 2 - b / (double) N_BODIES; */ + /* g_globals_ptr->dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */ + /* } */ + /* g_globals_ptr->dyn_bodies[0].addTorque (0, 0, 0.1); */ + + const int n = 10; + for (int i = 0; i < n; i ++) + { + dSpaceCollide (g_globals_ptr->coll_space_id, 0, &cb_near_collision); + g_globals_ptr->dyn_world.step (dReal(TIME_STEP/n)); + g_globals_ptr->coll_contacts.empty (); + } + } + +# if 1 /* [ */ + { + // @@@ hack Plane2D constraint error reduction here: + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id()); + const dReal *quat_ptr; + dReal quat[4], + quat_len; + + + quat_ptr = dBodyGetQuaternion (g_globals_ptr->dyn_bodies[b].id()); + quat[0] = quat_ptr[0]; + quat[1] = 0; + quat[2] = 0; + quat[3] = quat_ptr[3]; + quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]); + quat[0] /= quat_len; + quat[3] /= quat_len; + dBodySetQuaternion (g_globals_ptr->dyn_bodies[b].id(), quat); + dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(), 0, 0, rot[2]); + } + } +# endif /* ] */ + + +# if 0 /* [ */ + { + // @@@ friction + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *vel = dBodyGetLinearVel (g_globals_ptr->dyn_bodies[b].id()), + *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id()); + dReal s = 1.00; + dReal t = 0.99; + + dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]); + dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]); + } + } +# endif /* ] */ + + + { + // ode drawstuff + + dsSetTexture (DS_WOOD); + for (int b = 0; b < N_BODIES; b ++) + { + if (b == 0) + dsSetColor (1.0, 0.5, 1.0); + else + dsSetColor (0, 0.5, 1.0); +#ifdef dDOUBLE + dsDrawBoxD (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]); +#else + dsDrawBox (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]); +#endif + } + } +} + + + +extern int main +/******************/ +( + int argc, + char **argv +) +{ + int b; + dsFunctions drawstuff_functions; + + + dInitODE2(0); + + g_globals_ptr = new GlobalVars(); + + // dynamic world + + dReal cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP; + dReal err_reduct;// = TIME_STEP * K_SPRING * cf_mixing; + err_reduct = REAL( 0.5 ); + cf_mixing = REAL( 0.001 ); + dWorldSetERP (g_globals_ptr->dyn_world.id (), err_reduct); + dWorldSetCFM (g_globals_ptr->dyn_world.id (), cf_mixing); + g_globals_ptr->dyn_world.setGravity (0, 0.0, -1.0); + + g_globals_ptr->coll_space_id = dSimpleSpaceCreate (0); + + // dynamic bodies + for (b = 0; b < N_BODIES; b ++) + { + int l = (int) (1 + sqrt ((double) N_BODIES)); + dReal x = dReal( (0.5 + (b / l)) / l * STAGE_SIZE ); + dReal y = dReal( (0.5 + (b % l)) / l * STAGE_SIZE ); + dReal z = REAL( 1.0 ) + REAL( 0.1 ) * (dReal)drand48(); + + g_globals_ptr->bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); + g_globals_ptr->bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); + g_globals_ptr->bodies_sides[b][2] = z; + + g_globals_ptr->dyn_bodies[b].create (g_globals_ptr->dyn_world); + g_globals_ptr->dyn_bodies[b].setPosition (x, y, z/2); + g_globals_ptr->dyn_bodies[b].setData ((void*) (dsizeint)b); + dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id (), + dReal( 3 * (drand48 () - 0.5) ), + dReal( 3 * (drand48 () - 0.5) ), 0); + + dMass m; + m.setBox (1, g_globals_ptr->bodies_sides[b][0],g_globals_ptr->bodies_sides[b][1],g_globals_ptr->bodies_sides[b][2]); + m.adjust (REAL(0.1) * g_globals_ptr->bodies_sides[b][0] * g_globals_ptr->bodies_sides[b][1]); + g_globals_ptr->dyn_bodies[b].setMass (&m); + + g_globals_ptr->plane2d_joint_ids[b] = dJointCreatePlane2D (g_globals_ptr->dyn_world.id (), 0); + dJointAttach (g_globals_ptr->plane2d_joint_ids[b], g_globals_ptr->dyn_bodies[b].id (), 0); + } + + dJointSetPlane2DXParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10); + dJointSetPlane2DYParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10); + + + // collision geoms and joints + dCreatePlane (g_globals_ptr->coll_space_id, 1, 0, 0, 0); + dCreatePlane (g_globals_ptr->coll_space_id, -1, 0, 0, -STAGE_SIZE); + dCreatePlane (g_globals_ptr->coll_space_id, 0, 1, 0, 0); + dCreatePlane (g_globals_ptr->coll_space_id, 0, -1, 0, -STAGE_SIZE); + + for (b = 0; b < N_BODIES; b ++) + { + dGeomID coll_box_id; + coll_box_id = dCreateBox (g_globals_ptr->coll_space_id, + g_globals_ptr->bodies_sides[b][0], g_globals_ptr->bodies_sides[b][1], g_globals_ptr->bodies_sides[b][2]); + dGeomSetBody (coll_box_id, g_globals_ptr->dyn_bodies[b].id ()); + } + + g_globals_ptr->coll_contacts.create (); + + { + // simulation loop (by drawstuff lib) + drawstuff_functions.version = DS_VERSION; + drawstuff_functions.start = &cb_start; + drawstuff_functions.step = &cb_sim_step; + drawstuff_functions.command = 0; + drawstuff_functions.stop = 0; + drawstuff_functions.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions); + } + + delete g_globals_ptr; + g_globals_ptr = NULL; + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp b/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp new file mode 100644 index 0000000..61d1115 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp @@ -0,0 +1,258 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* +Angular friction demo: + +A bunch of ramps of different pitch. +A bunch of spheres with rolling friction. +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants +#define GRAVITY 10 // the global gravity to use +#define RAMP_COUNT 8 + +static const dReal rampX = 6.0f; +static const dReal rampY = 0.5f; +static const dReal rampZ = 0.25f; +static const dReal sphereRadius = 0.25f; +static const dReal maxRamp = M_PI/4.0f; // Needs to be less than pi/2 +static const dReal rampInc = maxRamp/RAMP_COUNT; + +// dynamics and collision objects +static dWorldID world = 0; +static dSpaceID space = 0; +static dJointGroupID contactgroup = 0; +static dGeomID ground; + +static dReal mu = REAL(0.37); // the global mu to use +static dReal rho = REAL(0.1); // the global rho to use +static dReal omega = REAL(25.0); + +static dGeomID rampGeom[RAMP_COUNT]; +static dBodyID sphereBody[RAMP_COUNT]; +static dGeomID sphereGeom[RAMP_COUNT]; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + if (b1==0 && b2==0) return; + + dContact contact[3]; + for (int ii=0; ii<3; ii++) { + contact[ii].surface.mode = dContactApprox1 | dContactRolling; + contact[ii].surface.mu = mu; + contact[ii].surface.rho = rho; + } + if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) { + for (i=0; i<numc; i++) { + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {0,-3.0f,3.0f}; + static float hpr[3] = {90.0000,-15.0000,0.0000}; + dsSetViewpoint (xyz,hpr); + printf ("Press:\n" + "\t'[' or ']' to change initial angular velocity\n" + "\t'm' to increase sliding friction\n" + "\t'n' to decrease sliding friction\n" + "\t'j' to increase rolling friction\n" + "\t'h' to decrease rolling friction\n" + "\t'r' to reset simulation.\n"); +} + +/** + Delete the bodies, etc. +*/ +static void clear() +{ + if (contactgroup) dJointGroupDestroy (contactgroup); + if (space) dSpaceDestroy (space); + if (world) dWorldDestroy (world); +} + + + +/** + Cleanup if necessary and rebuild the + world. +*/ +static void reset() +{ + clear(); + + // create world + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-GRAVITY); + ground = dCreatePlane (space,0,0,1,0); + + // Calculate mass for sphere a capsule with water density. + dMass sphereMass; + dMassSetSphere(&sphereMass,1000,sphereRadius); + + for (int ii=0;ii<RAMP_COUNT;++ii) { + dQuaternion q; + + dReal angle = (ii+1)*rampInc; + dReal cosA = dCos(angle); + dReal sinA = dSin(angle); + dReal rampW = rampX/cosA; // Box width that preserves ground distance + dReal zPos = REAL(0.5)*(sinA*rampW-cosA*rampZ); // Position that makes end meet ground + dReal yPos = ii*1.25*rampY; + dReal xPos = 0; + + + // Create the ramp + rampGeom[ii] = dCreateBox(space,rampW,rampY,rampZ); + dQFromAxisAndAngle(q,0,1,0,angle); + dGeomSetQuaternion(rampGeom[ii],q); + dGeomSetPosition(rampGeom[ii],xPos,yPos,zPos); + + // Create the spheres + xPos = -REAL(0.5)*rampX + sphereRadius; + zPos = sinA*rampW + sphereRadius; + sphereBody[ii] = dBodyCreate(world); + dBodySetMass(sphereBody[ii],&sphereMass); + sphereGeom[ii] = dCreateSphere(space,sphereRadius); + dGeomSetBody(sphereGeom[ii],sphereBody[ii]); + dBodySetPosition(sphereBody[ii],xPos,yPos,zPos); + dBodySetAngularVel(sphereBody[ii],0,omega,0); + } +} + + +static void command (int cmd) +{ + switch (cmd) { + case 'h': case 'H': + rho-=0.02; + if (rho<0) rho=0; + break; + case 'j': case 'J': + rho+=0.02; + if (rho>1) rho=1; + break; + case 'n': case 'N': + mu-=0.02; + if (mu<0) mu=0; + break; + case 'm': case 'M': + mu+=0.02; + if (mu>1) mu=1; + break; + case 'r': case 'R': + reset(); + break; + case ']': + omega+=1; + break; + case '[': + omega-=1; + break; + } +} + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.017); // 60 fps + // remove all contact joints + dJointGroupEmpty (contactgroup); + } + + // Render ramps and spheres + dsSetTexture (DS_WOOD); + for (int ii=0;ii<RAMP_COUNT;++ii) { + dVector3 sides; + + dsSetColor (1,0.5,0); + dGeomBoxGetLengths(rampGeom[ii],sides); + dsDrawBox (dGeomGetPosition(rampGeom[ii]),dGeomGetRotation(rampGeom[ii]),sides); + + dsSetColor(0,0,1); + dsDrawSphere (dGeomGetPosition(sphereGeom[ii]),dGeomGetRotation(sphereGeom[ii]), sphereRadius); + } +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + reset(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + clear(); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_slider.cpp b/libs/ode-0.16.1/ode/demo/demo_slider.cpp new file mode 100644 index 0000000..33be9ed --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_slider.cpp @@ -0,0 +1,171 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define SIDE (0.5f) // side length of a box +#define MASS (1.0) // mass of a box + + +// dynamics and collision objects +static dWorldID world; +static dBodyID body[2]; +static dJointID slider; + + +// state set by keyboard commands +static int occasional_error = 0; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("Press 'e' to start/stop occasional error.\n"); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == 'e' || cmd == 'E') { + occasional_error ^= 1; + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + const dReal kd = -0.3; // angular damping constant + const dReal ks = 0.5; // spring constant + if (!pause) { + // add an oscillating torque to body 0, and also damp its rotational motion + static dReal a=0; + const dReal *w = dBodyGetAngularVel (body[0]); + dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a)); + a += 0.01; + + // add a spring force to keep the bodies together, otherwise they will + // fly apart along the slider axis. + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]), + ks*(p2[2]-p1[2])); + dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]), + ks*(p1[2]-p2[2])); + + // occasionally re-orient one of the bodies to create a deliberate error. + if (occasional_error) { + static int count = 0; + if ((count % 20)==0) { + // randomly adjust orientation of body[0] + const dReal *R1; + dMatrix3 R2,R3; + R1 = dBodyGetRotation (body[0]); + dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, + dRandReal()-0.5,dRandReal()-0.5); + dMultiply0 (R3,R1,R2,3,3,3); + dBodySetRotation (body[0],R3); + + // randomly adjust position of body[0] + const dReal *pos = dBodyGetPosition (body[0]); + dBodySetPosition (body[0], + pos[0]+0.2*(dRandReal()-0.5), + pos[1]+0.2*(dRandReal()-0.5), + pos[2]+0.2*(dRandReal()-0.5)); + } + count++; + } + + dWorldStep (world,0.05); + } + + dReal sides1[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f}; + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0],0,0,1); + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dQuaternion q; + dQFromAxisAndAngle (q,-1,1,0,0.25*M_PI); + dBodySetPosition (body[1],0.2,0.2,1.2); + dBodySetQuaternion (body[1],q); + + slider = dJointCreateSlider (world,0); + dJointAttach (slider,body[0],body[1]); + dJointSetSliderAxis (slider,1,1,1); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_space.cpp b/libs/ode-0.16.1/ode/demo/demo_space.cpp new file mode 100644 index 0000000..6f871f6 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_space.cpp @@ -0,0 +1,232 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +testing procedure: + * create a bunch of random boxes + * test for intersections directly, put results in n^2 array + * get space to report collisions: + - all correct collisions reported + - no pair reported more than once + - no incorrect collisions reported + +*/ + + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 20 // number of boxes to test + + +// collision objects and globals + +static dSpaceID space; +static dGeomID geom[NUM]; +static dReal bounds[NUM][6]; +static dsizeint good_matrix[NUM][NUM]; // correct collision matrix +static dsizeint test_matrix[NUM][NUM]; // testing collision matrix +static dsizeint hits[NUM]; // number of collisions a box has +static unsigned long seed=37; + + +static void init_test() +{ + int i,j; + const dReal scale = 0.5; + + // set random boxes + dRandSetSeed (seed); + for (i=0; i < NUM; i++) { + bounds[i][0] = dRandReal()*2-1; + bounds[i][1] = bounds[i][0] + dRandReal()*scale; + bounds[i][2] = dRandReal()*2-1; + bounds[i][3] = bounds[i][2] + dRandReal()*scale; + bounds[i][4] = dRandReal()*2; + bounds[i][5] = bounds[i][4] + dRandReal()*scale; + + if (geom[i]) dGeomDestroy (geom[i]); + geom[i] = dCreateBox (space, + bounds[i][1] - bounds[i][0], + bounds[i][3] - bounds[i][2], + bounds[i][5] - bounds[i][4]); + dGeomSetPosition (geom[i], + (bounds[i][0] + bounds[i][1])*0.5, + (bounds[i][2] + bounds[i][3])*0.5, + (bounds[i][4] + bounds[i][5])*0.5); + dGeomSetData (geom[i],(void*)(dsizeint)(i)); + } + + // compute all intersections and put the results in "good_matrix" + for (i=0; i < NUM; i++) { + for (j=0; j < NUM; j++) good_matrix[i][j] = 0; + } + for (i=0; i < NUM; i++) hits[i] = 0; + + for (i=0; i < NUM; i++) { + for (j=i+1; j < NUM; j++) { + dReal *bounds1 = &bounds[i][0]; + dReal *bounds2 = &bounds[j][0]; + if (bounds1[0] > bounds2[1] || + bounds1[1] < bounds2[0] || + bounds1[2] > bounds2[3] || + bounds1[3] < bounds2[2] || + bounds1[4] > bounds2[5] || + bounds1[5] < bounds2[4]) continue; + good_matrix[i][j] = 1; + good_matrix[j][i] = 1; + hits[i]++; + hits[j]++; + } + } +} + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + dsizeint i,j; + i = (dsizeint) dGeomGetData (o1); + j = (dsizeint) dGeomGetData (o2); + if (i==j) + printf ("collision (%d,%d) is between the same object\n",(int)i,(int)j); + if (!good_matrix[i][j] || !good_matrix[j][i]) + printf ("collision (%d,%d) is incorrect\n",(int)i,(int)j); + if (test_matrix[i][j] || test_matrix[j][i]) + printf ("collision (%d,%d) reported more than once\n",(int)i,(int)j); + test_matrix[i][j] = 1; + test_matrix[j][i] = 1; +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +static void command (int cmd) +{ + if (cmd == ' ') { + seed++; + init_test(); + } +} + + +// simulation loop + +static void simLoop (int) +{ + int i,j; + + for (i=0; i < NUM; i++) { + for (j=0; j < NUM; j++) test_matrix[i][j] = 0; + } + dSpaceCollide (space,0,&nearCallback); + for (i=0; i < NUM; i++) { + for (j=i+1; j < NUM; j++) { + if (good_matrix[i][j] && !test_matrix[i][j]) { + printf ("failed to report collision (%d,%d) (seed=%ld)\n",i,j,seed); + } + } + } + + seed++; + init_test(); + + for (i=0; i<NUM; i++) { + dVector3 pos,side; + dMatrix3 R; + dRSetIdentity (R); + for (j=0; j<3; j++) pos[j] = (bounds[i][j*2+1] + bounds[i][j*2]) * 0.5; + for (j=0; j<3; j++) side[j] = bounds[i][j*2+1] - bounds[i][j*2]; + if (hits[i] > 0) dsSetColor (1,0,0); + else dsSetColor (1,1,0); + dsDrawBox (pos,R,side); + } +} + + +int main (int argc, char **argv) +{ + int i; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + + // test the simple space: + // space = dSimpleSpaceCreate(); + + // test the hash space: + // space = dHashSpaceCreate (0); + // dHashSpaceSetLevels (space,-10,10); + + // test the quadtree space + dVector3 Center = {0, 0, 0, 0}; + dVector3 Extents = {10, 0, 10, 0}; + space = dQuadTreeSpaceCreate(0, Center, Extents, 7); + + for (i=0; i < NUM; i++) geom[i] = 0; + init_test(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dSpaceDestroy (space); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_space_stress.cpp b/libs/ode-0.16.1/ode/demo/demo_space_stress.cpp new file mode 100644 index 0000000..dcbd9d7 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_space_stress.cpp @@ -0,0 +1,449 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <string> + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10000 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 4 // maximum number of contact points per body +#define WORLD_SIZE 20 +#define WORLD_HEIGHT 20 + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space = NULL; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int draw_geom = 1; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i<MAX_CONTACTS; i++) { + contact[i].surface.mode = dContactBounce | dContactSoftCFM; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.1; + contact[i].surface.bounce_vel = 0.1; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact))) { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.02,0.02,0.02}; + for (i=0; i<numc; i++) { + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,3.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("To drop another object, press:\n"); + printf (" o to disable rendering.\n"); + printf (" b for box.\n"); + printf (" s for sphere.\n"); + printf (" c for cylinder.\n"); + printf (" x for a composite object.\n"); + printf (" y for cylinder.\n"); + printf ("To select an object, press space.\n"); + printf ("To disable the selected object, press d.\n"); + printf ("To enable the selected object, press e.\n"); + printf ("To toggle showing the geom AABBs, press a.\n"); + printf ("To toggle showing the contact points, press t.\n"); + printf ("To toggle dropping from random position/orientation, press r.\n"); +} + + +char locase(char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + int i,j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase(cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y') { + if (num < NUM) { + // new object to be created + i = num; + num++; + } else { + // recycle existing object + i = nextobj++; + nextobj %= num; // wrap-around if needed + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + obj[i].body = 0; + + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) { + dGeomDestroy(obj[i].geom[k]); + obj[i].geom[k] = 0; + } + } + + obj[i].body = dBodyCreate(world); + + for (k=0; k<3; k++) + sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition(obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); + dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } else { + // higher than highest body position + dReal maxheight = 0; + for (k=0; k<num; k++) { + const dReal *pos = dBodyGetPosition(obj[k].body); + if (pos[2] > maxheight) + maxheight = pos[2]; + } + dBodySetPosition(obj[i].body, 0,0,maxheight+1); + dRSetIdentity(R); + //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); + } + + dBodySetRotation(obj[i].body,R); + + if (cmd == 'b') { + + dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]); + + } else if (cmd == 'c') { + + sides[0] *= 0.5; + dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + + } else if (cmd == 'y') { + + dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]); + + } else if (cmd == 's') { + + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + + } else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j<GPB; j++) + for (k=0; k<3; k++) + dpos[j][k] = dRandReal()*0.3-0.15; + + for (k=0; k<GPB; k++) { + if (k==0) { + dReal radius = dRandReal()*0.25+0.05; + obj[i].geom[k] = dCreateSphere (space,radius); + dMassSetSphere (&m2,DENSITY,radius); + } else if (k==1) { + obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); + dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); + } else { + dReal radius = dRandReal()*0.1+0.05; + dReal length = dRandReal()*1.0+0.1; + obj[i].geom[k] = dCreateCapsule(space,radius,length); + dMassSetCapsule(&m2,DENSITY,3,radius,length); + } + + dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dMassRotate(&m2,drot[k]); + + dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); + + // add to the total mass + dMassAdd(&m,&m2); + + } + for (k=0; k<GPB; k++) { + dGeomSetBody(obj[i].geom[k],obj[i].body); + dGeomSetOffsetPosition(obj[i].geom[k], + dpos[k][0]-m.c[0], + dpos[k][1]-m.c[1], + dpos[k][2]-m.c[2]); + dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); + } + dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); + dBodySetMass(obj[i].body,&m); + + } + + if (!setBody) { // avoid calling for composite geometries + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) + dGeomSetBody(obj[i].geom[k],obj[i].body); + + dBodySetMass(obj[i].body,&m); + } + } + + + if (cmd == ' ') { + selected++; + if (selected >= num) selected = 0; + if (selected < 0) selected = 0; + } + else if (cmd == 'd' && selected >= 0 && selected < num) { + dBodyDisable (obj[selected].body); + } + else if (cmd == 'e' && selected >= 0 && selected < num) { + dBodyEnable (obj[selected].body); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } + else if (cmd == 'o') { + draw_geom ^= 1; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!draw_geom){ + return; + } + + if (!g) return; + if (!pos) pos = dGeomGetPosition(g); + if (!R) R = dGeomGetRotation(g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths(g,sides); + dsDrawBox(pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere(pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams(g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams(g,&radius,&length); + dsDrawCylinder(pos,R,length,radius); + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB(g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) + bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int j=0; j<3; j++) + bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity(RI); + dsSetColorAlpha(1,0,0,0.5); + dsDrawBox(bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + //if (!pause) dWorldStep (world,0.05); + if (!pause) dWorldQuickStep (world,0.05); + //if (!pause) dWorldStepFast (world,0.05, 1); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i<num; i++) { + for (int j=0; j < GPB; j++) { + if (i==selected) { + dsSetColor (0,0.7,1); + } + else if (! dBodyIsEnabled (obj[i].body)) { + dsSetColor (1,0,0); + } + else { + dsSetColor (1,1,0); + } + drawGeom (obj[i].geom[j],0,0,show_aabb); + } + } +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dsSetSphereQuality(0); + + // create world + dInitODE2(0); + world = dWorldCreate(); + + for (int i=1; i<argc; ++i) { + if (argv[i] == std::string("quad")) { + dVector3 Center = {0, 0, 0, 0}; + dVector3 Extents = {WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, 0}; + puts(":::: Using dQuadTreeSpace"); + space = dQuadTreeSpaceCreate (0, Center, Extents, 6); + } else if (argv[i] == std::string("hash")) { + puts(":::: Using dHashSpace"); + space = dHashSpaceCreate (0); + } else if (argv[i] == std::string("sap")) { + puts(":::: Using dSweepAndPruneSpace"); + space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ); + } else if (argv[i] == std::string("simple")) { + puts(":::: Using dSimpleSpace"); + space = dSimpleSpaceCreate(0); + } + } + if (!space) { + puts(":::: You can specify 'quad', 'hash', 'sap' or 'simple' in the"); + puts(":::: command line to specify the type of space."); + puts(":::: Using SAP space by default."); + space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ); + } + + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + dWorldSetCFM (world,1e-5); + dCreatePlane (space,0,0,1,0); + memset (obj,0,sizeof(obj)); + + for (int i = 0; i < NUM; i++){ + command('s'); + } + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_step.cpp b/libs/ode-0.16.1/ode/demo/demo_step.cpp new file mode 100644 index 0000000..f19a99c --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_step.cpp @@ -0,0 +1,192 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// test the step function by comparing the output of the fast and the slow +// version, for various systems. currently you have to define COMPARE_METHODS +// in step.cpp for this to work properly. +// +// @@@ report MAX error + +#include <time.h> +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10 // number of bodies +#define NUMJ 9 // number of joints +#define SIDE (0.2) // side length of a box +#define MASS (1.0) // mass of a box +#define RADIUS (0.1732f) // sphere radius + + + +// dynamics and collision objects + +static dWorldID world=0; +static dBodyID body[NUM]; +static dJointID joint[NUMJ]; + + +// create the test system + +void createTest() +{ + int i,j; + if (world) dWorldDestroy (world); + + world = dWorldCreate(); + + // create random bodies + for (i=0; i<NUM; i++) { + // create bodies at random position and orientation + body[i] = dBodyCreate (world); + dBodySetPosition (body[i],dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2+RADIUS); + dReal q[4]; + for (j=0; j<4; j++) q[j] = dRandReal()*2-1; + dBodySetQuaternion (body[i],q); + + // set random velocity + dBodySetLinearVel (body[i], dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1); + dBodySetAngularVel (body[i], dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1); + + // set random mass (random diagonal mass rotated by a random amount) + dMass m; + dMatrix3 R; + dMassSetBox (&m,1,dRandReal()+0.1,dRandReal()+0.1,dRandReal()+0.1); + dMassAdjust (&m,dRandReal()+1); + for (j=0; j<4; j++) q[j] = dRandReal()*2-1; + dQtoR (q,R); + dMassRotate (&m,R); + dBodySetMass (body[i],&m); + } + + // create ball-n-socket joints at random positions, linking random bodies + // (but make sure not to link the same pair of bodies twice) + char linked[NUM*NUM]; + for (i=0; i<NUM*NUM; i++) linked[i] = 0; + for (i=0; i<NUMJ; i++) { + int b1,b2; + do { + b1 = dRandInt (NUM); + b2 = dRandInt (NUM); + } while (linked[b1*NUM + b2] || b1==b2); + linked[b1*NUM + b2] = 1; + linked[b2*NUM + b1] = 1; + joint[i] = dJointCreateBall (world,0); + dJointAttach (joint[i],body[b1],body[b2]); + dJointSetBallAnchor (joint[i],dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*2+RADIUS); + } + + for (i=0; i<NUM; i++) { + // move bodies a bit to get some joint error + const dReal *pos = dBodyGetPosition (body[i]); + dBodySetPosition (body[i],pos[0]+dRandReal()*0.2-0.1, + pos[1]+dRandReal()*0.2-0.1,pos[2]+dRandReal()*0.2-0.1); + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.6117f,-1.4433f,2.3700f}; + static float hpr[3] = {151.5000f,-30.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + // add random forces and torques to all bodies + int i; + const dReal scale1 = 5; + const dReal scale2 = 5; + for (i=0; i<NUM; i++) { + dBodyAddForce (body[i], + scale1*(dRandReal()*2-1), + scale1*(dRandReal()*2-1), + scale1*(dRandReal()*2-1)); + dBodyAddTorque (body[i], + scale2*(dRandReal()*2-1), + scale2*(dRandReal()*2-1), + scale2*(dRandReal()*2-1)); + } + + dWorldStep (world,0.05); + createTest(); + } + + // float sides[3] = {SIDE,SIDE,SIDE}; + dsSetColor (1,1,0); + for (int i=0; i<NUM; i++) + dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + dRandSetSeed (time(0)); + createTest(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_tracks.cpp b/libs/ode-0.16.1/ode/demo/demo_tracks.cpp new file mode 100644 index 0000000..9b4f5dd --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_tracks.cpp @@ -0,0 +1,498 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +//#include <iostream> +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawLine dsDrawLineD +#endif + + + +const dReal ball_radius = 0.4; +const dReal balls_sep = 2; // separation between the balls + +/* Choose one test case + */ +#define TEST_CASE 0 + +#if TEST_CASE == 0 +const dReal track_len = 10; +const dReal track_height = 1; +const dReal track_width = 0.1; +const dReal track_gauge = 1; +const dReal track_elevation = 2; +const dReal track_angle = 80 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#elif TEST_CASE == 1 +const dReal track_len = 10; +const dReal track_height = 1; +const dReal track_width = 0.1; +const dReal track_gauge = 1.9*ball_radius; +const dReal track_elevation = 2; +const dReal track_angle = 0 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#elif TEST_CASE == 2 +const dReal track_len = 10; +const dReal track_height = 1; +const dReal track_width = 0.1; +const dReal track_gauge = 1.9*ball_radius; +const dReal track_elevation = 2; +const dReal track_angle = 15 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#elif TEST_CASE == 3 +const dReal track_len = 10; +const dReal track_height = .7; +const dReal track_width = 0.1; +const dReal track_gauge = track_height*1.1; +const dReal track_elevation = 2; +const dReal track_angle = 90 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#else +#error "TEST_CAST to a valid value!" +#endif + + + +dWorldID world; +dSpaceID space; +dJointGroupID contact_group; +dGeomID ground; +dGeomID ball1_geom, ball2_geom; +dTriMeshDataID mesh_data; +dGeomID mesh_geom; + +dBodyID ball1_body, ball2_body; + +const unsigned n_box_verts = 8; +dVector3 box_verts[n_box_verts] = { + {-track_len/2, -track_width/2, track_height/2}, // 0 + { track_len/2, -track_width/2, track_height/2}, // 1 + { track_len/2, track_width/2, track_height/2}, // 2 + {-track_len/2, track_width/2, track_height/2}, // 3 + { track_len/2, -track_width/2, -track_height/2}, // 4 + {-track_len/2, -track_width/2, -track_height/2}, // 5 + {-track_len/2, track_width/2, -track_height/2}, // 6 + { track_len/2, track_width/2, -track_height/2} // 7 +}; + +const unsigned n_box_faces = 12; +dTriIndex box_faces[n_box_faces * 3] = { + 0, 1, 2, + 0, 2, 3, + 1, 4, 7, + 1, 7, 2, + 4, 5, 6, + 4, 6, 7, + 5, 0, 3, + 5, 3, 6, + 3, 2, 7, + 3, 7, 6, + 0, 5, 4, + 0, 4, 1 +}; + + +const unsigned n_track_verts = n_box_verts * 2; +const unsigned n_track_faces = n_box_faces * 2; + +dVector3 track_verts[n_track_verts]; +dTriIndex track_faces[n_track_faces * 3]; + + + +void resetBall(dBodyID b, unsigned idx) +{ + dBodySetPosition(b, + 0.5*track_len*cos(track_incl) // Z + - 0.5*track_height*sin(track_incl) + - ball_radius, // X + balls_sep*idx, // Y + track_elevation + ball_radius// Z + + 0.5*track_len*sin(track_incl) + + 0.5*track_height*cos(track_incl)); + dMatrix3 r = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0}; + dBodySetRotation(b, r); + dBodySetLinearVel(b, 0, 0, 0); + dBodySetAngularVel(b, 0, 0, 0); + +} + + +void resetSim() +{ + resetBall(ball1_body, 0); + resetBall(ball2_body, 1); +} + + +void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + contact_group = dJointGroupCreate(0); + + space = dSimpleSpaceCreate (0); + + + // first, the ground plane + // it has to coincide with the plane we have in drawstuff + ground = dCreatePlane(space, 0, 0, 1, 0); + + + // now a ball + dMass m; + dMassSetSphere(&m, 0.1, ball_radius); + + ball1_geom = dCreateSphere(space, ball_radius); + ball1_body = dBodyCreate(world); + dGeomSetBody(ball1_geom, ball1_body); + dBodySetMass(ball1_body, &m); + + ball2_geom = dCreateSphere(space, ball_radius); + ball2_body = dBodyCreate(world); + dGeomSetBody(ball2_geom, ball2_body); + dBodySetMass(ball2_body, &m); + + + + + // tracks made out of boxes + dGeomID trk; + dMatrix3 r1, r2, r3; + dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation}; + dMatrix3 s1, s2, s3; + dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation}; + + dRFromAxisAndAngle(r1, 1, 0, 0, track_angle); + dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl); + dMultiply0_333(r3, r2, r1); + + dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle); + dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl); + dMultiply0_333(s3, s2, s1); + + trk = dCreateBox(space, track_len, track_width, track_height); + dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]); + dGeomSetRotation(trk, r3); + + trk = dCreateBox(space, track_len, track_width, track_height); + dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]); + dGeomSetRotation(trk, s3); + + + + + + // tracks made out of trimesh + for (unsigned i=0; i<n_box_verts; ++i) { + dVector3 p; + dMultiply0_331(p, s3, box_verts[i]); + dAddVectors3(p, p, so); + dCopyVector3(track_verts[i], p); + } + // trimesh tracks 2, transform all vertices by s3 + for (unsigned i=0; i<n_box_verts; ++i) { + dVector3 p; + dMultiply0_331(p, r3, box_verts[i]); + dAddVectors3(p, p, ro); + dCopyVector3(track_verts[n_box_verts + i], p); + } + + // copy face indices + for (unsigned i=0; i<n_box_faces; ++i) + for (unsigned j=0; j<3; ++j) // each face index + track_faces[3*i+j] = box_faces[3*i+j]; + for (unsigned i=0; i<n_box_faces; ++i) + for (unsigned j=0; j<3; ++j) // each face index + track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts; + + mesh_data = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSimple(mesh_data, + track_verts[0], n_track_verts, + track_faces, 3*n_track_faces); + mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0); + + + + + + resetSim(); + + + // initial camera position + static float xyz[3] = {-5.9414,-0.4804,2.9800}; + static float hpr[3] = {32.5000,-10.0000,0.0000}; + dsSetViewpoint (xyz,hpr); + + dsSetSphereQuality(3); +} + + +void nearCallback(void *, dGeomID a, dGeomID b) +{ + const unsigned max_contacts = 8; + dContact contacts[max_contacts]; + + if (!dGeomGetBody(a) && !dGeomGetBody(b)) + return; // don't handle static geom collisions + + int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact)); + //clog << "got " << n << " contacts" << endl; + + /* Simple contact merging: + * If we have contacts that are too close with the same normal, keep only + * the one with maximum depth. + * The epsilon that defines what "too close" means can be a heuristic. + */ + int new_n = 0; + dReal epsilon = 1e-1; // default + /* If we know one of the geoms is a sphere, we can base the epsilon on the + * sphere's radius. + */ + dGeomID s = 0; + if ((dGeomGetClass(a) == dSphereClass && (s = a)) || + (dGeomGetClass(b) == dSphereClass && (s = b))) { + epsilon = dGeomSphereGetRadius(s) * 0.3; + } + + + for (int i=0; i<n; ++i) { + + // this block draws the contact points before merging, in red + dMatrix3 r; + dRSetIdentity(r); + dsSetColor(1, 0, 0); + dsSetTexture(DS_NONE); + dsDrawSphere(contacts[i].geom.pos, r, 0.008); + + // let's offset the line a bit to avoid drawing overlap issues + float xyzf[3], hprf[3]; + dsGetViewpoint(xyzf, hprf); + dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])}; + dVector3 v; + dSubtractVectors3(v, contacts[i].geom.pos, xyz); + dVector3 c; + dCalcVectorCross3(c, v, contacts[i].geom.pos); + dNormalize3(c); + dVector3 pos1; + dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005); + dVector3 pos2; + dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05); + dsDrawLine(pos1, pos2); + // end of contacts drawing code + + + + int closest_point = i; + for (int j=0; j<new_n; ++j) { + dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal); + if (alignment > 0.99 // about 8 degrees of difference + && + dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) { + // they are too close + closest_point = j; + //clog << "found close points: " << j << " and " << i << endl; + break; + } + } + + if (closest_point != i) { + // we discard one of the points + if (contacts[i].geom.depth > contacts[closest_point].geom.depth) + // the new point is deeper, copy it over closest_point + contacts[closest_point] = contacts[i]; + } else + contacts[new_n++] = contacts[i]; // the point is preserved + } + //clog << "reduced from " << n << " to " << new_n << endl; + n = new_n; + + for (int i=0; i<n; ++i) { + contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP; + contacts[i].surface.mu = 10; + contacts[i].surface.bounce = 0.2; + contacts[i].surface.bounce_vel = 0; + contacts[i].surface.soft_erp = 1e-3; + //clog << "depth: " << contacts[i].geom.depth << endl; + + + dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]); + dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b)); + + dMatrix3 r; + dRSetIdentity(r); + dsSetColor(0, 0, 1); + dsSetTexture(DS_NONE); + dsDrawSphere(contacts[i].geom.pos, r, 0.01); + dsSetColor(0, 1, 0); + dVector3 pos2; + dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1); + dsDrawLine(contacts[i].geom.pos, pos2); + } + //clog << "----" << endl; +} + + + + +void stop() +{ + dGeomDestroy(mesh_geom); + dGeomTriMeshDataDestroy(mesh_data); + + dBodyDestroy(ball1_body); + dBodyDestroy(ball2_body); + + dGeomDestroy(ground); + + dJointGroupDestroy(contact_group); + + dSpaceDestroy(space); // will destroy all geoms + + dWorldDestroy(world); +} + + +static void command (int cmd) +{ + switch (cmd) { + case ' ': + resetSim(); + break; + } +} + + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dSphereClass: + dsSetColorAlpha(0, 0.75, 0.5, 0.5); + dsSetTexture (DS_CHECKERED); + dsDrawSphere(pos, rot, dGeomSphereGetRadius(g)); + break; + case dBoxClass: + { + dVector3 lengths; + dsSetColorAlpha(1, 1, 0, 0.5); + dsSetTexture (DS_WOOD); + dGeomBoxGetLengths(g, lengths); + dsDrawBox(pos, rot, lengths); + break; + } + case dTriMeshClass: + { + int numi = dGeomTriMeshGetTriangleCount(g); + + for (int i=0; i<numi; ++i) { + dVector3 v0, v1, v2; + dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2); + + dsSetTexture (DS_WOOD); + + dsSetDrawMode(DS_WIREFRAME); + dsSetColorAlpha(0, 0, 0, 1.0); + dsDrawTriangle(pos, rot, v0, v1, v2, true); + + dsSetDrawMode(DS_POLYFILL); + dsSetColorAlpha(1, 1, 0, 0.5); + dsDrawTriangle(pos, rot, v0, v1, v2, true); + } + break; + } + + default: + {} + } +} + + + +void simLoop (int pause) +{ + if (!pause) { + + const dReal step = 0.02; + const unsigned nsteps = 1; + + for (unsigned i=0; i<nsteps; ++i) { + dSpaceCollide(space, 0, nearCallback); + dWorldQuickStep(world, step); + dJointGroupEmpty(contact_group); + } + } else { + dSpaceCollide(space, 0, nearCallback); + dJointGroupEmpty(contact_group); + } + + // now we draw everything + unsigned ngeoms = dSpaceGetNumGeoms(space); + for (unsigned i=0; i<ngeoms; ++i) { + dGeomID g = dSpaceGetGeom(space, i); + + if (g == ground) + continue; // drawstuff is already drawing it for us + + drawGeom(g); + } + + if (dBodyGetPosition(ball1_body)[0] < -track_len) + resetSim(); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = stop; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE(); + + // run demo + dsSimulationLoop (argc, argv, 800, 600, &fn); + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_transmission.cpp b/libs/ode-0.16.1/ode/demo/demo_transmission.cpp new file mode 100644 index 0000000..50cb820 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_transmission.cpp @@ -0,0 +1,414 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawLine dsDrawLineD +#define dsDrawSphere dsDrawSphereD +#endif + +dReal theta = M_PI / 4; +dReal ratio = 1, speed = 5, rho_1 = 1, rho_2 = 1, backlash = 0.1; +int mode = 0; + +dWorldID world; +dSpaceID space; +dBodyID body1, body2; +dGeomID geom1, geom2; +dJointID hinge1, hinge2, transmission; +dJointFeedback feedback; + +void setup() { + dMatrix3 R; + + switch (mode) { + case 0: + // Parallel axes. + + dBodySetPosition(body1, 1, 0, 1); + dBodySetPosition(body2, -1, 0, 1); + + dRSetIdentity (R); + dBodySetRotation (body1, R); + dBodySetRotation (body2, R); + + dJointSetHingeAnchor(hinge2, -1, 0, 1); + dJointSetHingeAxis(hinge2, 0, 0, 1); + + dJointSetHingeAnchor(hinge1, 1, 0, 1); + dJointSetHingeAxis(hinge1, 0, 0, 1); + + dJointSetTransmissionMode(transmission, dTransmissionParallelAxes); + dJointSetTransmissionRatio(transmission, ratio); + dJointSetTransmissionAnchor1(transmission, 1, 0, 1); + dJointSetTransmissionAnchor2(transmission, -1, 0, 1); + dJointSetTransmissionAxis(transmission, 0, 0, 1); + + break; + case 1: + // Intersecting axes. + + dBodySetPosition(body1, 1, 0, 1); + dBodySetPosition(body2, -1, 0, 2); + + dRSetIdentity (R); + dBodySetRotation (body1, R); + + dRFromZAxis (R, cos(theta), 0, sin(theta)); + dBodySetRotation (body2, R); + + dJointSetHingeAnchor(hinge2, -1, 0, 2); + dJointSetHingeAxis(hinge2, cos(theta), 0, sin(theta)); + + dJointSetHingeAnchor(hinge1, 1, 0, 1); + dJointSetHingeAxis(hinge1, 0, 0, 1); + + dJointSetTransmissionMode(transmission, dTransmissionIntersectingAxes); + dJointSetTransmissionAnchor1(transmission, 1, 0, 1); + dJointSetTransmissionAnchor2(transmission, -1, 0, 2); + dJointSetTransmissionAxis1(transmission, 0, 0, -1); + dJointSetTransmissionAxis2(transmission, cos(theta), 0, sin(theta)); + + break; + case 2: + // Chain. + + dBodySetPosition(body1, 2, 0, 1); + dBodySetPosition(body2, -2, 0, 1); + + dRSetIdentity (R); + dBodySetRotation (body1, R); + dBodySetRotation (body2, R); + + dJointSetHingeAnchor(hinge2, -2, 0, 1); + dJointSetHingeAxis(hinge2, 0, 0, 1); + + dJointSetHingeAnchor(hinge1, 2, 0, 1); + dJointSetHingeAxis(hinge1, 0, 0, 1); + + dJointSetTransmissionMode(transmission, dTransmissionChainDrive); + dJointSetTransmissionAnchor1(transmission, 2, 0, 1); + dJointSetTransmissionAnchor2(transmission, -2, 0, 1); + dJointSetTransmissionRadius1(transmission, rho_1); + dJointSetTransmissionRadius2(transmission, rho_2); + dJointSetTransmissionAxis(transmission, 0, 0, 1); + + break; + } + + dJointSetTransmissionBacklash(transmission, backlash); + + dJointSetHingeParam(hinge2, dParamVel, speed); + dJointSetHingeParam(hinge2, dParamFMax, 50); + + dJointSetHingeParam(hinge1, dParamVel, 0); + dJointSetHingeParam(hinge1, dParamFMax, 2); + + dBodySetLinearVel(body1, 0, 0, 0); + dBodySetLinearVel(body2, 0, 0, 0); + dBodySetAngularVel(body1, 0, 0, 0); + dBodySetAngularVel(body2, 0, 0, 0); +} + +void start() +{ + dMass mass; + + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + dWorldSetERP(world, 0.2); + + space = dSimpleSpaceCreate (0); + + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetFiniteRotationMode(body1, 1); + dBodySetFiniteRotationMode(body2, 1); + + geom1 = dCreateCylinder(space, 0.2, 0.5); + dGeomSetBody(geom1, body1); + dMassSetCylinder(&mass, 100, 3, 0.2, 0.5); + dBodySetMass(body1, &mass); + + geom2 = dCreateCylinder(space, 0.2, 0.5); + dGeomSetBody(geom2, body2); + dMassSetCylinder(&mass, 100, 3, 0.2, 0.5); + dBodySetMass(body2, &mass); + + hinge1 = dJointCreateHinge(world, 0); + dJointAttach(hinge1, body1, 0); + + hinge2 = dJointCreateHinge(world, 0); + dJointAttach(hinge2, body2, 0); + + transmission = dJointCreateTransmission(world, 0); + dJointAttach(transmission, body1, body2); + dJointSetFeedback(transmission, &feedback); + + setup(); + + // initial camera position + static float xyz[3] = {1.15,-2.78,4.1}; + static float hpr[3] = {105,-45.5,0}; + dsSetViewpoint (xyz,hpr); + + fprintf (stderr, + "The green wheel is driving the red one. To control it use the following:\n" + " '[' : decrease wheel ratio\n" + " ']' : increase wheel ratio\n" + " ',' : decrease driving wheel speed\n" + " '.' : increase driving wheel speed\n" + " '-' : decrease backlash\n" + " '=' : increase backlash\n" + " '1' : switch to parallel axes gears mode\n" + " '2' : switch to intersecting axes gears mode\n" + " '3' : switch to chain (or belt) mode\n" +); +} + +void stop() +{ + dSpaceDestroy(space); + + dWorldDestroy(world); +} + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dCylinderClass: + { + dReal length, radius; + + if (g == geom1) { + dsSetColorAlpha(1, 0, 0, 1); + } else { + dsSetColorAlpha(0, 1, 0, 1); + } + + dsSetTexture (DS_WOOD); + dGeomCylinderGetParams(g, &radius, &length); + dsDrawCylinder(pos, rot, length, radius); + break; + } + + default: + { + abort(); + } + } +} + +void simLoop(int pause) +{ + if (!pause) { + + const dReal step = 0.003; + const unsigned nsteps = 4; + + for (unsigned i=0; i<nsteps; ++i) { + dWorldQuickStep(world, step); + } + } + +#if 0 + { + const dReal *omega_1, *omega_2; + + omega_1 = dBodyGetAngularVel(body1); + omega_2 = dBodyGetAngularVel(body2); + + printf ("T1: %f, %f, %f\n", + feedback.t1[0], feedback.t1[1], feedback.t1[2]); + + printf ("T2: %f, %f, %f\n", + feedback.t2[0], feedback.t2[1], feedback.t2[2]); + + printf ("F1: %f, %f, %f\n", + feedback.f1[0], feedback.f1[1], feedback.f1[2]); + + printf ("F2: %f, %f, %f\n", + feedback.f2[0], feedback.f2[1], feedback.f2[2]); + } +#endif + + // now we draw everything + unsigned ngeoms = dSpaceGetNumGeoms(space); + for (unsigned i=0; i<ngeoms; ++i) { + dGeomID g = dSpaceGetGeom(space, i); + + drawGeom(g); + } + + const dReal *R_1 = dGeomGetRotation(geom1); + const dReal *R_2 = dGeomGetRotation(geom2); + dVector3 c_1, c_2, a_1, a_2; + + dJointGetTransmissionContactPoint1(transmission, c_1); + dJointGetTransmissionContactPoint2(transmission, c_2); + dJointGetTransmissionAnchor1(transmission, a_1); + dJointGetTransmissionAnchor2(transmission, a_2); + + dsSetColorAlpha(1, 0, 0, 0.5); + dsDrawCylinder(a_1, R_1, 0.05, dCalcPointsDistance3(c_1, a_1)); + dsSetColorAlpha(0, 1, 0, 0.5); + dsDrawCylinder(a_2, R_2, 0.05, dCalcPointsDistance3(c_2, a_2)); + + dsSetColorAlpha(1, 0, 0, 0.5); + dsDrawSphere (c_1, R_1, 0.05); + dsDrawSphere (c_2, R_1, 0.05); + + dsSetColorAlpha(1, 1, 0, 0.5); + if (mode == dTransmissionChainDrive) { + dsDrawLine(c_1, c_2); + } +} + +static void command (int cmd) +{ + if (cmd == '[') { + switch(mode) { + case dTransmissionParallelAxes: + if (ratio > 0.125) { + ratio *= 0.5; + + fprintf (stderr, "Gear ratio set to %.3f.\n", ratio); + } + break; + case dTransmissionIntersectingAxes: + if (theta > 0.1) { + theta -= 0.1; + + fprintf (stderr, "Gear angle set to %.3f deg.\n", + theta / M_PI * 180); + } + break; + case dTransmissionChainDrive: + if (rho_2 > 0.125) { + rho_2 /= 2; + + fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1); + } + break; + } + + setup(); + } else if (cmd == ']') { + switch(mode) { + case dTransmissionParallelAxes: + if (ratio < 8) { + ratio *= 2; + + fprintf (stderr, "Gear ratio set to %.3f.\n", ratio); + } + break; + case dTransmissionIntersectingAxes: + if (theta < 0.9) { + theta += 0.1; + + fprintf (stderr, "Gear angle set to %.3f deg.\n", + theta / M_PI * 180); + } + break; + case dTransmissionChainDrive: + if (rho_2 < 2) { + rho_2 *= 2; + + fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1); + } + break; + } + + setup(); + } else if (cmd == '.') { + speed += 5; + + fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed); + + dJointSetHingeParam(hinge2, dParamVel, speed); + } else if (cmd == ',') { + speed -= 5; + + fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed); + + dJointSetHingeParam(hinge2, dParamVel, speed); + } else if (cmd == '/') { + if (dJointGetHingeParam(hinge2, dParamFMax) > 0) { + dJointSetHingeParam(hinge2, dParamFMax, 0); + } else { + dJointSetHingeParam(hinge2, dParamFMax, 50); + } + + } else if (cmd == '-') { + backlash -= 0.1; + + fprintf (stderr, "Backlash set to %g m.\n", backlash); + + dJointSetTransmissionBacklash(transmission, backlash); + } else if (cmd == '=') { + backlash += 0.1; + + fprintf (stderr, "Backlash set to %g m.\n", backlash); + + dJointSetTransmissionBacklash(transmission, backlash); + } else if (cmd == '1') { + mode = dTransmissionParallelAxes; + setup(); + } else if (cmd == '2') { + mode = dTransmissionIntersectingAxes; + setup(); + } else if (cmd == '3') { + mode = dTransmissionChainDrive; + setup(); + } +} + +int main(int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = stop; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE(); + + // run demo + dsSimulationLoop (argc, argv, 800, 600, &fn); + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_trimesh.cpp b/libs/ode-0.16.1/ode/demo/demo_trimesh.cpp new file mode 100644 index 0000000..1c53334 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_trimesh.cpp @@ -0,0 +1,605 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh test by Erwin de Vries + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +//<---- Convex Object +static const dReal planes[] = // planes for a cube +{ + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ +}; +static const unsigned int planecount=6; + +static const dReal points[] = // points for a cube +{ + 0.25f,0.25f,0.25f, + -0.25f,0.25f,0.25f, + + 0.25f,-0.25f,0.25f, + -0.25f,-0.25f,0.25f, + + 0.25f,0.25f,-0.25f, + -0.25f,0.25f,-0.25f, + + 0.25f,-0.25f,-0.25f, + -0.25f,-0.25f,-0.25f, +}; +static const unsigned int pointcount=8; + +static const unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawLine dsDrawLineD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 200 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 40 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? + +#define VertexCount 5 +#define IndexCount 12 + +static dVector3 Size; +static float Vertices[VertexCount][3]; +static dTriIndex Indices[IndexCount]; + +static dGeomID TriMesh; +static dGeomID Ray; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i<MAX_CONTACTS; i++) { + contact[i].surface.mode = dContactBounce | dContactSoftCFM; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.1; + contact[i].surface.bounce_vel = 0.1; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact))) { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.02,0.02,0.02}; + for (i=0; i<numc; i++) { + if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){ + dMatrix3 Rotation; + dRSetIdentity(Rotation); + dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01)); + + dVector3 End; + End[0] = contact[i].geom.pos[0] + (contact[i].geom.normal[0] * contact[i].geom.depth); + End[1] = contact[i].geom.pos[1] + (contact[i].geom.normal[1] * contact[i].geom.depth); + End[2] = contact[i].geom.pos[2] + (contact[i].geom.normal[2] * contact[i].geom.depth); + End[3] = contact[i].geom.pos[3] + (contact[i].geom.normal[3] * contact[i].geom.depth); + + dsDrawLine(contact[i].geom.pos, End); + continue; + } + + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("To drop another object, press:\n"); + printf (" b for box.\n"); + printf (" s for sphere.\n"); + printf (" c for cylinder.\n"); + printf( " v for a convex.\n" ); + printf (" x for a composite object.\n"); + printf ("To select an object, press space.\n"); + printf ("To disable the selected object, press d.\n"); + printf ("To enable the selected object, press e.\n"); + printf ("To toggle showing the geom AABBs, press a.\n"); + printf ("To toggle showing the contact points, press t.\n"); + printf ("To toggle dropping from random position/orientation, press r.\n"); +} + + +char locase (char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + int i,j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v' + /* || cmd == 'l' */) { + if (num < NUM) { + i = num; + num++; + } + else { + i = nextobj; + nextobj++; + if (nextobj >= num) nextobj = 0; + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + } + memset (&obj[i],0,sizeof(obj[i])); + } + + obj[i].body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition (obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1); + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } + else { + dReal maxheight = 0; + for (k=0; k<num; k++) { + const dReal *pos = dBodyGetPosition (obj[k].body); + if (pos[2] > maxheight) maxheight = pos[2]; + } + dBodySetPosition (obj[i].body, 0,0,maxheight+1); + dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); + } + dBodySetRotation (obj[i].body,R); + dBodySetData (obj[i].body,(void*)(dsizeint)i); + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } +/* + // cylinder option not yet implemented + else if (cmd == 'l') { + sides[1] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } +*/ + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + } + else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j<GPB; j++) + for (k=0; k<3; k++) + dpos[j][k] = dRandReal()*0.3-0.15; + + for (k=0; k<GPB; k++) { + if (k==0) { + dReal radius = dRandReal()*0.25+0.05; + obj[i].geom[k] = dCreateSphere (space,radius); + dMassSetSphere (&m2,DENSITY,radius); + } else if (k==1) { + obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); + dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); + } else { + dReal radius = dRandReal()*0.1+0.05; + dReal length = dRandReal()*1.0+0.1; + obj[i].geom[k] = dCreateCapsule(space,radius,length); + dMassSetCapsule(&m2,DENSITY,3,radius,length); + } + + dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dMassRotate(&m2,drot[k]); + + dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); + + // add to the total mass + dMassAdd(&m,&m2); + + } + for (k=0; k<GPB; k++) { + dGeomSetBody(obj[i].geom[k],obj[i].body); + dGeomSetOffsetPosition(obj[i].geom[k], + dpos[k][0]-m.c[0], + dpos[k][1]-m.c[1], + dpos[k][2]-m.c[2]); + dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); + } + dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); + dBodySetMass(obj[i].body,&m); + + } else if (cmd == 'v') { + + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + + obj[i].geom[0] = dCreateConvex(space, + planes, + planecount, + points, + pointcount, + polygons); + } + + if (!setBody) { // avoid calling for composite geometries + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) + dGeomSetBody(obj[i].geom[k],obj[i].body); + + dBodySetMass(obj[i].body,&m); + } + } + + if (cmd == ' ') { + selected++; + if (selected >= num) selected = 0; + if (selected < 0) selected = 0; + } + else if (cmd == 'd' && selected >= 0 && selected < num) { + dBodyDisable (obj[selected].body); + } + else if (cmd == 'e' && selected >= 0 && selected < num) { + dBodyEnable (obj[selected].body); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } else if (type == dConvexClass) { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } +/* + // cylinder option not yet implemented + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } +*/ + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + if (!pause) dWorldStep (world,0.05); + //if (!pause) dWorldStepFast (world,0.05, 1); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i<num; i++) { + for (int j=0; j < GPB; j++) { + if (i==selected) { + dsSetColor (0,0.7,1); + } + else if (! dBodyIsEnabled (obj[i].body)) { + dsSetColor (1,0,0); + } + else { + dsSetColor (1,1,0); + } + drawGeom (obj[i].geom[j],0,0,show_aabb); + } + } + + /*{ + for (int i = 1; i < IndexCount; i++) { + dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]); + } + }*/ + + {const dReal* Pos = dGeomGetPosition(TriMesh); + const dReal* Rot = dGeomGetRotation(TriMesh); + + {for (int i = 0; i < IndexCount / 3; i++){ + const float *p = Vertices[Indices[i * 3 + 0]]; + const dVector3 v0 = { p[0], p[1], p[2] }; + p = Vertices[Indices[i * 3 + 1]]; + const dVector3 v1 = { p[0], p[1], p[2] }; + p = Vertices[Indices[i * 3 + 2]]; + const dVector3 v2 = { p[0], p[1], p[2] }; + dsDrawTriangle(Pos, Rot, v0, v1, v2, 0); + }}} + + if (Ray){ + dVector3 Origin, Direction; + dGeomRayGet(Ray, Origin, Direction); + + dReal Length = dGeomRayGetLength(Ray); + + dVector3 End; + End[0] = Origin[0] + (Direction[0] * Length); + End[1] = Origin[1] + (Direction[1] * Length); + End[2] = Origin[2] + (Direction[2] * Length); + End[3] = Origin[3] + (Direction[3] * Length); + + dsDrawLine(Origin, End); + } +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + + space = dSimpleSpaceCreate(0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + dWorldSetCFM (world,1e-5); + //dCreatePlane (space,0,0,1,0); + memset (obj,0,sizeof(obj)); + + Size[0] = 5.0f; + Size[1] = 5.0f; + Size[2] = 2.5f; + + Vertices[0][0] = -Size[0]; + Vertices[0][1] = -Size[1]; + Vertices[0][2] = Size[2]; + + Vertices[1][0] = Size[0]; + Vertices[1][1] = -Size[1]; + Vertices[1][2] = Size[2]; + + Vertices[2][0] = Size[0]; + Vertices[2][1] = Size[1]; + Vertices[2][2] = Size[2]; + + Vertices[3][0] = -Size[0]; + Vertices[3][1] = Size[1]; + Vertices[3][2] = Size[2]; + + Vertices[4][0] = 0; + Vertices[4][1] = 0; + Vertices[4][2] = 0; + + Indices[0] = 0; + Indices[1] = 1; + Indices[2] = 4; + + Indices[3] = 1; + Indices[4] = 2; + Indices[5] = 4; + + Indices[6] = 2; + Indices[7] = 3; + Indices[8] = 4; + + Indices[9] = 3; + Indices[10] = 0; + Indices[11] = 4; + + dTriMeshDataID Data = dGeomTriMeshDataCreate(); + + //dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices, IndexCount); + dGeomTriMeshDataBuildSingle(Data, Vertices[0], 3 * sizeof(float), VertexCount, &Indices[0], IndexCount, 3 * sizeof(dTriIndex)); + dGeomTriMeshDataPreprocess2(Data, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); + + TriMesh = dCreateTriMesh(space, Data, 0, 0, 0); + + //dGeomSetPosition(TriMesh, 0, 0, 1.0); + + Ray = dCreateRay(space, 0.9); + dVector3 Origin, Direction; + Origin[0] = 0.0; + Origin[1] = 0; + Origin[2] = 0.5; + Origin[3] = 0; + + Direction[0] = 0; + Direction[1] = 1.1f; + Direction[2] = -1; + Direction[3] = 0; + dNormalize3(Direction); + + dGeomRaySet(Ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]); + + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); + dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dWorldSetStepThreadingImplementation(world, NULL, NULL); + dThreadingFreeImplementation(threading); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/halton235_geom.h b/libs/ode-0.16.1/ode/demo/halton235_geom.h new file mode 100644 index 0000000..c571d4f --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/halton235_geom.h @@ -0,0 +1,2271 @@ +// Generated by ./Tools/mksrc.py + +// h00 +const int h00_numv = 14; +const int h00_numf = 9; +const dReal h00_volu = 0.105869; +const dReal h00_pos[3] = { -0.801161,-0.825905,-0.503586 }; +const dReal h00_verts[ h00_numv * 3 ] = { + -0.198839,-0.174095,-0.496414, -0.198839,-0.073069,-0.496414, 0.355754,-0.174095,0.317906, 0.266263,0.078525,-0.113353, -0.198839,0.418138,0.148658, -0.198839,0.151957,0.285583, -0.004749,-0.165186,-0.496414, -0.198839,0.342511,-0.068863, -0.198839,-0.174095,0.341490, 0.030157,-0.174095,0.397398, -0.005096,-0.174095,-0.496414, 0.428901,-0.174095,0.139324, -0.198839,0.419530,0.145078, -0.111897,0.386202,0.018538, +}; +const unsigned int h00_faces[] = { + 5, 7,13,3,6,1, + 4, 6,10,0,1, + 7, 0,8,5,4,12,7,1, + 6, 11,3,13,12,4,2, + 4, 4,5,9,2, + 6, 9,8,0,10,11,2, + 4, 11,10,6,3, + 3, 8,9,5, + 3, 12,13,7, +}; +const dReal h00_planes[ h00_numf * 4 ] = { + 0.322183,0.678839,-0.659831,0.213885,0,0,-1,0.496414,-1,3.46515e-16,1.34725e-16,0.198839,0.66261,0.69806,0.271405,0.200478,0.212159,0.447018,0.869002,0.273913,-0,-1,0,0.174095,0.825473,-0.0322069,-0.563523,0.281141,-0.233951,0.164312,0.958263,0.34515,-0.131169,0.932758,-0.335793,0.368685 +}; +// h01 +const int h01_numv = 16; +const int h01_numf = 10; +const dReal h01_volu = 0.123003; +const dReal h01_pos[3] = { -0.669502,-0.555335,-0.800913 }; +const dReal h01_verts[ h01_numv * 3 ] = { + 0.137874,0.266251,0.316069, -0.136407,-0.435756,-0.199087, -0.330498,0.127352,-0.199087, 0.378917,-0.091301,0.019367, 0.437409,0.033623,-0.016334, 0.038469,0.311615,0.180723, -0.330498,-0.343638,-0.199087, 0.193689,-0.157234,0.188948, 0.276656,-0.200503,-0.199087, 0.399981,0.033623,-0.199087, -0.039317,0.311615,-0.199087, -0.330498,0.071941,0.228464, 0.134604,-0.192045,0.183974, -0.243556,0.115632,0.315865, 0.128360,0.261346,0.324519, -0.053858,0.233367,0.333678, +}; +const unsigned int h01_faces[] = { + 5, 8,3,7,12,1, + 5, 12,13,11,6,1, + 6, 6,2,10,9,8,1, + 6, 11,13,15,5,10,2, + 3, 6,11,2, + 4, 8,9,4,3, + 5, 4,0,14,7,3, + 5, 9,10,5,0,4, + 4, 15,14,0,5, + 5, 14,15,13,12,7, +}; +const dReal h01_planes[ h01_numf * 4 ] = { + 0.485031,-0.851631,0.198669,0.265389,-0.322183,-0.678839,0.659831,0.208392,0,0,-1,0.199087,-0.53156,0.839996,0.108863,0.260981,-1,0,-0,0.330498,0.870584,-0.458578,-0.178296,0.368295,0.693474,-0.121763,0.710118,0.287639,0.531559,0.839996,-0.108863,0.26253,-0.120149,0.911352,0.393704,0.35052,0.0929092,-0.293639,0.951391,0.243929 +}; +// h02 +const int h02_numv = 22; +const int h02_numf = 13; +const dReal h02_volu = 0.069699; +const dReal h02_pos[3] = { -0.226726,-0.554920,-0.490680 }; +const dReal h02_verts[ h02_numv * 3 ] = { + 0.092179,-0.226103,0.260087, -0.296734,0.272195,0.041665, -0.063859,-0.091716,-0.290866, 0.291800,-0.116083,-0.104671, -0.005367,0.033207,-0.326567, 0.057499,0.071569,-0.315604, -0.314416,0.260931,0.014286, 0.001987,-0.310858,0.106391, 0.200664,0.041526,0.249449, -0.249087,-0.157649,-0.121285, 0.164143,-0.213455,-0.116915, 0.086533,-0.112211,0.311781, 0.301609,-0.100599,-0.031006, -0.013037,0.041374,0.269240, 0.214238,-0.145305,0.159971, -0.177599,0.337949,0.022880, -0.304902,0.265836,0.005836, 0.151444,0.029502,0.298046, 0.226514,0.065723,0.196331, -0.078156,0.329100,-0.024159, 0.053890,0.084483,0.272741, -0.198047,0.327898,0.050872, +}; +const unsigned int h02_faces[] = { + 4, 13,20,21,1, + 5, 21,15,16,6,1, + 7, 6,9,7,0,11,13,1, + 5, 9,6,16,4,2, + 5, 4,5,3,10,2, + 4, 10,7,9,2, + 5, 5,19,18,12,3, + 6, 12,14,0,7,10,3, + 5, 16,15,19,5,4, + 7, 18,19,15,21,20,17,8, + 5, 17,11,0,14,8, + 4, 14,12,18,8, + 4, 17,20,13,11, +}; +const dReal h02_planes[ h02_numf * 4 ] = { + -0.322885,0.43293,0.841615,0.248718,-0.488006,0.871758,-0.0434537,0.380285,-0.73548,-0.309929,0.602505,0.158984,-0.693474,0.121763,-0.710118,0.239666,0.398278,-0.419584,-0.815674,0.250302,-0.11362,-0.877785,-0.465387,0.223128,0.726907,0.646142,-0.23261,0.161453,0.598819,-0.796079,0.0875987,0.257978,-0.27553,0.653111,-0.705358,0.253514,0.315015,0.796483,0.516121,0.225033,0.709719,-0.26169,0.654077,0.294707,0.913105,-0.120244,0.389591,0.275417,-0.158394,0.166869,0.973173,0.270985 +}; +// h03 +const int h03_numv = 18; +const int h03_numf = 11; +const dReal h03_volu = 0.069585; +const dReal h03_pos[3] = { -0.013871,-0.718833,-0.869565 }; +const dReal h03_verts[ h03_numv * 3 ] = { + -0.207492,-0.281167,-0.130435, 0.113468,0.049972,0.264160, -0.007999,0.353837,-0.130435, 0.078945,0.047830,0.274214, -0.003048,-0.281167,0.119131, -0.048711,-0.049541,0.261970, -0.155356,0.235482,0.063281, 0.163083,-0.281167,0.065052, -0.378975,-0.037005,-0.130435, -0.276714,0.072197,0.088019, 0.263172,-0.281167,-0.130435, 0.247851,0.256254,-0.130435, 0.308871,0.008019,-0.130435, 0.268707,0.210552,0.010520, 0.116203,0.335344,-0.130435, 0.263499,0.226082,-0.009852, -0.255650,0.197121,-0.130435, -0.218222,0.197121,0.052318, +}; +const unsigned int h03_faces[] = { + 7, 13,15,14,2,6,3,1, + 5, 3,5,4,7,1, + 5, 7,10,12,13,1, + 4, 16,17,6,2, + 8, 14,11,12,10,0,8,16,2, + 5, 6,17,9,5,3, + 5, 5,9,8,0,4, + 4, 0,10,7,4, + 4, 9,17,16,8, + 3, 14,15,11, + 4, 15,13,12,11, +}; +const dReal h03_planes[ h03_numf * 4 ] = { + 0.119725,0.804079,0.582343,0.207598,0.274018,-0.465094,0.841785,0.230216,0.881437,-0.139289,0.451296,0.212269,-0.53156,0.839996,0.108863,0.287274,0,-0,-1,0.130435,-0.398278,0.419584,0.815674,0.212296,-0.679728,-0.477395,0.556833,0.202635,-0,-1,-0,0.281167,-0.870584,0.458578,0.178296,0.289704,0.509459,0.848006,0.146072,0.324522,0.968964,0.238186,-0.066148,0.309823 +}; +// h04 +const int h04_numv = 18; +const int h04_numf = 11; +const dReal h04_volu = 0.049776; +const dReal h04_pos[3] = { 0.037853,-0.892026,-0.431971 }; +const dReal h04_verts[ h04_numv * 3 ] = { + -0.100436,0.123651,-0.175624, 0.353999,-0.060233,0.083056, 0.061744,0.223165,-0.173434, -0.054772,-0.107974,-0.318463, 0.079577,0.232813,-0.134685, 0.365414,-0.107974,0.061633, 0.111359,-0.107974,-0.372542, -0.262592,0.026248,0.047682, -0.327682,-0.107974,0.107959, -0.050341,0.191801,0.101262, -0.248632,-0.107974,0.379486, -0.172400,0.111003,0.201378, 0.324182,-0.074827,0.146980, 0.312271,-0.107974,0.174077, -0.248842,-0.086035,0.364515, -0.264984,-0.107974,0.352871, 0.027221,0.221023,-0.163380, 0.037030,0.236507,-0.089715, +}; +const unsigned int h04_faces[] = { + 4, 12,13,5,1, + 5, 5,6,2,4,1, + 5, 4,17,9,12,1, + 4, 16,17,4,2, + 5, 6,3,0,16,2, + 4, 8,7,0,3, + 7, 6,5,13,10,15,8,3, + 5, 8,15,14,11,7, + 6, 11,9,17,16,0,7, + 6, 11,14,10,13,12,9, + 3, 14,15,10, +}; +const dReal h04_planes[ h04_numf * 4 ] = { + 0.903842,0.0244154,0.42717,0.353968,0.79204,0.397342,-0.463457,0.217956,0.496382,0.766976,0.406636,0.163295,-0.11569,0.975032,-0.189546,0.243324,-0.274018,0.465094,-0.841785,0.232868,-0.83085,0.164121,-0.531744,0.197127,0,-1,0,0.107974,-0.837582,0.502473,0.214421,0.243355,-0.598819,0.796079,-0.0875987,0.173963,0.289106,0.541464,0.789452,0.169241,-0.80443,0.32957,0.494242,0.35198 +}; +// h05 +const int h05_numv = 12; +const int h05_numf = 8; +const dReal h05_volu = 0.060052; +const dReal h05_pos[3] = { -0.361440,-0.882680,-0.728709 }; +const dReal h05_verts[ h05_numv * 3 ] = { + -0.444817,-0.117320,-0.271291, 0.140076,-0.117320,-0.271291, 0.136701,0.016901,0.344420, -0.031407,0.126842,-0.271291, 0.070855,0.236044,-0.052837, -0.114373,0.170111,0.116744, 0.344521,-0.117320,-0.021725, 0.298857,0.114305,0.121114, -0.444469,-0.108411,-0.271291, 0.071611,-0.117320,0.404697, -0.173458,0.135300,0.111770, -0.010820,-0.117320,0.364447, +}; +const unsigned int h05_faces[] = { + 5, 3,4,7,6,1, + 5, 6,9,11,0,1, + 4, 0,8,3,1, + 5, 5,10,11,9,2, + 4, 9,6,7,2, + 4, 7,4,5,2, + 5, 8,10,5,4,3, + 4, 0,11,10,8, +}; +const dReal h05_planes[ h05_numf * 4 ] = { + 0.679728,0.477395,-0.556833,0.190269,0,-1,0,0.11732,0,0,-1,0.271291,-0.373525,0.524676,0.76498,0.221281,0.83085,-0.164121,0.531744,0.293948,0.11362,0.877785,0.465387,0.190656,-0.485031,0.851631,-0.198669,0.177153,-0.825473,0.0322069,0.563523,0.210527 +}; +// h06 +const int h06_numv = 16; +const int h06_numf = 10; +const dReal h06_volu = 0.067458; +const dReal h06_pos[3] = { 0.885250,-0.616510,-0.689541 }; +const dReal h06_verts[ h06_numv * 3 ] = { + -0.230633,-0.043361,0.025874, -0.001305,-0.383490,-0.079103, 0.114750,-0.383490,0.055891, 0.114750,0.141993,-0.310459, 0.042572,0.157183,0.322917, 0.114750,0.349093,0.009138, 0.114750,-0.209139,-0.310459, 0.114750,-0.383490,-0.220772, 0.114750,0.143660,0.326163, -0.023969,0.120047,-0.310459, -0.230634,0.218132,-0.108639, -0.072958,0.349093,0.054965, -0.138827,-0.119612,0.199579, 0.020027,-0.383490,0.025057, -0.041036,-0.038390,0.301047, 0.114750,-0.091790,0.305977, +}; +const unsigned int h06_faces[] = { + 6, 0,10,9,6,7,1, + 4, 7,2,13,1, + 4, 13,12,0,1, + 5, 15,14,12,13,2, + 7, 7,6,3,5,8,15,2, + 5, 9,10,11,5,3, + 3, 6,9,3, + 4, 14,15,8,4, + 4, 8,5,11,4, + 6, 11,10,0,12,14,4, +}; +const dReal h06_planes[ h06_numf * 4 ] = { + -0.735479,-0.309932,-0.602505,0.167476,0,-1,0,0.38349,-0.794108,-0.585614,0.162633,0.212748,-0.239912,-0.631869,0.73701,0.255977,1,-0,0,0.11475,-0.131611,0.831909,-0.539079,0.270385,0,0,-1,0.310459,-0.0606999,-0.0852635,0.994508,0.305158,0.131611,0.831909,0.539078,0.310442,-0.793214,0.278548,0.541501,0.184874 +}; +// h07 +const int h07_numv = 18; +const int h07_numf = 11; +const dReal h07_volu = 0.100040; +const dReal h07_pos[3] = { 0.439796,-0.816642,-0.690430 }; +const dReal h07_verts[ h07_numv * 3 ] = { + -0.290584,-0.183358,-0.114083, 0.444149,-0.183358,-0.078214, -0.340199,0.147781,0.085025, -0.036530,-0.183358,0.320092, -0.047944,-0.135617,0.341515, 0.306627,0.080520,0.200468, -0.322367,0.157429,0.123774, 0.465481,-0.183358,0.025945, 0.214820,0.156771,0.026763, -0.144796,0.105827,-0.309570, 0.159858,-0.183358,-0.309570, -0.190496,-0.183358,-0.309570, -0.155412,0.298926,0.091748, -0.184960,0.308361,-0.168615, -0.134267,0.332806,-0.106403, 0.196572,0.110989,0.274357, -0.157992,0.325768,-0.131745, -0.142419,0.333870,-0.095406, +}; +const unsigned int h07_faces[] = { + 7, 10,9,13,16,14,8,1, + 4, 8,5,7,1, + 6, 7,3,0,11,10,1, + 6, 6,12,17,16,13,2, + 5, 13,9,11,0,2, + 5, 0,3,4,6,2, + 5, 7,5,15,4,3, + 4, 15,12,6,4, + 6, 8,14,17,12,15,5, + 3, 10,11,9, + 3, 16,17,14, +}; +const dReal h07_planes[ h07_numf * 4 ] = { + 0.525603,0.55372,-0.645861,0.182432,0.794108,0.585614,-0.162633,0.258045,0,-1,0,0.183358,-0.632147,0.768422,0.0995875,0.337082,-0.881437,0.139289,-0.451296,0.282078,-0.79204,-0.397342,0.463457,0.250137,0.488522,-0.25733,0.833743,0.296212,-0.212159,0.447016,0.869003,0.246326,0.381225,0.903643,0.195187,0.228784,-1.58443e-16,2.50379e-17,-1,0.30957,-0.105626,0.979232,-0.173057,0.35849 +}; +// h08 +const int h08_numv = 22; +const int h08_numf = 13; +const dReal h08_volu = 0.095115; +const dReal h08_pos[3] = { 0.582909,-0.382310,-0.514785 }; +const dReal h08_verts[ h08_numv * 3 ] = { + 0.039696,0.333763,-0.114253, -0.040720,0.073975,-0.343437, -0.281418,0.194479,0.107755, 0.071707,-0.016068,-0.283395, 0.071707,-0.277561,-0.148882, 0.163514,-0.353812,0.024824, 0.072380,0.310495,-0.182078, 0.344912,-0.077018,0.148161, 0.229382,0.114893,-0.119790, 0.056106,0.251986,0.176207, 0.261304,-0.272590,0.126291, 0.053460,-0.323343,0.098712, -0.280479,0.198156,0.096008, -0.285532,-0.100462,-0.271051, 0.094134,-0.250952,0.200497, -0.277380,-0.101526,-0.282048, -0.010614,0.002589,0.315575, -0.026292,0.211375,0.249710, 0.146296,0.123570,0.286302, 0.056041,0.119791,0.321947, -0.290185,0.102104,0.127875, -0.298525,-0.135406,-0.083897, +}; +const unsigned int h08_faces[] = { + 6, 15,13,12,0,6,1, + 4, 6,8,3,1, + 4, 3,4,15,1, + 5, 20,16,19,17,2, + 5, 17,9,0,12,2, + 5, 12,13,21,20,2, + 6, 8,7,10,5,4,3, + 6, 5,11,21,13,15,4, + 4, 10,14,11,5, + 6, 0,9,18,7,8,6, + 6, 18,19,16,14,10,7, + 4, 17,19,18,9, + 5, 14,16,20,21,11, +}; +const dReal h08_planes[ h08_numf * 4 ] = { + -0.595386,0.627237,-0.502085,0.243079,0.598205,0.236327,-0.765702,0.256094,0.107916,-0.45476,-0.884052,0.265581,-0.484779,0.229821,0.843903,0.272055,-0.215549,0.936705,0.275902,0.272559,-0.995125,0.0827659,-0.053632,0.290362,0.793214,-0.278548,-0.541501,0.214813,-0.381225,-0.903643,-0.195187,0.252539,0.131611,-0.83191,0.539077,0.329242,0.750032,0.646492,0.139642,0.229594,0.364346,-0.255892,0.895417,0.278042,0.228161,0.721101,0.654183,0.30978,-0.595564,-0.522855,0.609857,0.197424 +}; +// h09 +const int h09_numv = 16; +const int h09_numf = 10; +const dReal h09_volu = 0.059582; +const dReal h09_pos[3] = { 0.627742,-0.657906,-0.898694 }; +const dReal h09_verts[ h09_numv * 3 ] = { + -0.028087,-0.342094,-0.101306, 0.372258,-0.342094,-0.101306, 0.233539,0.161443,-0.101306, 0.372258,-0.167743,-0.101306, 0.256203,-0.342094,0.130050, 0.372258,-0.342094,-0.011619, -0.322212,0.174071,0.101861, 0.026875,-0.001965,0.235027, -0.098160,0.388838,-0.101306, 0.026875,0.259528,0.100514, -0.332742,-0.052909,-0.101306, -0.085552,0.349571,0.040471, -0.393762,0.195327,-0.101306, -0.372906,0.149626,0.039648, -0.378114,0.165155,0.019276, -0.345937,0.167033,0.076519, +}; +const unsigned int h09_faces[] = { + 7, 0,10,12,8,2,3,1, + 3, 3,5,1, + 4, 5,4,0,1, + 4, 8,11,9,2, + 6, 9,7,4,5,3,2, + 7, 7,6,15,13,10,0,4, + 6, 11,8,12,14,15,6, + 4, 7,9,11,6, + 4, 13,14,12,10, + 3, 15,14,13, +}; +const dReal h09_planes[ h09_numf * 4 ] = { + 0,0,-1,0.101306,1,0,0,0.372258,0,-1,0,0.342094,0.55667,0.812007,0.175394,0.243329,0.735479,0.309932,0.602505,0.160762,-0.525603,-0.55372,0.645861,0.138757,-0.527367,0.805591,0.270012,0.337657,-0.107916,0.45476,0.884052,0.203982,-0.968964,-0.238186,0.066148,0.328316,-0.830322,0.320028,0.456232,0.375605 +}; +// h10 +const int h10_numv = 18; +const int h10_numf = 11; +const dReal h10_volu = 0.082139; +const dReal h10_pos[3] = { -0.348593,-0.203405,-0.827574 }; +const dReal h10_verts[ h10_numv * 3 ] = { + 0.351136,-0.038001,-0.172426, 0.318157,0.110282,-0.172426, -0.183035,-0.085679,0.342730, -0.360226,-0.040315,-0.172426, 0.190400,0.248876,-0.172426, 0.296736,0.161596,-0.025823, 0.116500,-0.318307,0.010327, -0.282440,-0.040315,0.207384, -0.155944,0.334915,-0.140051, -0.055732,-0.013566,0.359774, 0.296359,0.169251,-0.045236, -0.155944,0.347503,-0.172426, 0.079072,-0.318307,-0.172426, 0.043711,-0.022416,0.312735, 0.326723,-0.161591,-0.172426, 0.179366,-0.279946,0.021290, 0.100706,0.105108,0.263039, 0.042697,0.111008,0.290353, +}; +const unsigned int h10_faces[] = { + 4, 10,5,0,1, + 7, 0,14,12,3,11,4,1, + 3, 4,10,1, + 5, 6,15,13,9,2, + 5, 9,17,8,7,2, + 5, 7,3,12,6,2, + 4, 7,8,11,3, + 7, 11,8,17,16,5,10,4, + 6, 16,13,15,14,0,5, + 4, 12,14,15,6, + 4, 13,16,17,9, +}; +const dReal h10_planes[ h10_numf * 4 ] = { + 0.973989,0.216619,0.066491,0.322306,-0,0,-1,0.172426,0.722579,0.66608,-0.18498,0.335245,0.27553,-0.653111,0.705358,0.247274,-0.46199,0.681388,0.567693,0.220745,-0.531559,-0.839996,0.108863,0.206575,-0.870584,0.458578,0.178296,0.264376,0.256529,0.900843,0.350248,0.212649,0.830851,-0.164118,0.531744,0.206291,0.53156,-0.839996,-0.108863,0.328179,0.433652,0.152283,0.888119,0.293288 +}; +// h11 +const int h11_numv = 18; +const int h11_numf = 11; +const dReal h11_volu = 0.058443; +const dReal h11_pos[3] = { -0.902277,0.042835,-0.283464 }; +const dReal h11_verts[ h11_numv * 3 ] = { + 0.065494,-0.341848,-0.057123, 0.194525,0.153131,-0.091230, -0.097723,-0.350111,0.163707, 0.194525,-0.144351,0.061796, 0.065493,0.122511,-0.295991, -0.097723,-0.183345,0.306682, -0.097723,0.152153,0.335446, -0.097723,-0.428001,-0.052654, 0.107269,-0.156775,0.196004, -0.097723,0.199975,-0.375687, 0.037223,0.358435,0.033582, -0.097723,0.406471,-0.057020, -0.067761,-0.416891,-0.051054, -0.040073,-0.340339,0.134552, 0.056142,-0.350169,-0.055126, 0.004412,-0.376327,-0.008857, 0.025793,-0.108438,0.290488, -0.096343,0.152412,0.335215, +}; +const unsigned int h11_faces[] = { + 5, 4,9,11,10,1, + 6, 10,17,16,8,3,1, + 4, 3,0,4,1, + 5, 7,12,15,13,2, + 5, 13,8,16,5,2, + 6, 5,6,11,9,7,2, + 6, 8,13,15,14,0,3, + 6, 0,14,12,7,9,4, + 4, 16,17,6,5, + 4, 17,10,11,6, + 3, 14,15,12, +}; +const dReal h11_planes[ h11_numf * 4 ] = { + 0.553062,0.69918,-0.453068,0.255983,0.793213,0.278551,0.5415,0.147553,0.835369,-0.251444,-0.48881,0.168589,0.314046,-0.893286,0.321583,0.334705,0.443092,-0.583496,0.68059,0.272405,-1,0,0,0.0977235,0.66062,-0.653782,0.368986,0.245683,0.212159,-0.447018,-0.869002,0.216347,0.179474,-0.0840327,0.980167,0.298468,-0.066237,0.837367,0.542613,0.315898,0.474713,-0.879502,0.0335246,0.332778 +}; +// h12 +const int h12_numv = 22; +const int h12_numf = 13; +const dReal h12_volu = 0.106128; +const dReal h12_pos[3] = { -0.838340,-0.076123,-0.758521 }; +const dReal h12_verts[ h12_numv * 3 ] = { + 0.207307,-0.167597,0.138331, 0.129521,-0.167597,-0.241479, -0.161660,0.318933,0.099371, 0.312802,0.272050,-0.241479, -0.161660,-0.330252,0.400013, -0.074718,-0.363580,0.273473, 0.001556,0.241469,0.179066, -0.161660,-0.309043,0.422403, -0.161660,-0.407271,0.186072, -0.161660,-0.351860,-0.241479, -0.007794,-0.231211,0.419931, 0.001557,-0.222890,0.417934, 0.309084,0.269226,-0.210613, -0.161660,0.422173,-0.241479, 0.072963,0.309250,0.057032, -0.161660,0.392766,-0.014569, -0.131698,-0.297933,0.424004, -0.161660,-0.327468,0.404309, 0.114980,-0.245845,0.291286, 0.009169,-0.227496,0.411012, 0.333803,0.207633,-0.209104, 0.333803,0.220221,-0.241479, +}; +const unsigned int h12_faces[] = { + 6, 0,18,5,8,9,1, + 5, 9,13,3,21,1, + 4, 21,20,0,1, + 4, 6,14,15,2, + 8, 15,13,9,8,4,17,7,2, + 6, 7,16,10,11,6,2, + 4, 12,20,21,3, + 5, 13,15,14,12,3, + 7, 5,18,19,10,16,17,4, + 3, 8,5,4, + 8, 11,19,18,0,20,12,14,6, + 3, 17,16,7, + 3, 19,11,10, +}; +const dReal h12_planes[ h12_numf * 4 ] = { + 0.53156,-0.839996,-0.108863,0.235917,0,0,-1,0.241479,0.870584,-0.458578,-0.178296,0.23267,0.131611,0.83191,0.539077,0.297616,-1,0,0,0.16166,-0.212159,0.447018,0.869002,0.263219,0.917084,0.371595,0.144476,0.353071,0.299389,0.946218,0.12263,0.321456,0.425203,-0.759567,0.4922,0.378997,0.131169,-0.932758,0.335793,0.421162,0.735479,0.309932,0.602505,0.183872,0.216442,-0.684063,0.696571,0.470649,0.494656,-0.366713,0.787932,0.411811 +}; +// h13 +const int h13_numv = 22; +const int h13_numf = 13; +const dReal h13_volu = 0.080573; +const dReal h13_pos[3] = { -0.579196,0.019592,-0.524795 }; +const dReal h13_verts[ h13_numv * 3 ] = { + -0.186181,0.213535,-0.176694, 0.154423,-0.034973,0.266436, 0.105286,0.335497,0.021835, 0.055737,-0.302317,0.075780, -0.128556,-0.121108,0.303127, 0.252786,0.058395,0.170378, -0.257588,0.145754,-0.054660, 0.097995,0.334920,0.024505, 0.093556,-0.038093,0.287537, 0.074659,0.111918,-0.442830, -0.051837,-0.263312,-0.095394, -0.257587,-0.318605,0.184208, 0.273300,-0.111989,-0.012426, 0.049940,0.173511,-0.444339, -0.026589,0.239892,0.129875, -0.128556,0.176374,0.150102, 0.038054,-0.313581,0.048402, 0.047568,-0.308676,0.039952, -0.249976,-0.323211,0.177286, -0.144164,-0.341560,0.057561, 0.174871,-0.236563,0.056995, 0.154423,-0.246614,0.084987, +}; +const unsigned int h13_faces[] = { + 5, 21,20,12,5,1, + 6, 5,2,7,14,8,1, + 7, 8,4,11,18,3,21,1, + 5, 5,12,9,13,2, + 4, 13,0,7,2, + 5, 16,17,20,21,3, + 4, 18,19,16,3, + 4, 8,14,15,4, + 4, 15,6,11,4, + 5, 15,14,7,0,6, + 8, 0,13,9,10,19,18,11,6, + 5, 12,20,17,10,9, + 4, 17,16,19,10, +}; +const dReal h13_planes[ h13_numf * 4 ] = { + 0.805487,-0.385719,0.449901,0.257745,0.24653,0.556538,0.793403,0.229997,0.284317,-0.624016,0.727852,0.259655,0.904761,0.357436,-0.231619,0.21012,-0.184883,0.934913,-0.302912,0.287581,0.488006,-0.871758,0.0434537,0.29404,0.159459,-0.944944,0.285751,0.316215,-0.107916,0.45476,0.884052,0.226779,-0.835369,0.251444,0.48881,0.225111,-0.493234,0.850287,0.183662,0.240945,-0.735479,-0.309932,-0.602505,0.17721,0.46199,-0.681388,-0.567693,0.209624,0.120149,-0.911352,-0.393704,0.271298 +}; +// h14 +const int h14_numv = 24; +const int h14_numf = 14; +const dReal h14_volu = 0.092625; +const dReal h14_pos[3] = { 0.037294,-0.326434,-0.629340 }; +const dReal h14_verts[ h14_numv * 3 ] = { + 0.244511,-0.164440,-0.192835, -0.034752,0.085028,-0.370660, -0.037506,-0.162763,0.334991, -0.206521,-0.156917,-0.176944, 0.151340,0.267484,0.166363, 0.265137,0.142280,0.210563, 0.260084,-0.156338,-0.156496, -0.285181,0.228137,0.064806, 0.037588,-0.329085,0.107655, -0.342176,0.100614,0.114501, 0.027780,-0.344569,0.033990, -0.089151,0.284625,-0.224057, -0.059164,-0.038562,-0.370660, 0.065038,-0.057055,-0.370660, 0.036995,0.362842,0.085406, 0.062303,-0.342427,0.023935, 0.217542,-0.181847,-0.229705, 0.212334,-0.166317,-0.250077, 0.224212,0.171137,0.236256, 0.080136,-0.332779,0.062684, 0.146779,-0.002116,0.303773, 0.264198,0.138603,0.222310, 0.247091,-0.191282,0.030659, 0.255431,0.046228,0.242430, +}; +const unsigned int h14_faces[] = { + 6, 12,3,9,7,11,1, + 9, 11,14,4,5,6,0,17,13,1, + 3, 13,12,1, + 6, 8,19,22,23,20,2, + 7, 20,18,4,14,7,9,2, + 5, 9,3,10,8,2, + 7, 12,13,17,16,15,10,3, + 4, 18,21,5,4, + 5, 21,23,22,6,5, + 6, 22,19,15,16,0,6, + 3, 14,11,7, + 4, 10,15,19,8, + 3, 17,0,16, + 4, 20,23,21,18, +}; +const dReal h14_planes[ h14_numf * 4 ] = { + -0.830851,0.164118,-0.531744,0.239924,0.738081,0.518376,-0.431883,0.178509,0,0,-1,0.37066,0.582463,-0.552262,0.596442,0.267845,-0.240814,0.443968,0.863077,0.225893,-0.726907,-0.646142,0.23261,0.210353,-0.119725,-0.804079,-0.582343,0.253941,0.662611,0.698059,0.271405,0.33215,0.995125,-0.0827659,0.053632,0.263362,0.632147,-0.768422,-0.0995875,0.300129,-0.380333,0.921564,-0.0778922,0.313659,0.11569,-0.975032,0.189546,0.345622,0.830322,-0.320028,-0.456232,0.343625,0.433652,0.152283,0.888119,0.333115 +}; +// h15 +const int h15_numv = 16; +const int h15_numf = 10; +const dReal h15_volu = 0.047330; +const dReal h15_pos[3] = { 0.221274,0.213040,-0.893886 }; +const dReal h15_verts[ h15_numv * 3 ] = { + -0.273508,-0.247194,0.021075, -0.214206,0.249387,-0.106114, -0.394680,0.092354,-0.106114, 0.468273,-0.026582,-0.106114, -0.379467,-0.167569,-0.106114, -0.251710,-0.306163,-0.106114, -0.130453,-0.090301,0.212222, 0.215618,0.100683,0.162583, 0.108399,0.111587,0.216806, 0.543500,0.089559,-0.106114, 0.556660,0.080198,-0.106114, 0.496376,0.018888,-0.040935, 0.000444,-0.021162,0.246756, 0.322381,0.024676,0.097420, 0.541635,0.087788,-0.096097, 0.556215,0.081512,-0.106114, +}; +const unsigned int h15_faces[] = { + 5, 2,6,12,8,1, + 5, 8,7,14,9,1, + 8, 9,15,10,3,5,4,2,1, + 4, 4,0,6,2, + 3, 10,11,3, + 7, 11,13,12,6,0,5,3, + 3, 5,0,4, + 6, 13,11,10,15,14,7, + 4, 8,12,13,7, + 3, 14,15,9, +}; +const dReal h15_planes[ h15_numf * 4 ] = { + -0.469359,0.539421,0.69909,0.160881,0.201934,0.957321,0.206781,0.173545,0,0,-1,0.106114,-0.781405,-0.0457332,0.622346,0.238142,0.765488,-0.633631,0.11198,0.363418,0.324513,-0.835692,0.443069,0.127159,-0.722579,-0.66608,0.18498,0.366181,0.613049,0.207593,0.762283,0.27702,0.433652,-0.152283,0.888119,0.222564,0.518906,0.82,0.241527,0.329834 +}; +// h16 +const int h16_numv = 24; +const int h16_numf = 14; +const dReal h16_volu = 0.105221; +const dReal h16_pos[3] = { -0.230932,0.200668,-0.677519 }; +const dReal h16_verts[ h16_numv * 3 ] = { + 0.072738,-0.155197,-0.322481, 0.178698,-0.234822,-0.195293, -0.074965,-0.293065,0.140297, -0.273605,-0.069158,-0.290106, -0.242978,0.154421,0.174559, 0.321753,-0.077929,-0.004145, 0.331656,-0.130769,0.158361, -0.234281,0.105965,-0.322481, 0.057526,0.104725,-0.322481, -0.070916,0.357222,-0.022485, 0.179075,-0.242477,-0.175878, 0.305221,-0.164260,0.133584, 0.289986,0.017556,0.326217, -0.205442,0.224195,0.181431, -0.273605,-0.056571,-0.322481, -0.016955,-0.298965,0.112984, -0.012263,0.183238,-0.322481, -0.122870,0.351842,-0.002882, -0.294606,-0.004741,-0.322481, -0.298324,-0.007565,-0.291615, 0.354068,-0.072999,0.110530, 0.306083,0.044665,0.287425, 0.072223,0.006072,0.394149, -0.095479,-0.122681,0.323101, +}; +const unsigned int h16_faces[] = { + 7, 10,15,2,3,14,0,1, + 4, 0,8,5,1, + 6, 5,20,6,11,10,1, + 7, 15,11,6,12,22,23,2, + 5, 23,4,19,3,2, + 4, 19,18,14,3, + 4, 23,22,13,4, + 6, 13,17,7,18,19,4, + 6, 8,16,9,21,20,5, + 4, 20,21,12,6, + 4, 17,9,16,7, + 6, 16,8,0,14,18,7, + 6, 17,13,22,12,21,9, + 3, 11,15,10, +}; +const dReal h16_planes[ h16_numf * 4 ] = { + -0.256529,-0.900843,-0.350248,0.234098,0.781405,0.0457332,-0.622346,0.250435,0.837582,-0.502473,-0.214421,0.309541,0.247318,-0.694798,0.675344,0.27983,-0.904761,-0.357436,0.231619,0.205073,-0.917084,-0.371595,-0.144476,0.318531,-0.501077,0.186311,0.84511,0.298042,-0.876412,0.477567,-0.0618929,0.275891,0.726908,0.64614,-0.232611,0.184496,0.941719,-0.0992098,0.32144,0.376203,-0.28037,0.805551,-0.521997,0.31938,0,0,-1,0.322481,0.148211,0.780702,0.607074,0.254723,0.380333,-0.921564,0.0778922,0.277867 +}; +// h17 +const int h17_numv = 26; +const int h17_numf = 15; +const dReal h17_volu = 0.108916; +const dReal h17_pos[3] = { 0.335926,-0.094685,-0.795267 }; +const dReal h17_verts[ h17_numv * 3 ] = { + 0.319363,0.022871,0.098404, -0.388160,0.060531,-0.077543, 0.353621,0.281144,-0.204733, 0.207729,0.332401,-0.001198, 0.193656,-0.174383,-0.204733, -0.101947,-0.367894,-0.204733, -0.366362,0.001562,-0.204733, -0.261637,0.131093,0.251333, 0.356211,0.239639,0.008049, -0.212790,0.222354,0.228279, -0.038549,-0.388087,0.009431, -0.033495,-0.089469,0.376490, -0.054122,-0.396189,-0.026907, 0.286680,0.046139,0.166229, 0.206263,-0.213650,-0.062955, -0.030396,-0.389151,-0.001566, -0.086298,-0.398066,-0.084150, -0.233594,-0.288804,-0.204733, -0.114208,0.286563,0.148137, -0.245105,0.217424,0.113603, 0.376946,0.258587,-0.036908, 0.381724,0.326613,-0.139554, -0.147292,0.035735,0.332290, -0.235202,0.164584,0.276109, -0.333383,-0.146721,-0.204733, -0.387783,0.052876,-0.058130, +}; +const unsigned int h17_faces[] = { + 4, 6,24,25,1, + 6, 25,7,23,9,19,1, + 7, 19,18,3,21,2,6,1, + 6, 21,20,0,14,4,2, + 6, 4,5,17,24,6,2, + 4, 8,20,21,3, + 8, 18,9,23,22,11,13,8,3, + 6, 14,15,12,16,5,4, + 3, 16,17,5, + 3, 22,23,7, + 9, 25,24,17,16,12,10,11,22,7, + 4, 13,0,20,8, + 3, 18,19,9, + 6, 15,14,0,13,11,10, + 3, 12,15,10, +}; +const dReal h17_planes[ h17_numf * 4 ] = { + -0.973989,-0.216619,-0.066491,0.370108,-0.837582,0.502473,0.214421,0.338905,-0.324513,0.835692,-0.443069,0.210905,0.929286,-0.326334,-0.173016,0.27229,-4.07036e-16,-9.05266e-17,-1,0.204733,0.43192,0.742413,0.512121,0.335888,0.289106,0.541464,0.789452,0.239093,0.527367,-0.805591,-0.270012,0.297889,-0.509459,-0.848006,-0.146072,0.39382,-0.621965,-0.0849382,0.778424,0.347238,-0.738081,-0.518376,0.431883,0.2337,0.903842,0.0244154,0.42717,0.331247,-0.485104,0.868795,0.0993492,0.319085,0.595386,-0.627237,0.502085,0.225206,0.105626,-0.979232,0.173057,0.377587 +}; +// h18 +const int h18_numv = 18; +const int h18_numf = 11; +const dReal h18_volu = 0.081077; +const dReal h18_pos[3] = { 0.870716,-0.039183,-0.528244 }; +const dReal h18_verts[ h18_numv * 3 ] = { + -0.102809,0.184137,0.110997, 0.129284,0.393063,-0.471756, 0.129284,0.325763,0.151471, 0.124364,0.395398,-0.471756, -0.058424,-0.228234,-0.106332, 0.057106,-0.420144,0.161620, -0.248110,-0.009363,-0.100794, -0.231701,-0.091141,0.189666, 0.129284,-0.228234,-0.152159, 0.129284,-0.433667,0.164866, 0.129284,-0.166980,0.404939, -0.141510,-0.219557,0.299761, 0.129284,0.397646,-0.471756, -0.215427,-0.032631,-0.168619, 0.102902,0.388713,-0.463077, 0.129284,0.404181,-0.453605, -0.178579,0.184137,-0.258974, -0.157844,0.203085,-0.303931, +}; +const unsigned int h18_faces[] = { + 7, 8,4,13,17,14,3,1, + 3, 3,12,1, + 7, 12,15,2,10,9,8,1, + 6, 15,14,17,16,0,2, + 5, 0,7,11,10,2, + 4, 14,15,12,3, + 6, 5,11,7,6,13,4, + 4, 8,9,5,4, + 4, 9,10,11,5, + 4, 16,17,13,6, + 4, 7,0,16,6, +}; +const dReal h18_planes[ h18_numf * 4 ] = { + -0.212159,-0.447018,-0.869002,0.206822,0,-0,-1,0.471756,1,0,-0,0.129284,-0.53156,0.839996,0.108863,0.221407,-0.398278,0.419585,0.815673,0.208745,-0.395039,0.86436,-0.311169,0.439434,-0.750032,-0.646492,-0.139642,0.206219,-0.131611,-0.831909,-0.539078,0.25488,-0.156805,-0.660773,0.734024,0.387299,-0.903842,-0.0244154,-0.42717,0.267537,-0.870583,0.45858,0.178295,0.193736 +}; +// h19 +const int h19_numv = 16; +const int h19_numf = 10; +const dReal h19_volu = 0.057269; +const dReal h19_pos[3] = { 0.808301,-0.134672,-0.879100 }; +const dReal h19_verts[ h19_numv * 3 ] = { + 0.052980,-0.361791,-0.120900, 0.191699,-0.339845,-0.120900, -0.030366,0.427910,-0.120900, 0.191699,0.488552,-0.120900, -0.118754,0.321131,-0.120900, 0.191699,-0.132745,0.198697, 0.186779,0.490887,-0.120900, 0.003992,-0.132745,0.244524, -0.153012,0.062857,0.182237, -0.153685,-0.263706,0.080920, -0.278719,-0.134396,-0.120900, -0.266112,-0.173663,0.020877, -0.090651,0.366600,-0.055721, -0.095429,0.298574,0.046924, -0.012422,0.433588,-0.120900, 0.165316,0.484202,-0.112221, +}; +const unsigned int h19_faces[] = { + 5, 5,7,9,0,1, + 8, 0,10,4,2,14,6,3,1, + 3, 3,5,1, + 5, 12,13,15,14,2, + 3, 4,12,2, + 7, 6,15,13,8,7,5,3, + 6, 10,11,8,13,12,4, + 3, 14,15,6, + 4, 8,11,9,7, + 4, 11,10,0,9, +}; +const dReal h19_planes[ h19_numf * 4 ] = { + 0.131611,-0.831909,0.539079,0.242775,-4.33777e-17,-9.13967e-17,-1,0.1209,1,0,0,0.191699,-0.256638,0.811103,0.525594,0.291328,-0.765488,0.633631,-0.11198,0.307921,0.212159,0.447018,0.869002,0.153999,-0.929286,0.326334,0.173016,0.194234,-0.275992,0.959497,0.0565231,0.412621,-0.598205,-0.236327,0.765702,0.216216,-0.55667,-0.812007,-0.175394,0.28549 +}; +// h20 +const int h20_numv = 30; +const int h20_numf = 17; +const dReal h20_volu = 0.107509; +const dReal h20_pos[3] = { 0.402458,0.140897,-0.404353 }; +const dReal h20_verts[ h20_numv * 3 ] = { + -0.100408,0.282050,-0.142576, 0.065914,0.236686,0.319509, -0.125820,0.203801,0.337118, 0.365449,0.004057,-0.012894, 0.056400,0.231781,0.327959, 0.034434,0.172826,-0.326950, -0.072786,0.183730,-0.272727, -0.247229,0.030507,0.295489, 0.136546,-0.281738,0.161639, 0.289679,0.004057,-0.382865, 0.141197,0.096819,-0.392113, -0.100966,-0.328728,-0.002677, -0.140953,-0.296194,0.011269, -0.180740,0.050981,-0.242777, -0.253566,0.024492,0.286206, -0.033492,0.282050,0.184162, 0.236557,-0.271221,0.065774, 0.154159,-0.311832,0.139278, -0.301735,-0.070997,-0.114805, -0.213824,-0.199847,-0.058625, -0.310451,0.100424,0.233258, -0.241567,0.131963,0.326249, -0.062874,0.247232,-0.227963, -0.327109,0.098788,0.164544, -0.279322,-0.013228,-0.162635, -0.343405,0.077327,0.053051, -0.313038,0.127071,0.015013, -0.327307,0.104436,0.014259, -0.100027,-0.325051,-0.014425, 0.220147,-0.189444,-0.224685, +}; +const unsigned int h20_faces[] = { + 8, 3,9,10,5,22,0,15,1, + 4, 15,2,4,1, + 6, 4,8,17,16,3,1, + 7, 15,0,26,23,20,21,2, + 5, 21,7,8,4,2, + 4, 16,29,9,3, + 3, 6,22,5, + 4, 10,13,6,5, + 7, 13,24,27,26,0,22,6, + 6, 14,12,11,17,8,7, + 4, 21,20,14,7, + 8, 29,28,19,18,24,13,10,9, + 5, 28,29,16,17,11, + 4, 12,19,28,11, + 7, 14,20,23,25,18,19,12, + 4, 25,27,24,18, + 4, 26,27,25,23, +}; +const dReal h20_planes[ h20_numf * 4 ] = { + 0.531561,0.839995,-0.108864,0.199069,-0.120149,0.911351,0.393704,0.333576,0.693474,-0.121763,0.710118,0.243778,-0.53156,0.839996,0.108863,0.274773,0.0929092,-0.293639,0.951391,0.249197,0.870583,-0.45858,-0.178295,0.318592,-0.320422,0.578682,-0.749971,0.334181,-0.433652,0.152283,-0.888119,0.301757,-0.720506,0.471135,-0.508826,0.277775,-0.322183,-0.678839,0.659831,0.253916,-0.770892,-0.145024,0.620237,0.369436,-0.289106,-0.541464,-0.789452,0.21631,0.215549,-0.936705,-0.275902,0.286897,-0.662611,-0.698059,-0.271405,0.297099,-0.847832,-0.483811,0.217045,0.265252,-0.941719,0.0992098,-0.32144,0.314009,-0.846082,0.532633,0.0211314,0.332856 +}; +// h21 +const int h21_numv = 20; +const int h21_numf = 12; +const dReal h21_volu = 0.107886; +const dReal h21_pos[3] = { -0.701261,0.516623,-0.734170 }; +const dReal h21_verts[ h21_numv * 3 ] = { + 0.220060,-0.162111,0.233880, -0.071194,0.139979,-0.265830, 0.326350,0.086982,0.076961, 0.175723,-0.320696,-0.265830, -0.194481,0.286846,0.262411, 0.236048,-0.209990,-0.265830, -0.298739,0.200252,0.166960, 0.264887,-0.091760,0.238083, 0.347459,0.035887,0.053770, -0.298739,0.031984,-0.265830, -0.298739,-0.199980,-0.038920, -0.298739,-0.170573,-0.265830, -0.064116,-0.283496,0.032682, 0.172005,-0.323520,-0.234964, 0.067327,0.317485,0.021628, 0.227351,-0.161534,0.231210, 0.015724,0.286846,0.365050, -0.065674,0.367082,0.311546, 0.153155,0.171345,0.362933, 0.139764,0.180879,0.371107, +}; +const unsigned int h21_faces[] = { + 6, 9,6,4,17,14,1, + 5, 14,2,8,5,1, + 5, 5,3,11,9,1, + 4, 18,7,8,2, + 6, 14,17,16,19,18,2, + 6, 5,8,7,15,13,3, + 5, 13,12,10,11,3, + 3, 16,17,4, + 7, 6,10,12,0,19,16,4, + 4, 9,11,10,6, + 5, 18,19,0,15,7, + 4, 13,15,0,12, +}; +const dReal h21_planes[ h21_numf * 4 ] = { + -0.404539,0.852363,-0.331398,0.236209,0.595564,0.522855,-0.609857,0.192906,-1.16998e-16,-0,-1,0.26583,0.864691,0.138024,0.482969,0.331367,0.636259,0.754085,0.162882,0.285771,0.876412,-0.477567,0.0618929,0.290707,-0.299389,-0.946218,-0.12263,0.283437,-0.433652,0.152285,0.888119,0.361071,-0.398278,-0.419584,0.815674,0.171144,-1,0,0,0.298739,0.349687,-0.276295,0.895198,0.331112,0.184883,-0.934913,0.302912,0.26309 +}; +// h22 +const int h22_numv = 14; +const int h22_numf = 9; +const dReal h22_volu = 0.065276; +const dReal h22_pos[3] = { -0.383570,0.726249,-0.891974 }; +const dReal h22_verts[ h22_numv * 3 ] = { + 0.403835,0.273751,-0.108026, -0.250365,0.107859,0.179431, -0.224444,0.273751,-0.108026, 0.311631,-0.076337,-0.108026, -0.388885,-0.069647,-0.108026, 0.349740,0.107545,0.033083, 0.400677,0.273751,-0.064850, -0.189576,0.273751,0.079255, 0.008658,-0.122644,0.234765, -0.081643,-0.419616,-0.108026, 0.029768,-0.173739,0.211573, 0.125191,-0.098941,0.194122, 0.140375,-0.342343,-0.108026, 0.081722,-0.168359,0.191970, +}; +const unsigned int h22_faces[] = { + 6, 8,11,5,6,7,1, + 4, 7,2,4,1, + 5, 4,9,10,8,1, + 4, 7,6,0,2, + 6, 0,3,12,9,4,2, + 5, 5,11,13,12,3, + 4, 0,6,5,3, + 4, 10,13,11,8, + 4, 12,13,10,9, +}; +const dReal h22_planes[ h22_numf * 4 ] = { + 0.212159,0.447018,0.869002,0.151024,-0.889469,0.425934,0.165603,0.298347,-0.595564,-0.522855,0.609857,0.202141,0,1,0,0.273751,0,0,-1,0.108026,0.758508,-0.488331,0.431507,0.227038,0.964613,-0.254054,0.0705546,0.312375,0.364346,-0.255892,0.895417,0.244751,0.28037,-0.805551,0.521997,0.258742 +}; +// h23 +const int h23_numv = 16; +const int h23_numf = 10; +const dReal h23_volu = 0.051599; +const dReal h23_pos[3] = { -0.800668,0.450995,-0.472891 }; +const dReal h23_verts[ h23_numv * 3 ] = { + 0.035291,-0.217869,-0.228598, -0.199332,-0.134353,-0.300199, -0.199332,0.265879,-0.094319, -0.095074,0.352473,0.001132, -0.199332,-0.001690,0.132407, -0.199332,-0.208186,-0.186260, 0.189555,0.299490,0.167366, -0.199332,0.309177,-0.027504, -0.064385,-0.049725,0.223009, -0.036116,-0.285650,-0.106564, 0.116475,0.143410,0.211970, 0.319467,-0.096483,-0.027399, 0.092916,-0.255030,0.098198, 0.194883,-0.191512,0.077971, 0.239171,0.246507,0.109828, 0.115131,0.352473,0.103771, +}; +const unsigned int h23_faces[] = { + 4, 0,9,5,1, + 5, 5,4,7,2,1, + 7, 2,3,15,14,11,0,1, + 3, 7,3,2, + 7, 7,4,8,10,6,15,3, + 5, 5,9,12,8,4, + 3, 14,15,6, + 5, 10,13,11,14,6, + 4, 12,13,10,8, + 5, 0,11,13,12,9, +}; +const dReal h23_planes[ h23_numf * 4 ] = { + -0.131611,-0.83191,-0.539077,0.299835,-1,6.69634e-17,-4.33923e-17,0.199332,0.398278,0.419584,-0.815674,0.109103,-0.195313,0.823048,-0.533334,0.308068,-0.398278,0.419585,0.815673,0.186681,-0.553062,-0.69918,0.453068,0.171414,0.647541,0.750402,-0.132616,0.325287,0.693474,-0.121763,0.710118,0.213834,0.349687,-0.276295,0.895198,0.190861,0.493234,-0.850287,-0.183662,0.244643 +}; +// h24 +const int h24_numv = 12; +const int h24_numf = 8; +const dReal h24_volu = 0.050770; +const dReal h24_pos[3] = { -0.845297,0.862613,-0.757515 }; +const dReal h24_verts[ h24_numv * 3 ] = { + 0.078361,0.021092,0.334891, -0.154703,-0.145738,0.190305, -0.154703,0.137387,-0.242485, -0.154703,-0.314006,-0.242485, 0.072842,-0.206011,-0.242485, 0.237283,0.137387,-0.242485, -0.154703,-0.102441,0.257120, -0.050445,-0.059144,0.285756, 0.211362,-0.028505,0.044972, -0.154703,0.137387,0.380489, 0.272151,0.137387,-0.055204, 0.060041,0.137387,0.393596, +}; +const unsigned int h24_faces[] = { + 3, 7,6,1, + 5, 6,9,2,3,1, + 6, 3,4,8,0,7,1, + 4, 5,4,3,2, + 5, 9,11,10,5,2, + 4, 5,10,8,4, + 5, 7,0,11,9,6, + 4, 10,11,0,8, +}; +const dReal h24_planes[ h24_numf * 4 ] = { + 0.195313,-0.823048,0.533334,0.19123,-1,0,0,0.154703,0.404539,-0.852363,0.331398,0.124705,0,0,-1,0.242485,0,1,-0,0.137387,0.889469,-0.425934,-0.165603,0.192694,-0.0541955,-0.456759,0.887938,0.283483,0.901694,-0.0730722,0.426155,0.211832 +}; +// h25 +const int h25_numv = 16; +const int h25_numf = 10; +const dReal h25_volu = 0.064196; +const dReal h25_pos[3] = { 0.265380,0.609017,-0.859766 }; +const dReal h25_verts[ h25_numv * 3 ] = { + 0.171512,-0.295294,0.128463, -0.112864,0.390983,-0.140234, -0.179972,0.258217,0.163631, -0.171482,0.390983,0.031499, -0.146465,0.312621,0.159756, 0.063572,-0.117444,0.267684, -0.245115,0.390983,-0.140234, -0.337319,0.040895,-0.140234, 0.499394,-0.306418,-0.140234, 0.112578,-0.021086,0.259138, 0.074204,-0.220888,0.227450, 0.064293,-0.284390,0.182686, -0.258312,-0.146590,-0.140234, 0.497529,-0.308188,-0.130217, -0.299210,0.224777,0.000875, -0.248273,0.390983,-0.097058, +}; +const unsigned int h25_faces[] = { + 6, 3,4,9,13,8,1, + 5, 8,12,7,6,1, + 4, 6,15,3,1, + 7, 14,7,12,11,10,5,2, + 4, 5,9,4,2, + 5, 4,3,15,14,2, + 5, 10,0,13,9,5, + 4, 7,14,15,6, + 5, 13,0,11,12,8, + 3, 11,0,10, +}; +const dReal h25_planes[ h25_numf * 4 ] = { + 0.727925,0.639055,0.248465,0.13286,0,-0,-1,0.140234,0,1,0,0.390983,-0.735479,-0.309932,0.602505,0.150925,-0.158394,0.166867,0.973173,0.230836,-0.747363,0.49209,0.446425,0.33462,0.598205,-0.236329,0.765702,0.270751,-0.964613,0.254054,-0.0705546,0.345666,-0.201934,-0.957321,-0.206781,0.221494,0.320422,-0.578682,0.749971,0.322182 +}; +// h26 +const int h26_numv = 24; +const int h26_numf = 14; +const dReal h26_volu = 0.101687; +const dReal h26_pos[3] = { -0.341007,0.877992,-0.537072 }; +const dReal h26_verts[ h26_numv * 3 ] = { + 0.358114,0.122008,-0.419752, -0.168425,-0.052359,0.397165, -0.220490,-0.180490,0.174009, 0.459922,0.043646,-0.162938, 0.324251,-0.109249,-0.070985, -0.202046,0.011183,0.424477, 0.252084,0.007514,0.202105, -0.286958,0.122008,0.388199, 0.307177,-0.044198,-0.321819, 0.426415,-0.010758,-0.159063, 0.082627,-0.250684,-0.160780, -0.033904,-0.274387,-0.120138, -0.232139,0.122008,-0.275647, -0.292928,-0.043884,-0.175471, -0.344531,-0.074523,0.167952, -0.207099,-0.190024,0.165835, -0.444249,0.122008,0.173153, -0.425929,0.005713,0.114448, 0.327136,0.122008,0.185091, -0.202046,0.122008,0.443480, 0.434905,0.122008,-0.291195, 0.483509,0.122008,-0.120325, -0.187210,-0.038985,0.408631, -0.270106,-0.127507,0.231547, +}; +const unsigned int h26_faces[] = { + 5, 22,23,2,15,1, + 6, 15,11,10,4,6,1, + 6, 6,18,19,5,22,1, + 3, 23,14,2, + 6, 14,17,13,11,15,2, + 3, 20,21,3, + 6, 21,18,6,4,9,3, + 5, 9,8,0,20,3, + 4, 10,8,9,4, + 3, 19,7,5, + 7, 7,16,17,14,23,22,5, + 8, 19,18,21,20,0,12,16,7, + 6, 10,11,13,12,0,8, + 4, 13,17,16,12, +}; +const dReal h26_planes[ h26_numf * 4 ] = { + -0.256638,-0.811103,0.525594,0.29444,0.324513,-0.835692,0.443069,0.165071,0.433652,-0.152285,0.888119,0.287665,-0.647541,-0.750402,0.132616,0.301294,-0.636259,-0.754085,-0.162882,0.248051,0.95246,-0.139363,-0.270922,0.476118,0.791095,-0.458379,0.40504,0.277839,0.747363,-0.49209,-0.446425,0.39499,0.555832,-0.7947,-0.243931,0.284365,-0.540055,-0.142236,0.829524,0.459639,-0.742791,-0.391264,0.543299,0.37632,0,1,0,0.122008,-0.212159,-0.447018,-0.869002,0.234249,-0.901694,0.0730722,-0.426155,0.335702 +}; +// h27 +const int h27_numv = 24; +const int h27_numf = 14; +const dReal h27_volu = 0.100588; +const dReal h27_pos[3] = { 0.167088,0.649940,-0.364014 }; +const dReal h27_verts[ h27_numv * 3 ] = { + -0.183844,0.118803,-0.244043, -0.044353,-0.266136,0.405236, -0.256011,0.235567,0.029047, 0.161864,-0.158366,-0.228068, 0.300539,0.065664,-0.127155, -0.024586,0.350060,-0.293383, -0.048173,0.271698,-0.335996, -0.180959,0.350060,0.012033, -0.081680,0.217294,-0.332121, -0.044357,0.350060,0.088262, -0.006196,-0.377080,0.285910, 0.201878,-0.226993,0.143823, -0.003872,-0.277036,0.420726, 0.210870,-0.062009,-0.236614, -0.003871,0.350060,0.098146, 0.183618,0.350060,-0.130723, -0.165155,-0.254165,0.331667, -0.075081,-0.408619,0.192919, -0.091738,-0.410255,0.124205, 0.005721,-0.287236,0.414262, -0.077668,-0.381972,-0.025325, -0.102748,-0.401170,0.182332, 0.134962,-0.226993,-0.182915, 0.109551,-0.305242,0.296779, +}; +const unsigned int h27_faces[] = { + 7, 16,21,17,10,19,12,1, + 4, 12,14,9,1, + 5, 9,7,2,16,1, + 6, 7,5,6,8,0,2, + 6, 0,20,18,21,16,2, + 5, 13,4,11,22,3, + 5, 22,20,0,8,3, + 4, 8,6,13,3, + 5, 13,6,5,15,4, + 7, 15,14,12,19,23,11,4, + 5, 7,9,14,15,5, + 7, 17,18,20,22,11,23,10, + 3, 23,19,10, + 3, 21,18,17, +}; +const dReal h27_planes[ h27_numf * 4 ] = { + -0.40667,-0.730271,0.54893,0.434835,-0.212159,0.447018,0.869002,0.242593,-0.444511,0.409753,0.796563,0.233461,-0.791095,0.458379,-0.40504,0.298742,-0.954535,-0.263372,-0.139635,0.178273,0.870583,-0.45858,-0.178295,0.254203,-0.309646,-0.434949,-0.84554,0.211601,0.158394,-0.166867,-0.973173,0.274014,0.598205,0.236328,-0.765702,0.292665,0.73548,0.309929,0.602505,0.16478,1.09327e-15,1,1.13815e-15,0.35006,0.53156,-0.839996,-0.108863,0.282326,0.425203,-0.759567,0.4922,0.424508,-0.292039,-0.95183,0.0934524,0.428891 +}; +// h28 +const int h28_numv = 18; +const int h28_numf = 11; +const dReal h28_volu = 0.075441; +const dReal h28_pos[3] = { 0.013616,0.472900,-0.719797 }; +const dReal h28_verts[ h28_numv * 3 ] = { + -0.187022,-0.167506,-0.280203, 0.109520,-0.345231,0.152809, -0.085555,0.177012,-0.280203, -0.271995,0.154408,0.021945, -0.030372,0.295843,0.111740, 0.061535,-0.227567,0.329703, 0.075804,-0.204932,0.330457, 0.071792,0.394334,0.023662, 0.208102,-0.281022,0.072668, -0.047446,0.360894,-0.139094, 0.077205,-0.350161,0.038133, -0.006548,-0.010473,-0.280203, 0.288434,-0.049953,0.172868, 0.315336,0.018674,0.127715, 0.316057,-0.148273,0.042717, 0.325968,-0.084770,0.087482, -0.256811,-0.088994,-0.280203, -0.315464,0.084990,0.019794, +}; +const unsigned int h28_faces[] = { + 7, 8,14,15,12,6,5,1, + 6, 5,17,16,0,10,1, + 3, 10,8,1, + 5, 16,17,3,9,2, + 7, 9,7,13,15,14,11,2, + 4, 11,0,16,2, + 5, 17,5,6,4,3, + 4, 4,7,9,3, + 5, 6,12,13,7,4, + 5, 10,0,11,14,8, + 3, 15,13,12, +}; +const dReal h28_planes[ h28_numf * 4 ] = { + 0.720506,-0.471135,0.508826,0.319313,-0.726908,-0.64614,0.232611,0.179002,0.485104,-0.868795,-0.0993492,0.337882,-0.758508,0.488331,-0.431507,0.272244,0.735479,0.309932,-0.602505,0.160762,0,0,-1,0.280203,-0.469484,0.267909,0.841315,0.187527,-0.555832,0.7947,0.243931,0.279245,0.309646,0.434949,0.84554,0.213753,0.469359,-0.539421,-0.69909,0.198463,0.901694,-0.0730722,0.426155,0.337399 +}; +// h29 +const int h29_numv = 20; +const int h29_numf = 12; +const dReal h29_volu = 0.090754; +const dReal h29_pos[3] = { -0.195482,0.549508,-0.348926 }; +const dReal h29_verts[ h29_numv * 3 ] = { + -0.062897,0.077801,-0.348926, 0.254536,-0.331284,-0.002375, -0.179429,0.054098,-0.308283, 0.178726,0.219235,-0.259131, -0.352624,0.138460,-0.022310, 0.259822,-0.300738,0.167244, 0.197415,-0.153733,0.316579, 0.065987,-0.327570,0.133724, 0.114982,-0.134660,0.333685, 0.270633,-0.304175,-0.041168, -0.106366,0.008382,-0.351078, 0.106560,0.335999,0.013959, -0.240892,-0.124645,-0.147161, -0.313950,0.276125,0.209019, -0.158320,0.003002,-0.331475, 0.036774,-0.342768,0.065557, 0.284902,-0.281540,-0.040413, 0.270832,-0.309823,0.109117, 0.013405,-0.084041,0.336165, 0.046528,-0.137097,0.329090, +}; +const unsigned int h29_faces[] = { + 4, 9,16,17,1, + 5, 17,5,7,15,1, + 6, 15,12,14,10,9,1, + 4, 0,10,14,2, + 4, 14,12,4,2, + 6, 4,13,11,3,0,2, + 5, 16,9,10,0,3, + 6, 11,6,5,17,16,3, + 7, 12,15,7,19,18,13,4, + 5, 6,8,19,7,5, + 5, 11,13,18,8,6, + 3, 18,19,8, +}; +const dReal h29_planes[ h29_numf * 4 ] = { + 0.846082,-0.532633,-0.0211314,0.391862,0.105626,-0.979232,0.173057,0.350878,-0.148211,-0.780702,-0.607074,0.22235,-0.364346,0.255892,-0.895417,0.355259,-0.864691,-0.138024,-0.482969,0.296575,-0.324513,0.835692,-0.443069,0.240027,0.469484,-0.267909,-0.841315,0.243184,0.954535,0.263372,0.139635,0.192157,-0.73808,-0.518378,0.431882,0.178854,-0.0212658,-0.716912,0.696839,0.326619,0.289106,0.541462,0.789453,0.223758,-0.0600487,-0.168696,0.983837,0.344105 +}; +// h30 +const int h30_numv = 26; +const int h30_numf = 15; +const dReal h30_volu = 0.141813; +const dReal h30_pos[3] = { 0.709749,0.432546,-0.520590 }; +const dReal h30_verts[ h30_numv * 3 ] = { + 0.086130,-0.133630,-0.479410, 0.290251,-0.067548,-0.461259, 0.290251,0.254276,-0.130163, -0.166094,-0.194830,-0.275875, -0.114073,0.017151,0.452790, 0.263868,-0.083016,-0.470730, -0.015645,0.141724,0.383369, -0.170984,0.316821,0.046789, -0.242122,0.283058,0.029421, 0.290251,0.110614,0.239339, -0.340783,-0.009598,0.300399, 0.290251,-0.018830,0.261535, 0.067740,-0.137994,-0.479410, -0.241377,-0.054963,0.435747, 0.058158,-0.287592,0.103343, 0.290251,-0.145966,0.143817, 0.068185,-0.139308,-0.479410, -0.017612,-0.287592,-0.266628, 0.053160,-0.131717,-0.469392, -0.272857,-0.118823,-0.210712, 0.007901,-0.200618,-0.414231, 0.003123,-0.268644,-0.311584, -0.331791,0.155385,-0.080037, -0.407699,-0.009598,-0.026339, -0.380797,0.059028,-0.071491, -0.370165,-0.044416,-0.111725, +}; +const unsigned int h30_faces[] = { + 6, 15,14,17,21,5,1, + 9, 5,0,12,18,22,8,7,2,1, + 5, 2,9,11,15,1, + 4, 7,6,9,2, + 4, 20,21,17,3, + 8, 17,14,13,10,23,25,19,3, + 6, 19,18,12,16,20,3, + 5, 13,14,15,11,4, + 4, 11,9,6,4, + 6, 6,7,8,10,13,4, + 5, 21,20,16,0,5, + 5, 22,24,23,10,8, + 3, 0,16,12, + 5, 19,25,24,22,18, + 3, 24,25,23, +}; +const dReal h30_planes[ h30_numf * 4 ] = { + 0.53156,-0.839996,-0.108863,0.26124,-0.167754,0.706913,-0.687119,0.220498,1,-0,0,0.290251,0.256529,0.900844,0.350247,0.257932,-0.43192,-0.742413,-0.512121,0.357665,-0.531561,-0.839995,0.108864,0.221912,-0.613049,-0.207593,-0.762283,0.352564,0.275531,-0.653109,0.705359,0.276747,0.433652,0.152285,0.888119,0.355275,-0.46199,0.681388,0.567693,0.321433,0.256638,-0.811103,-0.525594,0.382466,-0.870583,0.45858,0.178295,0.345838,-0,0,-1,0.479409,-0.598205,0.236329,-0.765702,0.296486,-0.901694,0.0730722,-0.426155,0.378142 +}; +// h31 +const int h31_numv = 20; +const int h31_numf = 12; +const dReal h31_volu = 0.145607; +const dReal h31_pos[3] = { 0.628702,0.773411,-0.793457 }; +const dReal h31_verts[ h31_numv * 3 ] = { + -0.189575,0.226589,0.317223, 0.371298,-0.414948,-0.206543, -0.476185,0.226589,-0.206543, 0.371298,0.226589,-0.206543, -0.250744,-0.185480,0.192828, 0.148787,-0.478859,-0.206543, -0.509787,0.148227,0.093447, -0.534804,0.226589,-0.034811, -0.277996,0.226589,0.298719, -0.486200,0.226589,0.136059, 0.371298,-0.408413,-0.188392, 0.167177,-0.474495,-0.206543, 0.366378,-0.417196,-0.206543, 0.344915,-0.423881,-0.197864, 0.371298,-0.086589,0.142703, 0.371298,0.226589,0.089002, -0.161075,-0.057807,0.302287, -0.089936,-0.024044,0.319655, 0.134207,-0.472582,-0.196526, 0.136072,-0.470812,-0.206543, +}; +const unsigned int h31_faces[] = { + 4, 10,13,12,1, + 7, 12,11,5,19,2,3,1, + 5, 3,15,14,10,1, + 6, 19,18,4,6,7,2, + 7, 7,9,8,0,15,3,2, + 5, 16,8,9,6,4, + 9, 18,5,11,13,10,14,17,16,4, + 3, 18,19,5, + 3, 9,7,6, + 4, 16,17,0,8, + 3, 12,13,11, + 4, 15,0,17,14, +}; +const dReal h31_planes[ h31_numf * 4 ] = { + 0.395039,-0.86436,0.311169,0.441071,-0,-0,-1,0.206543,1,-0,0,0.371298,-0.727925,-0.639055,-0.248465,0.253143,4.94863e-17,1,-0,0.226589,-0.598205,-0.236328,0.765702,0.341479,0.167754,-0.706913,0.687119,0.221551,-0.518906,-0.82,-0.241527,0.365343,-0.95246,0.139363,0.270922,0.531526,-0.204299,-0.0717432,0.976276,0.332171,0.275992,-0.959497,-0.0565231,0.51309,0.372229,0.156858,0.91479,0.255169 +}; +// h32 +const int h32_numv = 22; +const int h32_numf = 13; +const dReal h32_volu = 0.092552; +const dReal h32_pos[3] = { -0.648077,-0.694629,-0.055690 }; +const dReal h32_verts[ h32_numv * 3 ] = { + -0.351923,0.020682,-0.162313, -0.351923,0.291038,-0.298522, 0.116588,0.074450,0.304425, 0.180292,0.145010,-0.063690, -0.351923,0.286862,-0.299238, 0.082653,-0.305371,0.200452, 0.444715,-0.214767,0.086986, 0.440565,-0.305371,0.060643, 0.437298,-0.305371,0.003205, 0.441236,-0.180086,0.007980, 0.202670,-0.305371,-0.129990, -0.122926,-0.305371,-0.050498, -0.351923,0.343433,0.003710, 0.034975,0.067995,0.334312, -0.249787,0.361137,-0.236631, 0.059834,0.222599,0.089656, 0.420946,-0.305371,-0.023410, 0.437088,-0.283432,-0.011766, -0.351923,0.387353,-0.064067, -0.294272,0.397125,-0.093222, -0.351923,0.309463,-0.280428, -0.321961,0.320573,-0.278827, +}; +const unsigned int h32_faces[] = { + 3, 20,21,1, + 9, 21,14,3,9,17,16,10,4,1, + 6, 4,0,12,18,20,1, + 5, 13,5,7,6,2, + 5, 6,9,3,15,2, + 6, 15,19,18,12,13,2, + 4, 14,19,15,3, + 4, 10,11,0,4, + 5, 13,12,0,11,5, + 6, 11,10,16,8,7,5, + 5, 7,8,17,9,6, + 3, 16,17,8, + 5, 21,20,18,19,14, +}; +const dReal h32_planes[ h32_numf * 4 ] = { + -0.216442,0.684063,-0.696571,0.483201,0.433652,0.152285,-0.888119,0.156831,-1,0,0,0.351923,0.349687,-0.276295,0.895198,0.29272,0.727924,0.639056,0.248465,0.208084,0.131611,0.83191,0.539077,0.241389,0.471464,0.878752,-0.0742736,0.21716,-0.212159,-0.447018,-0.869002,0.206469,-0.73548,-0.309929,0.602505,0.154628,-3.74119e-15,-1,2.12832e-16,0.305371,0.99796,-0.0292037,-0.0567728,0.445142,0.80443,-0.32957,-0.494242,0.450833,-0.314046,0.893286,-0.321583,0.47714 +}; +// h33 +const int h33_numv = 18; +const int h33_numf = 11; +const dReal h33_volu = 0.056945; +const dReal h33_pos[3] = { -0.288352,-0.547292,0.050646 }; +const dReal h33_verts[ h33_numv * 3 ] = { + 0.084990,-0.362104,-0.019350, -0.079434,0.021164,0.334816, 0.019488,-0.072734,0.262363, 0.115516,0.076855,-0.268585, 0.213070,0.021874,-0.243280, 0.274323,0.089944,-0.132397, 0.019789,0.332949,0.053313, 0.257211,0.025988,-0.078610, 0.131191,0.288888,-0.060012, 0.118204,-0.320377,0.031687, 0.081511,-0.327423,-0.098356, -0.243137,-0.072887,0.198089, -0.179433,-0.002327,-0.170026, -0.299891,0.075262,-0.016680, -0.055817,0.322184,0.088384, -0.038680,0.277450,0.153234, 0.148159,-0.119839,-0.229545, 0.048588,0.033746,-0.272086, +}; +const unsigned int h33_faces[] = { + 5, 11,0,9,2,1, + 7, 2,7,5,8,6,15,1, + 5, 15,14,13,11,1, + 3, 9,7,2, + 4, 4,16,17,3, + 7, 17,12,13,14,6,8,3, + 4, 8,5,4,3, + 7, 5,7,9,0,10,16,4, + 3, 14,15,6, + 5, 0,11,13,12,10, + 4, 12,17,16,10, +}; +const dReal h33_planes[ h33_numf * 4 ] = { + -0.167754,-0.706913,0.687119,0.228423,0.73548,0.309929,0.602505,0.149866,-0.708232,0.479648,0.518021,0.239851,0.830851,-0.164118,0.531744,0.167639,0.158394,-0.166869,-0.973173,0.266852,-0.391463,0.659847,-0.641373,0.177755,0.49733,0.589428,-0.636583,0.273727,0.877503,-0.423705,-0.224641,0.232352,0.131611,0.83191,0.539077,0.308327,-0.727924,-0.639056,-0.248465,0.174346,-0.309646,-0.434949,-0.84554,0.200337 +}; +// h34 +const int h34_numv = 24; +const int h34_numf = 14; +const dReal h34_volu = 0.086482; +const dReal h34_pos[3] = { -0.499709,-0.643458,-0.327986 }; +const dReal h34_verts[ h34_numv * 3 ] = { + -0.500291,0.235691,-0.026942, -0.413349,0.203755,-0.157062, -0.223651,0.321490,-0.139249, 0.209880,-0.356542,0.003973, 0.365162,-0.137565,0.097393, 0.292868,-0.231257,0.280276, 0.359516,-0.023673,0.149087, -0.329463,0.339839,-0.019523, -0.041433,0.349469,-0.148408, 0.054302,-0.356542,0.142306, 0.272578,-0.356542,0.248886, 0.288720,-0.334603,0.260530, 0.023896,-0.069111,-0.283979, 0.274970,-0.222320,-0.056303, 0.127449,-0.356542,-0.036276, -0.035189,-0.103922,-0.288953, -0.023751,0.360733,-0.121029, -0.500291,0.237083,-0.030523, -0.346425,0.336124,-0.010604, 0.259945,0.129913,0.106546, -0.500291,0.239867,-0.026226, -0.470329,0.269402,-0.006532, -0.398155,0.309966,0.035665, 0.031924,0.093839,0.208606, +}; +const unsigned int h34_faces[] = { + 7, 17,20,21,18,7,2,1, + 5, 2,8,12,15,1, + 6, 15,14,9,0,17,1, + 4, 7,16,8,2, + 5, 14,15,12,13,3, + 5, 13,4,11,10,3, + 4, 10,9,14,3, + 7, 13,12,8,16,19,6,4, + 4, 6,5,11,4, + 4, 6,19,23,5, + 9, 23,22,21,20,0,9,10,11,5, + 6, 18,22,23,19,16,7, + 3, 0,20,17, + 3, 21,22,18, +}; +const dReal h34_planes[ h34_numf * 4 ] = { + -0.425203,0.759567,-0.4922,0.407829,-0.0929092,0.293639,-0.951391,0.247662,-0.66261,-0.69806,-0.271405,0.174284,-0.159459,0.944944,-0.285751,0.379244,0.373525,-0.524676,-0.76498,0.262425,0.837582,-0.502473,-0.214421,0.354093,0,-1,0,0.356542,0.73548,0.309929,-0.602505,0.167254,0.941719,-0.0992098,0.32144,0.388833,0.309646,0.434949,0.84554,0.227085,-0.433652,-0.152285,0.888119,0.157132,0.148211,0.780702,0.607074,0.204631,-1,0,-0,0.500291,-0.474713,0.879502,-0.0335246,0.460429 +}; +// h35 +const int h35_numv = 12; +const int h35_numf = 8; +const dReal h35_volu = 0.072358; +const dReal h35_pos[3] = { -0.852811,-0.781078,0.150064 }; +const dReal h35_verts[ h35_numv * 3 ] = { + -0.147189,-0.218922,0.332047, 0.103613,-0.074924,0.288589, -0.147189,0.429882,-0.202044, -0.147189,-0.218922,-0.312160, 0.239709,0.154443,0.128558, 0.287386,-0.218922,-0.005301, -0.043492,-0.218922,0.344705, 0.074717,-0.218922,0.306226, -0.147189,0.402063,0.012610, 0.208223,0.167113,0.176854, -0.147189,0.107130,-0.368067, 0.081807,-0.218922,-0.256252, +}; +const unsigned int h35_faces[] = { + 3, 6,7,1, + 5, 7,5,4,9,1, + 5, 9,8,0,6,1, + 4, 8,9,4,2, + 5, 4,5,11,10,2, + 5, 10,3,0,8,2, + 3, 10,11,3, + 6, 11,5,7,6,0,3, +}; +const dReal h35_planes[ h35_numf * 4 ] = { + 0.309078,0.0542691,0.949487,0.301971,0.822068,-0.096228,0.561199,0.254343,-0.107916,0.45476,0.884052,0.209874,0.512085,0.851811,0.110395,0.2685,0.73548,0.309929,-0.602505,0.146711,-1,-0,-0,0.147189,0.233951,-0.164312,-0.958263,0.300667,0,-1,0,0.218922 +}; +// h36 +const int h36_numv = 18; +const int h36_numf = 11; +const dReal h36_volu = 0.076715; +const dReal h36_pos[3] = { 0.330903,-0.599381,-0.308448 }; +const dReal h36_verts[ h36_numv * 3 ] = { + 0.235744,-0.093981,0.208985, 0.157036,0.091360,0.296934, -0.213473,-0.059832,-0.258208, 0.131366,0.084254,0.304768, 0.060949,-0.352878,-0.040467, 0.031132,-0.367472,0.023458, -0.038178,0.319175,-0.078462, 0.346140,-0.033881,-0.005840, 0.305466,-0.106272,-0.107625, -0.256020,-0.056138,-0.213238, -0.331115,0.110184,0.014099, -0.356965,0.085987,0.067218, 0.185622,-0.147504,0.212044, 0.241393,0.219660,0.109239, -0.343391,-0.100844,-0.022261, -0.046519,0.081665,-0.290234, -0.272849,0.156195,0.144404, -0.146830,0.270831,-0.017119, +}; +const unsigned int h36_faces[] = { + 6, 13,6,17,16,3,1, + 4, 3,12,0,1, + 4, 0,7,13,1, + 6, 9,10,17,6,15,2, + 4, 15,8,4,2, + 5, 4,5,14,9,2, + 6, 16,11,14,5,12,3, + 6, 8,7,0,12,5,4, + 5, 13,7,8,15,6, + 4, 14,11,10,9, + 4, 11,16,17,10, +}; +const dReal h36_planes[ h36_numf * 4 ] = { + -0.066237,0.837367,0.542613,0.227221,0.349687,-0.276295,0.895198,0.295486,0.864691,0.138024,0.482969,0.291807,-0.582463,0.552262,-0.596442,0.245303,0.212159,-0.447016,-0.869003,0.205839,-0.496382,-0.766976,-0.406636,0.25685,-0.398278,-0.419584,0.815674,0.16092,0.722578,-0.66608,0.18498,0.2716,0.595564,0.522855,-0.609857,0.191995,-0.913105,0.120244,-0.389591,0.310099,-0.654405,0.755705,0.0257735,0.300313 +}; +// h37 +const int h37_numv = 18; +const int h37_numf = 11; +const dReal h37_volu = 0.098217; +const dReal h37_pos[3] = { 0.055330,-0.588368,0.333127 }; +const dReal h37_verts[ h37_numv * 3 ] = { + -0.107417,0.016721,0.347927, 0.283844,-0.036335,0.197820, 0.129290,0.324988,-0.064466, -0.012100,0.128834,-0.356552, 0.344968,0.171444,-0.038138, -0.270436,0.127441,0.395275, -0.130084,-0.375511,-0.154724, 0.001812,-0.380727,-0.087638, -0.247911,0.142505,0.383526, 0.360906,0.133475,0.137346, 0.062042,-0.352141,-0.072933, 0.261216,0.073240,-0.194499, 0.127844,0.325300,-0.059106, 0.361773,0.137946,0.130076, -0.225478,-0.279302,-0.250794, -0.324194,-0.031659,-0.020118, -0.297963,0.086614,0.328910, -0.086471,0.067064,-0.361091, +}; +const unsigned int h37_faces[] = { + 6, 10,11,4,13,9,1, + 5, 9,8,5,0,1, + 4, 0,7,10,1, + 4, 12,13,4,2, + 4, 4,11,3,2, + 8, 3,17,15,16,5,8,12,2, + 7, 11,10,7,6,14,17,3, + 5, 16,6,7,0,5, + 4, 16,15,14,6, + 4, 9,13,12,8, + 3, 15,17,14, +}; +const dReal h37_planes[ h37_numf * 4 ] = { + 0.870583,-0.45858,-0.178295,0.228502,0.372229,0.156856,0.91479,0.280919,0.167754,-0.706913,0.687119,0.209227,0.568104,0.816131,0.105771,0.331865,0.49733,0.589428,-0.636583,0.296895,-0.616962,0.758294,-0.21059,0.180245,0.398278,-0.419585,-0.815673,0.231954,-0.32928,-0.737152,0.590069,0.228345,-0.799798,-0.547681,0.245698,0.271686,0.226312,0.817437,0.529699,0.263537,-0.830851,0.164118,-0.531744,0.274859 +}; +// h38 +const int h38_numv = 28; +const int h38_numf = 16; +const dReal h38_volu = 0.093559; +const dReal h38_pos[3] = { 0.133166,-0.773583,-0.050905 }; +const dReal h38_verts[ h38_numv * 3 ] = { + -0.076024,-0.195512,0.296394, 0.264846,-0.226417,0.026836, 0.183380,0.258456,0.189533, -0.015793,-0.166926,0.311098, -0.267713,-0.007440,-0.179688, -0.159228,0.260189,-0.190326, -0.343945,-0.226417,-0.001580, -0.273359,0.106452,-0.127994, 0.216958,-0.226417,-0.206989, 0.383359,0.026697,-0.045499, 0.329103,0.258456,0.047225, -0.089936,0.314049,0.027481, -0.145654,0.073358,-0.279804, 0.228869,-0.193270,-0.234086, -0.131382,0.326289,-0.026485, -0.075112,0.330397,-0.113139, -0.208448,0.248166,-0.141729, -0.011151,-0.226417,0.296364, -0.211663,-0.226417,0.216210, -0.072038,-0.226417,0.288931, -0.336528,-0.135813,0.082201, -0.147195,0.316236,-0.030846, -0.340678,-0.226417,0.055858, -0.164307,0.252279,0.022941, -0.344155,-0.204478,-0.016551, -0.340007,-0.101132,0.003195, -0.207920,-0.190296,0.229308, -0.303314,-0.094086,0.133238, +}; +const unsigned int h38_faces[] = { + 4, 8,13,9,1, + 6, 9,10,2,3,17,1, + 7, 17,19,18,22,6,8,1, + 5, 10,15,14,11,2, + 7, 11,23,27,26,0,3,2, + 4, 0,19,17,3, + 4, 24,25,7,4, + 5, 7,16,5,12,4, + 6, 12,13,8,6,24,4, + 5, 16,21,14,15,5, + 6, 15,10,9,13,12,5, + 5, 22,20,25,24,6, + 7, 25,20,27,23,21,16,7, + 4, 14,21,23,11, + 5, 26,27,20,22,18, + 4, 19,0,26,18, +}; +const dReal h38_planes[ h38_numf * 4 ] = { + 0.870583,-0.45858,-0.178295,0.329616,0.693474,-0.121763,0.710118,0.23029,0,-1,0,0.226417,0.124536,0.983986,0.127525,0.301324,-0.398278,0.419585,0.815673,0.190005,-0.117405,-0.247372,0.961781,0.342356,-0.941719,0.0992098,-0.32144,0.309131,-0.709719,0.26169,-0.654077,0.305583,-0.289106,-0.541464,-0.789452,0.223281,-0.465257,0.845503,-0.262033,0.343944,0.398278,0.419584,-0.815674,0.200998,-0.99796,0.0292037,0.0567728,0.336541,-0.877503,0.423705,0.224641,0.256225,-0.548603,0.619234,0.561769,0.259246,-0.770892,-0.145024,0.620237,0.330106,-0.445543,-0.264025,0.855443,0.33904 +}; +// h39 +const int h39_numv = 32; +const int h39_numf = 18; +const dReal h39_volu = 0.060173; +const dReal h39_pos[3] = { -0.130993,-0.349156,0.297470 }; +const dReal h39_verts[ h39_numv * 3 ] = { + 0.132777,-0.098138,-0.374861, 0.174223,-0.110378,-0.320895, -0.236793,-0.176972,0.087992, 0.120090,0.163784,-0.274197, -0.036262,0.126682,-0.293283, -0.239846,-0.040235,0.342234, -0.196039,0.079314,-0.093590, -0.026168,0.090752,-0.306837, -0.084113,-0.111771,0.430932, -0.132771,0.138527,-0.197817, -0.061588,-0.096707,0.419183, -0.167047,-0.030107,0.457226, 0.314167,0.086089,-0.023449, 0.140527,0.255266,-0.004320, -0.137571,0.134813,-0.193511, 0.315613,0.085777,-0.028809, 0.116964,-0.108191,-0.379221, 0.099852,-0.172148,-0.325434, -0.176687,-0.026318,0.447418, -0.168971,-0.023213,0.453768, -0.137871,-0.270870,0.015539, -0.111640,-0.152598,0.364567, -0.122362,-0.092357,0.456223, -0.127730,-0.079472,0.460978, -0.243030,-0.080780,0.324741, -0.136583,-0.114124,0.420896, -0.241856,-0.054541,0.348612, -0.178625,-0.028253,0.445248, -0.171209,-0.024409,0.453290, -0.169333,-0.024798,0.455225, 0.224076,0.244621,-0.088171, 0.213513,0.251212,-0.061379, +}; +const unsigned int h39_faces[] = { + 5, 0,3,30,15,1, + 8, 15,12,10,8,21,20,17,1, + 4, 17,16,0,1, + 5, 20,21,25,24,2, + 5, 24,26,5,6,2, + 7, 6,14,7,16,17,20,2, + 5, 0,16,7,4,3, + 6, 4,9,13,31,30,3, + 4, 7,14,9,4, + 4, 26,27,18,5, + 8, 18,28,19,13,9,14,6,5, + 4, 10,23,22,8, + 4, 22,25,21,8, + 8, 12,31,13,19,29,11,23,10, + 7, 27,26,24,25,22,23,11, + 5, 29,28,18,27,11, + 4, 15,30,31,12, + 3, 28,29,19, +}; +const dReal h39_planes[ h39_numf * 4 ] = { + 0.794364,0.251059,-0.553132,0.288182,0.616962,-0.758294,0.21059,0.12361,0.548603,-0.619234,-0.561769,0.344197,-0.526505,-0.792388,0.308081,0.292012,-0.991673,0.108076,-0.0700326,0.209533,-0.73548,-0.309929,-0.602505,0.17599,0.0284606,0.359798,-0.932596,0.318062,-0.184883,0.934913,-0.302912,0.213979,-0.696788,0.0734069,-0.713511,0.243827,-0.830322,0.320028,0.456232,0.342411,-0.493232,0.850288,0.183662,0.146944,0.512456,-0.101226,0.852726,0.335677,-0.0662371,-0.837367,0.542614,0.332994,0.553104,0.49165,0.672575,0.200322,-0.664689,-0.486283,0.567201,0.385014,-0.721996,-0.0507074,0.690036,0.437637,0.818514,0.542417,0.189261,0.299408,-0.474458,0.651966,0.591464,0.333423 +}; +// h40 +const int h40_numv = 24; +const int h40_numf = 14; +const dReal h40_volu = 0.115424; +const dReal h40_pos[3] = { 0.492476,-0.748212,0.263785 }; +const dReal h40_verts[ h40_numv * 3 ] = { + -0.375103,-0.192297,-0.003592, 0.259307,0.208511,0.137583, 0.097667,-0.251788,0.295436, 0.264741,-0.251788,-0.112461, -0.092178,0.331289,0.031204, 0.024049,0.001327,-0.360189, 0.074171,0.054850,-0.363248, 0.312772,-0.115013,-0.159366, -0.175930,0.233085,-0.125157, -0.094464,-0.251788,-0.287854, -0.004537,0.240191,-0.275299, -0.030207,0.233085,-0.267465, -0.370461,-0.251788,-0.018326, 0.056736,-0.251788,0.315422, -0.153302,0.123509,0.267162, -0.068302,0.078845,0.319783, 0.171724,0.348803,0.156293, 0.053571,0.367605,-0.023060, -0.075373,0.297791,0.199418, 0.109457,0.331072,0.189737, -0.005042,0.303901,0.226077, 0.014996,0.282164,0.244247, -0.076239,0.293319,0.206688, -0.068649,0.297216,0.207331, +}; +const unsigned int h40_faces[] = { + 4, 2,3,7,1, + 6, 7,6,10,17,16,1, + 7, 16,19,21,15,13,2,1, + 5, 13,12,9,3,2, + 5, 9,5,6,7,3, + 5, 17,10,11,8,4, + 6, 8,0,14,22,18,4, + 7, 18,23,20,19,16,17,4, + 6, 9,12,0,8,11,5, + 4, 11,10,6,5, + 5, 13,15,14,0,12, + 6, 15,21,20,23,22,14, + 3, 22,23,18, + 3, 20,21,19, +}; +const dReal h40_planes[ h40_numf * 4 ] = { + 0.908278,-0.191373,0.372031,0.246805,0.73808,0.518378,-0.431882,0.240058,0.433652,0.152285,0.888119,0.266393,5.15129e-17,-1,5.27492e-17,0.251788,0.398278,-0.419585,-0.815673,0.302818,-0.348894,0.86639,-0.357268,0.308038,-0.870583,0.45858,0.178295,0.237735,-0.162747,0.964426,0.208316,0.341006,-0.693474,0.121763,-0.710118,0.23926,-0.349687,0.276295,-0.895198,0.314397,-0.598205,-0.236328,0.765702,0.267084,-0.296224,0.4369,0.849335,0.326283,-0.440632,0.78713,0.431589,0.353679,0.071473,0.677671,0.731884,0.371046 +}; +// h41 +const int h41_numv = 18; +const int h41_numf = 11; +const dReal h41_volu = 0.076473; +const dReal h41_pos[3] = { 0.741085,-0.876917,-0.246261 }; +const dReal h41_verts[ h41_numv * 3 ] = { + 0.005338,0.140795,-0.243701, -0.174437,0.183555,0.146798, 0.258916,-0.123083,-0.387389, 0.103129,0.222017,-0.142233, 0.164192,-0.123083,-0.418224, 0.016133,-0.123083,0.397585, 0.258916,0.168617,-0.137303, 0.258916,-0.123083,0.279039, -0.343073,-0.123083,0.222192, -0.064041,0.243655,-0.068027, -0.104715,0.171264,-0.169812, -0.390960,-0.123083,-0.011633, -0.224559,0.130032,0.149857, -0.379049,-0.089936,-0.038730, -0.349233,-0.075342,-0.102654, -0.337817,-0.123083,-0.124077, 0.258916,-0.009416,0.259549, 0.064164,0.013692,0.350680, +}; +const unsigned int h41_faces[] = { + 6, 9,10,14,13,12,1, + 5, 12,8,5,17,1, + 6, 17,16,6,3,9,1, + 5, 4,0,3,6,2, + 4, 6,16,7,2, + 7, 7,5,8,11,15,4,2, + 4, 0,10,9,3, + 5, 15,14,10,0,4, + 4, 7,16,17,5, + 4, 12,13,11,8, + 4, 13,14,15,11, +}; +const dReal h41_planes[ h41_numf * 4 ] = { + -0.722578,0.66608,-0.18498,0.221152,-0.398278,0.419585,0.815673,0.266231,0.287163,0.873965,0.392074,0.167884,0.239912,0.631869,-0.73701,0.269854,1,0,0,0.258915,0,-1,0,0.123083,-0.131611,0.83191,-0.539077,0.2478,-0.488522,0.25733,-0.833743,0.236806,0.433652,0.152285,0.888119,0.341356,-0.870583,0.45858,0.178295,0.281846,-0.903842,-0.0244154,-0.42717,0.361341 +}; +// h42 +const int h42_numv = 26; +const int h42_numf = 15; +const dReal h42_volu = 0.137951; +const dReal h42_pos[3] = { 0.814994,-0.482932,0.027418 }; +const dReal h42_verts[ h42_numv * 3 ] = { + -0.063211,-0.056769,0.373950, 0.185005,0.097851,0.415613, -0.248347,-0.210430,-0.126881, 0.112828,0.023604,-0.394042, -0.108847,0.115253,0.400661, -0.327055,-0.025089,-0.038931, -0.085789,0.224192,-0.255901, 0.185005,-0.403401,-0.014130, 0.185005,-0.225368,-0.410982, -0.009747,-0.380293,0.077002, -0.175394,0.222730,-0.208732, 0.002670,0.221670,0.389944, -0.150794,0.083523,0.392660, 0.160182,0.229752,0.436714, 0.133897,0.345057,0.231744, -0.006362,0.255606,0.329122, 0.185005,0.234054,0.438967, 0.185005,0.352490,0.238788, 0.029219,-0.171968,-0.415912, -0.137952,-0.150330,-0.341706, 0.185005,0.276769,-0.150723, 0.185005,0.010082,-0.390796, -0.176045,0.220413,-0.220256, -0.242698,0.103211,-0.226628, -0.268948,0.102325,0.213307, -0.248100,0.136462,0.190657, +}; +const unsigned int h42_faces[] = { + 4, 0,9,7,1, + 7, 7,8,21,20,17,16,1, + 6, 16,13,4,12,0,1, + 4, 5,23,19,2, + 6, 19,18,8,7,9,2, + 6, 9,0,12,24,5,2, + 4, 6,20,21,3, + 4, 21,8,18,3, + 6, 18,19,23,22,6,3, + 3, 13,11,4, + 6, 11,15,25,24,12,4, + 6, 24,25,10,22,23,5, + 6, 22,10,14,17,20,6, + 4, 25,15,14,10, + 6, 13,16,17,14,15,11, +}; +const dReal h42_planes[ h42_numf * 4 ] = { + 0.267862,-0.627092,0.731441,0.29219,1,2.67045e-16,1.9344e-17,0.185006,-0.0600487,-0.168696,0.983837,0.381279,-0.864691,-0.138024,-0.482969,0.305068,-0.287163,-0.873965,-0.392074,0.304972,-0.73808,-0.518378,0.431882,0.237585,0.156805,0.660773,-0.734024,0.322525,0.0606999,0.0852635,-0.994508,0.400738,-0.364346,0.255892,-0.895417,0.317763,-0.280943,0.383024,0.879979,0.427298,-0.590109,0.664552,0.458413,0.324491,-0.86695,0.495807,-0.0507289,0.273077,-0.11569,0.975032,-0.189546,0.277024,-0.485104,0.868795,0.0993492,0.257853,-0.191731,0.844679,0.499756,0.381605 +}; +// h43 +const int h43_numv = 26; +const int h43_numf = 15; +const dReal h43_volu = 0.091347; +const dReal h43_pos[3] = { -0.439313,0.395062,-0.161996 }; +const dReal h43_verts[ h43_numv * 3 ] = { + -0.171800,0.355423,-0.143529, -0.041887,-0.040549,-0.338294, -0.088904,0.443945,0.033555, -0.050746,0.256028,0.267789, -0.120718,0.085236,0.331241, 0.112903,-0.317075,-0.192421, -0.024680,-0.380539,0.081401, 0.014540,-0.410443,-0.096363, -0.166472,-0.135578,-0.232924, -0.179653,0.199344,0.219565, 0.056744,-0.387608,-0.004660, -0.088904,0.425462,0.081094, -0.046327,-0.413563,-0.075262, -0.024725,0.299814,0.247575, -0.150602,0.151087,0.286944, -0.050613,-0.134365,0.334664, -0.244880,0.199344,-0.098926, -0.122184,0.302440,-0.201067, -0.108793,0.292906,-0.209241, -0.034597,-0.039973,-0.340964, 0.309817,-0.173124,-0.053206, -0.070119,0.430571,0.022089, 0.002939,0.029802,-0.334091, 0.280604,-0.188322,-0.121373, 0.290359,0.017349,0.142160, 0.257236,0.070405,0.149235, +}; +const unsigned int h43_faces[] = { + 5, 8,16,0,17,1, + 5, 17,18,22,19,1, + 6, 19,5,7,12,8,1, + 5, 21,18,17,0,2, + 5, 0,16,9,11,2, + 5, 11,13,25,21,2, + 6, 4,15,24,25,13,3, + 5, 13,11,9,14,3, + 3, 14,4,3, + 8, 14,9,16,8,12,6,15,4, + 4, 19,22,23,5, + 5, 23,20,10,7,5, + 5, 10,20,24,15,6, + 4, 12,7,10,6, + 7, 21,25,24,20,23,22,18, +}; +const dReal h43_planes[ h43_numf * 4 ] = { + -0.693474,0.121763,-0.710118,0.264339,-0.349687,0.276295,-0.895198,0.306284,-0.24653,-0.556538,-0.793403,0.301297,0.256638,0.811103,-0.525594,0.319633,-0.870583,0.45858,0.178295,0.286965,0.662611,0.698059,0.271405,0.260098,0.433652,0.152285,0.888119,0.254811,-0.49733,0.589429,0.636582,0.346617,-0.296224,0.4369,0.849335,0.354333,-0.935836,-0.295766,0.191659,0.151248,0.501077,-0.186311,-0.84511,0.278264,0.632147,-0.768422,-0.0995875,0.334181,0.582464,-0.552261,0.596443,0.244332,0.11569,-0.975032,0.189546,0.383611,0.73808,0.518378,-0.431882,0.161906 +}; +// h44 +const int h44_numv = 26; +const int h44_numf = 15; +const dReal h44_volu = 0.104111; +const dReal h44_pos[3] = { -0.656166,-0.300748,0.215302 }; +const dReal h44_verts[ h44_numv * 3 ] = { + -0.220317,0.235145,-0.208278, 0.078796,0.351295,-0.102059, 0.283317,-0.102949,0.430780, 0.285327,-0.088643,0.424402, 0.201177,0.226255,-0.138992, 0.124677,-0.319431,0.033433, 0.117727,0.304230,-0.184196, 0.043064,-0.325887,0.063320, -0.138842,0.186808,-0.302762, 0.311997,0.075640,-0.076273, 0.067923,-0.171282,-0.181336, -0.343834,-0.050448,-0.267282, -0.343834,-0.078267,-0.052628, -0.343834,0.035932,0.053112, 0.088073,0.239020,0.159535, 0.282143,-0.129188,0.406909, -0.286183,0.003244,-0.364214, -0.343834,0.054532,0.043544, 0.011578,-0.313217,0.111616, 0.329134,0.030906,-0.011422, 0.089881,-0.140507,0.355648, 0.092412,-0.225203,0.279944, 0.288380,-0.225380,0.170160, 0.101854,0.336248,-0.130909, -0.343834,-0.006528,-0.335059, -0.343834,0.160238,-0.192084, +}; +const unsigned int h44_faces[] = { + 7, 14,3,19,9,4,23,1, + 5, 23,6,8,0,1, + 5, 0,25,17,14,1, + 5, 15,22,19,3,2, + 6, 3,14,17,13,20,2, + 4, 20,21,15,2, + 6, 9,10,16,8,6,4, + 3, 6,23,4, + 6, 22,15,21,18,7,5, + 6, 7,11,24,16,10,5, + 5, 10,9,19,22,5, + 4, 18,12,11,7, + 5, 16,24,25,0,8, + 6, 12,13,17,25,24,11, + 5, 18,21,20,13,12, +}; +const dReal h44_planes[ h44_numf * 4 ] = { + 0.727924,0.639056,0.248465,0.256497,-0.131611,0.83191,-0.539077,0.336893,-0.446963,0.816184,0.366152,0.214133,0.991673,-0.108076,0.0700326,0.322253,-0.398278,0.419585,0.815673,0.19534,-0.156805,-0.660773,0.734024,0.339803,0.438192,-0.051293,-0.897417,0.201283,0.722578,0.66608,-0.18498,0.321781,0.20808,-0.904248,0.372879,0.327254,-0.131611,-0.83191,-0.539077,0.231306,0.708232,-0.479648,-0.518021,0.224196,-0.512085,-0.851811,-0.110395,0.248551,-0.443092,0.583496,-0.68059,0.376579,-1,6.26681e-16,4.55506e-17,0.343834,-0.619037,-0.533581,0.576268,0.22428 +}; +// h45 +const int h45_numv = 22; +const int h45_numf = 13; +const dReal h45_volu = 0.073607; +const dReal h45_pos[3] = { -0.692003,0.228592,-0.074767 }; +const dReal h45_verts[ h45_numv * 3 ] = { + 0.206363,-0.247093,-0.162491, 0.114633,-0.178045,0.188010, 0.007810,0.365814,-0.186155, -0.306616,-0.033345,0.126518, -0.055718,0.198405,0.254002, 0.153565,-0.225110,0.105873, -0.184480,-0.294195,0.081791, 0.073037,0.365814,0.132336, 0.155930,-0.080562,0.225951, -0.173050,0.172678,-0.175116, -0.103005,-0.342532,-0.012694, -0.015749,-0.330108,-0.146901, 0.086218,0.030892,-0.320153, -0.015750,-0.032626,-0.299926, 0.228010,-0.214069,-0.005829, 0.104078,0.176805,0.286842, 0.137691,-0.193092,0.159160, 0.102087,0.317557,0.199715, -0.062260,0.197254,0.252076, 0.131971,0.251706,0.244012, 0.186972,0.021095,0.262581, 0.202077,0.032105,0.247435, +}; +const unsigned int h45_faces[] = { + 8, 8,20,15,4,18,3,6,1, + 5, 6,10,5,16,1, + 3, 16,8,1, + 4, 12,13,9,2, + 5, 9,3,18,7,2, + 8, 7,17,19,21,14,0,12,2, + 6, 9,13,11,10,6,3, + 4, 15,19,17,4, + 4, 17,7,18,4, + 6, 14,21,20,8,16,5, + 5, 10,11,0,14,5, + 4, 13,12,0,11, + 4, 20,21,19,15, +}; +const dReal h45_planes[ h45_numf * 4 ] = { + -0.228531,-0.267508,0.936063,0.19742,0.131611,-0.83191,0.539077,0.264556,0.535431,-0.493566,0.685351,0.278107,-0.349687,0.276295,-0.895198,0.264987,-0.717787,0.680568,0.147003,0.21599,0.935836,0.295766,-0.191659,0.151183,-0.793213,-0.278551,-0.5415,0.183991,-0.103318,0.522457,0.846383,0.324397,-0.311731,0.703731,0.638425,0.319154,0.777333,-0.40946,0.477593,0.262108,0.35532,-0.923345,0.145539,0.277827,0.107916,-0.45476,-0.884052,0.278288,0.613049,0.207593,0.762283,0.319163 +}; +// h46 +const int h46_numv = 24; +const int h46_numf = 14; +const dReal h46_volu = 0.077166; +const dReal h46_pos[3] = { -0.506861,-0.263372,-0.148064 }; +const dReal h46_verts[ h46_numv * 3 ] = { + -0.435488,-0.034132,-0.000849, 0.039076,-0.286247,0.028683, -0.031577,0.266854,0.179170, 0.042868,0.277895,0.067468, 0.334025,-0.207064,-0.069876, -0.200891,0.161856,-0.073604, -0.329922,-0.035641,-0.192524, -0.391003,-0.070120,-0.144257, 0.124292,0.270826,-0.018593, -0.288147,0.149432,0.060603, 0.051872,0.188879,0.224374, -0.081382,-0.208658,0.182030, 0.243097,0.052743,0.247717, 0.267097,-0.250173,-0.073376, 0.162691,0.038264,0.287094, 0.339607,0.040898,0.152251, 0.238297,0.049029,0.252023, 0.349700,0.004968,0.138697, 0.021220,0.244871,-0.089194, -0.322311,-0.040247,-0.199445, -0.016599,-0.019353,-0.300951, 0.082088,0.247991,-0.110295, -0.339273,-0.043963,-0.190526, 0.082087,0.036350,-0.291745, +}; +const unsigned int h46_faces[] = { + 6, 7,22,19,20,13,1, + 7, 13,4,17,16,14,11,1, + 4, 11,0,7,1, + 6, 10,12,15,8,3,2, + 5, 3,18,5,9,2, + 6, 9,0,11,14,10,2, + 4, 8,21,18,3, + 4, 13,20,23,4, + 6, 23,21,8,15,17,4, + 7, 18,21,23,20,19,6,5, + 6, 6,22,7,0,9,5, + 3, 19,22,6, + 4, 14,16,12,10, + 4, 16,17,15,12, +}; +const dReal h46_planes[ h46_numf * 4 ] = { + -0.148211,-0.780702,-0.607074,0.200269,0.391463,-0.659847,0.641373,0.222573,-0.471464,-0.878752,0.0742736,0.235248,0.496382,0.766976,0.406636,0.261853,-0.35532,0.923345,-0.145539,0.231542,-0.438192,0.051293,0.897417,0.188315,-0.11569,0.975032,-0.189546,0.253209,0.322885,-0.43293,-0.841615,0.256305,0.792041,0.39734,-0.463457,0.214671,-0.284317,0.624016,-0.727852,0.21169,-0.66062,0.653782,-0.368986,0.26569,-0.494656,0.366713,-0.787932,0.301823,0.289106,0.541464,0.789452,0.2944,0.696788,-0.0734069,0.713511,0.342264 +}; +// h47 +const int h47_numv = 36; +const int h47_numf = 20; +const dReal h47_volu = 0.096745; +const dReal h47_pos[3] = { -0.132135,0.108975,0.072319 }; +const dReal h47_verts[ h47_numv * 3 ] = { + 0.298509,0.263511,-0.009995, -0.406303,-0.105494,-0.041213, 0.295351,0.263929,-0.015607, -0.362255,0.123840,0.142831, -0.016819,0.303436,-0.092155, 0.214655,-0.206919,0.163772, 0.119596,0.038081,0.333996, -0.357791,0.151721,0.100349, 0.141669,-0.202865,0.220831, -0.322854,-0.183468,0.003991, -0.422177,-0.073475,0.012073, 0.248529,0.277838,-0.020262, -0.331858,-0.094452,-0.152914, -0.250434,-0.101521,-0.238975, -0.131628,-0.319604,0.027334, 0.281027,0.056414,-0.190466, 0.134068,0.286800,-0.104666, 0.293847,0.266235,0.003176, -0.403938,0.039055,0.078865, -0.372896,0.140712,0.115495, -0.035119,-0.331449,-0.068132, 0.121232,-0.294347,-0.049046, 0.051634,0.305873,-0.087560, 0.254870,0.274829,-0.031097, 0.196475,0.139795,-0.254001, 0.002640,0.112963,-0.287521, 0.198447,0.073853,0.288417, 0.224142,0.132346,-0.243414, 0.225218,-0.213510,0.136980, 0.298178,-0.027388,0.023425, 0.292448,0.199019,0.106734, 0.304944,0.253729,-0.022071, 0.308556,0.186869,0.065231, 0.305066,0.254217,-0.020526, 0.293027,0.163885,-0.150423, 0.287364,0.062429,-0.181183, +}; +const unsigned int h47_faces[] = { + 6, 12,13,20,14,9,1, + 3, 9,10,1, + 6, 10,18,19,7,12,1, + 4, 0,33,31,2, + 7, 31,34,27,24,16,23,2, + 5, 23,11,17,0,2, + 10, 6,26,30,17,11,22,4,7,19,3, + 3, 19,18,3, + 7, 18,10,9,14,8,6,3, + 5, 22,16,24,25,4, + 5, 25,13,12,7,4, + 6, 28,29,32,30,26,5, + 4, 26,6,8,5, + 6, 8,14,20,21,28,5, + 4, 23,16,22,11, + 7, 25,24,27,15,21,20,13, + 4, 27,34,35,15, + 5, 35,29,28,21,15, + 5, 30,32,33,0,17, + 6, 35,34,31,33,32,29, +}; +const dReal h47_planes[ h47_numf * 4 ] = { + -0.496382,-0.766976,-0.406636,0.299351,-0.722578,-0.66608,0.18498,0.35623,-0.777333,0.40946,-0.477593,0.292321,0.60243,0.746152,-0.283435,0.379283,0.40667,0.730271,-0.54893,0.321417,0.292039,0.95183,-0.0934524,0.338928,-0.066237,0.837367,0.542613,0.205196,-0.897445,0.121213,0.424146,0.400696,-0.398278,-0.419584,0.815674,0.208821,0.0212658,0.716912,-0.696839,0.281397,-0.582464,0.552261,-0.596443,0.232337,0.913105,-0.120244,0.389591,0.284687,0.576312,-0.303571,0.758755,0.310786,0.184883,-0.934913,0.302912,0.282746,0.191593,0.968844,-0.156953,0.319978,0.212159,-0.447018,-0.869002,0.19992,0.770892,0.145024,-0.620237,0.342957,0.836212,-0.484521,-0.256884,0.256592,0.900933,0.358346,0.24476,0.360918,0.997066,-0.0350131,-0.0680664,0.296668 +}; +// h48 +const int h48_numv = 24; +const int h48_numf = 14; +const dReal h48_volu = 0.097516; +const dReal h48_pos[3] = { 0.292729,-0.243911,-0.041409 }; +const dReal h48_verts[ h48_numv * 3 ] = { + -0.303632,0.058539,0.064682, -0.108656,-0.084639,-0.284158, 0.263889,0.072976,-0.223666, 0.274166,-0.102559,0.259484, 0.253318,-0.136696,0.282134, 0.346872,-0.016291,-0.139905, -0.290945,-0.203383,-0.035982, 0.008763,0.056080,-0.365621, 0.246275,0.103070,-0.201305, 0.107569,-0.173012,0.336398, -0.249499,-0.215623,0.017984, -0.234675,-0.199275,-0.122635, 0.023817,-0.271216,0.180037, 0.169540,-0.271216,0.037729, -0.126686,0.325499,0.137154, 0.195210,-0.264110,0.029895, 0.346221,-0.018608,-0.151429, 0.279567,-0.135810,-0.157801, -0.143837,0.409300,-0.076739, -0.137500,0.415315,-0.067456, -0.031223,0.088614,-0.351675, -0.000004,-0.036295,-0.345501, -0.199646,0.139376,0.250708, -0.108109,-0.019468,0.310070, +}; +const unsigned int h48_faces[] = { + 4, 20,7,21,1, + 6, 21,17,15,13,11,1, + 6, 11,6,0,18,20,1, + 5, 16,17,21,7,2, + 6, 7,20,18,19,8,2, + 4, 8,5,16,2, + 6, 14,22,23,9,4,3, + 6, 4,15,17,16,5,3, + 5, 5,8,19,14,3, + 5, 9,12,13,15,4, + 5, 10,23,22,0,6, + 5, 11,13,12,10,6, + 4, 23,10,12,9, + 5, 19,18,0,22,14, +}; +const dReal h48_planes[ h48_numf * 4 ] = { + -0.433652,-0.152283,-0.888119,0.312375,0.066237,-0.837367,-0.542613,0.217865,-0.830851,0.164118,-0.531744,0.227486,0.484779,-0.229821,-0.843903,0.299909,0.322183,0.678839,-0.659831,0.282141,0.794108,0.585614,-0.162633,0.288667,0.212159,0.447018,0.869002,0.237812,0.86695,-0.495807,0.0507289,0.301701,0.66261,0.69806,0.271405,0.180498,0.348894,-0.86639,0.357268,0.30761,-0.794364,-0.251059,0.553132,0.262275,-0.124536,-0.983986,-0.127525,0.240948,-0.49733,-0.589428,0.636583,0.262626,-0.836212,0.484521,0.256884,0.29888 +}; +// h49 +const int h49_numv = 30; +const int h49_numf = 17; +const dReal h49_volu = 0.103417; +const dReal h49_pos[3] = { -0.082648,-0.117151,-0.277803 }; +const dReal h49_verts[ h49_numv * 3 ] = { + 0.344154,-0.038146,-0.115281, -0.165239,0.018854,-0.286732, -0.342125,0.101770,0.019444, -0.222234,-0.108669,-0.237036, -0.321677,-0.099820,-0.189997, 0.084432,-0.330143,0.200412, -0.076061,0.323891,-0.005566, -0.299921,0.124605,0.111147, -0.223248,0.024754,-0.259418, 0.141702,0.335375,-0.073499, 0.231540,0.282540,0.159656, 0.156937,0.153559,-0.266131, -0.243762,0.195138,-0.076614, -0.084607,-0.105324,0.281990, 0.266721,-0.211399,-0.047765, 0.071745,-0.068221,0.301076, -0.090188,-0.353285,0.059864, -0.342125,-0.109871,-0.162005, -0.074513,-0.141253,0.268436, 0.140702,-0.326035,0.113759, 0.082436,-0.372046,-0.016546, 0.056586,-0.396243,0.036572, 0.007365,-0.408266,0.085169, 0.068619,-0.340196,0.196052, 0.183372,0.187050,-0.241355, 0.271282,0.058201,-0.185174, 0.174655,0.358472,0.106708, -0.046848,0.339089,0.062601, 0.146988,0.365921,0.096121, 0.157998,0.356836,0.037994, +}; +const unsigned int h49_faces[] = { + 4, 3,4,8,1, + 7, 8,12,6,9,24,11,1, + 7, 11,25,0,14,20,3,1, + 6, 17,16,18,13,7,2, + 5, 7,27,6,12,2, + 5, 12,8,4,17,2, + 7, 20,21,22,16,17,4,3, + 5, 15,13,18,23,5, + 5, 23,22,21,19,5, + 6, 19,14,0,10,15,5, + 5, 27,28,29,9,6, + 7, 13,15,10,26,28,27,7, + 7, 29,26,10,0,25,24,9, + 3, 24,25,11, + 4, 19,21,20,14, + 4, 22,23,18,16, + 3, 29,28,26, +}; +const dReal h49_planes[ h49_numf * 4 ] = { + -0.433652,-0.152283,-0.888119,0.323437,-0.247318,0.694798,-0.675344,0.247609,0.240814,-0.443968,-0.863077,0.199309,-0.792041,-0.39734,0.463457,0.239551,-0.632147,0.768422,0.0995875,0.296413,-0.805487,0.385719,-0.449901,0.306085,-0.315015,-0.796483,-0.516121,0.2789,-0.0284606,-0.359798,0.932596,0.303286,0.465257,-0.845503,0.262033,0.370934,0.830851,-0.164118,0.531744,0.230901,-0.105626,0.979232,-0.173057,0.326162,-0.212159,0.447018,0.869002,0.215918,0.847832,0.483811,-0.217045,0.29835,0.621965,0.0849382,-0.778424,0.317815,0.654405,-0.755705,-0.0257735,0.33553,-0.49733,-0.589428,0.636583,0.291198,0.292039,0.95183,-0.0934524,0.382238 +}; +// h50 +const int h50_numv = 20; +const int h50_numf = 12; +const dReal h50_volu = 0.069183; +const dReal h50_pos[3] = { -0.304007,-0.060848,0.383791 }; +const dReal h50_verts[ h50_numv * 3 ] = { + 0.035443,-0.153495,-0.279832, 0.291468,0.207904,0.022524, -0.264086,-0.000880,-0.008953, -0.190383,0.293663,-0.168640, 0.004043,-0.311521,0.367447, -0.001962,-0.311376,0.364440, -0.150982,-0.013645,-0.307481, -0.250305,0.096348,-0.299398, 0.040243,-0.149781,-0.284138, -0.040162,-0.164260,-0.244761, -0.228192,0.064423,0.060664, 0.001805,-0.312717,0.366969, -0.232066,0.208878,-0.232607, -0.273363,0.111395,-0.270548, 0.313541,-0.033043,-0.090641, -0.003673,-0.314626,0.361097, -0.023025,-0.208994,-0.179911, -0.066832,-0.328543,0.255913, -0.170020,0.271491,-0.017447, 0.190786,0.198102,0.196479, +}; +const unsigned int h50_faces[] = { + 4, 3,18,19,1, + 4, 19,4,14,1, + 7, 14,8,6,7,12,3,1, + 6, 10,18,3,12,13,2, + 7, 13,7,6,9,16,17,2, + 5, 17,15,5,10,2, + 8, 11,15,17,16,0,8,14,4, + 6, 19,18,10,5,11,4, + 3, 15,11,5, + 4, 8,0,9,6, + 3, 13,12,7, + 3, 0,16,9, +}; +const dReal h50_planes[ h50_numf * 4 ] = { + 0.124536,0.983986,0.127525,0.243745,0.859244,-0.150868,0.488814,0.230086,0.398278,0.419584,-0.815674,0.184946,-0.929286,0.326334,0.173016,0.243575,-0.727924,-0.639056,-0.248465,0.195021,-0.848621,-0.0894025,0.521393,0.219519,0.493232,-0.850288,-0.183662,0.199392,-0.398278,0.419584,0.815674,0.167397,-0.604872,-0.393584,0.692258,0.376026,-0.289106,-0.541464,-0.789452,0.293779,-0.535431,0.493566,-0.685351,0.386768,-0.131611,-0.83191,-0.539077,0.273881 +}; +// h51 +const int h51_numv = 20; +const int h51_numf = 12; +const dReal h51_volu = 0.097496; +const dReal h51_pos[3] = { 0.796238,0.092236,-0.101631 }; +const dReal h51_verts[ h51_numv * 3 ] = { + -0.221011,0.347408,0.061823, -0.319697,0.291705,0.052616, 0.203762,-0.222678,0.367837, -0.200562,0.357460,0.033830, 0.203762,0.194344,-0.275142, -0.239620,-0.263171,-0.163444, 0.152653,-0.230111,0.360793, 0.203762,0.321480,-0.157424, -0.337380,0.280442,0.025237, -0.327866,0.285347,0.016786, -0.028332,0.052718,-0.315616, 0.203762,-0.298399,-0.021674, -0.257234,-0.233077,-0.141083, -0.157288,-0.354755,-0.091207, 0.203762,0.006585,0.328526, -0.156637,-0.352438,-0.079683, 0.030926,0.103995,0.283692, -0.036002,0.060886,0.280191, -0.157223,-0.222560,-0.236947, -0.067033,-0.350976,-0.126852, +}; +const unsigned int h51_faces[] = { + 4, 17,16,0,1, + 5, 0,3,9,8,1, + 6, 8,12,15,6,17,1, + 5, 14,16,17,6,2, + 6, 6,15,13,19,11,2, + 5, 11,4,7,14,2, + 5, 7,4,10,9,3, + 5, 0,16,14,7,3, + 5, 11,19,18,10,4, + 4, 18,19,13,5, + 4, 13,15,12,5, + 6, 12,8,9,10,18,5, +}; +const dReal h51_planes[ h51_numf * 4 ] = { + -0.322885,0.43293,0.841615,0.273796,-0.488006,0.871758,-0.0434538,0.408025,-0.73548,-0.309929,0.602505,0.176424,-0.158394,0.166869,0.973173,0.288536,0.11569,-0.975032,0.189546,0.310413,1,0,0,0.203762,-0.275531,0.653109,-0.705359,0.26486,0.315015,0.796483,0.516121,0.238991,0.398278,-0.419585,-0.815673,0.224036,-0.228161,-0.721101,-0.654183,0.351367,-0.794108,-0.585614,0.162633,0.31782,-0.693474,0.121763,-0.710118,0.250191 +}; +// h52 +const int h52_numv = 16; +const int h52_numf = 10; +const dReal h52_volu = 0.084293; +const dReal h52_pos[3] = { 0.513157,0.018456,0.067444 }; +const dReal h52_verts[ h52_numv * 3 ] = { + -0.347114,0.063132,0.028301, -0.352265,0.254404,-0.145548, 0.247079,0.134666,0.111116, -0.336736,0.277388,0.070106, 0.126444,-0.278658,-0.248758, -0.340226,0.344736,-0.015651, 0.295476,-0.245782,0.289096, -0.054299,0.354222,-0.143838, 0.053738,-0.364926,0.150631, -0.036616,0.365485,-0.116459, 0.025847,-0.159297,-0.310158, -0.357928,0.152948,-0.176308, 0.019058,0.098592,0.213176, 0.435734,-0.156331,0.191718, -0.340348,0.344248,-0.017196, -0.236518,0.326242,-0.134679, +}; +const unsigned int h52_faces[] = { + 3, 14,15,1, + 5, 15,7,10,11,1, + 6, 11,0,3,5,14,1, + 4, 12,6,13,2, + 6, 13,4,10,7,9,2, + 5, 9,5,3,12,2, + 5, 0,8,6,12,3, + 4, 13,6,8,4, + 5, 8,0,11,10,4, + 5, 9,7,15,14,5, +}; +const dReal h52_planes[ h52_numf * 4 ] = { + -0.425203,0.759567,-0.4922,0.41466,-0.0929092,0.293639,-0.951391,0.245904,-0.997066,0.0350131,0.0680664,0.350232,0.309646,0.434949,0.84554,0.229033,0.73548,0.309929,-0.602505,0.156511,0.148211,0.780702,0.607074,0.209209,-0.433652,-0.152285,0.888119,0.166047,0.485104,-0.868795,-0.0993492,0.328149,-0.66261,-0.69806,-0.271405,0.17825,-0.159459,0.944944,-0.285751,0.38448 +}; +// h53 +const int h53_numv = 16; +const int h53_numf = 10; +const dReal h53_volu = 0.086476; +const dReal h53_pos[3] = { -0.781555,0.807509,0.197497 }; +const dReal h53_verts[ h53_numv * 3 ] = { + 0.291496,-0.156419,-0.091704, -0.218445,0.192491,0.461269, -0.218445,0.192491,-0.255541, -0.218445,-0.397567,0.047987, 0.027292,-0.381663,-0.020188, -0.218445,-0.373860,0.169935, 0.033835,-0.380512,-0.018262, 0.352556,0.192491,0.043056, 0.162589,-0.213103,-0.139928, 0.191640,-0.261360,-0.072549, 0.238502,0.192491,-0.291089, 0.253338,0.013015,-0.278399, 0.326334,-0.087525,-0.081781, 0.317517,-0.112633,-0.111918, 0.238502,0.081667,-0.310092, 0.153590,0.192491,-0.346370, +}; +const unsigned int h53_faces[] = { + 7, 5,6,9,0,12,7,1, + 5, 7,10,15,2,1, + 4, 2,3,5,1, + 7, 15,14,11,8,4,3,2, + 4, 4,6,5,3, + 4, 8,9,6,4, + 6, 12,13,11,14,10,7, + 5, 11,13,0,9,8, + 3, 14,15,10, + 3, 0,13,12, +}; +const dReal h53_planes[ h53_numf * 4 ] = { + 0.545755,-0.383303,0.745138,0.150709,-0,1,0,0.192491,-1,0,-0,0.218445,-0.212159,-0.447018,-0.869002,0.182363,0.11569,-0.975032,0.189546,0.371464,0.311731,-0.703731,-0.638425,0.289984,0.944939,0.0553043,-0.322539,0.329903,0.49733,-0.589429,-0.636582,0.295545,0.540055,0.142236,-0.829524,0.397648,0.877441,-0.462191,0.128357,0.316295 +}; +// h54 +const int h54_numv = 20; +const int h54_numf = 12; +const dReal h54_volu = 0.089003; +const dReal h54_pos[3] = { -0.845912,0.679404,-0.140778 }; +const dReal h54_verts[ h54_numv * 3 ] = { + 0.317695,0.141120,0.059877, 0.091649,-0.253558,0.318087, -0.154088,0.080768,-0.359617, -0.152707,-0.484157,0.192529, -0.154088,0.320596,0.082735, 0.078976,0.204301,-0.281846, -0.154088,-0.269462,0.386262, -0.154088,0.320596,-0.236248, -0.154088,-0.484416,0.192760, 0.226946,-0.084997,0.198347, 0.060656,0.320596,-0.223141, 0.161719,-0.084997,-0.120143, 0.217947,0.320596,-0.008094, -0.049829,0.124065,-0.330981, -0.154088,-0.230098,-0.199706, -0.019141,-0.278134,-0.109104, 0.160374,0.124065,-0.228342, 0.234799,0.071081,-0.164747, 0.302859,0.209772,0.028184, 0.317695,0.159603,0.012337, +}; +const unsigned int h54_faces[] = { + 4, 6,8,3,1, + 5, 3,15,11,9,1, + 7, 9,0,18,12,4,6,1, + 5, 7,10,5,13,2, + 7, 13,16,17,11,15,14,2, + 6, 14,8,6,4,7,2, + 4, 8,14,15,3, + 4, 12,10,7,4, + 7, 10,12,18,19,17,16,5, + 3, 16,13,5, + 5, 11,17,19,0,9, + 3, 0,19,18, +}; +const dReal h54_planes[ h54_numf * 4 ] = { + 0.242073,-0.64915,0.721114,0.41616,0.717787,-0.680568,-0.147003,0.191588,0.212159,0.447018,0.869002,0.182518,0.0541955,0.456759,-0.887938,0.347858,0.398278,-0.419585,-0.815673,0.19807,-1,0,0,0.154088,0.066237,-0.837367,-0.542613,0.290833,0,1,0,0.320596,0.742791,0.391264,-0.543299,0.291725,0.433652,-0.152285,-0.888119,0.253448,0.870583,-0.45858,-0.178295,0.201189,0.962317,0.253449,0.0985412,0.34739 +}; +// h55 +const int h55_numv = 16; +const int h55_numf = 10; +const dReal h55_volu = 0.055290; +const dReal h55_pos[3] = { 0.087136,0.788999,0.139771 }; +const dReal h55_verts[ h55_numv * 3 ] = { + 0.076080,-0.416095,-0.083059, -0.323951,0.211001,0.286715, 0.205112,0.211001,-0.185127, 0.076081,0.211001,-0.405639, -0.148237,0.211001,0.332473, 0.101171,-0.226374,0.192117, -0.006832,-0.085411,0.277812, 0.205111,-0.223849,0.038561, -0.274823,0.078877,0.258729, 0.035595,0.211001,-0.415523, 0.074576,-0.413789,-0.064275, 0.079238,-0.416513,-0.077447, 0.107256,-0.249478,0.168347, 0.117855,-0.236273,0.172768, 0.029258,-0.402186,-0.087713, 0.035598,-0.405195,-0.098549, +}; +const unsigned int h55_faces[] = { + 4, 8,6,4,1, + 5, 4,2,3,9,1, + 5, 9,15,14,8,1, + 5, 7,11,0,3,2, + 6, 4,6,5,13,7,2, + 4, 0,15,9,3, + 3, 12,13,5, + 6, 6,8,14,10,12,5, + 5, 13,12,10,11,7, + 5, 14,15,0,11,10, +}; +const dReal h55_planes[ h55_numf * 4 ] = { + -0.241471,-0.286187,0.927248,0.283694,0,1,0,0.211001,-0.866617,-0.228251,-0.443708,0.105363,0.835368,-0.251447,-0.48881,0.20878,0.793214,0.278548,0.541501,0.121225,0.212159,-0.447018,-0.869002,0.274321,0.443092,-0.583496,0.68059,0.307669,-0.463507,-0.691764,0.553737,0.216086,0.66062,-0.653782,0.368986,0.296077,-0.292039,-0.95183,0.0934524,0.366071 +}; +// h56 +const int h56_numv = 20; +const int h56_numf = 12; +const dReal h56_volu = 0.096963; +const dReal h56_pos[3] = { -0.186579,0.800379,0.005269 }; +const dReal h56_verts[ h56_numv * 3 ] = { + 0.004502,-0.334912,-0.018030, 0.302973,-0.413566,0.046788, 0.309310,0.199621,-0.281021, -0.242420,0.199621,0.235284, -0.121421,0.199621,0.412527, 0.172708,0.199621,-0.357250, -0.050236,0.199621,0.421217, -0.341638,0.020145,-0.086171, -0.356474,0.199621,-0.098861, 0.097656,0.085128,-0.340236, -0.001108,0.067497,0.393231, -0.356474,0.088797,-0.117864, -0.277459,-0.105503,0.080310, 0.309314,-0.416575,0.035953, 0.106078,-0.385531,-0.020510, 0.188512,-0.404604,-0.037617, -0.268642,-0.080395,0.110447, -0.118494,0.123210,0.393231, -0.341638,0.038628,-0.133710, -0.322853,0.025254,-0.145176, +}; +const unsigned int h56_faces[] = { + 4, 14,15,13,1, + 5, 13,2,6,10,1, + 7, 10,17,16,12,0,14,1, + 5, 13,15,9,5,2, + 6, 5,8,3,4,6,2, + 4, 16,17,4,3, + 6, 8,11,7,12,16,3, + 4, 17,10,6,4, + 6, 9,19,18,11,8,5, + 5, 18,19,0,12,7, + 3, 11,18,7, + 5, 15,14,0,19,9, +}; +const dReal h56_planes[ h56_numf * 4 ] = { + -0.191593,-0.968844,0.156953,0.349977,0.866617,0.228251,0.443708,0.188925,-0.322183,-0.678838,0.659831,0.214003,0.444511,-0.409753,-0.796563,0.279548,4.04497e-16,1,-1.19541e-16,0.199621,-0.813673,-0.17144,0.555468,0.293719,-0.944939,-0.0553043,0.322539,0.29392,-0.117405,-0.247371,0.961782,0.361636,-0.433652,0.152285,-0.888119,0.272785,-0.662611,-0.698059,-0.271405,0.235698,-0.962317,-0.253449,-0.0985412,0.33215,-0.289106,-0.541462,-0.789453,0.194274 +}; +// h57 +const int h57_numv = 16; +const int h57_numf = 10; +const dReal h57_volu = 0.081294; +const dReal h57_pos[3] = { 0.851151,0.533342,0.098902 }; +const dReal h57_verts[ h57_numv * 3 ] = { + -0.184097,0.265595,0.004922, 0.148849,0.009818,-0.380153, 0.148849,-0.049459,0.325702, -0.275924,-0.093697,-0.138710, 0.148849,-0.119626,-0.357957, -0.275924,0.137573,0.059567, -0.112901,0.343928,0.059438, -0.249462,0.157589,0.121951, 0.148849,-0.434521,0.127992, 0.148849,0.447452,0.070089, 0.148849,-0.343521,0.212252, -0.023987,-0.337111,0.083159, -0.018406,-0.089149,0.305285, -0.008312,-0.125079,0.291731, -0.255475,-0.083645,-0.166703, -0.157047,0.040928,-0.236124, +}; +const unsigned int h57_faces[] = { + 4, 4,14,15,1, + 5, 15,0,6,9,1, + 6, 9,2,10,8,4,1, + 4, 12,13,10,2, + 5, 9,6,7,12,2, + 5, 14,4,8,11,3, + 6, 11,13,12,7,5,3, + 5, 5,0,15,14,3, + 4, 7,6,0,5, + 4, 10,13,11,8, +}; +const dReal h57_planes[ h57_numf * 4 ] = { + -0.433652,-0.152285,-0.888119,0.271577,-0.247318,0.694798,-0.675344,0.226742,1,0,-0,0.148849,-0.0284606,-0.359798,0.932596,0.317307,-0.212159,0.447018,0.869002,0.229347,-0.315015,-0.796483,-0.516121,0.233139,-0.792041,-0.39734,0.463457,0.191487,-0.805488,0.385717,-0.449902,0.248519,-0.777693,0.614473,0.132726,0.307026,-0.49733,-0.589428,0.636583,0.26357 +}; +// h58 +const int h58_numv = 20; +const int h58_numf = 12; +const dReal h58_volu = 0.077812; +const dReal h58_pos[3] = { 0.374418,0.849026,0.291935 }; +const dReal h58_verts[ h58_numv * 3 ] = { + -0.082171,-0.283876,-0.113603, 0.200809,-0.178111,-0.133466, -0.082170,0.150974,-0.337291, 0.089508,-0.034332,0.259248, 0.146093,0.150974,0.326019, 0.410849,0.150974,-0.104898, -0.435519,0.150974,0.180309, -0.294114,-0.145438,0.125648, 0.292636,-0.050089,-0.188111, 0.183309,0.150974,-0.304884, 0.227271,-0.158095,-0.071082, 0.363832,0.028244,-0.133595, -0.169427,-0.296300,0.020604, 0.087144,-0.178878,0.139171, 0.071270,-0.146859,0.192458, -0.429747,0.150974,0.185433, -0.186111,-0.286401,0.039952, 0.048213,-0.131812,0.221308, -0.268420,-0.171352,0.132706, -0.212551,-0.233071,0.128708, +}; +const unsigned int h58_faces[] = { + 5, 10,13,12,0,1, + 5, 0,2,9,8,1, + 4, 8,11,10,1, + 6, 0,12,16,7,6,2, + 6, 6,15,4,5,9,2, + 6, 4,15,18,19,17,3, + 3, 17,14,3, + 7, 14,13,10,11,5,4,3, + 4, 11,8,9,5, + 4, 7,18,15,6, + 4, 16,19,18,7, + 6, 13,14,17,19,16,12, +}; +const dReal h58_planes[ h58_numf * 4 ] = { + 0.35532,-0.923345,0.145539,0.216384,0.107916,-0.45476,-0.884052,0.220658,0.777693,-0.614473,-0.132726,0.283326,-0.793214,-0.278548,-0.541501,0.205768,0,1,-0,0.150974,-0.228531,-0.267508,0.936063,0.231401,0.535431,-0.493566,0.685351,0.242546,0.777334,-0.409459,0.477594,0.20745,0.658203,-0.077045,-0.748888,0.337347,-0.604377,-0.413861,0.68077,0.323484,-0.722579,-0.66608,0.18498,0.332636,0.131611,-0.83191,0.539078,0.235303 +}; +// h59 +const int h59_numv = 12; +const int h59_numf = 8; +const dReal h59_volu = 0.098632; +const dReal h59_pos[3] = { 0.795288,0.838821,-0.270144 }; +const dReal h59_verts[ h59_numv * 3 ] = { + -0.010021,0.161179,0.457181, -0.356162,0.161179,-0.206090, 0.204712,-0.152000,-0.380610, 0.204712,0.161179,-0.434310, -0.057037,0.038449,0.428484, -0.237561,0.161179,0.257195, -0.256523,-0.089455,-0.203657, -0.128234,-0.039885,0.373968, 0.204712,-0.295661,-0.011106, -0.101183,-0.264551,0.132922, 0.204712,0.141973,0.439135, 0.204712,0.161179,0.444075, +}; +const unsigned int h59_faces[] = { + 4, 3,2,6,1, + 5, 6,9,7,5,1, + 5, 5,0,11,3,1, + 5, 3,11,10,8,2, + 4, 8,9,6,2, + 4, 10,11,0,4, + 4, 0,5,7,4, + 5, 7,9,8,10,4, +}; +const dReal h59_planes[ h59_numf * 4 ] = { + -0.372229,-0.156858,-0.91479,0.29582,-0.90476,-0.357438,0.231619,0.216895,0,1,0,0.161178,1,1.92688e-15,-1.87292e-15,0.204712,-0.256529,-0.900844,-0.350247,0.217721,0.0590083,-0.248661,0.966792,0.401329,-0.658203,0.077045,0.748888,0.361391,0.247318,-0.694798,0.675344,0.248554 +}; +// h60 +const int h60_numv = 22; +const int h60_numf = 13; +const dReal h60_volu = 0.091310; +const dReal h60_pos[3] = { 0.415485,0.715407,-0.130668 }; +const dReal h60_verts[ h60_numv * 3 ] = { + -0.064779,0.284593,-0.364069, 0.142242,0.284593,0.117719, -0.242554,-0.352215,0.182461, 0.061056,-0.331466,0.081653, 0.180191,-0.265711,0.062868, 0.251569,0.083531,0.234492, -0.123237,0.284593,0.085312, -0.123238,-0.150257,0.309000, 0.159742,-0.044491,0.289138, 0.278619,-0.141137,-0.006553, -0.252268,0.284593,-0.135200, 0.123280,0.033960,-0.343133, 0.052142,0.000197,-0.360501, 0.023641,0.284593,-0.345566, 0.159742,-0.275763,0.090860, -0.138846,-0.370709,0.063434, -0.249111,-0.342921,0.192992, -0.046519,-0.292460,-0.089523, -0.252269,-0.342503,0.187380, -0.242676,-0.352703,0.180916, 0.052887,-0.337824,0.045824, 0.043373,-0.342729,0.054274, +}; +const unsigned int h60_faces[] = { + 5, 5,9,11,13,1, + 5, 13,0,10,6,1, + 5, 6,7,8,5,1, + 4, 16,18,19,2, + 5, 19,15,21,3,2, + 6, 3,14,8,7,16,2, + 5, 21,20,4,14,3, + 6, 20,17,12,11,9,4, + 5, 9,5,8,14,4, + 5, 10,18,16,7,6, + 7, 0,12,17,15,19,18,10, + 4, 12,0,13,11, + 4, 17,20,21,15, +}; +const dReal h60_planes[ h60_numf * 4 ] = { + 0.90476,0.357438,-0.231619,0.203154,8.63556e-17,1,-5.05304e-17,0.284593,-0.107916,0.45476,0.884052,0.218141,-0.60243,-0.746152,0.283435,0.460644,0.159459,-0.944944,0.285751,0.346285,0.284317,-0.624015,0.727852,0.28363,0.488006,-0.871758,0.0434538,0.322301,0.46199,-0.681388,-0.567693,0.228609,0.805488,-0.385717,0.449902,0.275915,-0.835368,0.251447,0.48881,0.21621,-0.73548,-0.309929,-0.602505,0.178793,0.204299,0.0717432,-0.976276,0.362615,0.120149,-0.911351,-0.393704,0.29619 +}; +// h61 +const int h61_numv = 24; +const int h61_numf = 14; +const dReal h61_volu = 0.075853; +const dReal h61_pos[3] = { 0.499694,0.403970,0.252554 }; +const dReal h61_verts[ h61_numv * 3 ] = { + 0.101994,0.286961,-0.031701, -0.294704,0.148756,0.059985, -0.023153,-0.020028,-0.301569, 0.156136,0.037589,0.286475, -0.323273,-0.108125,-0.115004, 0.032521,-0.286922,0.028066, -0.038132,0.266178,0.178552, 0.260541,-0.250848,-0.073994, 0.327470,-0.207739,-0.070493, 0.075532,0.266945,-0.094084, 0.075532,0.035675,-0.292362, 0.343145,0.004293,0.138079, 0.231743,0.048354,0.251404, 0.045317,0.188205,0.223756, -0.087937,-0.209333,0.181412, -0.330859,-0.089605,0.055955, 0.333052,0.040223,0.151633, 0.236542,0.052068,0.247099, -0.339382,-0.095975,-0.073501, -0.326763,-0.040778,-0.200761, -0.207448,0.161180,-0.074222, -0.333320,-0.031483,-0.190230, -0.305302,0.135551,0.055565, -0.337982,-0.028760,-0.177059, +}; +const unsigned int h61_faces[] = { + 5, 20,21,23,22,1, + 7, 22,15,14,3,13,6,1, + 5, 6,0,9,20,1, + 5, 7,5,4,19,2, + 6, 19,21,20,9,10,2, + 4, 10,8,7,2, + 7, 14,5,7,8,11,12,3, + 4, 12,17,13,3, + 5, 18,23,21,19,4, + 5, 5,14,15,18,4, + 5, 13,17,16,0,6, + 6, 10,9,0,16,11,8, + 4, 16,17,12,11, + 4, 22,23,18,15, +}; +const dReal h61_planes[ h61_numf * 4 ] = { + -0.66062,0.653782,-0.368986,0.269808,-0.438192,0.051293,0.897417,0.190598,-0.35532,0.923345,-0.145539,0.233338,-0.148211,-0.780702,-0.607074,0.202142,-0.284317,0.624015,-0.727852,0.213583,0.322885,-0.43293,-0.841615,0.255,0.391463,-0.659847,0.641373,0.220056,0.289106,0.541462,0.789453,0.291652,-0.900933,-0.358346,-0.24476,0.358143,-0.471464,-0.878752,0.0742736,0.238885,0.496382,0.766976,0.406636,0.25783,0.792041,0.39734,-0.463457,0.209497,0.696788,-0.0734069,0.713511,0.337305,-0.991833,0.112685,0.0597434,0.321404 +}; +// h62 +const int h62_numv = 18; +const int h62_numf = 11; +const dReal h62_volu = 0.089635; +const dReal h62_pos[3] = { -0.412607,-0.798932,0.393221 }; +const dReal h62_verts[ h62_numv * 3 ] = { + 0.143743,0.178906,-0.080212, -0.336591,-0.057070,0.045432, -0.365487,-0.201068,0.063069, -0.043446,-0.201068,0.325146, -0.118881,0.178753,-0.144486, -0.152818,-0.201068,-0.248459, 0.205095,-0.201068,-0.388268, 0.337853,-0.164947,-0.214818, 0.038585,0.368996,0.228990, 0.242459,-0.068737,-0.310888, -0.200495,0.172297,-0.114599, -0.231981,0.184967,-0.066303, 0.334110,-0.201068,-0.227916, 0.209245,-0.110463,-0.361925, 0.145031,0.335652,0.325145, 0.169974,0.297178,0.268816, -0.151147,0.272981,0.102025, 0.044821,0.272804,-0.007759, +}; +const unsigned int h62_faces[] = { + 5, 11,10,5,2,1, + 6, 2,3,14,8,16,1, + 3, 16,11,1, + 5, 5,6,12,3,2, + 5, 12,7,15,14,3, + 6, 10,11,16,8,17,4, + 5, 17,0,9,13,4, + 5, 13,6,5,10,4, + 5, 13,9,7,12,6, + 4, 9,0,15,7, + 5, 14,15,0,17,8, +}; +const dReal h62_planes[ h62_numf * 4 ] = { + -0.822068,0.096228,-0.561199,0.245712,-0.616244,0.216405,0.75724,0.229475,-0.870583,0.45858,0.178295,0.27496,4.16641e-16,-1,1.84056e-16,0.201068,0.793214,-0.278548,0.541501,0.197612,-0.20808,0.904248,-0.372879,0.24025,0.167754,0.706913,-0.687119,0.205699,-0.349687,0.276295,-0.895198,0.220304,0.770892,0.145024,-0.620237,0.369765,0.799798,0.547681,-0.245698,0.232657,0.526505,0.792388,-0.308081,0.242156 +}; +// h63 +const int h63_numv = 20; +const int h63_numf = 12; +const dReal h63_volu = 0.065070; +const dReal h63_pos[3] = { -0.683722,-0.408459,0.881106 }; +const dReal h63_verts[ h63_numv * 3 ] = { + 0.408763,0.050375,-0.050997, 0.001643,0.372246,0.104787, -0.316278,-0.373443,0.118894, -0.000569,0.379463,0.118894, -0.316278,-0.303582,-0.012874, 0.413107,-0.090210,0.118894, 0.072977,-0.026465,-0.250457, -0.316278,0.104758,0.118894, -0.133134,0.036421,-0.232486, -0.316278,-0.061367,-0.137470, 0.406500,0.014459,-0.083368, 0.416439,0.055265,0.118894, 0.411396,0.069115,-0.001513, 0.370009,0.144956,0.118894, 0.377753,0.036235,-0.132875, 0.381520,0.034894,-0.130346, 0.374104,0.031050,-0.138388, 0.376042,0.032985,-0.136218, 0.383396,0.034505,-0.128411, 0.385682,0.029196,-0.126410, +}; +const unsigned int h63_faces[] = { + 5, 8,9,7,3,1, + 8, 3,13,12,0,18,15,14,1, + 6, 14,17,16,6,8,1, + 7, 4,6,16,19,10,5,2, + 6, 5,11,13,3,7,2, + 4, 7,9,4,2, + 4, 9,8,6,4, + 5, 10,0,12,11,5, + 4, 19,18,0,10, + 3, 12,13,11, + 3, 15,17,14, + 5, 18,19,16,17,15, +}; +const dReal h63_planes[ h63_numf * 4 ] = { + -0.589724,0.677749,-0.439183,0.2053,0.50496,0.797961,-0.32905,0.263387,0.141863,0.672536,-0.72634,0.174471,0.324513,-0.835692,-0.443069,0.156769,2.1681e-17,-2.49173e-17,1,0.118894,-1,0,0,0.316278,-0.212159,-0.447018,-0.869002,0.213995,0.998749,-0.0228741,-0.044466,0.409367,0.901569,0.256913,-0.348094,0.399222,0.887955,0.459664,0.015677,0.397046,0.604872,0.393584,-0.692258,0.334738,0.721996,0.0507074,-0.690036,0.367169 +}; +// h64 +const int h64_numv = 14; +const int h64_numf = 9; +const dReal h64_volu = 0.063334; +const dReal h64_pos[3] = { -0.862240,-0.589074,0.504780 }; +const dReal h64_verts[ h64_numv * 3 ] = { + -0.034062,-0.410926,-0.010011, 0.045384,0.217035,0.143840, 0.295955,0.147819,0.066170, 0.251495,0.154150,0.125869, -0.137760,-0.410926,0.116573, -0.137760,0.324258,-0.236366, -0.137760,0.210059,-0.342106, -0.137760,-0.122967,0.363452, -0.137760,-0.410926,-0.022669, 0.298486,0.063123,-0.009534, -0.137760,0.298525,-0.037806, 0.113042,-0.266928,-0.066127, -0.137760,0.119248,0.238856, 0.217652,-0.024891,-0.177862, +}; +const unsigned int h64_faces[] = { + 3, 10,12,1, + 4, 12,7,3,1, + 5, 3,2,5,10,1, + 5, 9,13,6,5,2, + 7, 3,7,4,0,11,9,2, + 7, 7,12,10,5,6,8,4, + 3, 8,0,4, + 5, 13,11,0,8,6, + 3, 11,13,9, +}; +const dReal h64_planes[ h64_numf * 4 ] = { + -0.163718,0.827886,0.536471,0.249417,0.212159,0.447018,0.869002,0.231645,0.299389,0.946218,0.12263,0.23659,0.619037,0.533581,-0.576268,0.223949,0.679728,-0.477395,0.556833,0.167446,-1,0,0,0.13776,-0,-1,0,0.410926,0.107916,-0.45476,-0.884052,0.192047,0.870583,-0.45858,-0.178295,0.23261 +}; +// h65 +const int h65_numv = 22; +const int h65_numf = 13; +const dReal h65_volu = 0.115639; +const dReal h65_pos[3] = { -0.632833,-0.759826,0.781144 }; +const dReal h65_verts[ h65_numv * 3 ] = { + -0.367167,-0.022076,0.218856, 0.176779,-0.240174,-0.062777, -0.145261,-0.240174,-0.324854, -0.367167,-0.240174,-0.159791, -0.367167,-0.240174,0.218856, 0.229214,-0.240174,0.218856, -0.367167,0.047785,0.087088, 0.379478,0.318313,-0.027451, 0.258810,0.329890,-0.158933, 0.022088,0.324902,-0.150495, -0.263470,-0.240174,-0.286375, -0.116365,-0.096176,-0.342491, 0.384007,0.200623,0.218856, 0.365257,0.296546,-0.062778, 0.323215,0.382417,-0.038426, 0.362218,0.261157,0.218856, 0.374110,0.331198,-0.022696, 0.069079,0.233875,-0.285898, 0.066548,0.318571,-0.210194, 0.259984,0.356129,-0.135062, 0.334793,0.380563,-0.026448, 0.355611,0.365826,0.016594, +}; +const unsigned int h65_faces[] = { + 5, 13,7,12,5,1, + 6, 5,4,3,10,2,1, + 6, 2,11,17,8,13,1, + 3, 10,11,2, + 7, 6,9,18,17,11,10,3, + 4, 4,0,6,3, + 5, 5,12,15,0,4, + 7, 0,15,21,20,14,9,6, + 7, 13,8,19,14,20,16,7, + 5, 16,21,15,12,7, + 4, 17,18,19,8, + 4, 14,19,18,9, + 3, 20,21,16, +}; +const dReal h65_planes[ h65_numf * 4 ] = { + 0.929286,-0.326334,-0.173016,0.253517,5.14845e-16,-1,8.60294e-16,0.240174,0.616244,-0.216405,-0.75724,0.208451,-0.309078,-0.0542691,-0.949487,0.366376,-0.679728,0.477395,-0.556833,0.223893,-1,0,0,0.367167,-0,0,1,0.218856,-0.324513,0.835692,0.443069,0.19767,0.664689,0.486283,-0.567201,0.422595,0.931229,0.335191,0.143037,0.45615,0.156805,0.660773,-0.734024,0.375226,-0.11569,0.975032,-0.189546,0.34276,0.777249,0.606249,-0.168364,0.495386 +}; +// h66 +const int h66_numv = 12; +const int h66_numf = 8; +const dReal h66_volu = 0.090843; +const dReal h66_pos[3] = { 0.194452,-0.846656,0.702344 }; +const dReal h66_verts[ h66_numv * 3 ] = { + 0.144722,0.221953,-0.171396, 0.354760,-0.153344,-0.123136, 0.229722,0.177289,-0.118776, -0.077080,-0.093853,-0.442151, -0.263367,-0.153344,0.297656, 0.293204,-0.153344,0.297656, -0.291202,0.163722,0.297656, 0.229722,0.015381,0.297656, -0.072437,-0.153344,-0.456885, -0.246539,0.275009,-0.021290, -0.133324,-0.153344,-0.464317, -0.137311,-0.122439,-0.456854, +}; +const unsigned int h66_faces[] = { + 5, 8,3,0,2,1, + 4, 2,7,5,1, + 5, 5,4,10,8,1, + 5, 0,9,6,7,2, + 4, 8,10,11,3, + 4, 11,9,0,3, + 5, 6,9,11,10,4, + 4, 5,7,6,4, +}; +const dReal h66_planes[ h66_numf * 4 ] = { + 0.598205,0.236328,-0.765702,0.270266,0.927293,0.348894,0.13565,0.258762,2.17789e-18,-1,-1.78413e-17,0.153344,0.256529,0.900843,0.350248,0.17704,0.117405,0.247372,-0.961781,0.392986,-0.167754,0.706913,-0.687119,0.250394,-0.982077,-0.0862167,-0.167608,0.221978,0,0,1,0.297657 +}; +// h67 +const int h67_numv = 14; +const int h67_numf = 9; +const dReal h67_volu = 0.073139; +const dReal h67_pos[3] = { -0.182450,-0.819619,0.699488 }; +const dReal h67_verts[ h67_numv * 3 ] = { + 0.085699,0.136685,0.300512, -0.221169,-0.180381,0.300512, -0.273604,-0.180381,0.018878, 0.130363,0.247972,-0.018435, -0.085126,0.356339,0.018877, 0.113535,-0.180381,0.300512, -0.066376,0.260416,0.300512, -0.060183,0.317865,-0.037452, 0.103953,-0.180381,-0.534183, 0.107696,-0.144260,-0.521085, 0.239592,-0.149476,-0.453999, 0.243578,-0.180381,-0.461462, -0.070905,0.378106,0.054205, -0.032656,0.358692,0.028913, +}; +const unsigned int h67_faces[] = { + 5, 6,12,4,2,1, + 5, 2,8,11,5,1, + 4, 5,0,6,1, + 5, 4,7,9,8,2, + 5, 10,9,7,13,3, + 5, 13,12,6,0,3, + 5, 0,5,11,10,3, + 4, 12,13,7,4, + 4, 9,10,11,8, +}; +const dReal h67_planes[ h67_numf * 4 ] = { + -0.929286,0.326334,0.173016,0.198658,4.99955e-17,-1,-5.73911e-19,0.180381,0,0,1,0.300512,-0.793214,0.278548,-0.541501,0.156559,0.32928,0.737152,-0.590069,0.236597,0.59399,0.730061,0.337914,0.25224,0.982077,0.0862167,0.167608,0.146316,0.0662371,0.837367,-0.542614,0.282505,0.445543,0.264025,-0.855443,0.455654 +}; +// h68 +const int h68_numv = 24; +const int h68_numf = 14; +const dReal h68_volu = 0.110178; +const dReal h68_pos[3] = { 0.194790,-0.476753,0.823942 }; +const dReal h68_verts[ h68_numv * 3 ] = { + -0.387371,0.030890,-0.107289, 0.221446,0.021860,-0.353469, -0.465405,-0.021917,0.176058, -0.291540,-0.206181,0.176058, 0.076464,0.334736,-0.007133, 0.229384,-0.354523,0.176058, 0.292644,0.032441,-0.334080, -0.452945,0.146290,0.025179, 0.246219,0.093034,-0.297244, 0.229037,0.025757,-0.352826, 0.107091,0.321655,0.176058, 0.390186,-0.018723,0.176058, 0.312682,0.010704,-0.315910, 0.229384,-0.192615,-0.240374, 0.144384,-0.147950,-0.292995, -0.246877,-0.094895,-0.142888, -0.409896,0.015825,-0.095540, -0.443616,-0.082450,0.176058, -0.462073,0.123559,0.176058, -0.472012,0.082753,-0.026204, -0.453513,0.048124,-0.065494, -0.448145,0.035240,-0.070249, -0.469749,0.118668,0.006167, -0.467116,0.137408,0.055651, +}; +const unsigned int h68_faces[] = { + 5, 14,15,16,0,1, + 9, 0,20,19,22,7,4,8,9,1, + 6, 9,6,12,13,14,1, + 5, 19,20,21,17,2, + 7, 17,3,5,11,10,18,2, + 5, 18,23,22,19,2, + 5, 17,21,16,15,3, + 5, 15,14,13,5,3, + 6, 10,11,12,6,8,4, + 5, 7,23,18,10,4, + 4, 13,12,11,5, + 3, 9,8,6, + 3, 22,23,7, + 4, 21,20,0,16, +}; +const dReal h68_planes[ h68_numf * 4 ] = { + -0.372229,-0.156856,-0.91479,0.237492,-0.27553,0.653111,-0.705358,0.202583,0.296224,-0.4369,-0.849335,0.35626,-0.931229,-0.335191,-0.143037,0.415561,-0,0,1,0.176058,-0.998749,0.0228741,0.044466,0.472149,-0.59399,-0.730061,-0.337914,0.264205,-0.256529,-0.900843,-0.350248,0.198862,0.766208,0.637263,-0.0825892,0.272492,-0.326272,0.937436,0.121492,0.287979,0.88947,-0.425933,-0.165603,0.325878,0.16676,0.602337,-0.780629,0.329134,-0.777217,0.601031,-0.186267,0.435271,-0.512456,0.101226,-0.852726,0.293125 +}; +// h69 +const int h69_numv = 18; +const int h69_numf = 11; +const dReal h69_volu = 0.150904; +const dReal h69_pos[3] = { 0.751581,-0.668854,0.749549 }; +const dReal h69_verts[ h69_numv * 3 ] = { + 0.248419,0.357221,0.250451, -0.263925,-0.331146,0.250451, 0.248419,0.283773,-0.306518, 0.248419,-0.331146,0.009798, 0.000203,0.129153,-0.348181, 0.248419,-0.331146,0.250451, -0.202369,-0.331146,-0.170342, -0.161439,-0.331146,-0.190328, 0.248419,0.424683,-0.270088, -0.087381,0.269445,-0.329471, -0.149648,0.251714,-0.296027, -0.327407,-0.162422,0.250451, -0.166605,0.173379,0.250451, -0.045433,0.301175,-0.321470, -0.327407,-0.000512,-0.165981, -0.244109,0.202806,-0.241517, 0.223596,0.415674,-0.285417, 0.248419,0.419976,-0.283164, +}; +const unsigned int h69_faces[] = { + 4, 11,14,6,1, + 5, 6,7,3,5,1, + 5, 5,0,12,11,1, + 6, 4,9,13,16,17,2, + 6, 17,8,0,5,3,2, + 4, 3,7,4,2, + 7, 7,6,14,15,10,9,4, + 3, 17,16,8, + 7, 16,13,10,15,12,0,8, + 3, 10,13,9, + 4, 12,15,14,11, +}; +const dReal h69_planes[ h69_numf * 4 ] = { + -0.927293,-0.348894,-0.13565,0.326296,0,-1,-0,0.331146,8.48822e-17,-4.06468e-17,1,0.250451,0.0600487,0.168696,-0.983837,0.364353,1,-0,0,0.248419,0.398278,-0.419585,-0.815673,0.229892,-0.433652,-0.152285,-0.888119,0.28947,-0.131169,0.932758,-0.335793,0.454236,-0.402198,0.907959,0.117672,0.2539,-0.469026,0.741177,-0.480283,0.39893,-0.88947,0.425933,0.165603,0.263513 +}; +// h70 +const int h70_numv = 8; +const int h70_numf = 6; +const dReal h70_volu = 0.052841; +const dReal h70_pos[3] = { 0.870032,-0.820199,0.384721 }; +const dReal h70_verts[ h70_numv * 3 ] = { + 0.129968,-0.179801,0.374626, 0.129968,-0.179801,-0.351943, -0.279890,-0.179801,0.174500, 0.129968,0.435118,0.058310, -0.112815,-0.179801,-0.233397, -0.118249,0.280498,0.016648, 0.129968,-0.066134,-0.371433, -0.064784,-0.043027,-0.280302, +}; +const unsigned int h70_faces[] = { + 4, 4,7,6,1, + 4, 6,3,0,1, + 4, 0,2,4,1, + 4, 5,7,4,2, + 4, 0,3,5,2, + 4, 6,7,5,3, +}; +const dReal h70_planes[ h70_numf * 4 ] = { + -0.433652,-0.152285,-0.888119,0.283587,1,0,0,0.129968,0,-1,0,0.179801,-0.908278,0.191373,-0.372031,0.154889,-0.398278,0.419585,0.815673,0.178367,-0.267862,0.627092,-0.731441,0.195395 +}; +// h71 +const int h71_numv = 16; +const int h71_numf = 10; +const dReal h71_volu = 0.078575; +const dReal h71_pos[3] = { -0.715618,-0.146967,0.614144 }; +const dReal h71_verts[ h71_numv * 3 ] = { + -0.284382,-0.099249,-0.355298, 0.407938,-0.228507,0.130744, -0.284382,-0.117849,-0.345730, 0.406000,-0.230442,0.128574, 0.033539,0.110754,0.371749, 0.104873,-0.287957,0.016505, -0.197952,0.422046,0.249299, -0.284382,0.237452,0.048837, -0.101238,-0.225072,0.034476, -0.284382,-0.143582,-0.147170, 0.149333,-0.294288,-0.043194, 0.409649,-0.225257,0.134087, 0.147525,0.085239,-0.239307, 0.183419,0.150542,-0.169689, 0.342769,-0.256730,0.031938, 0.344779,-0.242424,0.025560, +}; +const unsigned int h71_faces[] = { + 4, 3,14,15,1, + 5, 15,12,13,11,1, + 6, 11,4,8,5,3,1, + 6, 0,12,15,14,10,2, + 5, 10,5,8,9,2, + 4, 9,7,0,2, + 4, 5,10,14,3, + 4, 11,13,6,4, + 5, 6,7,9,8,4, + 5, 13,12,0,7,6, +}; +const dReal h71_planes[ h71_numf * 4 ] = { + 0.830322,-0.320028,-0.456232,0.352199,0.848621,0.0894025,-0.521393,0.257586,-0.141863,-0.672536,0.72634,0.190772,0.398278,-0.419585,-0.815673,0.218187,-0.299389,-0.946218,-0.12263,0.239048,-1,-0,0,0.284382,0.11569,-0.975032,0.189546,0.296029,0.727925,0.639055,0.248465,0.187558,-0.735479,-0.309932,0.602505,0.164988,-0.1544,0.759081,-0.632422,0.193269 +}; +// h72 +const int h72_numv = 24; +const int h72_numf = 14; +const dReal h72_volu = 0.135103; +const dReal h72_pos[3] = { -0.469590,0.119774,0.755853 }; +const dReal h72_verts[ h72_numv * 3 ] = { + -0.212489,-0.155987,0.230040, -0.214701,-0.148769,0.244147, -0.443980,0.155306,0.107590, 0.375305,0.040715,-0.139275, -0.004437,0.090870,-0.389509, 0.311360,0.156806,0.046426, -0.480672,0.247962,0.244147, -0.005314,0.363706,-0.137567, 0.163621,-0.491997,-0.007622, 0.213688,0.055724,0.244147, -0.495580,0.247143,0.161130, -0.499540,0.269230,0.219387, 0.144703,0.290282,-0.088352, 0.355676,0.140088,-0.062598, -0.062609,-0.116199,-0.311398, -0.088334,0.254773,-0.303291, 0.356369,0.017480,-0.175583, 0.155877,-0.383277,0.244147, 0.169264,-0.493727,-0.003158, 0.197264,-0.459118,0.123740, 0.194631,-0.477857,0.074256, 0.211435,-0.450236,0.093267, 0.167388,-0.493339,-0.005093, 0.169626,-0.492143,-0.004615, +}; +const unsigned int h72_faces[] = { + 8, 0,8,22,18,20,19,17,1, + 4, 17,9,6,1, + 6, 6,11,10,2,0,1, + 5, 10,15,4,14,2, + 4, 14,8,0,2, + 7, 16,4,15,7,12,13,3, + 7, 13,5,9,17,19,21,3, + 6, 21,20,18,23,16,3, + 6, 16,23,22,8,14,4, + 3, 13,12,5, + 6, 12,7,11,6,9,5, + 4, 15,10,11,7, + 3, 22,23,18, + 3, 20,21,19, +}; +const dReal h72_planes[ h72_numf * 4 ] = { + -0.50496,-0.797961,0.32905,0.307465,0,0,1,0.244147,-0.820856,-0.550308,0.152828,0.29542,-0.751031,-0.0376746,-0.659191,0.256669,-0.727925,-0.639055,-0.248465,0.197203,0.49733,0.589429,-0.636582,0.299309,0.913105,-0.120244,0.389591,0.283536,0.892822,-0.397939,-0.210981,0.348263,0.398278,-0.419584,-0.815674,0.277818,0.568103,0.816132,0.105771,0.30977,0.226312,0.817437,0.529699,0.223235,-0.411928,0.842404,-0.347377,0.356365,0.474458,-0.651966,-0.591464,0.404071,0.777217,-0.601031,0.186267,0.452309 +}; +// h73 +const int h73_numv = 16; +const int h73_numf = 10; +const dReal h73_volu = 0.036032; +const dReal h73_pos[3] = { -0.912743,-0.085439,0.831075 }; +const dReal h73_verts[ h73_numv * 3 ] = { + 0.095887,-0.286600,-0.182455, -0.087257,0.175924,-0.168094, -0.087257,-0.205110,-0.364101, 0.230664,0.049226,0.154818, -0.037519,0.453174,0.168925, 0.228452,0.056443,0.168925, -0.087257,0.484247,0.168925, -0.000826,0.360518,0.032367, -0.087257,-0.218262,0.168925, -0.087257,-0.384387,-0.087440, -0.087257,0.485343,0.150329, -0.064760,0.476158,0.168925, -0.052427,0.452355,0.085908, -0.087257,0.457669,0.073457, -0.056387,0.474442,0.144165, -0.056885,0.474757,0.144652, +}; +const unsigned int h73_faces[] = { + 4, 13,12,7,1, + 5, 7,3,0,2,1, + 7, 2,9,8,6,10,13,1, + 3, 0,9,2, + 5, 5,8,9,0,3, + 6, 7,12,14,4,5,3, + 4, 14,15,11,4, + 5, 11,6,8,5,4, + 4, 11,15,10,6, + 5, 15,14,12,13,10, +}; +const dReal h73_planes[ h73_numf * 4 ] = { + 0.347582,0.610296,-0.711847,0.196695,0.735479,0.309932,-0.602505,0.091627,-1,0,0,0.0872566,0.163718,-0.827886,-0.536471,0.350852,0.589724,-0.677749,0.439183,0.170659,0.820856,0.550308,-0.152828,0.192771,0.636259,0.754085,0.162882,0.345375,3.43778e-17,-3.95092e-17,1,0.168925,0.337828,0.939579,0.0553497,0.434861,0.255819,0.909579,-0.327448,0.369911 +}; +// h74 +const int h74_numv = 22; +const int h74_numf = 13; +const dReal h74_volu = 0.115647; +const dReal h74_pos[3] = { -0.806688,0.162223,0.365754 }; +const dReal h74_verts[ h74_numv * 3 ] = { + -0.106882,0.112856,0.497689, -0.158482,0.204693,0.551229, 0.218764,0.243174,-0.153679, 0.270615,-0.014193,-0.214570, -0.193312,-0.071737,0.297227, 0.312298,0.070592,-0.150604, -0.069795,-0.227826,-0.358730, 0.248765,0.212323,0.086808, -0.193312,-0.408439,-0.106908, -0.193312,0.210007,0.538778, 0.301657,0.087464,-0.177940, -0.191931,0.033024,-0.314003, -0.193312,0.271426,0.001678, 0.058968,0.264774,-0.186519, -0.193312,0.247719,-0.120270, 0.052425,0.263623,-0.188445, 0.332661,0.048420,0.000590, 0.274489,-0.158648,0.078702, 0.238595,-0.223951,0.009084, 0.229318,-0.111676,-0.252511, -0.193312,-0.302733,-0.342536, -0.193312,0.032765,-0.313772, +}; +const unsigned int h74_faces[] = { + 6, 7,2,13,12,9,1, + 4, 9,4,0,1, + 5, 0,17,16,7,1, + 5, 7,16,5,10,2, + 8, 10,3,19,6,11,15,13,2, + 6, 5,16,17,18,19,3, + 3, 10,5,3, + 7, 9,12,14,21,20,8,4, + 5, 8,18,17,0,4, + 4, 20,21,11,6, + 5, 19,18,8,20,6, + 4, 21,14,15,11, + 4, 13,15,14,12, +}; +const dReal h74_planes[ h74_numf * 4 ] = { + 0.110273,0.987466,0.11292,0.246896,-0.347582,-0.610296,0.711847,0.322553,0.751031,0.0376746,0.659191,0.252053,0.878502,0.475257,-0.0486263,0.315228,0.228531,0.267508,-0.936063,0.258898,0.929286,-0.326334,-0.173016,0.293234,0.897445,-0.121213,-0.424146,0.335592,-1,0,0,0.193312,0.1544,-0.759081,0.632422,0.21258,-0.179474,0.0840327,-0.980167,0.344997,0.446963,-0.816184,-0.366152,0.286102,-0.242073,0.64915,-0.721114,0.29433,-0.11569,0.975032,-0.189546,0.286695 +}; +// h75 +const int h75_numv = 18; +const int h75_numf = 11; +const dReal h75_volu = 0.089620; +const dReal h75_pos[3] = { 0.052817,0.072055,0.868740 }; +const dReal h75_verts[ h75_numv * 3 ] = { + 0.433734,0.123433,0.131260, 0.005775,0.176863,-0.278988, 0.218437,-0.214071,-0.051932, -0.366530,-0.335558,0.131260, -0.308719,0.103443,0.131260, -0.147102,0.088434,-0.252162, -0.310972,-0.402518,-0.019619, -0.003079,0.231496,-0.237914, 0.112171,0.347564,-0.006639, 0.302595,-0.019353,-0.141822, 0.235603,0.353295,0.131260, 0.077918,0.377107,0.131260, 0.249063,-0.227153,0.131260, -0.166731,0.187806,-0.175486, -0.093909,0.310921,0.131260, -0.211047,0.204525,-0.066461, -0.325143,-0.411399,0.010853, -0.320100,-0.425249,0.131260, +}; +const unsigned int h75_faces[] = { + 6, 7,8,10,0,9,1, + 5, 9,2,6,5,1, + 4, 5,13,7,1, + 5, 12,17,16,6,2, + 4, 9,0,12,2, + 3, 16,17,3, + 8, 17,12,0,10,11,14,4,3, + 7, 4,15,13,5,6,16,3, + 3, 14,15,4, + 6, 13,15,14,11,8,7, + 3, 11,10,8, +}; +const dReal h75_planes[ h75_numf * 4 ] = { + 0.619037,0.533582,-0.576267,0.258718,0.107917,-0.454758,-0.884053,0.166834,-0.421977,0.500122,-0.756183,0.296982,0.326272,-0.937436,-0.121492,0.278257,0.870584,-0.458578,-0.178296,0.297595,-0.887955,-0.459664,-0.015677,0.477648,0,0,1,0.13126,-0.913105,0.120244,-0.389591,0.243193,-0.694517,0.719058,0.0245236,0.292011,-0.345296,0.896431,-0.277815,0.27468,0.14709,0.974029,-0.172138,0.356179 +}; +// h76 +const int h76_numv = 24; +const int h76_numf = 14; +const dReal h76_volu = 0.095138; +const dReal h76_pos[3] = { 0.088788,-0.142804,0.576459 }; +const dReal h76_verts[ h76_numv * 3 ] = { + 0.352221,-0.240915,-0.049761, -0.386828,-0.236459,0.178237, 0.182466,0.000788,0.240349, -0.006268,0.044860,-0.340368, 0.298110,0.182836,0.102163, -0.389114,-0.231150,0.176236, -0.030195,0.391722,0.013292, -0.363747,-0.215280,0.253650, -0.388752,-0.229565,0.174779, -0.101327,0.289860,-0.170144, 0.328315,-0.307617,-0.113256, -0.079254,0.048914,-0.283309, 0.094387,-0.120263,-0.302438, -0.281369,-0.303059,0.140194, 0.335039,-0.308192,-0.105343, 0.327448,-0.312089,-0.105986, 0.022142,0.379302,-0.133650, -0.346943,-0.187659,0.272661, -0.202009,0.280058,0.003811, -0.183073,0.303293,0.040119, 0.266624,0.195506,0.150459, -0.022476,0.325632,-0.215723, -0.347511,-0.285824,0.181989, -0.366010,-0.251196,0.221279, +}; +const unsigned int h76_faces[] = { + 3, 22,23,1, + 4, 23,7,5,1, + 8, 5,8,11,3,12,13,22,1, + 9, 17,7,23,22,13,15,14,0,2, + 4, 0,4,20,2, + 5, 20,6,19,17,2, + 4, 11,9,21,3, + 8, 21,16,4,0,14,10,12,3, + 4, 16,6,20,4, + 6, 7,17,19,18,8,5, + 6, 16,21,9,18,19,6, + 4, 18,9,11,8, + 3, 14,15,10, + 4, 15,13,12,10, +}; +const dReal h76_planes[ h76_numf * 4 ] = { + -0.777249,-0.606249,0.168364,0.474024,-0.901569,-0.256913,0.348094,0.471545,-0.553104,-0.49165,-0.672575,0.210334,0.27553,-0.653111,0.705358,0.219292,0.822068,-0.096228,0.561199,0.284807,-0.107917,0.454758,0.884053,0.193148,-0.576312,0.303571,-0.758755,0.275486,0.73548,0.309929,-0.602505,0.214367,0.512085,0.851811,0.110395,0.319678,-0.892822,0.397939,0.210981,0.292608,-0.517294,0.817451,-0.253339,0.332466,-0.859244,0.150868,-0.488814,0.213964,0.440632,-0.78713,-0.431589,0.435681,-0.226312,-0.817437,-0.529699,0.237148 +}; +// h77 +const int h77_numv = 26; +const int h77_numf = 15; +const dReal h77_volu = 0.087804; +const dReal h77_pos[3] = { 0.390778,-0.073451,0.362494 }; +const dReal h77_verts[ h77_numv * 3 ] = { + -0.003880,0.113483,0.316128, 0.315369,-0.294228,0.065585, -0.206158,-0.189928,-0.093832, 0.020979,0.268088,0.071472, -0.324466,0.256279,-0.001758, -0.230465,0.381445,-0.183441, 0.426887,-0.187811,0.054869, 0.211155,-0.343689,0.091028, -0.224735,0.155039,-0.266749, 0.077733,0.119939,0.286241, 0.026325,-0.376970,0.100709, -0.297695,-0.031084,-0.153195, 0.009520,-0.343472,-0.067504, 0.155269,-0.307156,-0.121769, -0.207604,-0.189616,-0.088472, -0.308258,-0.024493,-0.126403, 0.050231,-0.310268,0.164204, 0.176117,-0.273019,-0.144419, 0.417855,-0.153875,-0.005953, 0.273422,-0.325958,0.057585, 0.096656,-0.370860,0.127368, 0.033049,-0.377545,0.108622, -0.214357,0.369295,-0.224944, 0.141437,0.190499,-0.081874, -0.279848,0.309949,0.080316, -0.221943,0.387815,-0.053985, +}; +const unsigned int h77_faces[] = { + 6, 19,13,17,18,6,1, + 7, 6,9,0,16,20,7,1, + 3, 7,19,1, + 4, 14,15,11,2, + 6, 11,8,17,13,12,2, + 4, 12,10,14,2, + 5, 25,24,0,9,3, + 5, 9,6,18,23,3, + 5, 23,22,5,25,3, + 4, 24,25,5,4, + 6, 5,22,8,11,15,4, + 8, 15,14,10,21,16,0,24,4, + 7, 20,21,10,12,13,19,7, + 5, 22,23,18,17,8, + 3, 21,20,16, +}; +const dReal h77_planes[ h77_numf * 4 ] = { + 0.590109,-0.664552,-0.458413,0.351567,0.349687,-0.276295,0.895198,0.250286,0.469026,-0.741177,0.480283,0.397491,-0.818514,-0.542417,-0.189261,0.289522,-0.212159,-0.447018,-0.869002,0.21018,-0.568104,-0.816131,-0.105771,0.28205,0.131611,0.83191,0.539077,0.264315,0.727924,0.639056,0.248465,0.204352,0.471464,0.878752,-0.0742736,0.240165,-0.784459,0.61982,0.0211391,0.41334,-0.913105,0.120244,-0.389591,0.327772,-0.73548,-0.309929,0.602505,0.158151,0.162747,-0.964426,-0.208316,0.346865,0.433652,0.152285,-0.888119,0.163058,-0.16676,-0.602337,0.780629,0.306692 +}; +// h78 +const int h78_numv = 20; +const int h78_numf = 12; +const dReal h78_volu = 0.126407; +const dReal h78_pos[3] = { 0.652845,-0.171711,0.785711 }; +const dReal h78_verts[ h78_numv * 3 ] = { + -0.350965,0.016612,0.214289, 0.347155,-0.072460,-0.306250, -0.381591,0.029695,0.031097, 0.347155,-0.139922,0.214289, 0.322332,-0.081469,-0.321579, -0.265947,0.211743,-0.107089, -0.020631,0.312250,-0.000249, 0.347155,0.151286,0.214289, 0.347155,-0.036860,-0.269624, -0.184333,0.218199,-0.136976, -0.297433,0.224413,-0.058793, -0.211836,-0.212008,-0.259013, -0.050911,-0.245429,-0.332189, -0.067869,-0.323764,0.214289, -0.166294,0.367199,0.214289, -0.026282,0.399418,0.214289, -0.145373,-0.294337,-0.277679, -0.165410,-0.272600,-0.295849, 0.053302,-0.195968,-0.357632, 0.164820,-0.089551,-0.368349, +}; +const unsigned int h78_faces[] = { + 7, 3,13,16,12,18,4,1, + 6, 4,19,9,6,8,1, + 4, 8,7,3,1, + 6, 11,17,16,13,0,2, + 4, 0,14,10,2, + 4, 10,5,11,2, + 6, 7,15,14,0,13,3, + 3, 18,19,4, + 6, 10,14,15,6,9,5, + 7, 9,19,18,12,17,11,5, + 4, 15,7,8,6, + 3, 16,17,12, +}; +const dReal h78_planes[ h78_numf * 4 ] = { + 0.402198,-0.907959,-0.117672,0.241453,0.167754,0.706913,-0.687119,0.217444,1,-0,0,0.347155,-0.766208,-0.637263,0.0825892,0.276023,-0.870584,0.458578,0.178296,0.351369,-0.822068,0.096228,-0.561199,0.2991,0,-0,1,0.214289,0.280943,-0.383024,-0.879979,0.404745,-0.20808,0.904248,-0.372879,0.286737,-0.349687,0.276295,-0.895198,0.247367,0.526505,0.792388,-0.308081,0.236638,-0.071473,-0.677671,-0.731884,0.413083 +}; +// h79 +const int h79_numv = 20; +const int h79_numf = 12; +const dReal h79_volu = 0.069132; +const dReal h79_pos[3] = { 0.747670,0.086302,0.441993 }; +const dReal h79_verts[ h79_numv * 3 ] = { + 0.252330,0.012519,-0.215098, 0.012566,0.066820,-0.263433, 0.201221,-0.224177,-0.182831, 0.095169,0.321961,-0.051360, -0.091840,0.355257,0.097036, -0.215455,0.030746,-0.161373, -0.279159,-0.039814,0.206742, 0.060963,-0.313628,-0.085453, -0.115456,0.054238,0.343469, 0.252330,-0.294873,0.074094, -0.016233,0.366022,0.061965, -0.074702,0.310522,0.161887, 0.252330,0.103519,-0.130839, 0.252330,-0.216744,-0.175787, -0.335913,0.108335,-0.008027, 0.079494,0.109929,-0.259932, 0.069995,-0.347564,-0.024631, 0.252330,-0.330473,0.037468, 0.252330,-0.335180,0.024392, 0.227507,-0.339482,0.022139, +}; +const unsigned int h79_faces[] = { + 7, 5,14,4,10,3,15,1, + 5, 15,0,13,2,1, + 4, 2,7,5,1, + 6, 13,18,19,16,7,2, + 6, 10,11,8,9,12,3, + 4, 12,0,15,3, + 5, 14,6,8,11,4, + 3, 11,10,4, + 5, 7,16,6,14,5, + 6, 16,19,17,9,8,6, + 6, 17,18,13,0,12,9, + 3, 19,18,17, +}; +const dReal h79_planes[ h79_numf * 4 ] = { + -0.391463,0.659847,-0.641373,0.208131,0.158394,-0.166869,-0.973173,0.247207,-0.309646,-0.434949,-0.84554,0.189789,0.191731,-0.844679,-0.499756,0.319309,0.73548,0.309929,0.602505,0.138836,0.49733,0.589428,-0.636583,0.269798,-0.708232,0.479648,0.518021,0.285709,0.131611,0.831909,0.539078,0.335765,-0.727924,-0.639056,-0.248465,0.177281,-0.167754,-0.706913,0.687119,0.217031,1,0,0,0.25233,0.131169,-0.932758,0.335793,0.35393 +}; +// h80 +const int h80_numv = 14; +const int h80_numf = 9; +const dReal h80_volu = 0.076324; +const dReal h80_pos[3] = { 0.872729,0.237609,0.718600 }; +const dReal h80_verts[ h80_numv * 3 ] = { + 0.127271,0.331095,-0.032200, 0.127271,-0.258034,0.281400, 0.127271,-0.446180,-0.202512, -0.199761,0.159215,-0.114719, -0.141292,0.214715,-0.214641, -0.136492,0.218429,-0.218946, -0.240515,-0.097070,0.066863, -0.246166,-0.009903,0.281400, -0.239577,0.050556,0.281400, 0.127271,0.263358,0.281400, 0.127271,0.246274,-0.293996, 0.127271,-0.047788,-0.407445, -0.039983,0.206584,-0.314412, -0.029889,0.170654,-0.327966, +}; +const unsigned int h80_faces[] = { + 4, 7,6,2,1, + 6, 2,11,10,0,9,1, + 4, 9,8,7,1, + 6, 6,3,4,13,11,2, + 6, 8,9,0,5,4,3, + 4, 6,7,8,3, + 4, 5,12,13,4, + 4, 0,10,12,5, + 4, 11,13,12,10, +}; +const dReal h80_planes[ h80_numf * 4 ] = { + -0.526505,-0.792388,0.308081,0.224148,1,0,0,0.127271,0,0,1,0.281401,-0.73548,-0.309929,-0.602505,0.166694,-0.493234,0.850287,0.183662,0.212838,-0.991673,0.108076,-0.0700326,0.223339,-0.696788,0.0734069,-0.713511,0.267361,-0.184883,0.934913,-0.302912,0.295769,0.0284606,0.359798,-0.932596,0.36641 +}; +// h81 +const int h81_numv = 10; +const int h81_numf = 7; +const dReal h81_volu = 0.092471; +const dReal h81_pos[3] = { -0.647649,0.823554,0.820209 }; +const dReal h81_verts[ h81_numv * 3 ] = { + 0.457657,0.176446,0.179791, -0.352351,-0.423650,0.161195, -0.352351,0.176446,-0.147497, -0.352351,-0.424746,0.179791, -0.352351,0.176446,0.179791, 0.395926,0.059255,0.179791, 0.203225,-0.169390,-0.173056, 0.338792,0.176446,-0.400601, -0.321979,-0.434236,0.155518, -0.329854,-0.432835,0.179791, +}; +const unsigned int h81_faces[] = { + 4, 8,9,3,1, + 4, 3,4,2,1, + 5, 2,7,6,8,1, + 4, 4,0,7,2, + 5, 9,5,0,4,3, + 4, 6,7,0,5, + 4, 9,8,6,5, +}; +const dReal h81_planes[ h81_numf * 4 ] = { + -0.337828,-0.939579,-0.0553497,0.508165,-1,-9.23352e-17,0,0.352351,-0.309646,-0.434949,-0.84554,0.157073,0,1,-0,0.176446,-0,0,1,0.179791,0.870584,-0.458578,-0.178296,0.285458,0.546922,-0.806654,0.224019,0.209019 +}; +// h82 +const int h82_numv = 22; +const int h82_numf = 13; +const dReal h82_volu = 0.133984; +const dReal h82_pos[3] = { -0.704013,0.655334,0.510913 }; +const dReal h82_verts[ h82_numv * 3 ] = { + -0.295987,0.344666,0.147853, 0.143982,-0.175036,-0.341668, -0.295987,-0.255430,0.470491, 0.398940,0.268255,-0.112413, -0.265615,-0.266016,0.464814, -0.043708,-0.228337,-0.331678, 0.395156,0.344666,-0.091306, 0.146090,-0.280788,-0.058352, 0.259589,-0.001170,0.136240, -0.295987,0.344666,0.161799, 0.268488,-0.087750,0.102566, -0.295987,-0.221685,-0.143481, -0.265117,-0.266331,0.464327, 0.229109,-0.171854,0.107373, -0.295987,-0.283104,0.393619, -0.261157,-0.288418,0.406070, 0.116089,-0.249937,-0.298838, 0.396013,0.344666,-0.093117, 0.248792,0.064649,-0.395197, 0.275014,0.344666,-0.270360, 0.114098,-0.109185,-0.385965, 0.213954,-0.004245,-0.405120, +}; +const unsigned int h82_faces[] = { + 4, 16,5,20,1, + 3, 20,21,1, + 8, 21,18,3,10,13,7,16,1, + 5, 14,15,12,4,2, + 5, 4,8,6,9,2, + 5, 9,0,11,14,2, + 4, 18,19,17,3, + 5, 17,6,8,10,3, + 5, 12,13,10,8,4, + 6, 16,7,15,14,11,5, + 7, 11,0,19,18,21,20,5, + 5, 17,19,0,9,6, + 4, 13,12,15,7, +}; +const dReal h82_planes[ h82_numf * 4 ] = { + 0.103318,-0.522457,-0.846383,0.395508,0.296224,-0.4369,-0.849335,0.409315,0.88947,-0.425933,-0.165603,0.259202,-0.255819,-0.909579,0.327448,0.462115,0.309646,0.434949,0.84554,0.195068,-1,0,0,0.295987,0.813673,0.17144,-0.555468,0.433038,0.901694,-0.0730707,0.426155,0.292215,0.598205,-0.236328,0.765702,0.259883,-0.110273,-0.987466,-0.11292,0.267748,-0.545755,0.383303,-0.745138,0.183477,-9.72172e-17,1,0,0.344666,0.411928,-0.842404,0.347377,0.276446 +}; +// h83 +const int h83_numv = 12; +const int h83_numf = 8; +const dReal h83_volu = 0.056796; +const dReal h83_pos[3] = { -0.429347,0.488254,0.882979 }; +const dReal h83_verts[ h83_numv * 3 ] = { + -0.006178,0.079330,-0.269501, -0.548156,-0.097535,0.117020, -0.015077,0.165910,-0.235826, -0.540281,-0.098936,0.092747, 0.104460,-0.078198,-0.215479, 0.388255,-0.105278,0.117020, 0.173445,-0.312756,0.117020, 0.177624,0.394555,0.117020, -0.045557,-0.004774,-0.264694, 0.271117,-0.211674,-0.080700, -0.539783,-0.099251,0.092261, -0.520915,-0.120519,0.117020, +}; +const unsigned int h83_faces[] = { + 4, 3,10,11,1, + 5, 11,6,5,7,1, + 4, 7,2,3,1, + 6, 7,5,9,4,0,2, + 5, 0,8,10,3,2, + 6, 9,6,11,10,8,4, + 3, 8,0,4, + 3, 6,9,5, +}; +const dReal h83_planes[ h83_numf * 4 ] = { + -0.636259,-0.754085,-0.162882,0.403258,0,0,1,0.117021,-0.546922,0.806654,-0.224019,0.194907,0.735479,0.309932,-0.602505,0.182419,-0.598205,0.236328,-0.765702,0.228801,-0.226312,-0.817437,-0.529699,0.15442,0.233951,-0.164311,-0.958264,0.243773,0.694517,-0.719058,-0.0245236,0.342481 +}; +// h84 +const int h84_numv = 22; +const int h84_numf = 13; +const dReal h84_volu = 0.109330; +const dReal h84_pos[3] = { -0.121674,0.670783,0.713170 }; +const dReal h84_verts[ h84_numv * 3 ] = { + -0.313851,-0.103198,-0.099691, -0.010109,0.329217,0.286830, 0.252410,-0.221620,0.286830, -0.068318,0.329217,0.286830, 0.080582,-0.287807,0.286830, 0.201978,0.032805,-0.295587, -0.187183,0.329217,-0.293562, 0.227672,0.006891,-0.288529, -0.130049,0.212026,0.286830, -0.322750,-0.016619,-0.066017, -0.036556,-0.394202,0.089109, 0.171412,-0.367231,-0.082344, -0.203213,-0.260726,-0.045669, 0.007760,-0.410921,-0.019916, 0.265234,-0.083360,-0.251274, 0.286662,-0.251163,0.148930, -0.115141,0.329217,-0.286684, -0.066013,0.197094,-0.314670, -0.183399,0.252807,-0.314670, -0.186326,0.329217,-0.295374, 0.060573,0.329217,-0.240926, 0.066345,0.329217,-0.235802, +}; +const unsigned int h84_faces[] = { + 6, 2,15,14,7,21,1, + 7, 21,20,16,19,6,3,1, + 5, 3,8,4,2,1, + 6, 4,10,13,11,15,2, + 4, 6,9,8,3, + 6, 8,9,0,12,10,4, + 4, 20,21,7,5, + 9, 7,14,11,13,12,0,18,17,5, + 4, 17,16,20,5, + 5, 19,18,0,9,6, + 3, 12,13,10, + 3, 14,15,11, + 4, 17,18,19,16, +}; +const dReal h84_planes[ h84_numf * 4 ] = { + 0.894954,0.426518,0.130919,0.168922,0,1,0,0.329217,0,0,1,0.28683,0.345296,-0.896431,0.277815,0.365509,-0.870584,0.458578,0.178296,0.261589,-0.735479,-0.309932,0.602505,0.202751,0.604377,0.413861,-0.68077,0.336874,-0.212159,-0.447018,-0.869002,0.19935,0.241471,0.286187,-0.927248,0.332242,-0.901694,0.0730707,-0.426155,0.317941,-0.568103,-0.816132,-0.105771,0.333064,0.877503,-0.423705,-0.224641,0.32451,0.117405,0.247371,-0.961782,0.343649 +}; +// h85 +const int h85_numv = 36; +const int h85_numf = 20; +const dReal h85_volu = 0.148789; +const dReal h85_pos[3] = { -0.198763,0.456116,0.345386 }; +const dReal h85_verts[ h85_numv * 3 ] = { + 0.248501,-0.152565,0.285440, 0.011075,0.411760,0.053114, 0.304761,0.221558,0.079254, 0.360475,-0.080906,-0.269891, -0.389161,-0.050719,-0.133311, 0.279067,0.247472,0.072197, 0.342323,0.131307,0.116509, -0.236762,0.111468,0.268093, 0.084849,-0.196255,0.347868, 0.186224,-0.309060,0.060929, -0.295627,-0.223301,-0.130236, 0.016686,0.009351,-0.358147, 0.315157,-0.069303,-0.293328, -0.106311,0.467473,0.053114, 0.257356,-0.207198,0.244366, 0.367598,-0.141752,-0.036878, 0.104478,-0.295627,0.271192, -0.126124,-0.046060,0.322115, -0.256458,0.263868,-0.229670, 0.085542,-0.318862,0.234884, -0.291296,0.194974,-0.239593, -0.265275,0.238760,-0.259807, -0.361268,0.024182,-0.176141, -0.291163,-0.195420,-0.172718, -0.276141,0.027364,0.272900, -0.306268,-0.206429,-0.157572, -0.275264,-0.245473,0.020958, -0.359160,-0.081570,0.107176, 0.360630,0.159839,0.075257, 0.387070,0.106509,-0.013499, 0.393155,0.083405,-0.037268, 0.359076,-0.148122,-0.166333, 0.309693,-0.219618,0.097423, 0.265075,-0.273288,0.015350, 0.118262,-0.041268,-0.360627, 0.049809,-0.043705,-0.365222, +}; +const unsigned int h85_faces[] = { + 7, 12,34,11,21,18,13,1, + 9, 13,7,17,8,0,6,2,5,1, + 6, 5,29,30,3,12,1, + 4, 28,29,5,2, + 3, 6,28,2, + 4, 30,15,31,3, + 10, 31,33,9,10,25,23,35,34,12,3, + 5, 25,10,26,27,4, + 8, 27,24,7,13,18,20,22,4, + 4, 22,23,25,4, + 8, 0,14,32,15,30,29,28,6, + 3, 24,17,7, + 7, 17,24,27,26,19,16,8, + 4, 16,14,0,8, + 4, 19,26,10,9, + 6, 33,32,14,16,19,9, + 3, 34,35,11, + 6, 35,23,22,20,21,11, + 4, 32,33,31,15, + 3, 21,20,18, +}; +const dReal h85_planes[ h85_numf * 4 ] = { + 0.322183,0.678838,-0.659831,0.24804,0.212159,0.447018,0.869002,0.23257,0.463507,0.691764,-0.553737,0.260563,0.722579,0.66608,-0.18498,0.353129,0.602768,0.50566,0.617235,0.344652,0.991833,-0.112685,-0.0597434,0.382771,0.066237,-0.837367,-0.542613,0.238071,-0.878502,-0.475257,0.0486263,0.359502,-0.88947,0.425933,0.165603,0.302468,-0.613049,-0.207593,-0.762283,0.350725,0.933932,-0.105417,0.341552,0.345658,-0.233951,0.164311,0.958264,0.33061,-0.49733,-0.589429,0.636582,0.294927,0.421977,-0.500122,0.756183,0.397007,-0.124536,-0.983986,-0.127525,0.273149,0.517294,-0.817451,0.253339,0.364409,0.0600487,0.168696,-0.983837,0.354938,-0.433652,-0.152285,-0.888119,0.309417,0.784459,-0.61982,-0.0211391,0.377005,-0.877441,0.462191,-0.128357,0.376465 +}; +// h86 +const int h86_numv = 14; +const int h86_numf = 9; +const dReal h86_volu = 0.108919; +const dReal h86_pos[3] = { 0.238541,0.789776,0.775739 }; +const dReal h86_verts[ h86_numv * 3 ] = { + -0.094981,-0.202353,-0.313843, -0.370325,0.210224,0.224261, -0.076674,-0.173821,-0.355096, 0.281969,0.210224,-0.157785, 0.225385,0.024918,-0.224556, 0.374020,0.210224,0.224261, 0.316487,0.210224,-0.084750, -0.107806,-0.340614,0.224261, 0.201351,-0.281476,0.224261, 0.184090,-0.072562,-0.262496, -0.073553,-0.370157,0.086361, 0.049879,-0.364426,0.224261, -0.132543,-0.112102,-0.351098, -0.293871,0.210224,-0.298371, +}; +const unsigned int h86_faces[] = { + 6, 13,12,0,10,7,1, + 5, 7,11,8,5,1, + 5, 5,6,3,13,1, + 6, 12,13,3,4,9,2, + 6, 9,8,11,10,0,2, + 3, 0,12,2, + 3, 6,4,3, + 5, 6,5,8,9,4, + 3, 10,11,7, +}; +const dReal h86_planes[ h86_numf * 4 ] = { + -0.894954,-0.426518,-0.130919,0.212399,0,0,1,0.224261,0,1,0,0.210224,0.228531,0.267508,-0.936063,0.268372,0.446964,-0.816183,-0.366153,0.237618,-0.602768,-0.50566,-0.617235,0.353289,0.897445,-0.121212,-0.424146,0.294495,0.929286,-0.326334,-0.173016,0.240167,-0.14709,-0.974029,0.172138,0.386228 +}; +// h87 +const int h87_numv = 16; +const int h87_numf = 10; +const dReal h87_volu = 0.089360; +const dReal h87_pos[3] = { 0.798384,0.791714,0.481127 }; +const dReal h87_verts[ h87_numv * 3 ] = { + -0.277873,0.208286,0.136827, -0.253373,-0.199539,-0.004817, 0.201616,0.208286,-0.307196, 0.201616,0.189080,-0.312136, 0.201616,-0.223010,0.205272, 0.201616,0.208286,0.427133, 0.201616,-0.307831,-0.056523, -0.352696,-0.089547,0.003266, -0.060134,0.085556,-0.322787, -0.013117,0.208286,-0.294090, -0.336822,-0.121565,-0.050021, -0.196695,-0.100782,-0.260274, -0.062147,-0.335676,0.018525, 0.034362,-0.347521,-0.076940, -0.334458,0.022980,0.070056, -0.243356,0.208286,0.209862, +}; +const unsigned int h87_faces[] = { + 5, 10,11,13,12,1, + 7, 12,4,5,15,14,7,1, + 3, 7,10,1, + 4, 3,8,9,2, + 5, 9,0,15,5,2, + 5, 5,4,6,3,2, + 5, 6,13,11,8,3, + 4, 12,13,6,4, + 7, 14,0,9,8,11,10,7, + 3, 15,0,14, +}; +const dReal h87_planes[ h87_numf * 4 ] = { + -0.496382,-0.766976,-0.406636,0.28077,-0.398278,-0.419587,0.815673,0.180707,-0.722578,-0.66608,0.18498,0.3151,-0.0590083,0.248661,-0.966792,0.336891,0,1,0,0.208286,1,1.89246e-16,1.50725e-16,0.201616,0.212159,-0.447018,-0.869002,0.229499,0.184883,-0.934913,0.302912,0.307949,-0.777334,0.409459,-0.477594,0.235937,-0.897445,0.121212,0.424146,0.332657 +}; +// h88 +const int h88_numv = 26; +const int h88_numf = 15; +const dReal h88_volu = 0.102785; +const dReal h88_pos[3] = { 0.372790,0.365732,0.656967 }; +const dReal h88_verts[ h88_numv * 3 ] = { + -0.084370,0.059618,0.343033, 0.095722,-0.319244,-0.008232, 0.172221,0.226443,-0.180656, 0.300178,0.031092,-0.053086, 0.259424,-0.225193,0.128495, 0.253773,-0.138025,0.343033, -0.207802,0.053887,0.205134, 0.072899,0.336435,-0.172573, -0.203955,-0.051368,-0.348458, 0.260362,-0.077566,0.343033, 0.038967,-0.171095,-0.223000, 0.088772,0.304416,-0.225861, 0.113761,-0.170244,0.343033, 0.067103,0.142568,0.343033, -0.229230,0.221691,-0.195070, -0.323052,-0.062181,-0.026140, 0.014109,-0.325700,0.021656, -0.261860,-0.129234,-0.214158, -0.017378,-0.313030,0.069951, -0.314197,-0.116814,-0.067215, 0.049841,0.351482,-0.143724, -0.178398,0.173789,-0.348849, -0.210923,0.250223,-0.236324, 0.283040,0.075827,-0.117937, -0.184483,0.196893,-0.325078, -0.167799,0.186994,-0.344427, +}; +const unsigned int h88_faces[] = { + 5, 10,23,3,4,1, + 6, 4,5,12,18,16,1, + 5, 16,17,8,10,1, + 7, 23,10,8,21,25,11,2, + 3, 11,7,2, + 7, 7,20,13,9,3,23,2, + 4, 9,5,4,3, + 5, 9,13,0,12,5, + 6, 15,19,18,12,0,6, + 6, 0,13,20,22,14,6, + 3, 14,15,6, + 6, 11,25,24,22,20,7, + 8, 17,19,15,14,22,24,21,8, + 4, 18,19,17,16, + 3, 24,25,21, +}; +const dReal h88_planes[ h88_numf * 4 ] = { + 0.708232,-0.479648,-0.518021,0.225182,0.20808,-0.904248,0.372879,0.305524,-0.131611,-0.83191,-0.539077,0.257422,0.438192,-0.051293,-0.897417,0.225975,0.722578,0.66608,-0.18498,0.30869,0.727925,0.639055,0.248465,0.225187,0.991673,-0.108076,0.0700326,0.2906,0,-0,1,0.343033,-0.619037,-0.533582,0.576267,0.218095,-0.446964,0.816183,0.366153,0.211972,-0.877503,0.423705,0.224641,0.25126,-0.131611,0.83191,-0.539078,0.36332,-0.933932,0.105417,-0.341552,0.304082,-0.512085,-0.851811,-0.110395,0.267819,-0.443092,0.583496,-0.68059,0.417874 +}; +// h89 +const int h89_numv = 16; +const int h89_numf = 10; +const dReal h89_volu = 0.098811; +const dReal h89_pos[3] = { 0.713605,0.673779,0.814484 }; +const dReal h89_verts[ h89_numv * 3 ] = { + -0.273713,-0.165479,0.185516, -0.080453,-0.385613,0.185516, 0.286395,-0.172812,0.185516, 0.286395,0.326221,0.093776, -0.101044,0.326221,0.185516, 0.286395,0.326221,0.185516, 0.017832,-0.221455,-0.310526, 0.286395,-0.105074,-0.128085, -0.040637,-0.276955,-0.210604, -0.057774,-0.232220,-0.275455, -0.158577,0.326221,-0.123495, -0.267917,0.028388,-0.330091, -0.290974,0.043435,-0.301241, -0.249679,0.140915,-0.263301, -0.168594,-0.081604,-0.338174, 0.022632,-0.217741,-0.314831, +}; +const unsigned int h89_faces[] = { + 6, 8,6,15,7,2,1, + 5, 2,5,4,0,1, + 7, 0,12,11,14,9,8,1, + 4, 7,3,5,2, + 7, 7,15,14,11,13,10,3, + 4, 10,4,5,3, + 5, 10,13,12,0,4, + 4, 9,14,15,6, + 3, 8,9,6, + 3, 12,13,11, +}; +const dReal h89_planes[ h89_numf * 4 ] = { + 0.493234,-0.850287,-0.183662,0.254128,-0,1.12896e-16,1,0.185516,-0.727925,-0.639055,-0.248465,0.258897,1,0,0,0.286395,0.398278,0.419587,-0.815673,0.174452,0,1,0,0.326221,-0.929286,0.326334,0.173016,0.232453,-0.289106,-0.541462,-0.789453,0.3599,-0.131611,-0.831909,-0.539078,0.349281,-0.535431,0.493566,-0.685351,0.38369 +}; + +//Aggregate of all cells. +const int halton_numc = 90; +const int halton_numv[ halton_numc ] = { + h00_numv,h01_numv,h02_numv,h03_numv,h04_numv,h05_numv,h06_numv,h07_numv,h08_numv,h09_numv,h10_numv,h11_numv,h12_numv,h13_numv,h14_numv,h15_numv,h16_numv,h17_numv,h18_numv,h19_numv,h20_numv,h21_numv,h22_numv,h23_numv,h24_numv,h25_numv,h26_numv,h27_numv,h28_numv,h29_numv,h30_numv,h31_numv,h32_numv,h33_numv,h34_numv,h35_numv,h36_numv,h37_numv,h38_numv,h39_numv,h40_numv,h41_numv,h42_numv,h43_numv,h44_numv,h45_numv,h46_numv,h47_numv,h48_numv,h49_numv,h50_numv,h51_numv,h52_numv,h53_numv,h54_numv,h55_numv,h56_numv,h57_numv,h58_numv,h59_numv,h60_numv,h61_numv,h62_numv,h63_numv,h64_numv,h65_numv,h66_numv,h67_numv,h68_numv,h69_numv,h70_numv,h71_numv,h72_numv,h73_numv,h74_numv,h75_numv,h76_numv,h77_numv,h78_numv,h79_numv,h80_numv,h81_numv,h82_numv,h83_numv,h84_numv,h85_numv,h86_numv,h87_numv,h88_numv,h89_numv, +}; +const int halton_numf[ halton_numc ] = { + h00_numf,h01_numf,h02_numf,h03_numf,h04_numf,h05_numf,h06_numf,h07_numf,h08_numf,h09_numf,h10_numf,h11_numf,h12_numf,h13_numf,h14_numf,h15_numf,h16_numf,h17_numf,h18_numf,h19_numf,h20_numf,h21_numf,h22_numf,h23_numf,h24_numf,h25_numf,h26_numf,h27_numf,h28_numf,h29_numf,h30_numf,h31_numf,h32_numf,h33_numf,h34_numf,h35_numf,h36_numf,h37_numf,h38_numf,h39_numf,h40_numf,h41_numf,h42_numf,h43_numf,h44_numf,h45_numf,h46_numf,h47_numf,h48_numf,h49_numf,h50_numf,h51_numf,h52_numf,h53_numf,h54_numf,h55_numf,h56_numf,h57_numf,h58_numf,h59_numf,h60_numf,h61_numf,h62_numf,h63_numf,h64_numf,h65_numf,h66_numf,h67_numf,h68_numf,h69_numf,h70_numf,h71_numf,h72_numf,h73_numf,h74_numf,h75_numf,h76_numf,h77_numf,h78_numf,h79_numf,h80_numf,h81_numf,h82_numf,h83_numf,h84_numf,h85_numf,h86_numf,h87_numf,h88_numf,h89_numf, +}; +const dReal halton_volu[ halton_numc ] = { + h00_volu,h01_volu,h02_volu,h03_volu,h04_volu,h05_volu,h06_volu,h07_volu,h08_volu,h09_volu,h10_volu,h11_volu,h12_volu,h13_volu,h14_volu,h15_volu,h16_volu,h17_volu,h18_volu,h19_volu,h20_volu,h21_volu,h22_volu,h23_volu,h24_volu,h25_volu,h26_volu,h27_volu,h28_volu,h29_volu,h30_volu,h31_volu,h32_volu,h33_volu,h34_volu,h35_volu,h36_volu,h37_volu,h38_volu,h39_volu,h40_volu,h41_volu,h42_volu,h43_volu,h44_volu,h45_volu,h46_volu,h47_volu,h48_volu,h49_volu,h50_volu,h51_volu,h52_volu,h53_volu,h54_volu,h55_volu,h56_volu,h57_volu,h58_volu,h59_volu,h60_volu,h61_volu,h62_volu,h63_volu,h64_volu,h65_volu,h66_volu,h67_volu,h68_volu,h69_volu,h70_volu,h71_volu,h72_volu,h73_volu,h74_volu,h75_volu,h76_volu,h77_volu,h78_volu,h79_volu,h80_volu,h81_volu,h82_volu,h83_volu,h84_volu,h85_volu,h86_volu,h87_volu,h88_volu,h89_volu, +}; +const dReal* halton_pos[ halton_numc ] = { + h00_pos,h01_pos,h02_pos,h03_pos,h04_pos,h05_pos,h06_pos,h07_pos,h08_pos,h09_pos,h10_pos,h11_pos,h12_pos,h13_pos,h14_pos,h15_pos,h16_pos,h17_pos,h18_pos,h19_pos,h20_pos,h21_pos,h22_pos,h23_pos,h24_pos,h25_pos,h26_pos,h27_pos,h28_pos,h29_pos,h30_pos,h31_pos,h32_pos,h33_pos,h34_pos,h35_pos,h36_pos,h37_pos,h38_pos,h39_pos,h40_pos,h41_pos,h42_pos,h43_pos,h44_pos,h45_pos,h46_pos,h47_pos,h48_pos,h49_pos,h50_pos,h51_pos,h52_pos,h53_pos,h54_pos,h55_pos,h56_pos,h57_pos,h58_pos,h59_pos,h60_pos,h61_pos,h62_pos,h63_pos,h64_pos,h65_pos,h66_pos,h67_pos,h68_pos,h69_pos,h70_pos,h71_pos,h72_pos,h73_pos,h74_pos,h75_pos,h76_pos,h77_pos,h78_pos,h79_pos,h80_pos,h81_pos,h82_pos,h83_pos,h84_pos,h85_pos,h86_pos,h87_pos,h88_pos,h89_pos, +}; +const dReal* halton_verts[ halton_numc ] = { + h00_verts,h01_verts,h02_verts,h03_verts,h04_verts,h05_verts,h06_verts,h07_verts,h08_verts,h09_verts,h10_verts,h11_verts,h12_verts,h13_verts,h14_verts,h15_verts,h16_verts,h17_verts,h18_verts,h19_verts,h20_verts,h21_verts,h22_verts,h23_verts,h24_verts,h25_verts,h26_verts,h27_verts,h28_verts,h29_verts,h30_verts,h31_verts,h32_verts,h33_verts,h34_verts,h35_verts,h36_verts,h37_verts,h38_verts,h39_verts,h40_verts,h41_verts,h42_verts,h43_verts,h44_verts,h45_verts,h46_verts,h47_verts,h48_verts,h49_verts,h50_verts,h51_verts,h52_verts,h53_verts,h54_verts,h55_verts,h56_verts,h57_verts,h58_verts,h59_verts,h60_verts,h61_verts,h62_verts,h63_verts,h64_verts,h65_verts,h66_verts,h67_verts,h68_verts,h69_verts,h70_verts,h71_verts,h72_verts,h73_verts,h74_verts,h75_verts,h76_verts,h77_verts,h78_verts,h79_verts,h80_verts,h81_verts,h82_verts,h83_verts,h84_verts,h85_verts,h86_verts,h87_verts,h88_verts,h89_verts, +}; +const unsigned int* halton_faces[ halton_numc ] = { + h00_faces,h01_faces,h02_faces,h03_faces,h04_faces,h05_faces,h06_faces,h07_faces,h08_faces,h09_faces,h10_faces,h11_faces,h12_faces,h13_faces,h14_faces,h15_faces,h16_faces,h17_faces,h18_faces,h19_faces,h20_faces,h21_faces,h22_faces,h23_faces,h24_faces,h25_faces,h26_faces,h27_faces,h28_faces,h29_faces,h30_faces,h31_faces,h32_faces,h33_faces,h34_faces,h35_faces,h36_faces,h37_faces,h38_faces,h39_faces,h40_faces,h41_faces,h42_faces,h43_faces,h44_faces,h45_faces,h46_faces,h47_faces,h48_faces,h49_faces,h50_faces,h51_faces,h52_faces,h53_faces,h54_faces,h55_faces,h56_faces,h57_faces,h58_faces,h59_faces,h60_faces,h61_faces,h62_faces,h63_faces,h64_faces,h65_faces,h66_faces,h67_faces,h68_faces,h69_faces,h70_faces,h71_faces,h72_faces,h73_faces,h74_faces,h75_faces,h76_faces,h77_faces,h78_faces,h79_faces,h80_faces,h81_faces,h82_faces,h83_faces,h84_faces,h85_faces,h86_faces,h87_faces,h88_faces,h89_faces, +}; +const dReal* halton_planes[ halton_numc ] = { + h00_planes,h01_planes,h02_planes,h03_planes,h04_planes,h05_planes,h06_planes,h07_planes,h08_planes,h09_planes,h10_planes,h11_planes,h12_planes,h13_planes,h14_planes,h15_planes,h16_planes,h17_planes,h18_planes,h19_planes,h20_planes,h21_planes,h22_planes,h23_planes,h24_planes,h25_planes,h26_planes,h27_planes,h28_planes,h29_planes,h30_planes,h31_planes,h32_planes,h33_planes,h34_planes,h35_planes,h36_planes,h37_planes,h38_planes,h39_planes,h40_planes,h41_planes,h42_planes,h43_planes,h44_planes,h45_planes,h46_planes,h47_planes,h48_planes,h49_planes,h50_planes,h51_planes,h52_planes,h53_planes,h54_planes,h55_planes,h56_planes,h57_planes,h58_planes,h59_planes,h60_planes,h61_planes,h62_planes,h63_planes,h64_planes,h65_planes,h66_planes,h67_planes,h68_planes,h69_planes,h70_planes,h71_planes,h72_planes,h73_planes,h74_planes,h75_planes,h76_planes,h77_planes,h78_planes,h79_planes,h80_planes,h81_planes,h82_planes,h83_planes,h84_planes,h85_planes,h86_planes,h87_planes,h88_planes,h89_planes, +}; diff --git a/libs/ode-0.16.1/ode/demo/icosahedron_geom.h b/libs/ode-0.16.1/ode/demo/icosahedron_geom.h new file mode 100644 index 0000000..eb0a0ea --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/icosahedron_geom.h @@ -0,0 +1,216 @@ +//<---- Icosahedron ----> +/* + This is a description of a convex icosahedron, to test + the convex collision detection. +*/ +unsigned int Sphere_pointcount = 42; +unsigned int Sphere_planecount = 80; +dReal Sphere_points[126]={ +0.000000,0.000000,-0.300000, +0.217080,-0.157716,-0.134164, +-0.082915,-0.255192,-0.134164, +-0.268327,0.000000,-0.134164, +-0.082915,0.255192,-0.134164, +0.217080,0.157716,-0.134164, +0.082915,-0.255192,0.134164, +-0.217080,-0.157716,0.134164, +-0.217080,0.157716,0.134164, +0.082915,0.255192,0.134164, +0.268327,0.000000,0.134164, +0.000000,0.000000,0.300000, +0.127597,-0.092703,-0.255196, +-0.048737,-0.149999,-0.255196, +0.078861,-0.242703,-0.157721, +0.127597,0.092703,-0.255196, +0.255194,0.000000,-0.157721, +-0.157719,0.000000,-0.255195, +-0.206457,-0.149999,-0.157721, +-0.048737,0.149999,-0.255196, +-0.206457,0.149999,-0.157721, +0.078861,0.242703,-0.157721, +0.285317,0.092704,0.000000, +0.285317,-0.092704,0.000000, +0.176336,-0.242705,0.000000, +0.000000,-0.300000,0.000000, +-0.176336,-0.242705,0.000000, +-0.285317,-0.092704,0.000000, +-0.285317,0.092704,0.000000, +-0.176336,0.242705,0.000000, +0.000000,0.300000,0.000000, +0.176336,0.242705,0.000000, +0.206457,-0.149999,0.157721, +-0.078861,-0.242703,0.157721, +-0.255194,0.000000,0.157721, +-0.078861,0.242703,0.157721, +0.206457,0.149999,0.157721, +0.157719,0.000000,0.255195, +0.048737,-0.149999,0.255196, +-0.127597,-0.092703,0.255196, +-0.127597,0.092703,0.255196, +0.048737,0.149999,0.255196 +}; +unsigned int Sphere_polygons[]={ +3,14,12,1, +3,12,14,13, +3,2,13,14, +3,13,0,12, +3,16,1,12, +3,12,15,16, +3,5,16,15, +3,12,0,15, +3,18,13,2, +3,13,18,17, +3,3,17,18, +3,17,0,13, +3,20,17,3, +3,17,20,19, +3,4,19,20, +3,19,0,17, +3,21,19,4, +3,19,21,15, +3,5,15,21, +3,15,0,19, +3,23,1,16, +3,16,22,23, +3,10,23,22, +3,22,16,5, +3,25,2,14, +3,14,24,25, +3,6,25,24, +3,24,14,1, +3,27,3,18, +3,18,26,27, +3,7,27,26, +3,26,18,2, +3,29,4,20, +3,20,28,29, +3,8,29,28, +3,28,20,3, +3,31,5,21, +3,21,30,31, +3,9,31,30, +3,30,21,4, +3,32,23,10, +3,23,32,24, +3,6,24,32, +3,24,1,23, +3,33,25,6, +3,25,33,26, +3,7,26,33, +3,26,2,25, +3,34,27,7, +3,27,34,28, +3,8,28,34, +3,28,3,27, +3,35,29,8, +3,29,35,30, +3,9,30,35, +3,30,4,29, +3,36,31,9, +3,31,36,22, +3,10,22,36, +3,22,5,31, +3,38,6,32, +3,32,37,38, +3,11,38,37, +3,37,32,10, +3,39,7,33, +3,33,38,39, +3,11,39,38, +3,38,33,6, +3,40,8,34, +3,34,39,40, +3,11,40,39, +3,39,34,7, +3,41,9,35, +3,35,40,41, +3,11,41,40, +3,40,35,8, +3,37,10,36, +3,36,41,37, +3,11,37,41, +3,41,36,9, +}; +dReal Sphere_planes[]={ +0.471317,-0.583121,-0.661687,0.283056, +0.187594,-0.577345,-0.794658,0.280252, +-0.038547,-0.748789,-0.661687,0.283056, +0.102381,-0.315090,-0.943523,0.283057, +0.700228,-0.268049,-0.661688,0.283056, +0.607060,0.000000,-0.794656,0.280252, +0.700228,0.268049,-0.661688,0.283056, +0.331305,0.000000,-0.943524,0.283057, +-0.408939,-0.628443,-0.661686,0.283056, +-0.491119,-0.356821,-0.794657,0.280252, +-0.724044,-0.194735,-0.661694,0.283057, +-0.268034,-0.194737,-0.943523,0.283057, +-0.724044,0.194735,-0.661694,0.283057, +-0.491119,0.356821,-0.794657,0.280252, +-0.408939,0.628443,-0.661686,0.283056, +-0.268034,0.194737,-0.943523,0.283057, +-0.038547,0.748789,-0.661687,0.283056, +0.187594,0.577345,-0.794658,0.280252, +0.471317,0.583121,-0.661687,0.283056, +0.102381,0.315090,-0.943523,0.283057, +0.904981,-0.268049,-0.330393,0.283056, +0.982246,0.000000,-0.187599,0.280252, +0.992077,0.000000,0.125631,0.283057, +0.904981,0.268049,-0.330393,0.283056, +0.024726,-0.943519,-0.330396,0.283056, +0.303531,-0.934171,-0.187598,0.280251, +0.306568,-0.943519,0.125651,0.283056, +0.534590,-0.777851,-0.330395,0.283056, +-0.889698,-0.315092,-0.330386,0.283056, +-0.794656,-0.577348,-0.187595,0.280251, +-0.802607,-0.583125,0.125648,0.283055, +-0.574584,-0.748793,-0.330397,0.283055, +-0.574584,0.748793,-0.330397,0.283055, +-0.794656,0.577348,-0.187595,0.280251, +-0.802607,0.583125,0.125648,0.283055, +-0.889698,0.315092,-0.330386,0.283056, +0.534590,0.777851,-0.330395,0.283056, +0.303531,0.934171,-0.187598,0.280251, +0.306568,0.943519,0.125651,0.283056, +0.024726,0.943519,-0.330396,0.283056, +0.889698,-0.315092,0.330386,0.283056, +0.794656,-0.577348,0.187595,0.280251, +0.574584,-0.748793,0.330397,0.283055, +0.802607,-0.583125,-0.125648,0.283055, +-0.024726,-0.943519,0.330396,0.283055, +-0.303531,-0.934171,0.187598,0.280251, +-0.534590,-0.777851,0.330395,0.283056, +-0.306568,-0.943519,-0.125651,0.283056, +-0.904981,-0.268049,0.330393,0.283056, +-0.982246,0.000000,0.187599,0.280252, +-0.904981,0.268049,0.330393,0.283056, +-0.992077,0.000000,-0.125631,0.283057, +-0.534590,0.777851,0.330395,0.283056, +-0.303531,0.934171,0.187598,0.280251, +-0.024726,0.943519,0.330396,0.283055, +-0.306568,0.943519,-0.125651,0.283056, +0.574584,0.748793,0.330397,0.283055, +0.794656,0.577348,0.187595,0.280251, +0.889698,0.315092,0.330386,0.283056, +0.802607,0.583125,-0.125648,0.283055, +0.408939,-0.628443,0.661686,0.283056, +0.491119,-0.356821,0.794657,0.280252, +0.268034,-0.194737,0.943523,0.283057, +0.724044,-0.194735,0.661694,0.283057, +-0.471317,-0.583121,0.661687,0.283056, +-0.187594,-0.577345,0.794658,0.280252, +-0.102381,-0.315090,0.943523,0.283057, +0.038547,-0.748789,0.661687,0.283056, +-0.700228,0.268049,0.661688,0.283056, +-0.607060,0.000000,0.794656,0.280252, +-0.331305,0.000000,0.943524,0.283057, +-0.700228,-0.268049,0.661688,0.283056, +0.038547,0.748789,0.661687,0.283056, +-0.187594,0.577345,0.794658,0.280252, +-0.102381,0.315090,0.943523,0.283057, +-0.471317,0.583121,0.661687,0.283056, +0.724044,0.194735,0.661694,0.283057, +0.491119,0.356821,0.794657,0.280252, +0.268034,0.194737,0.943523,0.283057, +0.408939,0.628443,0.661686,0.283056, +}; + diff --git a/libs/ode-0.16.1/ode/demo/texturepath.h b/libs/ode-0.16.1/ode/demo/texturepath.h new file mode 100644 index 0000000..5815138 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/texturepath.h @@ -0,0 +1,26 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef DRAWSTUFF_TEXTURE_PATH +#define DRAWSTUFF_TEXTURE_PATH "../../drawstuff/textures" +#endif + diff --git a/libs/ode-0.16.1/ode/demo/world_geom3.h b/libs/ode-0.16.1/ode/demo/world_geom3.h new file mode 100644 index 0000000..27cd69e --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/world_geom3.h @@ -0,0 +1,9 @@ +// mesh for a world model, to be used with test_cyl.cpp + +static float world_vertices[] = {10.000000f,-10.000000f,1.000000f,-10.000000f,-10.000000f,1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,10.000000f,-10.000000f,-1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,-10.000000f,-1.000000f,10.000000f,-10.000000f,-1.000000f,10.000000f,10.000000f,-1.000000f,10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,-1.000000f,10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,10.000000f,-1.000000f,10.000000f,10.000000f,-1.000000f,0.000000f,9.000000f,-0.000000f,0.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,0.000000f,0.000000f,9.000000f,-0.000000f,9.000000f,-9.000000f,0.000000f,9.000000f,9.000000f,-0.000000f,10.000000f,10.000000f,-1.000000f,-10.000000f,10.000000f,-1.000000f,-10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,-1.000000f,-10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-10.000000f,10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,9.000000f,-9.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,-10.000000f,-10.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,9.000000f,9.000000f,1.000000f,9.000000f,-9.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,9.000000f,9.000000f,1.000000f,10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-10.000000f,-10.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,0.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,1.000000f,0.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,1.000000f,9.000000f,-9.000000f,1.000000f,0.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,1.000000f,9.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,1.000000f,9.000000f,9.000000f,1.000000f,9.000000f,9.000000f,1.000000f,9.000000f,9.000000f,-0.000000f,9.000000f,-9.000000f,0.000000f,0.000000f,9.000000f,-0.000000f,9.000000f,9.000000f,-0.000000f,9.000000f,9.000000f,1.000000f,0.000000f,9.000000f,-0.000000f,9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,0.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,-9.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,0.000000f,-9.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,1.000000f,-2.997000f,-1.748874f,0.000000f,-2.997000f,-2.001000f,0.000000f,0.000000f,-9.000000f,0.000000f,-2.997000f,-1.748874f,0.000000f,0.000000f,-9.000000f,0.000000f,-2.997000f,1.748874f,-0.000000f,-2.997000f,-2.001000f,0.000000f,-2.997000f,-6.003000f,0.002697f,0.000000f,-9.000000f,0.000000f,0.000000f,9.000000f,-0.000000f,-2.997000f,2.001000f,-0.000000f,-2.997000f,1.748874f,-0.000000f,0.000000f,9.000000f,-0.000000f,-2.997000f,1.748874f,-0.000000f,0.000000f,-9.000000f,0.000000f,-2.997000f,2.001000f,-0.000000f,0.000000f,9.000000f,-0.000000f,-2.997000f,6.003000f,0.002697f,-6.003000f,6.003000f,0.002697f,-2.997000f,6.003000f,0.002697f,0.000000f,9.000000f,-0.000000f,0.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,-0.000000f,-6.003000f,6.003000f,0.002697f,-6.003000f,1.748874f,-0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,2.001000f,-0.000000f,-6.003000f,6.003000f,0.002697f,-9.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,-0.000000f,-6.003000f,1.748874f,-0.000000f,-6.003000f,2.001000f,-0.000000f,-9.000000f,9.000000f,-0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,1.748874f,-0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,-6.003000f,0.002697f,-6.003000f,-2.001000f,0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-6.003000f,0.002697f,-9.000000f,-9.000000f,0.000000f,0.000000f,-9.000000f,0.000000f,-6.003000f,-6.003000f,0.002697f,0.000000f,-9.000000f,0.000000f,-2.997000f,-6.003000f,0.002697f,-2.997000f,1.748874f,1.237951f,-2.997000f,1.748874f,-0.000000f,-2.997000f,2.001000f,-0.000000f,-2.997000f,1.748874f,1.237951f,-2.997000f,2.001000f,-0.000000f,-2.997000f,2.001000f,1.515748f,-6.003000f,-2.001000f,1.515748f,-6.003000f,-6.003000f,0.002697f,-2.997000f,-6.003000f,0.002697f,-6.003000f,-2.001000f,1.515748f,-2.997000f,-6.003000f,0.002697f,-2.997000f,-2.001000f,1.515748f,-2.997000f,2.001000f,1.515748f,-2.997000f,6.003000f,0.002697f,-6.003000f,6.003000f,0.002697f,-6.003000f,6.003000f,0.002697f,-6.003000f,2.001000f,1.515748f,-2.997000f,2.001000f,1.515748f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-6.003000f,0.002697f,-6.003000f,-2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,6.003000f,0.002697f,-6.003000f,2.001000f,-0.000000f,-2.997000f,-2.001000f,1.515748f,-2.997000f,-6.003000f,0.002697f,-2.997000f,-2.001000f,0.000000f,-2.997000f,2.001000f,-0.000000f,-2.997000f,6.003000f,0.002697f,-2.997000f,2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-2.997000f,2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,-2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-2.997000f,-1.748874f,1.237951f,-2.997000f,1.748874f,1.237951f,-2.997000f,2.001000f,1.515748f,-2.997000f,-1.748874f,1.237951f,-2.997000f,2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-6.003000f,-1.748874f,1.237951f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-1.748874f,1.237951f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-2.997000f,-2.001000f,0.000000f,-2.997000f,-1.748874f,1.237951f,-2.997000f,-2.001000f,0.000000f,-2.997000f,-1.748874f,0.000000f,-2.997000f,-1.748874f,1.237951f,-6.003000f,1.748874f,1.237951f,-6.003000f,2.001000f,1.515748f,-6.003000f,2.001000f,-0.000000f,-6.003000f,1.748874f,1.237951f,-6.003000f,2.001000f,-0.000000f,-6.003000f,1.748874f,-0.000000f,-6.003000f,1.748874f,1.237951f,-6.003000f,-1.748874f,1.237951f,-6.003000f,-2.001000f,1.515748f,-6.003000f,1.748874f,1.237951f,-6.003000f,-2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,1.748874f,1.237951f,-6.003000f,1.748874f,-0.000000f,-2.997000f,1.748874f,1.237951f,-6.003000f,1.748874f,-0.000000f,-2.997000f,1.748874f,-0.000000f,-2.997000f,1.748874f,1.237951f,-6.003000f,1.748874f,-0.000000f,-6.003000f,-1.748874f,0.000000f,-2.997000f,-1.748874f,0.000000f,-6.003000f,1.748874f,-0.000000f,-2.997000f,-1.748874f,0.000000f,-2.997000f,1.748874f,-0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f,-2.997000f,-1.748874f,0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-1.748874f,1.237951f,-6.003000f,1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f,-6.003000f,1.748874f,1.237951f,-2.997000f,1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f}; + +static float world_normals[] = {0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000225f,0.000161f,1.000000f,0.000225f,-0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,0.000225f,0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,-0.000000f,0.000000f,1.000000f,0.000787f,0.000337f,1.000000f,0.000225f,-0.000161f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000225f,-0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,0.000787f,0.000337f,1.000000f,0.000400f,-0.179805f,0.983702f,0.000225f,-0.000161f,1.000000f,0.000225f,0.000161f,1.000000f,0.000787f,-0.000337f,1.000000f,0.000000f,0.000000f,1.000000f,0.000225f,0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,0.000225f,-0.000161f,1.000000f,0.000787f,-0.000337f,1.000000f,0.000225f,0.000161f,1.000000f,0.000532f,0.119686f,0.992812f,-0.000320f,0.143927f,0.989588f,0.000532f,0.119686f,0.992812f,0.000225f,0.000161f,1.000000f,0.000225f,0.000161f,1.000000f,-0.000393f,0.000056f,1.000000f,-0.000320f,0.143927f,0.989588f,-0.000000f,0.000000f,1.000000f,-0.000315f,-0.000045f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000787f,-0.000337f,1.000000f,-0.000320f,0.143927f,0.989588f,-0.000393f,0.000056f,1.000000f,-0.000393f,0.000056f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000787f,-0.000337f,1.000000f,-0.000393f,0.000056f,1.000000f,-0.000315f,-0.000045f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000315f,-0.000045f,1.000000f,-0.000398f,-0.089784f,0.995961f,-0.000787f,0.000337f,1.000000f,-0.000315f,-0.000045f,1.000000f,-0.000787f,0.000337f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000398f,-0.089784f,0.995961f,-0.000315f,-0.000045f,1.000000f,0.000225f,-0.000161f,1.000000f,-0.000398f,-0.089784f,0.995961f,0.000225f,-0.000161f,1.000000f,0.000400f,-0.179805f,0.983702f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,-0.239222f,0.970965f,-0.000398f,-0.089784f,0.995961f,0.000400f,-0.179805f,0.983702f,0.000000f,-0.239222f,0.970965f,0.000400f,-0.179805f,0.983702f,0.000000f,-0.119611f,0.992821f,0.000000f,0.239222f,0.970965f,0.000532f,0.119686f,0.992812f,-0.000320f,0.143927f,0.989588f,-0.000320f,0.143927f,0.989588f,0.000000f,0.119611f,0.992821f,0.000000f,0.239222f,0.970965f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,-0.119611f,0.992821f,0.000000f,0.239222f,0.970965f,0.000000f,0.119611f,0.992821f,0.000000f,0.119611f,0.992821f,0.000000f,-0.239222f,0.970965f,0.000000f,-0.119611f,0.992821f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f}; + + +static dTriIndex world_indices[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143,144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159,160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175,176,177,178,179,180,181,182,183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207,208,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223,224,225,226,227}; + diff --git a/libs/ode-0.16.1/ode/doc/Doxyfile.in b/libs/ode-0.16.1/ode/doc/Doxyfile.in new file mode 100644 index 0000000..bfb9412 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/Doxyfile.in @@ -0,0 +1,2331 @@ +# Doxyfile 1.8.5 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv +# for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = ODE + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = @ODE_VERSION@ + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify an logo or icon that is included in +# the documentation. The maximum height of the logo should not exceed 55 pixels +# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo +# to the output directory. + +PROJECT_LOGO = @top_srcdir@/web/ODElogo.png + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese- +# Traditional, Croatian, Czech, Danish, Dutch, English, Esperanto, Farsi, +# Finnish, French, German, Greek, Hungarian, Italian, Japanese, Japanese-en, +# Korean, Korean-en, Latvian, Norwegian, Macedonian, Persian, Polish, +# Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, Swedish, +# Turkish, Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a +# new page for each member. If set to NO, the documentation of a member will be +# part of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = YES + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran, VHDL. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. +# +# Note For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by by putting a % sign in front of the word +# or globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO these classes will be included in the various overviews. This option has +# no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = NO + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = YES + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO the members will appear in declaration order. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the +# todo list. This list is created by putting \todo commands in the +# documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the +# test list. This list is created by putting \test commands in the +# documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if <section_label> ... \endif and \cond <section_label> +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES the list +# will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. Do not use file names with spaces, bibtex cannot handle them. See +# also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO doxygen will only warn about wrong or incomplete parameter +# documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. +# Note: If this tag is empty the current directory is searched. + +INPUT = @top_srcdir@/include/ode @top_builddir@/include/ode + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank the +# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii, +# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, +# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, +# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf, +# *.qsf, *.as and *.js. + +FILE_PATTERNS = *.c \ + *.cc \ + *.cxx \ + *.cpp \ + *.c++ \ + *.java \ + *.ii \ + *.ixx \ + *.ipp \ + *.i++ \ + *.inl \ + *.idl \ + *.ddl \ + *.odl \ + *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ \ + *.cs \ + *.d \ + *.php \ + *.php4 \ + *.php5 \ + *.phtml \ + *.inc \ + *.m \ + *.markdown \ + *.md \ + *.mm \ + *.dox \ + *.py \ + *.f90 \ + *.f \ + *.for \ + *.tcl \ + *.vhd \ + *.vhdl \ + *.ucf \ + *.qsf \ + *.as \ + *.js + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = ODE_API + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# <filter> <input-file> +# +# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER ) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES, then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see http://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = d + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional user- +# defined cascading style sheet that is included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefor more robust against future updates. +# Doxygen will copy the style sheet file to the output directory. For an example +# see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the stylesheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 243 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 187 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 92 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to NO can help when comparing the output of multiple runs. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler ( hhc.exe). If non-empty +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated ( +# YES) or that it should be included in the master .chm file ( NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated ( +# YES) or a normal table of contents ( NO) in the .chm file. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = YES + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# http://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using prerendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = YES + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = NativeMML + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from http://www.mathjax.org before deployment. +# The default value is: http://cdn.mathjax.org/mathjax/latest. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use <access key> + S +# (what the <access key> is depends on the OS and browser, but it is typically +# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down +# key> to jump into the search results window, the results can be navigated +# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel +# the search. The filter options can be selected when the cursor is inside the +# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys> +# to select a filter and <Enter> or <escape> to activate or cancel the filter +# option. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a web server instead of a web client using Javascript. There +# are two flavours of web server based searching depending on the +# EXTERNAL_SEARCH setting. When disabled, doxygen will generate a PHP script for +# searching and an index file used by the script. When EXTERNAL_SEARCH is +# enabled the indexing and searching needs to be provided by external tools. See +# the section "External Indexing and Searching" for details. +# The default value is: NO. +# This tag requires that the tag SEARCHENGINE is set to YES. + +SERVER_BASED_SEARCH = NO + +# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP +# script for searching. Instead the search results are written to an XML file +# which needs to be processed by an external indexer. Doxygen will invoke an +# external search engine pointed to by the SEARCHENGINE_URL option to obtain the +# search results. +# +# Doxygen ships with an example indexer ( doxyindexer) and search engine +# (doxysearch.cgi) which are based on the open source search engine library +# Xapian (see: http://xapian.org/). +# +# See the section "External Indexing and Searching" for details. +# The default value is: NO. +# This tag requires that the tag SEARCHENGINE is set to YES. + +EXTERNAL_SEARCH = NO + +# The SEARCHENGINE_URL should point to a search engine hosted by a web server +# which will return the search results when EXTERNAL_SEARCH is enabled. +# +# Doxygen ships with an example indexer ( doxyindexer) and search engine +# (doxysearch.cgi) which are based on the open source search engine library +# Xapian (see: http://xapian.org/). See the section "External Indexing and +# Searching" for details. +# This tag requires that the tag SEARCHENGINE is set to YES. + +SEARCHENGINE_URL = + +# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed +# search data is written to a file for indexing by an external tool. With the +# SEARCHDATA_FILE tag the name of this file can be specified. +# The default file is: searchdata.xml. +# This tag requires that the tag SEARCHENGINE is set to YES. + +SEARCHDATA_FILE = searchdata.xml + +# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the +# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is +# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple +# projects and redirect the results back to the right project. +# This tag requires that the tag SEARCHENGINE is set to YES. + +EXTERNAL_SEARCH_ID = + +# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen +# projects other than the one defined by this configuration file, but that are +# all added to the same external search index. Each project needs to have a +# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of +# to a relative location where the documentation can be found. The format is: +# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ... +# This tag requires that the tag SEARCHENGINE is set to YES. + +EXTRA_SEARCH_MAPPINGS = + +#--------------------------------------------------------------------------- +# Configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES doxygen will generate LaTeX output. +# The default value is: YES. + +GENERATE_LATEX = NO + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: latex. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. +# +# Note that when enabling USE_PDFLATEX this option is only used for generating +# bitmaps for formulas in the HTML output, but not in the Makefile that is +# written to the output directory. +# The default file is: latex. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate +# index for LaTeX. +# The default file is: makeindex. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES doxygen generates more compact LaTeX +# documents. This may be useful for small projects and may help to save some +# trees in general. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used by the +# printer. +# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x +# 14 inches) and executive (7.25 x 10.5 inches). +# The default value is: a4. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +PAPER_TYPE = a4 + +# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names +# that should be included in the LaTeX output. To get the times font for +# instance you can specify +# EXTRA_PACKAGES=times +# If left blank no extra packages will be included. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the +# generated LaTeX document. The header should contain everything until the first +# chapter. If it is left blank doxygen will generate a standard header. See +# section "Doxygen usage" for information on how to let doxygen write the +# default header to a separate file. +# +# Note: Only use a user-defined header if you know what you are doing! The +# following commands have a special meaning inside the header: $title, +# $datetime, $date, $doxygenversion, $projectname, $projectnumber. Doxygen will +# replace them by respectively the title of the page, the current date and time, +# only the current date, the version number of doxygen, the project name (see +# PROJECT_NAME), or the project number (see PROJECT_NUMBER). +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the +# generated LaTeX document. The footer should contain everything after the last +# chapter. If it is left blank doxygen will generate a standard footer. +# +# Note: Only use a user-defined footer if you know what you are doing! +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_FOOTER = + +# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the LATEX_OUTPUT output +# directory. Note that the files will be copied as-is; there are no commands or +# markers available. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_EXTRA_FILES = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is +# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will +# contain links (just like the HTML output) instead of page references. This +# makes the output suitable for online browsing using a PDF viewer. +# The default value is: YES. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +PDF_HYPERLINKS = YES + +# If the LATEX_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate +# the PDF file directly from the LaTeX files. Set this option to YES to get a +# higher quality PDF documentation. +# The default value is: YES. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode +# command to the generated LaTeX files. This will instruct LaTeX to keep running +# if errors occur, instead of asking the user for help. This option is also used +# when generating formulas in HTML. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_BATCHMODE = NO + +# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the +# index chapters (such as File Index, Compound Index, etc.) in the output. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_HIDE_INDICES = NO + +# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source +# code with syntax highlighting in the LaTeX output. +# +# Note that which sources are shown also depends on other settings such as +# SOURCE_BROWSER. +# The default value is: NO. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. See +# http://en.wikipedia.org/wiki/BibTeX and \cite for more info. +# The default value is: plain. +# This tag requires that the tag GENERATE_LATEX is set to YES. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# Configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES doxygen will generate RTF output. The +# RTF output is optimized for Word 97 and may not look too pretty with other RTF +# readers/editors. +# The default value is: NO. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: rtf. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES doxygen generates more compact RTF +# documents. This may be useful for small projects and may help to save some +# trees in general. +# The default value is: NO. +# This tag requires that the tag GENERATE_RTF is set to YES. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will +# contain hyperlink fields. The RTF file will contain links (just like the HTML +# output) instead of page references. This makes the output suitable for online +# browsing using Word or some other Word compatible readers that support those +# fields. +# +# Note: WordPad (write) and others do not support links. +# The default value is: NO. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_HYPERLINKS = NO + +# Load stylesheet definitions from file. Syntax is similar to doxygen's config +# file, i.e. a series of assignments. You only have to provide replacements, +# missing definitions are set to their default value. +# +# See also section "Doxygen usage" for information on how to generate the +# default style sheet that doxygen normally uses. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an RTF document. Syntax is +# similar to doxygen's config file. A template extensions file can be generated +# using doxygen -e rtf extensionFile. +# This tag requires that the tag GENERATE_RTF is set to YES. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES doxygen will generate man pages for +# classes and files. +# The default value is: NO. + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. A directory man3 will be created inside the directory specified by +# MAN_OUTPUT. +# The default directory is: man. +# This tag requires that the tag GENERATE_MAN is set to YES. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to the generated +# man pages. In case the manual section does not start with a number, the number +# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is +# optional. +# The default value is: .3. +# This tag requires that the tag GENERATE_MAN is set to YES. + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it +# will generate one additional man file for each entity documented in the real +# man page(s). These additional files only source the real man page, but without +# them the man command would be unable to find the correct page. +# The default value is: NO. +# This tag requires that the tag GENERATE_MAN is set to YES. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# Configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES doxygen will generate an XML file that +# captures the structure of the code including all documentation. +# The default value is: NO. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: xml. +# This tag requires that the tag GENERATE_XML is set to YES. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify a XML schema, which can be used by a +# validating XML parser to check the syntax of the XML files. +# This tag requires that the tag GENERATE_XML is set to YES. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify a XML DTD, which can be used by a +# validating XML parser to check the syntax of the XML files. +# This tag requires that the tag GENERATE_XML is set to YES. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES doxygen will dump the program +# listings (including syntax highlighting and cross-referencing information) to +# the XML output. Note that enabling this will significantly increase the size +# of the XML output. +# The default value is: YES. +# This tag requires that the tag GENERATE_XML is set to YES. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# Configuration options related to the DOCBOOK output +#--------------------------------------------------------------------------- + +# If the GENERATE_DOCBOOK tag is set to YES doxygen will generate Docbook files +# that can be used to generate PDF. +# The default value is: NO. + +GENERATE_DOCBOOK = NO + +# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in +# front of it. +# The default directory is: docbook. +# This tag requires that the tag GENERATE_DOCBOOK is set to YES. + +DOCBOOK_OUTPUT = docbook + +#--------------------------------------------------------------------------- +# Configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES doxygen will generate an AutoGen +# Definitions (see http://autogen.sf.net) file that captures the structure of +# the code including all documentation. Note that this feature is still +# experimental and incomplete at the moment. +# The default value is: NO. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# Configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES doxygen will generate a Perl module +# file that captures the structure of the code including all documentation. +# +# Note that this feature is still experimental and incomplete at the moment. +# The default value is: NO. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES doxygen will generate the necessary +# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI +# output from the Perl module output. +# The default value is: NO. +# This tag requires that the tag GENERATE_PERLMOD is set to YES. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be nicely +# formatted so it can be parsed by a human reader. This is useful if you want to +# understand what is going on. On the other hand, if this tag is set to NO the +# size of the Perl module output will be much smaller and Perl will parse it +# just the same. +# The default value is: YES. +# This tag requires that the tag GENERATE_PERLMOD is set to YES. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file are +# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful +# so different doxyrules.make files included by the same Makefile don't +# overwrite each other's variables. +# This tag requires that the tag GENERATE_PERLMOD is set to YES. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES doxygen will evaluate all +# C-preprocessor directives found in the sources and include files. +# The default value is: YES. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES doxygen will expand all macro names +# in the source code. If set to NO only conditional compilation will be +# performed. Macro expansion can be done in a controlled way by setting +# EXPAND_ONLY_PREDEF to YES. +# The default value is: NO. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +MACRO_EXPANSION = YES + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then +# the macro expansion is limited to the macros specified with the PREDEFINED and +# EXPAND_AS_DEFINED tags. +# The default value is: NO. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +EXPAND_ONLY_PREDEF = YES + +# If the SEARCH_INCLUDES tag is set to YES the includes files in the +# INCLUDE_PATH will be searched if a #include is found. +# The default value is: YES. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by the +# preprocessor. +# This tag requires that the tag SEARCH_INCLUDES is set to YES. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will be +# used. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that are +# defined before the preprocessor is started (similar to the -D option of e.g. +# gcc). The argument of the tag is a list of macros of the form: name or +# name=definition (no spaces). If the definition and the "=" are omitted, "=1" +# is assumed. To prevent a macro definition from being undefined via #undef or +# recursively expanded use the := operator instead of the = operator. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +PREDEFINED = ODE_API= + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this +# tag can be used to specify a list of macro names that should be expanded. The +# macro definition that is found in the sources will be used. Use the PREDEFINED +# tag if you want to use a different macro definition that overrules the +# definition found in the source code. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will +# remove all refrences to function-like macros that are alone on a line, have an +# all uppercase name, and do not end with a semicolon. Such function macros are +# typically used for boiler-plate code, and will confuse the parser if not +# removed. +# The default value is: YES. +# This tag requires that the tag ENABLE_PREPROCESSING is set to YES. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration options related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES tag can be used to specify one or more tag files. For each tag +# file the location of the external documentation should be added. The format of +# a tag file without this location is as follows: +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where loc1 and loc2 can be relative or absolute paths or URLs. See the +# section "Linking to external documentation" for more information about the use +# of tag files. +# Note: Each tag file must have an unique name (where the name does NOT include +# the path). If a tag file is not located in the directory in which doxygen is +# run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create a +# tag file that is based on the input files it reads. See section "Linking to +# external documentation" for more information about the usage of tag files. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external class will be listed in the +# class index. If set to NO only the inherited external classes will be listed. +# The default value is: NO. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed in +# the modules index. If set to NO, only the current project's groups will be +# listed. +# The default value is: YES. + +EXTERNAL_GROUPS = YES + +# If the EXTERNAL_PAGES tag is set to YES all external pages will be listed in +# the related pages index. If set to NO, only the current project's pages will +# be listed. +# The default value is: YES. + +EXTERNAL_PAGES = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of 'which perl'). +# The default file (with absolute path) is: /usr/bin/perl. + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES doxygen will generate a class diagram +# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to +# NO turns the diagrams off. Note that this option also works with HAVE_DOT +# disabled, but it is recommended to install and use dot, since it yields more +# powerful graphs. +# The default value is: YES. + +CLASS_DIAGRAMS = NO + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see: +# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide inheritance +# and usage relations if the target is undocumented or is not a class. +# The default value is: YES. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz (see: +# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent +# Bell Labs. The other options in this section have no effect if this option is +# set to NO +# The default value is: NO. + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed +# to run in parallel. When set to 0 doxygen will base this on the number of +# processors available in the system. You can set it explicitly to a value +# larger than 0 to get control over the balance between CPU load and processing +# speed. +# Minimum value: 0, maximum value: 32, default value: 0. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_NUM_THREADS = 0 + +# When you want a differently looking font n the dot files that doxygen +# generates you can specify the font name using DOT_FONTNAME. You need to make +# sure dot is able to find the font, which can be done by putting it in a +# standard location or by setting the DOTFONTPATH environment variable or by +# setting DOT_FONTPATH to the directory containing the font. +# The default value is: Helvetica. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_FONTNAME = Helvetica + +# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of +# dot graphs. +# Minimum value: 4, maximum value: 24, default value: 10. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the default font as specified with +# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set +# the path where dot can find it using this tag. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_FONTPATH = + +# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for +# each documented class showing the direct and indirect inheritance relations. +# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a +# graph for each documented class showing the direct and indirect implementation +# dependencies (inheritance, containment, and class references variables) of the +# class with other documented classes. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +COLLABORATION_GRAPH = NO + +# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for +# groups, showing the direct groups dependencies. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +UML_LOOK = NO + +# If the UML_LOOK tag is enabled, the fields and methods are shown inside the +# class node. If there are many fields or methods and many nodes the graph may +# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the +# number of items for each type to make the size more manageable. Set this to 0 +# for no limit. Note that the threshold may be exceeded by 50% before the limit +# is enforced. So when you set the threshold to 10, up to 15 fields may appear, +# but if the number exceeds 15, the total amount of fields shown is limited to +# 10. +# Minimum value: 0, maximum value: 100, default value: 10. +# This tag requires that the tag HAVE_DOT is set to YES. + +UML_LIMIT_NUM_FIELDS = 10 + +# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and +# collaboration graphs will show the relations between templates and their +# instances. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +TEMPLATE_RELATIONS = NO + +# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to +# YES then doxygen will generate a graph for each documented file showing the +# direct and indirect include dependencies of the file with other documented +# files. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +INCLUDE_GRAPH = NO + +# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are +# set to YES then doxygen will generate a graph for each documented file showing +# the direct and indirect include dependencies of the file with other documented +# files. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +INCLUDED_BY_GRAPH = NO + +# If the CALL_GRAPH tag is set to YES then doxygen will generate a call +# dependency graph for every global function or class method. +# +# Note that enabling this option will significantly increase the time of a run. +# So in most cases it will be better to enable call graphs for selected +# functions only using the \callgraph command. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller +# dependency graph for every global function or class method. +# +# Note that enabling this option will significantly increase the time of a run. +# So in most cases it will be better to enable caller graphs for selected +# functions only using the \callergraph command. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical +# hierarchy of all classes instead of a textual one. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +GRAPHICAL_HIERARCHY = NO + +# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the +# dependencies a directory has on other directories in a graphical way. The +# dependency relations are determined by the #include relations between the +# files in the directories. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. +# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order +# to make the SVG files visible in IE 9+ (other browsers do not have this +# requirement). +# Possible values are: png, jpg, gif and svg. +# The default value is: png. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# +# Note that this requires a modern browser other than Internet Explorer. Tested +# and working are Firefox, Chrome, Safari, and Opera. +# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make +# the SVG files visible. Older versions of IE do not have SVG support. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +INTERACTIVE_SVG = NO + +# The DOT_PATH tag can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the \dotfile +# command). +# This tag requires that the tag HAVE_DOT is set to YES. + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the \mscfile +# command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes +# that will be shown in the graph. If the number of nodes in a graph becomes +# larger than this value, doxygen will truncate the graph, which is visualized +# by representing a node as a red box. Note that doxygen if the number of direct +# children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that +# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. +# Minimum value: 0, maximum value: 10000, default value: 50. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs +# generated by dot. A depth value of 3 means that only nodes reachable from the +# root by following a path via at most 3 edges will be shown. Nodes that lay +# further from the root node will be omitted. Note that setting this option to 1 +# or 2 may greatly reduce the computation time needed for large code bases. Also +# note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. +# Minimum value: 0, maximum value: 1000, default value: 0. +# This tag requires that the tag HAVE_DOT is set to YES. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not seem +# to support this out of the box. +# +# Warning: Depending on the platform used, enabling this option may lead to +# badly anti-aliased labels on the edges of a graph (i.e. they become hard to +# read). +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) support +# this, this feature is disabled by default. +# The default value is: NO. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_MULTI_TARGETS = NO + +# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page +# explaining the meaning of the various boxes and arrows in the dot generated +# graphs. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES doxygen will remove the intermediate dot +# files that are used to generate the various graphs. +# The default value is: YES. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_CLEANUP = YES diff --git a/libs/ode-0.16.1/ode/doc/Makefile.am b/libs/ode-0.16.1/ode/doc/Makefile.am new file mode 100644 index 0000000..a7a006e --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/Makefile.am @@ -0,0 +1,11 @@ +EXTRA_DIST = Doxyfile.in main.dox pix + +if HAVE_DOXYGEN + +mostlyclean-local: + rm -rf html + +html-local: Doxyfile + $(DOXYGEN) Doxyfile + +endif diff --git a/libs/ode-0.16.1/ode/doc/Makefile.in b/libs/ode-0.16.1/ode/doc/Makefile.in new file mode 100644 index 0000000..f031226 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/Makefile.in @@ -0,0 +1,472 @@ +# Makefile.in generated by automake 1.15 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994-2014 Free Software Foundation, Inc. + +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ +VPATH = @srcdir@ +am__is_gnu_make = { \ + if test -z '$(MAKELEVEL)'; then \ + false; \ + elif test -n '$(MAKE_HOST)'; then \ + true; \ + elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \ + true; \ + else \ + false; \ + fi; \ +} +am__make_running_with_option = \ + case $${target_option-} in \ + ?) ;; \ + *) echo "am__make_running_with_option: internal error: invalid" \ + "target option '$${target_option-}' specified" >&2; \ + exit 1;; \ + esac; \ + has_opt=no; \ + sane_makeflags=$$MAKEFLAGS; \ + if $(am__is_gnu_make); then \ + sane_makeflags=$$MFLAGS; \ + else \ + case $$MAKEFLAGS in \ + *\\[\ \ ]*) \ + bs=\\; \ + sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \ + | sed "s/$$bs$$bs[$$bs $$bs ]*//g"`;; \ + esac; \ + fi; \ + skip_next=no; \ + strip_trailopt () \ + { \ + flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \ + }; \ + for flg in $$sane_makeflags; do \ + test $$skip_next = yes && { skip_next=no; continue; }; \ + case $$flg in \ + *=*|--*) continue;; \ + -*I) strip_trailopt 'I'; skip_next=yes;; \ + -*I?*) strip_trailopt 'I';; \ + -*O) strip_trailopt 'O'; skip_next=yes;; \ + -*O?*) strip_trailopt 'O';; \ + -*l) strip_trailopt 'l'; skip_next=yes;; \ + -*l?*) strip_trailopt 'l';; \ + -[dEDm]) skip_next=yes;; \ + -[JT]) skip_next=yes;; \ + esac; \ + case $$flg in \ + *$$target_option*) has_opt=yes; break;; \ + esac; \ + done; \ + test $$has_opt = yes +am__make_dryrun = (target_option=n; $(am__make_running_with_option)) +am__make_keepgoing = (target_option=k; $(am__make_running_with_option)) +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ +subdir = ode/doc +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \ + $(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \ + $(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \ + $(top_srcdir)/m4/pkg.m4 $(top_srcdir)/configure.ac +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +DIST_COMMON = $(srcdir)/Makefile.am $(am__DIST_COMMON) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/ode/src/config.h +CONFIG_CLEAN_FILES = Doxyfile +CONFIG_CLEAN_VPATH_FILES = +AM_V_P = $(am__v_P_@AM_V@) +am__v_P_ = $(am__v_P_@AM_DEFAULT_V@) +am__v_P_0 = false +am__v_P_1 = : +AM_V_GEN = $(am__v_GEN_@AM_V@) +am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@) +am__v_GEN_0 = @echo " GEN " $@; +am__v_GEN_1 = +AM_V_at = $(am__v_at_@AM_V@) +am__v_at_ = $(am__v_at_@AM_DEFAULT_V@) +am__v_at_0 = @ +am__v_at_1 = +SOURCES = +DIST_SOURCES = +am__can_run_installinfo = \ + case $$AM_UPDATE_INFO_DIR in \ + n|no|NO) false;; \ + *) (install-info --version) >/dev/null 2>&1;; \ + esac +am__tagged_files = $(HEADERS) $(SOURCES) $(TAGS_FILES) $(LISP) +am__DIST_COMMON = $(srcdir)/Doxyfile.in $(srcdir)/Makefile.in +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +ACLOCAL = @ACLOCAL@ +ALLOCA = @ALLOCA@ +AMTAR = @AMTAR@ +AM_DEFAULT_VERBOSITY = @AM_DEFAULT_VERBOSITY@ +AR = @AR@ +AS = @AS@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CCD_CFLAGS = @CCD_CFLAGS@ +CCD_LIBS = @CCD_LIBS@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXCPP = @CXXCPP@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DLLTOOL = @DLLTOOL@ +DOXYGEN = @DOXYGEN@ +DSYMUTIL = @DSYMUTIL@ +DUMPBIN = @DUMPBIN@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +EXTRA_LIBTOOL_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ +FGREP = @FGREP@ +GL_LIBS = @GL_LIBS@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LD = @LD@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LIBSTDCXX = @LIBSTDCXX@ +LIBTOOL = @LIBTOOL@ +LIPO = @LIPO@ +LN_S = @LN_S@ +LTLIBOBJS = @LTLIBOBJS@ +LT_SYS_LIBRARY_PATH = @LT_SYS_LIBRARY_PATH@ +MAKEINFO = @MAKEINFO@ +MANIFEST_TOOL = @MANIFEST_TOOL@ +MKDIR_P = @MKDIR_P@ +NM = @NM@ +NMEDIT = @NMEDIT@ +OBJDUMP = @OBJDUMP@ +OBJEXT = @OBJEXT@ +ODE_PRECISION = @ODE_PRECISION@ +ODE_VERSION = @ODE_VERSION@ +ODE_VERSION_INFO = @ODE_VERSION_INFO@ +OTOOL = @OTOOL@ +OTOOL64 = @OTOOL64@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_URL = @PACKAGE_URL@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +PKG_CONFIG = @PKG_CONFIG@ +PKG_CONFIG_LIBDIR = @PKG_CONFIG_LIBDIR@ +PKG_CONFIG_PATH = @PKG_CONFIG_PATH@ +RANLIB = @RANLIB@ +SED = @SED@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +VERSION = @VERSION@ +WINDRES = @WINDRES@ +X11_CFLAGS = @X11_CFLAGS@ +X11_LIBS = @X11_LIBS@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_AR = @ac_ct_AR@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +ac_ct_DUMPBIN = @ac_ct_DUMPBIN@ +ac_ct_WINDRES = @ac_ct_WINDRES@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +build = @build@ +build_alias = @build_alias@ +build_cpu = @build_cpu@ +build_os = @build_os@ +build_vendor = @build_vendor@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host = @host@ +host_alias = @host_alias@ +host_cpu = @host_cpu@ +host_os = @host_os@ +host_vendor = @host_vendor@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +runstatedir = @runstatedir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +subdirs = @subdirs@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +top_build_prefix = @top_build_prefix@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +EXTRA_DIST = Doxyfile.in main.dox pix +all: all-am + +.SUFFIXES: +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \ + && { if test -f $@; then exit 0; else break; fi; }; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/doc/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/doc/Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(am__aclocal_m4_deps): +Doxyfile: $(top_builddir)/config.status $(srcdir)/Doxyfile.in + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ + +mostlyclean-libtool: + -rm -f *.lo + +clean-libtool: + -rm -rf .libs _libs +tags TAGS: + +ctags CTAGS: + +cscope cscopelist: + + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d "$(distdir)/$$file"; then \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \ + else \ + test -f "$(distdir)/$$file" \ + || cp -p $$d/$$file "$(distdir)/$$file" \ + || exit 1; \ + fi; \ + done +check-am: all-am +check: check-am +all-am: Makefile +installdirs: +install: install-am +install-exec: install-exec-am +install-data: install-data-am +uninstall: uninstall-am + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-am +install-strip: + if test -z '$(STRIP)'; then \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + install; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \ + fi +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +@HAVE_DOXYGEN_FALSE@html-local: +@HAVE_DOXYGEN_FALSE@mostlyclean-local: +clean: clean-am + +clean-am: clean-generic clean-libtool mostlyclean-am + +distclean: distclean-am + -rm -f Makefile +distclean-am: clean-am distclean-generic + +dvi: dvi-am + +dvi-am: + +html: html-am + +html-am: html-local + +info: info-am + +info-am: + +install-data-am: + +install-dvi: install-dvi-am + +install-dvi-am: + +install-exec-am: + +install-html: install-html-am + +install-html-am: + +install-info: install-info-am + +install-info-am: + +install-man: + +install-pdf: install-pdf-am + +install-pdf-am: + +install-ps: install-ps-am + +install-ps-am: + +installcheck-am: + +maintainer-clean: maintainer-clean-am + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-am + +mostlyclean-am: mostlyclean-generic mostlyclean-libtool \ + mostlyclean-local + +pdf: pdf-am + +pdf-am: + +ps: ps-am + +ps-am: + +uninstall-am: + +.MAKE: install-am install-strip + +.PHONY: all all-am check check-am clean clean-generic clean-libtool \ + cscopelist-am ctags-am distclean distclean-generic \ + distclean-libtool distdir dvi dvi-am html html-am html-local \ + info info-am install install-am install-data install-data-am \ + install-dvi install-dvi-am install-exec install-exec-am \ + install-html install-html-am install-info install-info-am \ + install-man install-pdf install-pdf-am install-ps \ + install-ps-am install-strip installcheck installcheck-am \ + installdirs maintainer-clean maintainer-clean-generic \ + mostlyclean mostlyclean-generic mostlyclean-libtool \ + mostlyclean-local pdf pdf-am ps ps-am tags-am uninstall \ + uninstall-am + +.PRECIOUS: Makefile + + +@HAVE_DOXYGEN_TRUE@mostlyclean-local: +@HAVE_DOXYGEN_TRUE@ rm -rf html + +@HAVE_DOXYGEN_TRUE@html-local: Doxyfile +@HAVE_DOXYGEN_TRUE@ $(DOXYGEN) Doxyfile + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/libs/ode-0.16.1/ode/doc/main.dox b/libs/ode-0.16.1/ode/doc/main.dox new file mode 100644 index 0000000..5136c33 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/main.dox @@ -0,0 +1,22 @@ +/**
+@mainpage Open Dynamics Engine API Reference
+
+<center><em>This document is © Russell Smith and the ODE Project</em></center>
+
+The Open Dynamics Engine (ODE) is a free, industrial quality library for
+simulating articulated rigid body dynamics. ODE is being developed by
+<a href="http://www.q12.org/">Russell Smith</a> with help from several
+<a href="http://ode.org/community.html">contributors</a>.
+
+This document describes the library API. For a more general introduction
+to ODE, please see the <a href="http://opende.sourceforge.net/wiki/index.php/Manual">
+Online Handbook</a>.
+
+<h2>Important: this document is not yet complete!</h2>
+
+We are still working on getting the full API documentated. In the meantime,
+please refer to the <a href="http://opende.sourceforge.net/wiki/index.php/Manual">
+Online Handbook</a>
+
+*/
+
diff --git a/libs/ode-0.16.1/ode/doc/pix/amotor.jpg b/libs/ode-0.16.1/ode/doc/pix/amotor.jpg Binary files differnew file mode 100644 index 0000000..bb80810 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/amotor.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/ball-and-socket-bad.jpg b/libs/ode-0.16.1/ode/doc/pix/ball-and-socket-bad.jpg Binary files differnew file mode 100644 index 0000000..90abaf3 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/ball-and-socket-bad.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/ball-and-socket.jpg b/libs/ode-0.16.1/ode/doc/pix/ball-and-socket.jpg Binary files differnew file mode 100644 index 0000000..050e136 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/ball-and-socket.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/body.jpg b/libs/ode-0.16.1/ode/doc/pix/body.jpg Binary files differnew file mode 100644 index 0000000..e026b93 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/body.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/contact.jpg b/libs/ode-0.16.1/ode/doc/pix/contact.jpg Binary files differnew file mode 100644 index 0000000..5d96809 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/contact.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/hinge.jpg b/libs/ode-0.16.1/ode/doc/pix/hinge.jpg Binary files differnew file mode 100644 index 0000000..ffd634e --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/hinge.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/hinge2.jpg b/libs/ode-0.16.1/ode/doc/pix/hinge2.jpg Binary files differnew file mode 100644 index 0000000..fda5e94 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/hinge2.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/joints.jpg b/libs/ode-0.16.1/ode/doc/pix/joints.jpg Binary files differnew file mode 100644 index 0000000..188ed01 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/joints.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/rollingcontact.jpg b/libs/ode-0.16.1/ode/doc/pix/rollingcontact.jpg Binary files differnew file mode 100644 index 0000000..b20bd1b --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/rollingcontact.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/sf-graph1.jpg b/libs/ode-0.16.1/ode/doc/pix/sf-graph1.jpg Binary files differnew file mode 100644 index 0000000..aee4ca7 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/sf-graph1.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/sf-graph2.jpg b/libs/ode-0.16.1/ode/doc/pix/sf-graph2.jpg Binary files differnew file mode 100644 index 0000000..67d08bc --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/sf-graph2.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/slider.jpg b/libs/ode-0.16.1/ode/doc/pix/slider.jpg Binary files differnew file mode 100644 index 0000000..040f76f --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/slider.jpg diff --git a/libs/ode-0.16.1/ode/doc/pix/universal.jpg b/libs/ode-0.16.1/ode/doc/pix/universal.jpg Binary files differnew file mode 100644 index 0000000..bc63523 --- /dev/null +++ b/libs/ode-0.16.1/ode/doc/pix/universal.jpg diff --git a/libs/ode-0.16.1/ode/src/Makefile.am b/libs/ode-0.16.1/ode/src/Makefile.am new file mode 100644 index 0000000..609044b --- /dev/null +++ b/libs/ode-0.16.1/ode/src/Makefile.am @@ -0,0 +1,201 @@ +SUBDIRS = joints + +AM_CPPFLAGS = -I$(top_srcdir)/include \ + -I$(top_builddir)/include \ + -D__ODE__ + + + +lib_LTLIBRARIES = libode.la + +libode_la_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ @ODE_VERSION_INFO@ +libode_la_LIBADD = joints/libjoints.la + + +# please, let's keep the filenames sorted +libode_la_SOURCES = nextafterf.c \ + array.cpp array.h \ + box.cpp \ + capsule.cpp \ + collision_cylinder_box.cpp \ + collision_cylinder_plane.cpp \ + collision_cylinder_sphere.cpp \ + collision_kernel.cpp collision_kernel.h \ + collision_quadtreespace.cpp \ + collision_sapspace.cpp \ + collision_space.cpp \ + collision_space_internal.h \ + collision_std.h \ + collision_transform.cpp collision_transform.h \ + collision_trimesh_colliders.h \ + collision_trimesh_disabled.cpp \ + collision_trimesh_internal.h \ + collision_trimesh_opcode.h \ + collision_trimesh_gimpact.h \ + collision_util.cpp collision_util.h \ + common.h \ + convex.cpp \ + coop_matrix_types.h \ + cylinder.cpp \ + default_threading.cpp default_threading.h \ + error.cpp error.h \ + export-dif.cpp \ + fastdot.cpp fastdot_impl.h \ + fastldltfactor.cpp fastldltfactor_impl.h \ + fastldltsolve.cpp fastldltsolve_impl.h \ + fastlsolve.cpp fastlsolve_impl.h \ + fastltsolve.cpp fastltsolve_impl.h \ + fastvecscale.cpp fastvecscale_impl.h \ + heightfield.cpp heightfield.h \ + lcp.cpp lcp.h \ + mass.cpp \ + mat.cpp mat.h \ + matrix.cpp matrix.h \ + memory.cpp \ + misc.cpp \ + objects.cpp objects.h \ + obstack.cpp obstack.h \ + ode.cpp \ + odeinit.cpp \ + odemath.cpp odemath.h \ + odeou.h \ + odetls.h \ + plane.cpp \ + quickstep.cpp quickstep.h \ + ray.cpp \ + resource_control.cpp resource_control.h \ + rotation.cpp \ + simple_cooperative.cpp simple_cooperative.h \ + sphere.cpp \ + step.cpp step.h \ + timer.cpp \ + threaded_solver_ldlt.h \ + threading_atomics_provs.h \ + threading_base.cpp threading_base.h \ + threading_fake_sync.h \ + threading_impl.cpp threading_impl.h \ + threading_impl_posix.h \ + threading_impl_templates.h \ + threading_impl_win.h \ + threading_pool_posix.cpp \ + threading_pool_win.cpp \ + threadingutils.h \ + typedefs.h \ + util.cpp util.h + + +################################### +# O U S T U F F +################################### + + +if ENABLE_OU + +AM_CPPFLAGS += -I$(top_srcdir)/ou/include +libode_la_LIBADD += $(top_builddir)/ou/src/ou/libou.la +libode_la_SOURCES += odetls.cpp odetls.h \ + odeou.cpp odeou.h + +endif + + +################################### +# G I M P A C T S T U F F +################################### + + +if GIMPACT +AM_CPPFLAGS += -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT -I$(top_srcdir)/GIMPACT/include + +libode_la_LIBADD += $(top_builddir)/GIMPACT/src/libGIMPACT.la +libode_la_SOURCES += collision_trimesh_gimpact.cpp \ + collision_trimesh_internal.cpp collision_trimesh_internal_impl.h \ + gimpact_contact_export_helper.cpp gimpact_contact_export_helper.h \ + gimpact_gim_contact_accessor.h \ + gimpact_plane_contact_accessor.h \ + collision_trimesh_trimesh.cpp \ + collision_trimesh_sphere.cpp \ + collision_trimesh_ray.cpp \ + collision_trimesh_box.cpp \ + collision_trimesh_ccylinder.cpp \ + collision_trimesh_internal.h \ + collision_cylinder_trimesh.cpp \ + collision_trimesh_plane.cpp \ + collision_convex_trimesh.cpp +endif + + + +################################# +# O P C O D E S T U F F +################################# + + +if OPCODE +AM_CPPFLAGS += -I$(top_srcdir)/OPCODE -I$(top_srcdir)/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE +libode_la_LIBADD += $(top_builddir)/OPCODE/libOPCODE.la \ + $(top_builddir)/OPCODE/Ice/libIce.la + +libode_la_SOURCES+= collision_trimesh_opcode.cpp \ + collision_trimesh_internal.cpp collision_trimesh_internal_impl.h \ + collision_trimesh_trimesh.cpp \ + collision_trimesh_trimesh_old.cpp \ + collision_trimesh_sphere.cpp \ + collision_trimesh_ray.cpp \ + collision_trimesh_box.cpp \ + collision_trimesh_ccylinder.cpp \ + collision_trimesh_internal.h \ + collision_cylinder_trimesh.cpp \ + collision_trimesh_plane.cpp \ + collision_convex_trimesh.cpp +endif + + +if LIBCCD + +AM_CPPFLAGS += -DdLIBCCD_ENABLED +AM_CPPFLAGS += -I$(top_srcdir)/libccd/src/custom + +if LIBCCD_INTERNAL +AM_CPPFLAGS += -I$(top_srcdir)/libccd/src -I$(top_builddir)/libccd/src +libode_la_LIBADD += $(top_builddir)/libccd/src/libccd.la +AM_CPPFLAGS += -DdLIBCCD_INTERNAL +else +AM_CPPFLAGS += $(CCD_CFLAGS) +libode_la_LIBADD += $(CCD_LIBS) +AM_CPPFLAGS += -DdLIBCCD_SYSTEM +endif + + +libode_la_SOURCES += collision_libccd.cpp collision_libccd.h + +if LIBCCD_BOX_CYL +AM_CPPFLAGS += -DdLIBCCD_BOX_CYL +endif + +if LIBCCD_CYL_CYL +AM_CPPFLAGS += -DdLIBCCD_CYL_CYL +endif + +if LIBCCD_CAP_CYL +AM_CPPFLAGS += -DdLIBCCD_CAP_CYL +endif + +if LIBCCD_CONVEX_BOX +AM_CPPFLAGS += -DdLIBCCD_CONVEX_BOX +endif +if LIBCCD_CONVEX_CAP +AM_CPPFLAGS += -DdLIBCCD_CONVEX_CAP +endif +if LIBCCD_CONVEX_CYL +AM_CPPFLAGS += -DdLIBCCD_CONVEX_CYL +endif +if LIBCCD_CONVEX_SPHERE +AM_CPPFLAGS += -DdLIBCCD_CONVEX_SPHERE +endif +if LIBCCD_CONVEX_CONVEX +AM_CPPFLAGS += -DdLIBCCD_CONVEX_CONVEX +endif + + +endif diff --git a/libs/ode-0.16.1/ode/src/Makefile.in b/libs/ode-0.16.1/ode/src/Makefile.in new file mode 100644 index 0000000..330d599 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/Makefile.in @@ -0,0 +1,1100 @@ +# Makefile.in generated by automake 1.15 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994-2014 Free Software Foundation, Inc. + +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ + +VPATH = @srcdir@ +am__is_gnu_make = { \ + if test -z '$(MAKELEVEL)'; then \ + false; \ + elif test -n '$(MAKE_HOST)'; then \ + true; \ + elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \ + true; \ + else \ + false; \ + fi; \ +} +am__make_running_with_option = \ + case $${target_option-} in \ + ?) ;; \ + *) echo "am__make_running_with_option: internal error: invalid" \ + "target option '$${target_option-}' specified" >&2; \ + exit 1;; \ + esac; \ + has_opt=no; \ + sane_makeflags=$$MAKEFLAGS; \ + if $(am__is_gnu_make); then \ + sane_makeflags=$$MFLAGS; \ + else \ + case $$MAKEFLAGS in \ + *\\[\ \ ]*) \ + bs=\\; \ + sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \ + | sed "s/$$bs$$bs[$$bs $$bs ]*//g"`;; \ + esac; \ + fi; \ + skip_next=no; \ + strip_trailopt () \ + { \ + flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \ + }; \ + for flg in $$sane_makeflags; do \ + test $$skip_next = yes && { skip_next=no; continue; }; \ + case $$flg in \ + *=*|--*) continue;; \ + -*I) strip_trailopt 'I'; skip_next=yes;; \ + -*I?*) strip_trailopt 'I';; \ + -*O) strip_trailopt 'O'; skip_next=yes;; \ + -*O?*) strip_trailopt 'O';; \ + -*l) strip_trailopt 'l'; skip_next=yes;; \ + -*l?*) strip_trailopt 'l';; \ + -[dEDm]) skip_next=yes;; \ + -[JT]) skip_next=yes;; \ + esac; \ + case $$flg in \ + *$$target_option*) has_opt=yes; break;; \ + esac; \ + done; \ + test $$has_opt = yes +am__make_dryrun = (target_option=n; $(am__make_running_with_option)) +am__make_keepgoing = (target_option=k; $(am__make_running_with_option)) +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ + +################################### +# O U S T U F F +################################### +@ENABLE_OU_TRUE@am__append_1 = -I$(top_srcdir)/ou/include +@ENABLE_OU_TRUE@am__append_2 = $(top_builddir)/ou/src/ou/libou.la +@ENABLE_OU_TRUE@am__append_3 = odetls.cpp odetls.h \ +@ENABLE_OU_TRUE@ odeou.cpp odeou.h + + +################################### +# G I M P A C T S T U F F +################################### +@GIMPACT_TRUE@am__append_4 = -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT -I$(top_srcdir)/GIMPACT/include +@GIMPACT_TRUE@am__append_5 = $(top_builddir)/GIMPACT/src/libGIMPACT.la +@GIMPACT_TRUE@am__append_6 = collision_trimesh_gimpact.cpp \ +@GIMPACT_TRUE@ collision_trimesh_internal.cpp collision_trimesh_internal_impl.h \ +@GIMPACT_TRUE@ gimpact_contact_export_helper.cpp gimpact_contact_export_helper.h \ +@GIMPACT_TRUE@ gimpact_gim_contact_accessor.h \ +@GIMPACT_TRUE@ gimpact_plane_contact_accessor.h \ +@GIMPACT_TRUE@ collision_trimesh_trimesh.cpp \ +@GIMPACT_TRUE@ collision_trimesh_sphere.cpp \ +@GIMPACT_TRUE@ collision_trimesh_ray.cpp \ +@GIMPACT_TRUE@ collision_trimesh_box.cpp \ +@GIMPACT_TRUE@ collision_trimesh_ccylinder.cpp \ +@GIMPACT_TRUE@ collision_trimesh_internal.h \ +@GIMPACT_TRUE@ collision_cylinder_trimesh.cpp \ +@GIMPACT_TRUE@ collision_trimesh_plane.cpp \ +@GIMPACT_TRUE@ collision_convex_trimesh.cpp + + +################################# +# O P C O D E S T U F F +################################# +@OPCODE_TRUE@am__append_7 = -I$(top_srcdir)/OPCODE -I$(top_srcdir)/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE +@OPCODE_TRUE@am__append_8 = $(top_builddir)/OPCODE/libOPCODE.la \ +@OPCODE_TRUE@ $(top_builddir)/OPCODE/Ice/libIce.la + +@OPCODE_TRUE@am__append_9 = collision_trimesh_opcode.cpp \ +@OPCODE_TRUE@ collision_trimesh_internal.cpp collision_trimesh_internal_impl.h \ +@OPCODE_TRUE@ collision_trimesh_trimesh.cpp \ +@OPCODE_TRUE@ collision_trimesh_trimesh_old.cpp \ +@OPCODE_TRUE@ collision_trimesh_sphere.cpp \ +@OPCODE_TRUE@ collision_trimesh_ray.cpp \ +@OPCODE_TRUE@ collision_trimesh_box.cpp \ +@OPCODE_TRUE@ collision_trimesh_ccylinder.cpp \ +@OPCODE_TRUE@ collision_trimesh_internal.h \ +@OPCODE_TRUE@ collision_cylinder_trimesh.cpp \ +@OPCODE_TRUE@ collision_trimesh_plane.cpp \ +@OPCODE_TRUE@ collision_convex_trimesh.cpp + +@LIBCCD_TRUE@am__append_10 = -DdLIBCCD_ENABLED \ +@LIBCCD_TRUE@ -I$(top_srcdir)/libccd/src/custom +@LIBCCD_INTERNAL_TRUE@@LIBCCD_TRUE@am__append_11 = \ +@LIBCCD_INTERNAL_TRUE@@LIBCCD_TRUE@ -I$(top_srcdir)/libccd/src \ +@LIBCCD_INTERNAL_TRUE@@LIBCCD_TRUE@ -I$(top_builddir)/libccd/src \ +@LIBCCD_INTERNAL_TRUE@@LIBCCD_TRUE@ -DdLIBCCD_INTERNAL +@LIBCCD_INTERNAL_TRUE@@LIBCCD_TRUE@am__append_12 = $(top_builddir)/libccd/src/libccd.la +@LIBCCD_INTERNAL_FALSE@@LIBCCD_TRUE@am__append_13 = $(CCD_CFLAGS) \ +@LIBCCD_INTERNAL_FALSE@@LIBCCD_TRUE@ -DdLIBCCD_SYSTEM +@LIBCCD_INTERNAL_FALSE@@LIBCCD_TRUE@am__append_14 = $(CCD_LIBS) +@LIBCCD_TRUE@am__append_15 = collision_libccd.cpp collision_libccd.h +@LIBCCD_BOX_CYL_TRUE@@LIBCCD_TRUE@am__append_16 = -DdLIBCCD_BOX_CYL +@LIBCCD_CYL_CYL_TRUE@@LIBCCD_TRUE@am__append_17 = -DdLIBCCD_CYL_CYL +@LIBCCD_CAP_CYL_TRUE@@LIBCCD_TRUE@am__append_18 = -DdLIBCCD_CAP_CYL +@LIBCCD_CONVEX_BOX_TRUE@@LIBCCD_TRUE@am__append_19 = -DdLIBCCD_CONVEX_BOX +@LIBCCD_CONVEX_CAP_TRUE@@LIBCCD_TRUE@am__append_20 = -DdLIBCCD_CONVEX_CAP +@LIBCCD_CONVEX_CYL_TRUE@@LIBCCD_TRUE@am__append_21 = -DdLIBCCD_CONVEX_CYL +@LIBCCD_CONVEX_SPHERE_TRUE@@LIBCCD_TRUE@am__append_22 = -DdLIBCCD_CONVEX_SPHERE +@LIBCCD_CONVEX_CONVEX_TRUE@@LIBCCD_TRUE@am__append_23 = -DdLIBCCD_CONVEX_CONVEX +subdir = ode/src +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \ + $(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \ + $(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \ + $(top_srcdir)/m4/pkg.m4 $(top_srcdir)/configure.ac +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +DIST_COMMON = $(srcdir)/Makefile.am $(am__DIST_COMMON) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = config.h +CONFIG_CLEAN_FILES = +CONFIG_CLEAN_VPATH_FILES = +am__vpath_adj_setup = srcdirstrip=`echo "$(srcdir)" | sed 's|.|.|g'`; +am__vpath_adj = case $$p in \ + $(srcdir)/*) f=`echo "$$p" | sed "s|^$$srcdirstrip/||"`;; \ + *) f=$$p;; \ + esac; +am__strip_dir = f=`echo $$p | sed -e 's|^.*/||'`; +am__install_max = 40 +am__nobase_strip_setup = \ + srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*|]/\\\\&/g'` +am__nobase_strip = \ + for p in $$list; do echo "$$p"; done | sed -e "s|$$srcdirstrip/||" +am__nobase_list = $(am__nobase_strip_setup); \ + for p in $$list; do echo "$$p $$p"; done | \ + sed "s| $$srcdirstrip/| |;"' / .*\//!s/ .*/ ./; s,\( .*\)/[^/]*$$,\1,' | \ + $(AWK) 'BEGIN { files["."] = "" } { files[$$2] = files[$$2] " " $$1; \ + if (++n[$$2] == $(am__install_max)) \ + { print $$2, files[$$2]; n[$$2] = 0; files[$$2] = "" } } \ + END { for (dir in files) print dir, files[dir] }' +am__base_list = \ + sed '$$!N;$$!N;$$!N;$$!N;$$!N;$$!N;$$!N;s/\n/ /g' | \ + sed '$$!N;$$!N;$$!N;$$!N;s/\n/ /g' +am__uninstall_files_from_dir = { \ + test -z "$$files" \ + || { test ! -d "$$dir" && test ! -f "$$dir" && test ! -r "$$dir"; } \ + || { echo " ( cd '$$dir' && rm -f" $$files ")"; \ + $(am__cd) "$$dir" && rm -f $$files; }; \ + } +am__installdirs = "$(DESTDIR)$(libdir)" +LTLIBRARIES = $(lib_LTLIBRARIES) +am__DEPENDENCIES_1 = +@LIBCCD_INTERNAL_FALSE@@LIBCCD_TRUE@am__DEPENDENCIES_2 = \ +@LIBCCD_INTERNAL_FALSE@@LIBCCD_TRUE@ $(am__DEPENDENCIES_1) +libode_la_DEPENDENCIES = joints/libjoints.la $(am__append_2) \ + $(am__append_5) $(am__append_8) $(am__append_12) \ + $(am__DEPENDENCIES_2) +am__libode_la_SOURCES_DIST = nextafterf.c array.cpp array.h box.cpp \ + capsule.cpp collision_cylinder_box.cpp \ + collision_cylinder_plane.cpp collision_cylinder_sphere.cpp \ + collision_kernel.cpp collision_kernel.h \ + collision_quadtreespace.cpp collision_sapspace.cpp \ + collision_space.cpp collision_space_internal.h collision_std.h \ + collision_transform.cpp collision_transform.h \ + collision_trimesh_colliders.h collision_trimesh_disabled.cpp \ + collision_trimesh_internal.h collision_trimesh_opcode.h \ + collision_trimesh_gimpact.h collision_util.cpp \ + collision_util.h common.h convex.cpp coop_matrix_types.h \ + cylinder.cpp default_threading.cpp default_threading.h \ + error.cpp error.h export-dif.cpp fastdot.cpp fastdot_impl.h \ + fastldltfactor.cpp fastldltfactor_impl.h fastldltsolve.cpp \ + fastldltsolve_impl.h fastlsolve.cpp fastlsolve_impl.h \ + fastltsolve.cpp fastltsolve_impl.h fastvecscale.cpp \ + fastvecscale_impl.h heightfield.cpp heightfield.h lcp.cpp \ + lcp.h mass.cpp mat.cpp mat.h matrix.cpp matrix.h memory.cpp \ + misc.cpp objects.cpp objects.h obstack.cpp obstack.h ode.cpp \ + odeinit.cpp odemath.cpp odemath.h odeou.h odetls.h plane.cpp \ + quickstep.cpp quickstep.h ray.cpp resource_control.cpp \ + resource_control.h rotation.cpp simple_cooperative.cpp \ + simple_cooperative.h sphere.cpp step.cpp step.h timer.cpp \ + threaded_solver_ldlt.h threading_atomics_provs.h \ + threading_base.cpp threading_base.h threading_fake_sync.h \ + threading_impl.cpp threading_impl.h threading_impl_posix.h \ + threading_impl_templates.h threading_impl_win.h \ + threading_pool_posix.cpp threading_pool_win.cpp \ + threadingutils.h typedefs.h util.cpp util.h odetls.cpp \ + odeou.cpp collision_trimesh_gimpact.cpp \ + collision_trimesh_internal.cpp \ + collision_trimesh_internal_impl.h \ + gimpact_contact_export_helper.cpp \ + gimpact_contact_export_helper.h gimpact_gim_contact_accessor.h \ + gimpact_plane_contact_accessor.h collision_trimesh_trimesh.cpp \ + collision_trimesh_sphere.cpp collision_trimesh_ray.cpp \ + collision_trimesh_box.cpp collision_trimesh_ccylinder.cpp \ + collision_cylinder_trimesh.cpp collision_trimesh_plane.cpp \ + collision_convex_trimesh.cpp collision_trimesh_opcode.cpp \ + collision_trimesh_trimesh_old.cpp collision_libccd.cpp \ + collision_libccd.h +@ENABLE_OU_TRUE@am__objects_1 = odetls.lo odeou.lo +@GIMPACT_TRUE@am__objects_2 = collision_trimesh_gimpact.lo \ +@GIMPACT_TRUE@ collision_trimesh_internal.lo \ +@GIMPACT_TRUE@ gimpact_contact_export_helper.lo \ +@GIMPACT_TRUE@ collision_trimesh_trimesh.lo \ +@GIMPACT_TRUE@ collision_trimesh_sphere.lo \ +@GIMPACT_TRUE@ collision_trimesh_ray.lo \ +@GIMPACT_TRUE@ collision_trimesh_box.lo \ +@GIMPACT_TRUE@ collision_trimesh_ccylinder.lo \ +@GIMPACT_TRUE@ collision_cylinder_trimesh.lo \ +@GIMPACT_TRUE@ collision_trimesh_plane.lo \ +@GIMPACT_TRUE@ collision_convex_trimesh.lo +@OPCODE_TRUE@am__objects_3 = collision_trimesh_opcode.lo \ +@OPCODE_TRUE@ collision_trimesh_internal.lo \ +@OPCODE_TRUE@ collision_trimesh_trimesh.lo \ +@OPCODE_TRUE@ collision_trimesh_trimesh_old.lo \ +@OPCODE_TRUE@ collision_trimesh_sphere.lo \ +@OPCODE_TRUE@ collision_trimesh_ray.lo collision_trimesh_box.lo \ +@OPCODE_TRUE@ collision_trimesh_ccylinder.lo \ +@OPCODE_TRUE@ collision_cylinder_trimesh.lo \ +@OPCODE_TRUE@ collision_trimesh_plane.lo \ +@OPCODE_TRUE@ collision_convex_trimesh.lo +@LIBCCD_TRUE@am__objects_4 = collision_libccd.lo +am_libode_la_OBJECTS = nextafterf.lo array.lo box.lo capsule.lo \ + collision_cylinder_box.lo collision_cylinder_plane.lo \ + collision_cylinder_sphere.lo collision_kernel.lo \ + collision_quadtreespace.lo collision_sapspace.lo \ + collision_space.lo collision_transform.lo \ + collision_trimesh_disabled.lo collision_util.lo convex.lo \ + cylinder.lo default_threading.lo error.lo export-dif.lo \ + fastdot.lo fastldltfactor.lo fastldltsolve.lo fastlsolve.lo \ + fastltsolve.lo fastvecscale.lo heightfield.lo lcp.lo mass.lo \ + mat.lo matrix.lo memory.lo misc.lo objects.lo obstack.lo \ + ode.lo odeinit.lo odemath.lo plane.lo quickstep.lo ray.lo \ + resource_control.lo rotation.lo simple_cooperative.lo \ + sphere.lo step.lo timer.lo threading_base.lo threading_impl.lo \ + threading_pool_posix.lo threading_pool_win.lo util.lo \ + $(am__objects_1) $(am__objects_2) $(am__objects_3) \ + $(am__objects_4) +libode_la_OBJECTS = $(am_libode_la_OBJECTS) +AM_V_lt = $(am__v_lt_@AM_V@) +am__v_lt_ = $(am__v_lt_@AM_DEFAULT_V@) +am__v_lt_0 = --silent +am__v_lt_1 = +libode_la_LINK = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CXXLD) $(AM_CXXFLAGS) \ + $(CXXFLAGS) $(libode_la_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_P = $(am__v_P_@AM_V@) +am__v_P_ = $(am__v_P_@AM_DEFAULT_V@) +am__v_P_0 = false +am__v_P_1 = : +AM_V_GEN = $(am__v_GEN_@AM_V@) +am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@) +am__v_GEN_0 = @echo " GEN " $@; +am__v_GEN_1 = +AM_V_at = $(am__v_at_@AM_V@) +am__v_at_ = $(am__v_at_@AM_DEFAULT_V@) +am__v_at_0 = @ +am__v_at_1 = +DEFAULT_INCLUDES = -I.@am__isrc@ +depcomp = $(SHELL) $(top_srcdir)/depcomp +am__depfiles_maybe = depfiles +am__mv = mv -f +COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \ + $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) +LTCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CC) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CFLAGS) $(CFLAGS) +AM_V_CC = $(am__v_CC_@AM_V@) +am__v_CC_ = $(am__v_CC_@AM_DEFAULT_V@) +am__v_CC_0 = @echo " CC " $@; +am__v_CC_1 = +CCLD = $(CC) +LINK = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CCLD) $(AM_CFLAGS) $(CFLAGS) \ + $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CCLD = $(am__v_CCLD_@AM_V@) +am__v_CCLD_ = $(am__v_CCLD_@AM_DEFAULT_V@) +am__v_CCLD_0 = @echo " CCLD " $@; +am__v_CCLD_1 = +CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \ + $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) +LTCXXCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CXX) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CXXFLAGS) $(CXXFLAGS) +AM_V_CXX = $(am__v_CXX_@AM_V@) +am__v_CXX_ = $(am__v_CXX_@AM_DEFAULT_V@) +am__v_CXX_0 = @echo " CXX " $@; +am__v_CXX_1 = +CXXLD = $(CXX) +CXXLINK = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CXXLD) $(AM_CXXFLAGS) \ + $(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CXXLD = $(am__v_CXXLD_@AM_V@) +am__v_CXXLD_ = $(am__v_CXXLD_@AM_DEFAULT_V@) +am__v_CXXLD_0 = @echo " CXXLD " $@; +am__v_CXXLD_1 = +SOURCES = $(libode_la_SOURCES) +DIST_SOURCES = $(am__libode_la_SOURCES_DIST) +RECURSIVE_TARGETS = all-recursive check-recursive cscopelist-recursive \ + ctags-recursive dvi-recursive html-recursive info-recursive \ + install-data-recursive install-dvi-recursive \ + install-exec-recursive install-html-recursive \ + install-info-recursive install-pdf-recursive \ + install-ps-recursive install-recursive installcheck-recursive \ + installdirs-recursive pdf-recursive ps-recursive \ + tags-recursive uninstall-recursive +am__can_run_installinfo = \ + case $$AM_UPDATE_INFO_DIR in \ + n|no|NO) false;; \ + *) (install-info --version) >/dev/null 2>&1;; \ + esac +RECURSIVE_CLEAN_TARGETS = mostlyclean-recursive clean-recursive \ + distclean-recursive maintainer-clean-recursive +am__recursive_targets = \ + $(RECURSIVE_TARGETS) \ + $(RECURSIVE_CLEAN_TARGETS) \ + $(am__extra_recursive_targets) +AM_RECURSIVE_TARGETS = $(am__recursive_targets:-recursive=) TAGS CTAGS \ + distdir +am__tagged_files = $(HEADERS) $(SOURCES) $(TAGS_FILES) \ + $(LISP)config.h.in +# Read a list of newline-separated strings from the standard input, +# and print each of them once, without duplicates. Input order is +# *not* preserved. +am__uniquify_input = $(AWK) '\ + BEGIN { nonempty = 0; } \ + { items[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in items) print i; }; } \ +' +# Make sure the list of sources is unique. This is necessary because, +# e.g., the same source file might be shared among _SOURCES variables +# for different programs/libraries. +am__define_uniq_tagged_files = \ + list='$(am__tagged_files)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | $(am__uniquify_input)` +ETAGS = etags +CTAGS = ctags +DIST_SUBDIRS = $(SUBDIRS) +am__DIST_COMMON = $(srcdir)/Makefile.in $(srcdir)/config.h.in \ + $(top_srcdir)/depcomp +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +am__relativize = \ + dir0=`pwd`; \ + sed_first='s,^\([^/]*\)/.*$$,\1,'; \ + sed_rest='s,^[^/]*/*,,'; \ + sed_last='s,^.*/\([^/]*\)$$,\1,'; \ + sed_butlast='s,/*[^/]*$$,,'; \ + while test -n "$$dir1"; do \ + first=`echo "$$dir1" | sed -e "$$sed_first"`; \ + if test "$$first" != "."; then \ + if test "$$first" = ".."; then \ + dir2=`echo "$$dir0" | sed -e "$$sed_last"`/"$$dir2"; \ + dir0=`echo "$$dir0" | sed -e "$$sed_butlast"`; \ + else \ + first2=`echo "$$dir2" | sed -e "$$sed_first"`; \ + if test "$$first2" = "$$first"; then \ + dir2=`echo "$$dir2" | sed -e "$$sed_rest"`; \ + else \ + dir2="../$$dir2"; \ + fi; \ + dir0="$$dir0"/"$$first"; \ + fi; \ + fi; \ + dir1=`echo "$$dir1" | sed -e "$$sed_rest"`; \ + done; \ + reldir="$$dir2" +ACLOCAL = @ACLOCAL@ +ALLOCA = @ALLOCA@ +AMTAR = @AMTAR@ +AM_DEFAULT_VERBOSITY = @AM_DEFAULT_VERBOSITY@ +AR = @AR@ +AS = @AS@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CCD_CFLAGS = @CCD_CFLAGS@ +CCD_LIBS = @CCD_LIBS@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXCPP = @CXXCPP@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DLLTOOL = @DLLTOOL@ +DOXYGEN = @DOXYGEN@ +DSYMUTIL = @DSYMUTIL@ +DUMPBIN = @DUMPBIN@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +EXTRA_LIBTOOL_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ +FGREP = @FGREP@ +GL_LIBS = @GL_LIBS@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LD = @LD@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LIBSTDCXX = @LIBSTDCXX@ +LIBTOOL = @LIBTOOL@ +LIPO = @LIPO@ +LN_S = @LN_S@ +LTLIBOBJS = @LTLIBOBJS@ +LT_SYS_LIBRARY_PATH = @LT_SYS_LIBRARY_PATH@ +MAKEINFO = @MAKEINFO@ +MANIFEST_TOOL = @MANIFEST_TOOL@ +MKDIR_P = @MKDIR_P@ +NM = @NM@ +NMEDIT = @NMEDIT@ +OBJDUMP = @OBJDUMP@ +OBJEXT = @OBJEXT@ +ODE_PRECISION = @ODE_PRECISION@ +ODE_VERSION = @ODE_VERSION@ +ODE_VERSION_INFO = @ODE_VERSION_INFO@ +OTOOL = @OTOOL@ +OTOOL64 = @OTOOL64@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_URL = @PACKAGE_URL@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +PKG_CONFIG = @PKG_CONFIG@ +PKG_CONFIG_LIBDIR = @PKG_CONFIG_LIBDIR@ +PKG_CONFIG_PATH = @PKG_CONFIG_PATH@ +RANLIB = @RANLIB@ +SED = @SED@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +VERSION = @VERSION@ +WINDRES = @WINDRES@ +X11_CFLAGS = @X11_CFLAGS@ +X11_LIBS = @X11_LIBS@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_AR = @ac_ct_AR@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +ac_ct_DUMPBIN = @ac_ct_DUMPBIN@ +ac_ct_WINDRES = @ac_ct_WINDRES@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +build = @build@ +build_alias = @build_alias@ +build_cpu = @build_cpu@ +build_os = @build_os@ +build_vendor = @build_vendor@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host = @host@ +host_alias = @host_alias@ +host_cpu = @host_cpu@ +host_os = @host_os@ +host_vendor = @host_vendor@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +runstatedir = @runstatedir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +subdirs = @subdirs@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +top_build_prefix = @top_build_prefix@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +SUBDIRS = joints +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include \ + -D__ODE__ $(am__append_1) $(am__append_4) $(am__append_7) \ + $(am__append_10) $(am__append_11) $(am__append_13) \ + $(am__append_16) $(am__append_17) $(am__append_18) \ + $(am__append_19) $(am__append_20) $(am__append_21) \ + $(am__append_22) $(am__append_23) +lib_LTLIBRARIES = libode.la +libode_la_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ @ODE_VERSION_INFO@ +libode_la_LIBADD = joints/libjoints.la $(am__append_2) $(am__append_5) \ + $(am__append_8) $(am__append_12) $(am__append_14) + +# please, let's keep the filenames sorted +libode_la_SOURCES = nextafterf.c array.cpp array.h box.cpp capsule.cpp \ + collision_cylinder_box.cpp collision_cylinder_plane.cpp \ + collision_cylinder_sphere.cpp collision_kernel.cpp \ + collision_kernel.h collision_quadtreespace.cpp \ + collision_sapspace.cpp collision_space.cpp \ + collision_space_internal.h collision_std.h \ + collision_transform.cpp collision_transform.h \ + collision_trimesh_colliders.h collision_trimesh_disabled.cpp \ + collision_trimesh_internal.h collision_trimesh_opcode.h \ + collision_trimesh_gimpact.h collision_util.cpp \ + collision_util.h common.h convex.cpp coop_matrix_types.h \ + cylinder.cpp default_threading.cpp default_threading.h \ + error.cpp error.h export-dif.cpp fastdot.cpp fastdot_impl.h \ + fastldltfactor.cpp fastldltfactor_impl.h fastldltsolve.cpp \ + fastldltsolve_impl.h fastlsolve.cpp fastlsolve_impl.h \ + fastltsolve.cpp fastltsolve_impl.h fastvecscale.cpp \ + fastvecscale_impl.h heightfield.cpp heightfield.h lcp.cpp \ + lcp.h mass.cpp mat.cpp mat.h matrix.cpp matrix.h memory.cpp \ + misc.cpp objects.cpp objects.h obstack.cpp obstack.h ode.cpp \ + odeinit.cpp odemath.cpp odemath.h odeou.h odetls.h plane.cpp \ + quickstep.cpp quickstep.h ray.cpp resource_control.cpp \ + resource_control.h rotation.cpp simple_cooperative.cpp \ + simple_cooperative.h sphere.cpp step.cpp step.h timer.cpp \ + threaded_solver_ldlt.h threading_atomics_provs.h \ + threading_base.cpp threading_base.h threading_fake_sync.h \ + threading_impl.cpp threading_impl.h threading_impl_posix.h \ + threading_impl_templates.h threading_impl_win.h \ + threading_pool_posix.cpp threading_pool_win.cpp \ + threadingutils.h typedefs.h util.cpp util.h $(am__append_3) \ + $(am__append_6) $(am__append_9) $(am__append_15) +all: config.h + $(MAKE) $(AM_MAKEFLAGS) all-recursive + +.SUFFIXES: +.SUFFIXES: .c .cpp .lo .o .obj +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \ + && { if test -f $@; then exit 0; else break; fi; }; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/src/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/src/Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(am__aclocal_m4_deps): + +config.h: stamp-h1 + @test -f $@ || rm -f stamp-h1 + @test -f $@ || $(MAKE) $(AM_MAKEFLAGS) stamp-h1 + +stamp-h1: $(srcdir)/config.h.in $(top_builddir)/config.status + @rm -f stamp-h1 + cd $(top_builddir) && $(SHELL) ./config.status ode/src/config.h +$(srcdir)/config.h.in: $(am__configure_deps) + ($(am__cd) $(top_srcdir) && $(AUTOHEADER)) + rm -f stamp-h1 + touch $@ + +distclean-hdr: + -rm -f config.h stamp-h1 + +install-libLTLIBRARIES: $(lib_LTLIBRARIES) + @$(NORMAL_INSTALL) + @list='$(lib_LTLIBRARIES)'; test -n "$(libdir)" || list=; \ + list2=; for p in $$list; do \ + if test -f $$p; then \ + list2="$$list2 $$p"; \ + else :; fi; \ + done; \ + test -z "$$list2" || { \ + echo " $(MKDIR_P) '$(DESTDIR)$(libdir)'"; \ + $(MKDIR_P) "$(DESTDIR)$(libdir)" || exit 1; \ + echo " $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=install $(INSTALL) $(INSTALL_STRIP_FLAG) $$list2 '$(DESTDIR)$(libdir)'"; \ + $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=install $(INSTALL) $(INSTALL_STRIP_FLAG) $$list2 "$(DESTDIR)$(libdir)"; \ + } + +uninstall-libLTLIBRARIES: + @$(NORMAL_UNINSTALL) + @list='$(lib_LTLIBRARIES)'; test -n "$(libdir)" || list=; \ + for p in $$list; do \ + $(am__strip_dir) \ + echo " $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=uninstall rm -f '$(DESTDIR)$(libdir)/$$f'"; \ + $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=uninstall rm -f "$(DESTDIR)$(libdir)/$$f"; \ + done + +clean-libLTLIBRARIES: + -test -z "$(lib_LTLIBRARIES)" || rm -f $(lib_LTLIBRARIES) + @list='$(lib_LTLIBRARIES)'; \ + locs=`for p in $$list; do echo $$p; done | \ + sed 's|^[^/]*$$|.|; s|/[^/]*$$||; s|$$|/so_locations|' | \ + sort -u`; \ + test -z "$$locs" || { \ + echo rm -f $${locs}; \ + rm -f $${locs}; \ + } + +libode.la: $(libode_la_OBJECTS) $(libode_la_DEPENDENCIES) $(EXTRA_libode_la_DEPENDENCIES) + $(AM_V_CXXLD)$(libode_la_LINK) -rpath $(libdir) $(libode_la_OBJECTS) $(libode_la_LIBADD) $(LIBS) + +mostlyclean-compile: + -rm -f *.$(OBJEXT) + +distclean-compile: + -rm -f *.tab.c + +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/array.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/box.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/capsule.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_convex_trimesh.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_cylinder_box.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_cylinder_plane.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_cylinder_sphere.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_cylinder_trimesh.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_kernel.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_libccd.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_quadtreespace.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_sapspace.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_space.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_transform.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_box.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_ccylinder.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_disabled.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_gimpact.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_internal.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_opcode.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_plane.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_ray.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_sphere.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_trimesh.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_trimesh_trimesh_old.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/collision_util.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/convex.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/cylinder.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/default_threading.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/error.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/export-dif.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fastdot.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fastldltfactor.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fastldltsolve.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fastlsolve.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fastltsolve.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fastvecscale.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/gimpact_contact_export_helper.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/heightfield.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/lcp.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/mass.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/mat.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/matrix.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/memory.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/misc.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/nextafterf.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/objects.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/obstack.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/ode.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/odeinit.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/odemath.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/odeou.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/odetls.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/plane.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/quickstep.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/ray.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/resource_control.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/rotation.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/simple_cooperative.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/sphere.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/step.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/threading_base.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/threading_impl.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/threading_pool_posix.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/threading_pool_win.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/timer.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/util.Plo@am__quote@ + +.c.o: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(COMPILE) -c -o $@ $< + +.c.obj: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(COMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.c.lo: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(LTCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(LTCOMPILE) -c -o $@ $< + +.cpp.o: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ $< + +.cpp.obj: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.cpp.lo: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(LTCXXCOMPILE) -c -o $@ $< + +mostlyclean-libtool: + -rm -f *.lo + +clean-libtool: + -rm -rf .libs _libs + +# This directory's subdirectories are mostly independent; you can cd +# into them and run 'make' without going through this Makefile. +# To change the values of 'make' variables: instead of editing Makefiles, +# (1) if the variable is set in 'config.status', edit 'config.status' +# (which will cause the Makefiles to be regenerated when you run 'make'); +# (2) otherwise, pass the desired values on the 'make' command line. +$(am__recursive_targets): + @fail=; \ + if $(am__make_keepgoing); then \ + failcom='fail=yes'; \ + else \ + failcom='exit 1'; \ + fi; \ + dot_seen=no; \ + target=`echo $@ | sed s/-recursive//`; \ + case "$@" in \ + distclean-* | maintainer-clean-*) list='$(DIST_SUBDIRS)' ;; \ + *) list='$(SUBDIRS)' ;; \ + esac; \ + for subdir in $$list; do \ + echo "Making $$target in $$subdir"; \ + if test "$$subdir" = "."; then \ + dot_seen=yes; \ + local_target="$$target-am"; \ + else \ + local_target="$$target"; \ + fi; \ + ($(am__cd) $$subdir && $(MAKE) $(AM_MAKEFLAGS) $$local_target) \ + || eval $$failcom; \ + done; \ + if test "$$dot_seen" = "no"; then \ + $(MAKE) $(AM_MAKEFLAGS) "$$target-am" || exit 1; \ + fi; test -z "$$fail" + +ID: $(am__tagged_files) + $(am__define_uniq_tagged_files); mkid -fID $$unique +tags: tags-recursive +TAGS: tags + +tags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + set x; \ + here=`pwd`; \ + if ($(ETAGS) --etags-include --version) >/dev/null 2>&1; then \ + include_option=--etags-include; \ + empty_fix=.; \ + else \ + include_option=--include; \ + empty_fix=; \ + fi; \ + list='$(SUBDIRS)'; for subdir in $$list; do \ + if test "$$subdir" = .; then :; else \ + test ! -f $$subdir/TAGS || \ + set "$$@" "$$include_option=$$here/$$subdir/TAGS"; \ + fi; \ + done; \ + $(am__define_uniq_tagged_files); \ + shift; \ + if test -z "$(ETAGS_ARGS)$$*$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + if test $$# -gt 0; then \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + "$$@" $$unique; \ + else \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$unique; \ + fi; \ + fi +ctags: ctags-recursive + +CTAGS: ctags +ctags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + $(am__define_uniq_tagged_files); \ + test -z "$(CTAGS_ARGS)$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && $(am__cd) $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) "$$here" +cscopelist: cscopelist-recursive + +cscopelist-am: $(am__tagged_files) + list='$(am__tagged_files)'; \ + case "$(srcdir)" in \ + [\\/]* | ?:[\\/]*) sdir="$(srcdir)" ;; \ + *) sdir=$(subdir)/$(srcdir) ;; \ + esac; \ + for i in $$list; do \ + if test -f "$$i"; then \ + echo "$(subdir)/$$i"; \ + else \ + echo "$$sdir/$$i"; \ + fi; \ + done >> $(top_builddir)/cscope.files + +distclean-tags: + -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d "$(distdir)/$$file"; then \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \ + else \ + test -f "$(distdir)/$$file" \ + || cp -p $$d/$$file "$(distdir)/$$file" \ + || exit 1; \ + fi; \ + done + @list='$(DIST_SUBDIRS)'; for subdir in $$list; do \ + if test "$$subdir" = .; then :; else \ + $(am__make_dryrun) \ + || test -d "$(distdir)/$$subdir" \ + || $(MKDIR_P) "$(distdir)/$$subdir" \ + || exit 1; \ + dir1=$$subdir; dir2="$(distdir)/$$subdir"; \ + $(am__relativize); \ + new_distdir=$$reldir; \ + dir1=$$subdir; dir2="$(top_distdir)"; \ + $(am__relativize); \ + new_top_distdir=$$reldir; \ + echo " (cd $$subdir && $(MAKE) $(AM_MAKEFLAGS) top_distdir="$$new_top_distdir" distdir="$$new_distdir" \\"; \ + echo " am__remove_distdir=: am__skip_length_check=: am__skip_mode_fix=: distdir)"; \ + ($(am__cd) $$subdir && \ + $(MAKE) $(AM_MAKEFLAGS) \ + top_distdir="$$new_top_distdir" \ + distdir="$$new_distdir" \ + am__remove_distdir=: \ + am__skip_length_check=: \ + am__skip_mode_fix=: \ + distdir) \ + || exit 1; \ + fi; \ + done +check-am: all-am +check: check-recursive +all-am: Makefile $(LTLIBRARIES) config.h +installdirs: installdirs-recursive +installdirs-am: + for dir in "$(DESTDIR)$(libdir)"; do \ + test -z "$$dir" || $(MKDIR_P) "$$dir"; \ + done +install: install-recursive +install-exec: install-exec-recursive +install-data: install-data-recursive +uninstall: uninstall-recursive + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-recursive +install-strip: + if test -z '$(STRIP)'; then \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + install; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \ + fi +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-recursive + +clean-am: clean-generic clean-libLTLIBRARIES clean-libtool \ + mostlyclean-am + +distclean: distclean-recursive + -rm -rf ./$(DEPDIR) + -rm -f Makefile +distclean-am: clean-am distclean-compile distclean-generic \ + distclean-hdr distclean-tags + +dvi: dvi-recursive + +dvi-am: + +html: html-recursive + +html-am: + +info: info-recursive + +info-am: + +install-data-am: + +install-dvi: install-dvi-recursive + +install-dvi-am: + +install-exec-am: install-libLTLIBRARIES + +install-html: install-html-recursive + +install-html-am: + +install-info: install-info-recursive + +install-info-am: + +install-man: + +install-pdf: install-pdf-recursive + +install-pdf-am: + +install-ps: install-ps-recursive + +install-ps-am: + +installcheck-am: + +maintainer-clean: maintainer-clean-recursive + -rm -rf ./$(DEPDIR) + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-recursive + +mostlyclean-am: mostlyclean-compile mostlyclean-generic \ + mostlyclean-libtool + +pdf: pdf-recursive + +pdf-am: + +ps: ps-recursive + +ps-am: + +uninstall-am: uninstall-libLTLIBRARIES + +.MAKE: $(am__recursive_targets) all install-am install-strip + +.PHONY: $(am__recursive_targets) CTAGS GTAGS TAGS all all-am check \ + check-am clean clean-generic clean-libLTLIBRARIES \ + clean-libtool cscopelist-am ctags ctags-am distclean \ + distclean-compile distclean-generic distclean-hdr \ + distclean-libtool distclean-tags distdir dvi dvi-am html \ + html-am info info-am install install-am install-data \ + install-data-am install-dvi install-dvi-am install-exec \ + install-exec-am install-html install-html-am install-info \ + install-info-am install-libLTLIBRARIES install-man install-pdf \ + install-pdf-am install-ps install-ps-am install-strip \ + installcheck installcheck-am installdirs installdirs-am \ + maintainer-clean maintainer-clean-generic mostlyclean \ + mostlyclean-compile mostlyclean-generic mostlyclean-libtool \ + pdf pdf-am ps ps-am tags tags-am uninstall uninstall-am \ + uninstall-libLTLIBRARIES + +.PRECIOUS: Makefile + + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/libs/ode-0.16.1/ode/src/array.cpp b/libs/ode-0.16.1/ode/src/array.cpp new file mode 100644 index 0000000..4d63925 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/array.cpp @@ -0,0 +1,81 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/memory.h> +#include <ode/error.h> +#include "config.h" +#include "array.h" + + +static inline int roundUpToPowerOfTwo (int x) +{ + int i = 1; + while (i < x) i <<= 1; + return i; +} + + +void dArrayBase::_freeAll (int sizeofT) +{ + if (_data) { + if (_data == this+1) return; // if constructLocalArray() was called + dFree (_data,_anum * sizeofT); + } +} + + +void dArrayBase::_setSize (int newsize, int sizeofT) +{ + if (newsize < 0) return; + if (newsize > _anum) { + if (_data == this+1) { + // this is a no-no, because constructLocalArray() was called + dDebug (0,"setSize() out of space in LOCAL array"); + } + int newanum = roundUpToPowerOfTwo (newsize); + if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT); + else _data = dAlloc (newanum*sizeofT); + _anum = newanum; + } + _size = newsize; +} + + +void * dArrayBase::operator new (size_t size) +{ + return dAlloc (size); +} + + +void dArrayBase::operator delete (void *ptr, size_t size) +{ + dFree (ptr,size); +} + + +void dArrayBase::constructLocalArray (int __anum) +{ + _size = 0; + _anum = __anum; + _data = this+1; +} diff --git a/libs/ode-0.16.1/ode/src/array.h b/libs/ode-0.16.1/ode/src/array.h new file mode 100644 index 0000000..7ce9e48 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/array.h @@ -0,0 +1,135 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* this comes from the `reuse' library. copy any changes back to the source. + * + * Variable sized array template. The array is always stored in a contiguous + * chunk. The array can be resized. A size increase will cause more memory + * to be allocated, and may result in relocation of the array memory. + * A size decrease has no effect on the memory allocation. + * + * Array elements with constructors or destructors are not supported! + * But if you must have such elements, here's what to know/do: + * - Bitwise copy is used when copying whole arrays. + * - When copying individual items (via push(), insert() etc) the `=' + * (equals) operator is used. Thus you should define this operator to do + * a bitwise copy. You should probably also define the copy constructor. + */ + + +#ifndef _ODE_ARRAY_H_ +#define _ODE_ARRAY_H_ + +#include <ode/odeconfig.h> + + +// this base class has no constructors or destructor, for your convenience. + +class dArrayBase { +protected: + int _size; // number of elements in `data' + int _anum; // allocated number of elements in `data' + void *_data; // array data + + void _freeAll (int sizeofT); + void _setSize (int newsize, int sizeofT); + // set the array size to `newsize', allocating more memory if necessary. + // if newsize>_anum and is a power of two then this is guaranteed to + // set _size and _anum to newsize. + +public: + // not: dArrayBase () { _size=0; _anum=0; _data=0; } + + int size() const { return _size; } + int allocatedSize() const { return _anum; } + void * operator new (size_t size); + void operator delete (void *ptr, size_t size); + + void constructor() { _size=0; _anum=0; _data=0; } + // if this structure is allocated with malloc() instead of new, you can + // call this to set it up. + + void constructLocalArray (int __anum); + // this helper function allows non-reallocating arrays to be constructed + // on the stack (or in the heap if necessary). this is something of a + // kludge and should be used with extreme care. this function acts like + // a constructor - it is called on uninitialized memory that will hold the + // Array structure and the data. __anum is the number of elements that + // are allocated. the memory MUST be allocated with size: + // sizeof(ArrayBase) + __anum*sizeof(T) + // arrays allocated this way will never try to reallocate or free the + // memory - that's your job. +}; + + +template <class T> class dArray : public dArrayBase { +public: + void equals (const dArray<T> &x) { + setSize (x.size()); + memcpy (_data,x._data,x._size * sizeof(T)); + } + + dArray () { constructor(); } + dArray (const dArray<T> &x) { constructor(); equals (x); } + ~dArray () { _freeAll(sizeof(T)); } + void setSize (int newsize) { _setSize (newsize,sizeof(T)); } + T *data() const { return (T*) _data; } + T & operator[] (int i) const { return ((T*)_data)[i]; } + void operator = (const dArray<T> &x) { equals (x); } + + void push (const T item) { + if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); + memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T)); + } + + void swap (dArray<T> &x) { + int tmp1; + void *tmp2; + tmp1=_size; _size=x._size; x._size=tmp1; + tmp1=_anum; _anum=x._anum; x._anum=tmp1; + tmp2=_data; _data=x._data; x._data=tmp2; + } + + // insert the item at the position `i'. if i<0 then add the item to the + // start, if i >= size then add the item to the end of the array. + void insert (int i, const T item) { + if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); + if (i >= (_size-1)) i = _size-1; // add to end + else { + if (i < 0) i=0; // add to start + int n = _size-1-i; + if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T)); + } + ((T*)_data)[i] = item; + } + + void remove (int i) { + if (i >= 0 && i < _size) { // passing this test guarantees size>0 + int n = _size-1-i; + if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T)); + _size--; + } + } +}; + + +#endif diff --git a/libs/ode-0.16.1/ode/src/box.cpp b/libs/ode-0.16.1/ode/src/box.cpp new file mode 100644 index 0000000..cfedb01 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/box.cpp @@ -0,0 +1,878 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + + standard ODE geometry primitives: public API and pairwise collision functions. + + the rule is that only the low level primitive collision functions should set + dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// box public API + +dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1) +{ + dAASSERT (lx >= 0 && ly >= 0 && lz >= 0); + type = dBoxClass; + side[0] = lx; + side[1] = ly; + side[2] = lz; + updateZeroSizedFlag(!lx || !ly || !lz); +} + + +void dxBox::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal xrange = REAL(0.5) * (dFabs (R[0] * side[0]) + + dFabs (R[1] * side[1]) + dFabs (R[2] * side[2])); + dReal yrange = REAL(0.5) * (dFabs (R[4] * side[0]) + + dFabs (R[5] * side[1]) + dFabs (R[6] * side[2])); + dReal zrange = REAL(0.5) * (dFabs (R[8] * side[0]) + + dFabs (R[9] * side[1]) + dFabs (R[10] * side[2])); + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz) +{ + return new dxBox (space,lx,ly,lz); +} + + +void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + dAASSERT (lx >= 0 && ly >= 0 && lz >= 0); + dxBox *b = (dxBox*) g; + b->side[0] = lx; + b->side[1] = ly; + b->side[2] = lz; + b->updateZeroSizedFlag(!lx || !ly || !lz); + dGeomMoved (g); +} + + +void dGeomBoxGetLengths (dGeomID g, dVector3 result) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + dxBox *b = (dxBox*) g; + result[0] = b->side[0]; + result[1] = b->side[1]; + result[2] = b->side[2]; +} + + +dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + g->recomputePosr(); + dxBox *b = (dxBox*) g; + + // Set p = (x,y,z) relative to box center + // + // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2) + + dVector3 p,q; + + p[0] = x - b->final_posr->pos[0]; + p[1] = y - b->final_posr->pos[1]; + p[2] = z - b->final_posr->pos[2]; + + // Rotate p into box's coordinate frame, so we can + // treat the OBB as an AABB + + dMultiply1_331 (q,b->final_posr->R,p); + + // Record distance from point to each successive box side, and see + // if the point is inside all six sides + + dReal dist[6]; + int i; + + bool inside = true; + + for (i=0; i < 3; i++) { + dReal side = b->side[i] * REAL(0.5); + + dist[i ] = side - q[i]; + dist[i+3] = side + q[i]; + + if ((dist[i] < 0) || (dist[i+3] < 0)) { + inside = false; + } + } + + // If point is inside the box, the depth is the smallest positive distance + // to any side + + if (inside) { + dReal smallest_dist = (dReal) (unsigned) -1; + + for (i=0; i < 6; i++) { + if (dist[i] < smallest_dist) smallest_dist = dist[i]; + } + + return smallest_dist; + } + + // Otherwise, if point is outside the box, the depth is the largest + // distance to any side. This is an approximation to the 'proper' + // solution (the proper solution may be larger in some cases). + + dReal largest_dist = 0; + + for (i=0; i < 6; i++) { + if (dist[i] > largest_dist) largest_dist = dist[i]; + } + + return -largest_dist; +} + +//**************************************************************************** +// box-box collision utility + + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). + +static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq=4,nr; + dReal buffer[16]; + dReal *q = p; + dReal *r = ret; + for (int dir=0; dir <= 1; dir++) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign=-1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + dReal *pq = q; + dReal *pr = r; + nr = 0; + for (int i=nq; i > 0; i--) { + // go through all points in q and all lines between adjacent points + if (sign*pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + dReal *nextq = (i > 1) ? pq+2 : q; + if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q==ret) ? buffer : ret; + nq = nr; + } + } +done: + if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal)); + return nr; +} + + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. + +void cullPoints (int n, dReal p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + int i,j; + dReal a,cx,cy,q; + if (n==1) { + cx = p[0]; + cy = p[1]; + } + else if (n==2) { + cx = REAL(0.5)*(p[0] + p[2]); + cy = REAL(0.5)*(p[1] + p[3]); + } + else { + a = 0; + cx = 0; + cy = 0; + for (i=0; i<(n-1); i++) { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + a = dRecip(REAL(3.0)*(a+q)); + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + // compute the angle of each point w.r.t. the centroid + dReal A[8]; + for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx); + + // search for points that have angles closest to A[i0] + i*(2*pi/m). + int avail[8]; + for (i=0; i<n; i++) avail[i] = 1; + avail[i0] = 0; + iret[0] = i0; + iret++; + for (j=1; j<m; j++) { + a = (dReal)(dReal(j)*(2*M_PI/m) + A[i0]); + if (a > M_PI) a -= (dReal)(2*M_PI); + dReal maxdiff=1e9,diff; +#ifndef dNODEBUG + *iret = i0; // iret is not allowed to keep this value +#endif + for (i=0; i<n; i++) { + if (avail[i]) { + diff = dFabs (A[i]-a); + if (diff > M_PI) diff = (dReal) (2*M_PI - diff); + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; + } + } + } +#ifndef dNODEBUG + dIASSERT (*iret != i0); // ensure iret got set +#endif + avail[*iret] = 0; + iret++; + } +} + + +// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and +// generate contact points. this returns 0 if there is no contact otherwise +// it returns the number of contacts generated. +// `normal' returns the contact normal. +// `depth' returns the maximum penetration depth along that normal. +// `return_code' returns a number indicating the type of contact that was +// detected: +// 1,2,3 = box 2 intersects with a face of box 1 +// 4,5,6 = box 1 intersects with a face of box 2 +// 7..15 = edge-edge contact +// `maxc' is the maximum number of contacts allowed to be generated, i.e. +// the size of the `contact' array. +// `contact' and `skip' are the contact array information provided to the +// collision functions. this function only fills in the position and depth +// fields. + + +int dBoxBox (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2, + dVector3 normal, dReal *depth, int *return_code, + int flags, dContactGeom *contact, int skip) +{ + const dReal fudge_factor = REAL(1.05); + dVector3 p,pp,normalC={0,0,0}; + const dReal *normalR = 0; + dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val; + int i,j,invert_normal,code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMultiply1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0]*REAL(0.5); + A[1] = side1[1]*REAL(0.5); + A[2] = side1[2]*REAL(0.5); + B[0] = side2[0]*REAL(0.5); + B[1] = side2[1]*REAL(0.5); + B[2] = side2[2]*REAL(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2); + R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2); + R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2); + + Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); + Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); + Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + + do { +#define TST(expr1,expr2,norm,cc) \ + expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ + s2 = dFabs(expr1_val) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1_val) < 0); \ + code = (cc); \ + if (flags & CONTACTS_UNIMPORTANT) break; \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis = u1,u2,u3 + TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); + TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); + TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + + // separating axis = v1,v2,v3 + TST (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); + TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); + TST (dCalcVectorDot3_41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. +#undef TST +#define TST(expr1,expr2,n1,n2,n3,cc) \ + expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ + s2 = dFabs(expr1_val) - (expr2); \ + if (s2 > 0) return 0; \ + l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2*fudge_factor > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1_val) < 0); \ + code = (cc); \ + if (flags & CONTACTS_UNIMPORTANT) break; \ + } \ + } + + // We only need to check 3 edges per box + // since parallel edges are equivalent. + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); + TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); + TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); + TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); + TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); + TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); + TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); +#undef TST + } while (0); + + if (!code) return 0; + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMultiply0_331 (normal,R1,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) { + // An edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + dVector3 pa; + dReal sign; + // Copy p1 into pa + for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy? + // Get world position of p2 into pa + for (j=0; j<3; j++) { + sign = (dCalcVectorDot3_14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; + } + + // find a point pb on the intersecting edge of box 2 + dVector3 pb; + // Copy p2 into pb + for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy? + // Get world position of p2 into pb + for (j=0; j<3; j++) { + sign = (dCalcVectorDot3_14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; + } + + dReal alpha,beta; + dVector3 ua,ub; + // Get direction of first edge + for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; + // Get direction of second edge + for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; + // Get closest points between edges (one at each) + dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + // Set the contact point as halfway between the 2 closest points + for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); + contact[0].depth = *depth; + *return_code = code; + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + // Note: Unmodified parameter values are being used here + const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb; + if (code <= 3) { // One of the faces of box 1 is the reference face + Ra = R1; // Rotation of 'a' + Rb = R2; // Rotation of 'b' + pa = p1; // Center (location) of 'a' + pb = p2; // Center (location) of 'b' + Sa = A; // Side Lenght of 'a' + Sb = B; // Side Lenght of 'b' + } + else { // One of the faces of box 2 is the reference face + Ra = R2; // Rotation of 'a' + Rb = R1; // Rotation of 'b' + pa = p2; // Center (location) of 'a' + pb = p1; // Center (location) of 'b' + Sa = B; // Side Lenght of 'a' + Sb = A; // Side Lenght of 'b' + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + /* + The normal is flipped if necessary so it always points outward from box 'a', + box 'b' is thus always the incident box + */ + dVector3 normal2,nr,anr; + if (code <= 3) { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + // Rotate normal2 in incident box opposite direction + dMultiply1_331 (nr,Rb,normal2); + anr[0] = dFabs (nr[0]); + anr[1] = dFabs (nr[1]); + anr[2] = dFabs (nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the incident face. the other axis numbers of the incident face + // are stored in a1,a2. + int lanr,a1,a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + dVector3 center; + if (nr[lanr] < 0) { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; + } + else { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN,code1,code2; + if (code <= 3) codeN = code-1; else codeN = code-4; + if (codeN==0) { + code1 = 1; + code2 = 2; + } + else if (codeN==1) { + code1 = 0; + code2 = 2; + } + else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + dReal quad[8]; // 2D coordinate of incident face (x,y pairs) + dReal c1,c2,m11,m12,m21,m22; + c1 = dCalcVectorDot3_14 (center,Ra+code1); + c2 = dCalcVectorDot3_14 (center,Ra+code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dCalcVectorDot3_44 (Ra+code1,Rb+a1); + m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2); + m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1); + m22 = dCalcVectorDot3_44 (Ra+code2,Rb+a2); + { + dReal k1 = m11*Sb[a1]; + dReal k2 = m21*Sb[a1]; + dReal k3 = m12*Sb[a2]; + dReal k4 = m22*Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + dReal rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + dReal ret[16]; + int n = intersectRectQuad (rect,quad,ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + dReal point[3*8]; // penetrating contact points + dReal dep[8]; // depths for those points + dReal det1 = dRecip(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j=0; j < n; j++) { + dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + for (i=0; i<3; i++) point[cnum*3+i] = + center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; + dep[cnum] = Sa[codeN] - dCalcVectorDot3(normal2,point+cnum*3); + if (dep[cnum] >= 0) { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + if (cnum < 1) { + return 0; // this should not happen, yet does at times (demo_plane2d single precision). + } + + // we can't generate more contacts than we actually have + int maxc = flags & NUMC_MASK; + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; // Even though max count must not be zero this check is kept for backward compatibility as this is a public function + + if (cnum <= maxc) { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) { + dContactGeom *con = CONTACT(contact,skip*j); + for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; + con->depth = dep[j]; + } + } + else { + dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + dReal maxdepth = dep[0]; + for (i=1; i<cnum; i++) { + if (dep[i] > maxdepth) { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints (cnum,ret,maxc,i1,iret); + + for (j=0; j < maxc; j++) { + dContactGeom *con = CONTACT(contact,skip*j); + for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + con->depth = dep[iret[j]]; + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + + + +int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dBoxClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dVector3 normal; + dReal depth; + int code; + dxBox *b1 = (dxBox*) o1; + dxBox *b2 = (dxBox*) o2; + int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side, + normal,&depth,&code,flags,contact,skip); + for (int i=0; i<num; i++) { + dContactGeom *currContact = CONTACT(contact,i*skip); + currContact->normal[0] = -normal[0]; + currContact->normal[1] = -normal[1]; + currContact->normal[2] = -normal[2]; + currContact->g1 = o1; + currContact->g2 = o2; + currContact->side1 = -1; + currContact->side2 = -1; + } + return num; +} + + +int dCollideBoxPlane (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dBoxClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxBox *box = (dxBox*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + + int ret = 0; + + //@@@ problem: using 4-vector (plane->p) as 3-vector (normal). + const dReal *R = o1->final_posr->R; // rotation of box + const dReal *n = plane->p; // normal vector + + // project sides lengths along normal vector, get absolute values + dReal Q1 = dCalcVectorDot3_14(n,R+0); + dReal Q2 = dCalcVectorDot3_14(n,R+1); + dReal Q3 = dCalcVectorDot3_14(n,R+2); + dReal A1 = box->side[0] * Q1; + dReal A2 = box->side[1] * Q2; + dReal A3 = box->side[2] * Q3; + dReal B1 = dFabs(A1); + dReal B2 = dFabs(A2); + dReal B3 = dFabs(A3); + + // early exit test + dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dCalcVectorDot3(n,o1->final_posr->pos); + if (depth < 0) return 0; + + // find number of contacts requested + int maxc = flags & NUMC_MASK; + // if (maxc < 1) maxc = 1; // an assertion is made on entry + if (maxc > 4) maxc = 4; // not more than 4 contacts per box allowed + + // find deepest point + dVector3 p; + p[0] = o1->final_posr->pos[0]; + p[1] = o1->final_posr->pos[1]; + p[2] = o1->final_posr->pos[2]; +#define FOO(i,op) \ + p[0] op REAL(0.5)*box->side[i] * R[0+i]; \ + p[1] op REAL(0.5)*box->side[i] * R[4+i]; \ + p[2] op REAL(0.5)*box->side[i] * R[8+i]; +#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) } + BAR(0,1); + BAR(1,2); + BAR(2,3); +#undef FOO +#undef BAR + + // the deepest point is the first contact point + contact->pos[0] = p[0]; + contact->pos[1] = p[1]; + contact->pos[2] = p[2]; + contact->depth = depth; + ret = 1; // ret is number of contact points found so far + if (maxc == 1) goto done; + + // get the second and third contact points by starting from `p' and going + // along the two sides with the smallest projected length. + +#define FOO(i,j,op) \ + CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \ + CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \ + CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j]; +#define BAR(ctact,side,sideinc) \ + if (depth - B ## sideinc < 0) goto done; \ + if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO(ctact,side,-); } \ + CONTACT(contact,ctact*skip)->depth = depth - B ## sideinc; \ + ret++; + + if (B1 < B2) { + if (B3 < B1) goto use_side_3; else { + BAR(1,0,1); // use side 1 + if (maxc == 2) goto done; + if (B2 < B3) goto contact2_2; else goto contact2_3; + } + } + else { + if (B3 < B2) { +use_side_3: // use side 3 + BAR(1,2,3); + if (maxc == 2) goto done; + if (B1 < B2) goto contact2_1; else goto contact2_2; + } + else { + BAR(1,1,2); // use side 2 + if (maxc == 2) goto done; + if (B1 < B3) goto contact2_1; else goto contact2_3; + } + } + +contact2_1: BAR(2,0,1); goto done; +contact2_2: BAR(2,1,2); goto done; +contact2_3: BAR(2,2,3); goto done; +#undef FOO +#undef BAR + +done: + + if (maxc == 4 && ret == 3) { // If user requested 4 contacts, and the first 3 were created... + // Combine contacts 2 and 3 (vectorial sum) and get the fourth one + // Result: if a box face is completely inside a plane, contacts are created for all the 4 vertices + dReal d4 = CONTACT(contact,1*skip)->depth + CONTACT(contact,2*skip)->depth - depth; // depth is the depth for first contact + if (d4 > 0) { + CONTACT(contact,3*skip)->pos[0] = CONTACT(contact,1*skip)->pos[0] + CONTACT(contact,2*skip)->pos[0] - p[0]; // p is the position of first contact + CONTACT(contact,3*skip)->pos[1] = CONTACT(contact,1*skip)->pos[1] + CONTACT(contact,2*skip)->pos[1] - p[1]; + CONTACT(contact,3*skip)->pos[2] = CONTACT(contact,1*skip)->pos[2] + CONTACT(contact,2*skip)->pos[2] - p[2]; + CONTACT(contact,3*skip)->depth = d4; + ret++; + } + } + + for (int i=0; i<ret; i++) { + dContactGeom *currContact = CONTACT(contact,i*skip); + currContact->g1 = o1; + currContact->g2 = o2; + currContact->side1 = -1; + currContact->side2 = -1; + + currContact->normal[0] = n[0]; + currContact->normal[1] = n[1]; + currContact->normal[2] = n[2]; + } + return ret; +} diff --git a/libs/ode-0.16.1/ode/src/capsule.cpp b/libs/ode-0.16.1/ode/src/capsule.cpp new file mode 100644 index 0000000..80e24ac --- /dev/null +++ b/libs/ode-0.16.1/ode/src/capsule.cpp @@ -0,0 +1,416 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// capped cylinder public API + +dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) : +dxGeom (space,1) +{ + dAASSERT (_radius >= 0 && _length >= 0); + type = dCapsuleClass; + radius = _radius; + lz = _length; + updateZeroSizedFlag(!_radius/* || !_length -- zero length capsule is not a zero sized capsule*/); +} + + +void dxCapsule::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal xrange = dFabs(R[2] * lz) * REAL(0.5) + radius; + dReal yrange = dFabs(R[6] * lz) * REAL(0.5) + radius; + dReal zrange = dFabs(R[10] * lz) * REAL(0.5) + radius; + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateCapsule (dSpaceID space, dReal radius, dReal length) +{ + return new dxCapsule (space,radius,length); +} + + +void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) +{ + dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); + dAASSERT (radius >= 0 && length >= 0); + dxCapsule *c = (dxCapsule*) g; + c->radius = radius; + c->lz = length; + c->updateZeroSizedFlag(!radius/* || !length -- zero length capsule is not a zero sized capsule*/); + dGeomMoved (g); +} + + +void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length) +{ + dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); + dxCapsule *c = (dxCapsule*) g; + *radius = c->radius; + *length = c->lz; +} + + +dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); + g->recomputePosr(); + dxCapsule *c = (dxCapsule*) g; + + const dReal* R = g->final_posr->R; + const dReal* pos = g->final_posr->pos; + + dVector3 a; + a[0] = x - pos[0]; + a[1] = y - pos[1]; + a[2] = z - pos[2]; + dReal beta = dCalcVectorDot3_14(a,R+2); + dReal lz2 = c->lz*REAL(0.5); + if (beta < -lz2) beta = -lz2; + else if (beta > lz2) beta = lz2; + a[0] = c->final_posr->pos[0] + beta*R[0*4+2]; + a[1] = c->final_posr->pos[1] + beta*R[1*4+2]; + a[2] = c->final_posr->pos[2] + beta*R[2*4+2]; + return c->radius - + dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2])); +} + + + +int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxCapsule *ccyl = (dxCapsule*) o1; + dxSphere *sphere = (dxSphere*) o2; + + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + + // find the point on the cylinder axis that is closest to the sphere + dReal alpha = + o1->final_posr->R[2] * (o2->final_posr->pos[0] - o1->final_posr->pos[0]) + + o1->final_posr->R[6] * (o2->final_posr->pos[1] - o1->final_posr->pos[1]) + + o1->final_posr->R[10] * (o2->final_posr->pos[2] - o1->final_posr->pos[2]); + dReal lz2 = ccyl->lz * REAL(0.5); + if (alpha > lz2) alpha = lz2; + if (alpha < -lz2) alpha = -lz2; + + // collide the spheres + dVector3 p; + p[0] = o1->final_posr->pos[0] + alpha * o1->final_posr->R[2]; + p[1] = o1->final_posr->pos[1] + alpha * o1->final_posr->R[6]; + p[2] = o1->final_posr->pos[2] + alpha * o1->final_posr->R[10]; + return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact); +} + + +int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxCapsule *cyl = (dxCapsule*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + + // get p1,p2 = cylinder axis endpoints, get radius + dVector3 p1,p2; + dReal clen = cyl->lz * REAL(0.5); + p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2]; + p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6]; + p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10]; + p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2]; + p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6]; + p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10]; + dReal radius = cyl->radius; + + // copy out box center, rotation matrix, and side array + dReal *c = o2->final_posr->pos; + dReal *R = o2->final_posr->R; + const dReal *side = box->side; + + // get the closest point between the cylinder axis and the box + dVector3 pl,pb; + dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb); + + // if the capsule is penetrated further than radius + // then pl and pb are equal (up to mindist) -> unknown normal + // use normal vector of closest box surface +#ifdef dSINGLE + dReal mindist = REAL(1e-6); +#else + dReal mindist = REAL(1e-15); +#endif + if (dCalcPointsDistance3(pl, pb)<mindist) { + // consider capsule as box + dVector3 normal; + dReal depth; + int code; + // WARNING! rad2 is declared as #define in Microsoft headers (as well as psh2, chx2, grp2, frm2, rct2, ico2, stc2, lst2, cmb2, edt2, scr2). Avoid abbreviations! + /* dReal rad2 = radius*REAL(2.0); */ dReal radiusMul2 = radius * REAL(2.0); + const dVector3 capboxside = {radiusMul2, radiusMul2, cyl->lz + radiusMul2}; + int num = dBoxBox (c, R, side, + o1->final_posr->pos, o1->final_posr->R, capboxside, + normal, &depth, &code, flags, contact, skip); + + for (int i=0; i<num; i++) { + dContactGeom *currContact = CONTACT(contact,i*skip); + currContact->normal[0] = normal[0]; + currContact->normal[1] = normal[1]; + currContact->normal[2] = normal[2]; + currContact->g1 = o1; + currContact->g2 = o2; + currContact->side1 = -1; + currContact->side2 = -1; + } + return num; + } else { + // generate contact point + return dCollideSpheres (pl,radius,pb,0,contact); + } +} + + +int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int i; + const dReal tolerance = REAL(1e-5); + + dxCapsule *cyl1 = (dxCapsule*) o1; + dxCapsule *cyl2 = (dxCapsule*) o2; + + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + + // copy out some variables, for convenience + dReal lz1 = cyl1->lz * REAL(0.5); + dReal lz2 = cyl2->lz * REAL(0.5); + dReal *pos1 = o1->final_posr->pos; + dReal *pos2 = o2->final_posr->pos; + dReal axis1[3],axis2[3]; + axis1[0] = o1->final_posr->R[2]; + axis1[1] = o1->final_posr->R[6]; + axis1[2] = o1->final_posr->R[10]; + axis2[0] = o2->final_posr->R[2]; + axis2[1] = o2->final_posr->R[6]; + axis2[2] = o2->final_posr->R[10]; + + // if the cylinder axes are close to parallel, we'll try to detect up to + // two contact points along the body of the cylinder. if we can't find any + // points then we'll fall back to the closest-points algorithm. note that + // we are not treating this special case for reasons of degeneracy, but + // because we want two contact points in some situations. the closet-points + // algorithm is robust in all casts, but it can return only one contact. + + dVector3 sphere1,sphere2; + dReal a1a2 = dCalcVectorDot3 (axis1,axis2); + dReal det = REAL(1.0)-a1a2*a1a2; + if (det < tolerance) { + // the cylinder axes (almost) parallel, so we will generate up to two + // contacts. alpha1 and alpha2 (line position parameters) are related by: + // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) + // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) + // first compute where the two cylinders overlap in alpha1 space: + if (a1a2 < 0) { + axis2[0] = -axis2[0]; + axis2[1] = -axis2[1]; + axis2[2] = -axis2[2]; + } + dReal q[3]; + for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; + dReal k = dCalcVectorDot3 (axis1,q); + dReal a1lo = -lz1; + dReal a1hi = lz1; + dReal a2lo = -lz2 - k; + dReal a2hi = lz2 - k; + dReal lo = (a1lo > a2lo) ? a1lo : a2lo; + dReal hi = (a1hi < a2hi) ? a1hi : a2hi; + if (lo <= hi) { + int num_contacts = flags & NUMC_MASK; + if (num_contacts >= 2 && lo < hi) { + // generate up to two contacts. if one of those contacts is + // not made, fall back on the one-contact strategy. + for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i]; + int n1 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + if (n1) { + for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i]; + dContactGeom *c2 = CONTACT(contact,skip); + int n2 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius, c2); + if (n2) { + c2->g1 = o1; + c2->g2 = o2; + c2->side1 = -1; + c2->side2 = -1; + return 2; + } + } + } + + // just one contact to generate, so put it in the middle of + // the range + dReal alpha1 = (lo + hi) * REAL(0.5); + dReal alpha2 = alpha1 + k; + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + return dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + } + } + + // use the closest point algorithm + dVector3 a1,a2,b1,b2; + a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1; + a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1; + a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1; + a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1; + a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1; + a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1; + b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2; + b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2; + b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2; + b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2; + b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2; + b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2; + + dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2); + return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact); +} + + +int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxCapsule *ccyl = (dxCapsule*) o1; + dxPlane *plane = (dxPlane*) o2; + + // collide the deepest capping sphere with the plane + dReal sign = (dCalcVectorDot3_14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0); + dVector3 p; + p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign; + p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign; + p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign; + + dReal k = dCalcVectorDot3 (p,plane->p); + dReal depth = plane->p[3] - k + ccyl->radius; + if (depth < 0) return 0; + contact->normal[0] = plane->p[0]; + contact->normal[1] = plane->p[1]; + contact->normal[2] = plane->p[2]; + contact->pos[0] = p[0] - plane->p[0] * ccyl->radius; + contact->pos[1] = p[1] - plane->p[1] * ccyl->radius; + contact->pos[2] = p[2] - plane->p[2] * ccyl->radius; + contact->depth = depth; + + int ncontacts = 1; + if ((flags & NUMC_MASK) >= 2) { + // collide the other capping sphere with the plane + p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign; + p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign; + p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign; + + k = dCalcVectorDot3 (p,plane->p); + depth = plane->p[3] - k + ccyl->radius; + if (depth >= 0) { + dContactGeom *c2 = CONTACT(contact,skip); + c2->normal[0] = plane->p[0]; + c2->normal[1] = plane->p[1]; + c2->normal[2] = plane->p[2]; + c2->pos[0] = p[0] - plane->p[0] * ccyl->radius; + c2->pos[1] = p[1] - plane->p[1] * ccyl->radius; + c2->pos[2] = p[2] - plane->p[2] * ccyl->radius; + c2->depth = depth; + ncontacts = 2; + } + } + + for (int i=0; i < ncontacts; i++) { + dContactGeom *currContact = CONTACT(contact,i*skip); + currContact->g1 = o1; + currContact->g2 = o2; + currContact->side1 = -1; + currContact->side2 = -1; + } + return ncontacts; +} + diff --git a/libs/ode-0.16.1/ode/src/collision_convex_trimesh.cpp b/libs/ode-0.16.1/ode/src/collision_convex_trimesh.cpp new file mode 100644 index 0000000..651f236 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_convex_trimesh.cpp @@ -0,0 +1,120 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+
+typedef struct _sLocalContactData
+{
+ dVector3 vPos;
+ dVector3 vNormal;
+ dReal fDepth;
+ int triIndex;
+ int nFlags; // 0 = filtered out, 1 = OK
+}sLocalContactData;
+
+
+#if dTRIMESH_ENABLED
+
+#include "collision_util.h"
+#include "collision_std.h"
+#include "collision_trimesh_internal.h"
+#if dLIBCCD_ENABLED
+#include "collision_libccd.h"
+#endif
+
+int dCollideConvexTrimesh( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
+{
+ int contactcount = 0;
+ dIASSERT( skip >= (int)sizeof( dContactGeom ) );
+ dIASSERT( o1->type == dConvexClass );
+ dIASSERT( o2->type == dTriMeshClass );
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+
+#if dLIBCCD_ENABLED
+
+#if dTRIMESH_OPCODE
+ const dVector3 &meshPosition = *(const dVector3 *)dGeomGetPosition(o2);
+ // Find convex OBB in trimesh coordinates
+ Point convexAABBMin(o1->aabb[0] - meshPosition[0], o1->aabb[2] - meshPosition[1], o1->aabb[4] - meshPosition[2]);
+ Point convexAABBMax(o1->aabb[1] - meshPosition[0], o1->aabb[3] - meshPosition[1], o1->aabb[5] - meshPosition[2]);
+
+ const Point convexCenter = 0.5f * (convexAABBMax + convexAABBMin);
+ const Point convexExtents = 0.5f * (convexAABBMax - convexAABBMin);
+ const Matrix3x3 convexRotation(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+ OBB convexOOB(convexCenter, convexExtents, convexRotation);
+
+ Matrix4x4 meshTransformation;
+ const dMatrix3 &meshRotation = *(const dMatrix3 *)dGeomGetRotation(o2);
+ const dVector3 zeroVector = { REAL(0.0), };
+ MakeMatrix(zeroVector, meshRotation, meshTransformation);
+
+ OBBCollider collider;
+ collider.SetFirstContact(false);
+ collider.SetTemporalCoherence(false);
+ collider.SetPrimitiveTests(false);
+
+ OBBCache cache;
+ dxTriMesh *trimesh = (dxTriMesh *)o2;
+ if (collider.Collide(cache, convexOOB, trimesh->retrieveMeshBVTreeRef(), null, &meshTransformation)) {
+ int triCount = collider.GetNbTouchedPrimitives();
+ if (triCount > 0) {
+ int* triangles = (int*)collider.GetTouchedPrimitives();
+ contactcount = dCollideConvexTrimeshTrianglesCCD(o1, o2, triangles, triCount, flags, contacts, skip);
+ }
+ }
+
+#elif dTRIMESH_GIMPACT
+ dxTriMesh *trimesh = (dxTriMesh *)o2;
+
+ aabb3f test_aabb(o1->aabb[0], o1->aabb[1], o1->aabb[2], o1->aabb[3], o1->aabb[4], o1->aabb[5]);
+
+ GDYNAMIC_ARRAY collision_result;
+ GIM_CREATE_BOXQUERY_LIST(collision_result);
+
+ gim_aabbset_box_collision(&test_aabb, &trimesh->m_collision_trimesh.m_aabbset, &collision_result);
+
+ if (collision_result.m_size != 0)
+ {
+ GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
+ GIM_TRIMESH * ptrimesh = &trimesh->m_collision_trimesh;
+ gim_trimesh_locks_work_data(ptrimesh);
+
+ contactcount = dCollideConvexTrimeshTrianglesCCD(o1, o2, (int *)boxesresult, collision_result.m_size, flags, contacts, skip);
+
+ gim_trimesh_unlocks_work_data(ptrimesh);
+ }
+
+ GIM_DYNARRAY_DESTROY(collision_result);
+#endif // dTRIMESH_GIMPACT
+
+#endif // dLIBCCD_ENABLED
+
+ return contactcount;
+}
+
+#endif // dTRIMESH_ENABLED
+
diff --git a/libs/ode-0.16.1/ode/src/collision_cylinder_box.cpp b/libs/ode-0.16.1/ode/src/collision_cylinder_box.cpp new file mode 100644 index 0000000..4eaf92d --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_cylinder_box.cpp @@ -0,0 +1,1038 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Cylinder-box collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_util.h" + +static const int MAX_CYLBOX_CLIP_POINTS = 16; +static const int nCYLINDER_AXIS = 2; +// Number of segment of cylinder base circle. +// Must be divisible by 4. +static const int nCYLINDER_SEGMENT = 8; + +#define MAX_FLOAT dInfinity + +// Data that passed through the collider's functions +struct sCylinderBoxData +{ + sCylinderBoxData(dxGeom *Cylinder, dxGeom *Box, int flags, dContactGeom *contact, int skip): + m_gBox(Box), m_gCylinder(Cylinder), m_gContact(contact), m_iFlags(flags), m_iSkip(skip), m_nContacts(0) + { + } + + void _cldInitCylinderBox(); + int _cldTestAxis( dVector3& vInputNormal, int iAxis ); + int _cldTestEdgeCircleAxis( const dVector3 &vCenterPoint, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis ); + int _cldTestSeparatingAxes(); + int _cldClipCylinderToBox(); + void _cldClipBoxToCylinder(); + int PerformCollisionChecking(); + + // cylinder parameters + dMatrix3 m_mCylinderRot; + dVector3 m_vCylinderPos; + dVector3 m_vCylinderAxis; + dReal m_fCylinderRadius; + dReal m_fCylinderSize; + dVector3 m_avCylinderNormals[nCYLINDER_SEGMENT]; + + // box parameters + + dMatrix3 m_mBoxRot; + dVector3 m_vBoxPos; + dVector3 m_vBoxHalfSize; + // box vertices array : 8 vertices + dVector3 m_avBoxVertices[8]; + + // global collider data + dVector3 m_vDiff; + dVector3 m_vNormal; + dReal m_fBestDepth; + dReal m_fBestrb; + dReal m_fBestrc; + int m_iBestAxis; + + // contact data + dVector3 m_vEp0, m_vEp1; + dReal m_fDepth0, m_fDepth1; + + // ODE stuff + dGeomID m_gBox; + dGeomID m_gCylinder; + dContactGeom* m_gContact; + int m_iFlags; + int m_iSkip; + int m_nContacts; + +}; + + +// initialize collision data +void sCylinderBoxData::_cldInitCylinderBox() +{ + // get cylinder position, orientation + const dReal* pRotCyc = dGeomGetRotation(m_gCylinder); + dMatrix3Copy(pRotCyc,m_mCylinderRot); + + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(m_gCylinder); + dVector3Copy(*pPosCyc,m_vCylinderPos); + + dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis); + + // get cylinder radius and size + dGeomCylinderGetParams(m_gCylinder,&m_fCylinderRadius,&m_fCylinderSize); + + // get box position, orientation, size + const dReal* pRotBox = dGeomGetRotation(m_gBox); + dMatrix3Copy(pRotBox,m_mBoxRot); + const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(m_gBox); + dVector3Copy(*pPosBox,m_vBoxPos); + + dGeomBoxGetLengths(m_gBox, m_vBoxHalfSize); + m_vBoxHalfSize[0] *= REAL(0.5); + m_vBoxHalfSize[1] *= REAL(0.5); + m_vBoxHalfSize[2] *= REAL(0.5); + + // vertex 0 + m_avBoxVertices[0][0] = -m_vBoxHalfSize[0]; + m_avBoxVertices[0][1] = m_vBoxHalfSize[1]; + m_avBoxVertices[0][2] = -m_vBoxHalfSize[2]; + + // vertex 1 + m_avBoxVertices[1][0] = m_vBoxHalfSize[0]; + m_avBoxVertices[1][1] = m_vBoxHalfSize[1]; + m_avBoxVertices[1][2] = -m_vBoxHalfSize[2]; + + // vertex 2 + m_avBoxVertices[2][0] = -m_vBoxHalfSize[0]; + m_avBoxVertices[2][1] = -m_vBoxHalfSize[1]; + m_avBoxVertices[2][2] = -m_vBoxHalfSize[2]; + + // vertex 3 + m_avBoxVertices[3][0] = m_vBoxHalfSize[0]; + m_avBoxVertices[3][1] = -m_vBoxHalfSize[1]; + m_avBoxVertices[3][2] = -m_vBoxHalfSize[2]; + + // vertex 4 + m_avBoxVertices[4][0] = m_vBoxHalfSize[0]; + m_avBoxVertices[4][1] = m_vBoxHalfSize[1]; + m_avBoxVertices[4][2] = m_vBoxHalfSize[2]; + + // vertex 5 + m_avBoxVertices[5][0] = m_vBoxHalfSize[0]; + m_avBoxVertices[5][1] = -m_vBoxHalfSize[1]; + m_avBoxVertices[5][2] = m_vBoxHalfSize[2]; + + // vertex 6 + m_avBoxVertices[6][0] = -m_vBoxHalfSize[0]; + m_avBoxVertices[6][1] = -m_vBoxHalfSize[1]; + m_avBoxVertices[6][2] = m_vBoxHalfSize[2]; + + // vertex 7 + m_avBoxVertices[7][0] = -m_vBoxHalfSize[0]; + m_avBoxVertices[7][1] = m_vBoxHalfSize[1]; + m_avBoxVertices[7][2] = m_vBoxHalfSize[2]; + + // temp index + int i = 0; + dVector3 vTempBoxVertices[8]; + // transform vertices in absolute space + for(i=0; i < 8; i++) + { + dMultiplyMat3Vec3(m_mBoxRot,m_avBoxVertices[i], vTempBoxVertices[i]); + dVector3Add(vTempBoxVertices[i], m_vBoxPos, m_avBoxVertices[i]); + } + + // find relative position + dVector3Subtract(m_vCylinderPos,m_vBoxPos,m_vDiff); + m_fBestDepth = MAX_FLOAT; + m_vNormal[0] = REAL(0.0); + m_vNormal[1] = REAL(0.0); + m_vNormal[2] = REAL(0.0); + + // calculate basic angle for nCYLINDER_SEGMENT-gon + dReal fAngle = (dReal) (M_PI/nCYLINDER_SEGMENT); + + // calculate angle increment + dReal fAngleIncrement = fAngle * REAL(2.0); + + // calculate nCYLINDER_SEGMENT-gon points + for(i = 0; i < nCYLINDER_SEGMENT; i++) + { + m_avCylinderNormals[i][0] = -dCos(fAngle); + m_avCylinderNormals[i][1] = -dSin(fAngle); + m_avCylinderNormals[i][2] = 0; + + fAngle += fAngleIncrement; + } + + m_fBestrb = 0; + m_fBestrc = 0; + m_iBestAxis = 0; + m_nContacts = 0; + +} + +// test for given separating axis +int sCylinderBoxData::_cldTestAxis( dVector3& vInputNormal, int iAxis ) +{ + // check length of input normal + dReal fL = dVector3Length(vInputNormal); + // if not long enough + if ( fL < REAL(1e-5) ) + { + // do nothing + return 1; + } + + // otherwise make it unit for sure + dNormalize3(vInputNormal); + + // project box and Cylinder on mAxis + dReal fdot1 = dVector3Dot(m_vCylinderAxis, vInputNormal); + + dReal frc; + + if (fdot1 > REAL(1.0)) + { + // assume fdot1 = 1 + frc = m_fCylinderSize*REAL(0.5); + } + else if (fdot1 < REAL(-1.0)) + { + // assume fdot1 = -1 + frc = m_fCylinderSize*REAL(0.5); + } + else + { + // project box and capsule on iAxis + frc = dFabs( fdot1 * (m_fCylinderSize*REAL(0.5))) + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); + } + + dVector3 vTemp1; + + dMat3GetCol(m_mBoxRot,0,vTemp1); + dReal frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[0]; + + dMat3GetCol(m_mBoxRot,1,vTemp1); + frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[1]; + + dMat3GetCol(m_mBoxRot,2,vTemp1); + frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[2]; + + // project their distance on separating axis + dReal fd = dVector3Dot(m_vDiff,vInputNormal); + + // get depth + + dReal fDepth = frc + frb; // Calculate partial depth + + // if they do not overlap exit, we have no intersection + if ( dFabs(fd) > fDepth ) + { + return 0; + } + + // Finalyze the depth calculation + fDepth -= dFabs(fd); + + // get maximum depth + if ( fDepth < m_fBestDepth ) + { + m_fBestDepth = fDepth; + dVector3Copy(vInputNormal,m_vNormal); + m_iBestAxis = iAxis; + m_fBestrb = frb; + m_fBestrc = frc; + + // flip normal if interval is wrong faced + if (fd > 0) + { + dVector3Inv(m_vNormal); + } + } + + return 1; +} + + +// check for separation between box edge and cylinder circle edge +int sCylinderBoxData::_cldTestEdgeCircleAxis( + const dVector3 &vCenterPoint, + const dVector3 &vVx0, const dVector3 &vVx1, + int iAxis ) +{ + // calculate direction of edge + dVector3 vDirEdge; + dVector3Subtract(vVx1,vVx0,vDirEdge); + dNormalize3(vDirEdge); + // starting point of edge + dVector3 vEStart; + dVector3Copy(vVx0,vEStart);; + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot (vDirEdge,m_vCylinderAxis); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2) < REAL(1e-5)) + { + // this can't be separating axis, because edge is parallel to circle plane + return 1; + } + + // find point of intersection between edge line and circle plane + dVector3 vTemp1; + dVector3Subtract(vCenterPoint,vEStart,vTemp1); + dReal fdot1 = dVector3Dot(vTemp1,m_vCylinderAxis); + dVector3 vpnt; + vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2); + vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2); + vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2); + + // find tangent vector on circle with same center (vCenterPoint) that + // touches point of intersection (vpnt) + dVector3 vTangent; + dVector3Subtract(vCenterPoint,vpnt,vTemp1); + dVector3Cross(vTemp1,m_vCylinderAxis,vTangent); + + // find vector orthogonal both to tangent and edge direction + dVector3 vAxis; + dVector3Cross(vTangent,vDirEdge,vAxis); + + // use that vector as separating axis + return _cldTestAxis( vAxis, iAxis ); +} + +// Test separating axis for collision +int sCylinderBoxData::_cldTestSeparatingAxes() +{ + // reset best axis + m_fBestDepth = MAX_FLOAT; + m_iBestAxis = 0; + m_fBestrb = 0; + m_fBestrc = 0; + m_nContacts = 0; + + dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + + // Epsilon value for checking axis vector length + const dReal fEpsilon = REAL(1e-6); + + // axis A0 + dMat3GetCol(m_mBoxRot, 0 , vAxis); + if (!_cldTestAxis( vAxis, 1 )) + { + return 0; + } + + // axis A1 + dMat3GetCol(m_mBoxRot, 1 , vAxis); + if (!_cldTestAxis( vAxis, 2 )) + { + return 0; + } + + // axis A2 + dMat3GetCol(m_mBoxRot, 2 , vAxis); + if (!_cldTestAxis( vAxis, 3 )) + { + return 0; + } + + // axis C - Cylinder Axis + //vAxis = vCylinderAxis; + dVector3Copy(m_vCylinderAxis , vAxis); + if (!_cldTestAxis( vAxis, 4 )) + { + return 0; + } + + // axis CxA0 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 )); + dVector3CrossMat3Col(m_mBoxRot, 0 ,m_vCylinderAxis, vAxis); + if(dVector3LengthSquare( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( vAxis, 5 )) + { + return 0; + } + } + + // axis CxA1 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 )); + dVector3CrossMat3Col(m_mBoxRot, 1 ,m_vCylinderAxis, vAxis); + if(dVector3LengthSquare( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( vAxis, 6 )) + { + return 0; + } + } + + // axis CxA2 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 )); + dVector3CrossMat3Col(m_mBoxRot, 2 ,m_vCylinderAxis, vAxis); + if(dVector3LengthSquare( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( vAxis, 7 )) + { + return 0; + } + } + + int i = 0; + dVector3 vTemp1; + dVector3 vTemp2; + // here we check box's vertices axis + for(i=0; i< 8; i++) + { + //vAxis = ( vCylinderAxis cross (m_avBoxVertices[i] - vCylinderPos)); + dVector3Subtract(m_avBoxVertices[i],m_vCylinderPos,vTemp1); + dVector3Cross(m_vCylinderAxis,vTemp1,vTemp2); + //vAxis = ( vCylinderAxis cross vAxis ); + dVector3Cross(m_vCylinderAxis,vTemp2,vAxis); + if(dVector3LengthSquare( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( vAxis, 8 + i )) + { + return 0; + } + } + } + + // ************************************ + // this is defined for first 12 axes + // normal of plane that contains top circle of cylinder + // center of top circle of cylinder + dVector3 vcc; + vcc[0] = (m_vCylinderPos)[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vcc[1] = (m_vCylinderPos)[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vcc[2] = (m_vCylinderPos)[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + // ************************************ + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[1], m_avBoxVertices[0], 16)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[1], m_avBoxVertices[3], 17)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[2], m_avBoxVertices[3], 18)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[2], m_avBoxVertices[0], 19)) + { + return 0; + } + + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[4], m_avBoxVertices[1], 20)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[4], m_avBoxVertices[7], 21)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[0], m_avBoxVertices[7], 22)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[5], m_avBoxVertices[3], 23)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[5], m_avBoxVertices[6], 24)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[2], m_avBoxVertices[6], 25)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[4], m_avBoxVertices[5], 26)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[6], m_avBoxVertices[7], 27)) + { + return 0; + } + + // ************************************ + // this is defined for second 12 axes + // normal of plane that contains bottom circle of cylinder + // center of bottom circle of cylinder + // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5)); + vcc[0] = (m_vCylinderPos)[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vcc[1] = (m_vCylinderPos)[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vcc[2] = (m_vCylinderPos)[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + // ************************************ + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[1], m_avBoxVertices[0], 28)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[1], m_avBoxVertices[3], 29)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[2], m_avBoxVertices[3], 30)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[2], m_avBoxVertices[0], 31)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[4], m_avBoxVertices[1], 32)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[4], m_avBoxVertices[7], 33)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[0], m_avBoxVertices[7], 34)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[5], m_avBoxVertices[3], 35)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[5], m_avBoxVertices[6], 36)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[2], m_avBoxVertices[6], 37)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[4], m_avBoxVertices[5], 38)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( vcc, m_avBoxVertices[6], m_avBoxVertices[7], 39)) + { + return 0; + } + + return 1; +} + +int sCylinderBoxData::_cldClipCylinderToBox() +{ + dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK)); + + // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal + dVector3 vN; + dReal fTemp1 = dVector3Dot(m_vCylinderAxis,m_vNormal); + vN[0] = m_vNormal[0] - m_vCylinderAxis[0]*fTemp1; + vN[1] = m_vNormal[1] - m_vCylinderAxis[1]*fTemp1; + vN[2] = m_vNormal[2] - m_vCylinderAxis[2]*fTemp1; + + // normalize that vector + dNormalize3(vN); + + // translate cylinder end points by the vector + dVector3 vCposTrans; + vCposTrans[0] = m_vCylinderPos[0] + vN[0] * m_fCylinderRadius; + vCposTrans[1] = m_vCylinderPos[1] + vN[1] * m_fCylinderRadius; + vCposTrans[2] = m_vCylinderPos[2] + vN[2] * m_fCylinderRadius; + + m_vEp0[0] = vCposTrans[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + m_vEp0[1] = vCposTrans[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + m_vEp0[2] = vCposTrans[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + m_vEp1[0] = vCposTrans[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + m_vEp1[1] = vCposTrans[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + m_vEp1[2] = vCposTrans[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + // transform edge points in box space + m_vEp0[0] -= m_vBoxPos[0]; + m_vEp0[1] -= m_vBoxPos[1]; + m_vEp0[2] -= m_vBoxPos[2]; + + m_vEp1[0] -= m_vBoxPos[0]; + m_vEp1[1] -= m_vBoxPos[1]; + m_vEp1[2] -= m_vBoxPos[2]; + + dVector3 vTemp1; + // clip the edge to box + dVector4 plPlane; + // plane 0 +x + dMat3GetCol(m_mBoxRot,0,vTemp1); + dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane); + if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) + { + return 0; + } + + // plane 1 +y + dMat3GetCol(m_mBoxRot,1,vTemp1); + dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane); + if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) + { + return 0; + } + + // plane 2 +z + dMat3GetCol(m_mBoxRot,2,vTemp1); + dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane); + if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) + { + return 0; + } + + // plane 3 -x + dMat3GetCol(m_mBoxRot,0,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane); + if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) + { + return 0; + } + + // plane 4 -y + dMat3GetCol(m_mBoxRot,1,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane); + if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) + { + return 0; + } + + // plane 5 -z + dMat3GetCol(m_mBoxRot,2,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane); + if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) + { + return 0; + } + + // calculate depths for both contact points + m_fDepth0 = m_fBestrb + dVector3Dot(m_vEp0, m_vNormal); + m_fDepth1 = m_fBestrb + dVector3Dot(m_vEp1, m_vNormal); + + // clamp depths to 0 + if(m_fDepth0<0) + { + m_fDepth0 = REAL(0.0); + } + + if(m_fDepth1<0) + { + m_fDepth1 = REAL(0.0); + } + + // back transform edge points from box to absolute space + m_vEp0[0] += m_vBoxPos[0]; + m_vEp0[1] += m_vBoxPos[1]; + m_vEp0[2] += m_vBoxPos[2]; + + m_vEp1[0] += m_vBoxPos[0]; + m_vEp1[1] += m_vBoxPos[1]; + m_vEp1[2] += m_vBoxPos[2]; + + dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); + Contact0->depth = m_fDepth0; + dVector3Copy(m_vNormal,Contact0->normal); + dVector3Copy(m_vEp0,Contact0->pos); + Contact0->g1 = m_gCylinder; + Contact0->g2 = m_gBox; + Contact0->side1 = -1; + Contact0->side2 = -1; + dVector3Inv(Contact0->normal); + m_nContacts++; + + if (m_nContacts != (m_iFlags & NUMC_MASK)) + { + dContactGeom* Contact1 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); + Contact1->depth = m_fDepth1; + dVector3Copy(m_vNormal,Contact1->normal); + dVector3Copy(m_vEp1,Contact1->pos); + Contact1->g1 = m_gCylinder; + Contact1->g2 = m_gBox; + Contact1->side1 = -1; + Contact1->side2 = -1; + dVector3Inv(Contact1->normal); + m_nContacts++; + } + + return 1; +} + + +void sCylinderBoxData::_cldClipBoxToCylinder() +{ + dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK)); + + dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; + // check which circle from cylinder we take for clipping + if ( dVector3Dot(m_vCylinderAxis, m_vNormal) > REAL(0.0) ) + { + // get top circle + vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[0] = REAL(0.0); + vCylinderCircleNormal_Rel[1] = REAL(0.0); + vCylinderCircleNormal_Rel[2] = REAL(0.0); + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[0] = REAL(0.0); + vCylinderCircleNormal_Rel[1] = REAL(0.0); + vCylinderCircleNormal_Rel[2] = REAL(0.0); + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + // vNr is normal in Box frame, pointing from Cylinder to Box + dVector3 vNr; + dMatrix3 mBoxInv; + + // Find a way to use quaternion + dMatrix3Inv(m_mBoxRot,mBoxInv); + dMultiplyMat3Vec3(mBoxInv,m_vNormal,vNr); + + dVector3 vAbsNormal; + + vAbsNormal[0] = dFabs( vNr[0] ); + vAbsNormal[1] = dFabs( vNr[1] ); + vAbsNormal[2] = dFabs( vNr[2] ); + + // find which face in box is closest to cylinder + int iB0, iB1, iB2; + + // Different from Croteam's code + if (vAbsNormal[1] > vAbsNormal[0]) + { + // 1 > 0 + if (vAbsNormal[0]> vAbsNormal[2]) + { + // 0 > 2 -> 1 > 0 >2 + iB0 = 1; iB1 = 0; iB2 = 2; + } + else + { + // 2 > 0-> Must compare 1 and 2 + if (vAbsNormal[1] > vAbsNormal[2]) + { + // 1 > 2 -> 1 > 2 > 0 + iB0 = 1; iB1 = 2; iB2 = 0; + } + else + { + // 2 > 1 -> 2 > 1 > 0; + iB0 = 2; iB1 = 1; iB2 = 0; + } + } + } + else + { + // 0 > 1 + if (vAbsNormal[1] > vAbsNormal[2]) + { + // 1 > 2 -> 0 > 1 > 2 + iB0 = 0; iB1 = 1; iB2 = 2; + } + else + { + // 2 > 1 -> Must compare 0 and 2 + if (vAbsNormal[0] > vAbsNormal[2]) + { + // 0 > 2 -> 0 > 2 > 1; + iB0 = 0; iB1 = 2; iB2 = 1; + } + else + { + // 2 > 0 -> 2 > 0 > 1; + iB0 = 2; iB1 = 0; iB2 = 1; + } + } + } + + dVector3 vCenter; + // find center of box polygon + dVector3 vTemp; + if (vNr[iB0] > 0) + { + dMat3GetCol(m_mBoxRot,iB0,vTemp); + vCenter[0] = m_vBoxPos[0] - m_vBoxHalfSize[iB0]*vTemp[0]; + vCenter[1] = m_vBoxPos[1] - m_vBoxHalfSize[iB0]*vTemp[1]; + vCenter[2] = m_vBoxPos[2] - m_vBoxHalfSize[iB0]*vTemp[2]; + } + else + { + dMat3GetCol(m_mBoxRot,iB0,vTemp); + vCenter[0] = m_vBoxPos[0] + m_vBoxHalfSize[iB0]*vTemp[0]; + vCenter[1] = m_vBoxPos[1] + m_vBoxHalfSize[iB0]*vTemp[1]; + vCenter[2] = m_vBoxPos[2] + m_vBoxHalfSize[iB0]*vTemp[2]; + } + + // find the vertices of box polygon + dVector3 avPoints[4]; + dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS]; + dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS]; + + int i=0; + for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++) + { + avTempArray1[i][0] = REAL(0.0); + avTempArray1[i][1] = REAL(0.0); + avTempArray1[i][2] = REAL(0.0); + + avTempArray2[i][0] = REAL(0.0); + avTempArray2[i][1] = REAL(0.0); + avTempArray2[i][2] = REAL(0.0); + } + + dVector3 vAxis1, vAxis2; + + dMat3GetCol(m_mBoxRot,iB1,vAxis1); + dMat3GetCol(m_mBoxRot,iB2,vAxis2); + + avPoints[0][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0]; + avPoints[0][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1]; + avPoints[0][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2]; + + avPoints[1][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0]; + avPoints[1][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1]; + avPoints[1][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2]; + + avPoints[2][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0]; + avPoints[2][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1]; + avPoints[2][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2]; + + avPoints[3][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0]; + avPoints[3][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1]; + avPoints[3][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2]; + + // transform box points to space of cylinder circle + dMatrix3 mCylinderInv; + dMatrix3Inv(m_mCylinderRot,mCylinderInv); + + for(i=0; i<4; i++) + { + dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp); + dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]); + } + + int iTmpCounter1 = 0; + int iTmpCounter2 = 0; + dVector4 plPlane; + + // plane of cylinder that contains circle for intersection + dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); + dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane); + + + // Body of base circle of Cylinder + int nCircleSegment = 0; + for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++) + { + dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane); + + if (0 == (nCircleSegment % 2)) + { + dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); + } + else + { + dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); + } + + dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + if (nCircleSegment % 2) + { + for( i=0; i<iTmpCounter2; i++) + { + dMultiply0_331(vPoint,m_mCylinderRot,avTempArray2[i]); + vPoint[0] += vCylinderCirclePos[0]; + vPoint[1] += vCylinderCirclePos[1]; + vPoint[2] += vCylinderCirclePos[2]; + + dVector3Subtract(vPoint,m_vCylinderPos,vTemp); + ftmpdot = dVector3Dot(vTemp, m_vNormal); + fTempDepth = m_fBestrc - ftmpdot; + // Depth must be positive + if (fTempDepth > REAL(0.0)) + { + // generate contacts + dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); + Contact0->depth = fTempDepth; + dVector3Copy(m_vNormal,Contact0->normal); + dVector3Copy(vPoint,Contact0->pos); + Contact0->g1 = m_gCylinder; + Contact0->g2 = m_gBox; + Contact0->side1 = -1; + Contact0->side2 = -1; + dVector3Inv(Contact0->normal); + m_nContacts++; + + if (m_nContacts == (m_iFlags & NUMC_MASK)) + { + break; + } + } + } + } + else + { + for( i=0; i<iTmpCounter1; i++) + { + dMultiply0_331(vPoint,m_mCylinderRot,avTempArray1[i]); + vPoint[0] += vCylinderCirclePos[0]; + vPoint[1] += vCylinderCirclePos[1]; + vPoint[2] += vCylinderCirclePos[2]; + + dVector3Subtract(vPoint,m_vCylinderPos,vTemp); + ftmpdot = dVector3Dot(vTemp, m_vNormal); + fTempDepth = m_fBestrc - ftmpdot; + // Depth must be positive + if (fTempDepth > REAL(0.0)) + { + // generate contacts + dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); + Contact0->depth = fTempDepth; + dVector3Copy(m_vNormal,Contact0->normal); + dVector3Copy(vPoint,Contact0->pos); + Contact0->g1 = m_gCylinder; + Contact0->g2 = m_gBox; + Contact0->side1 = -1; + Contact0->side2 = -1; + dVector3Inv(Contact0->normal); + m_nContacts++; + + if (m_nContacts == (m_iFlags & NUMC_MASK)) + { + break; + } + } + } + } +} + +int sCylinderBoxData::PerformCollisionChecking() +{ + // initialize collider + _cldInitCylinderBox(); + + // do intersection test and find best separating axis + if ( !_cldTestSeparatingAxes() ) + { + // if not found do nothing + return 0; + } + + // if best separation axis is not found + if ( m_iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + dIASSERT(0); + // do nothing + return 0; + } + + dReal fdot = dVector3Dot(m_vNormal,m_vCylinderAxis); + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + // clip cylinder over box + if(!_cldClipCylinderToBox()) + { + return 0; + } + } + else + { + _cldClipBoxToCylinder(); + } + + return m_nContacts; +} + +// Cylinder - Box by CroTeam +// Ported by Nguyen Binh +int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCylinderClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + sCylinderBoxData cData(o1, o2, flags, contact, skip); + + return cData.PerformCollisionChecking(); +} + + diff --git a/libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp b/libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp new file mode 100644 index 0000000..67424ad --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp @@ -0,0 +1,266 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +/* + * Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de ) + * + * This testing basically comes down to testing the intersection + * of the cylinder caps (discs) with the plane. + * + */ + +#include <ode/collision.h> +#include <ode/rotation.h> +#include <ode/objects.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" // for dxGeom +#include "collision_util.h" + + +int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (Cylinder->type == dCylinderClass); + dIASSERT (Plane->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int GeomCount = 0; // count of used contactgeoms + +#ifdef dSINGLE + const dReal toleranz = REAL(0.0001); +#endif +#ifdef dDOUBLE + const dReal toleranz = REAL(0.0000001); +#endif + + // Get the properties of the cylinder (length+radius) + dReal radius, length; + dGeomCylinderGetParams(Cylinder, &radius, &length); + dVector3 &cylpos = Cylinder->final_posr->pos; + // and the plane + dVector4 planevec; + dGeomPlaneGetParams(Plane, planevec); + dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; + //dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]}; + + dVector3 G1Pos1, G1Pos2, vDir1; + vDir1[0] = Cylinder->final_posr->R[2]; + vDir1[1] = Cylinder->final_posr->R[6]; + vDir1[2] = Cylinder->final_posr->R[10]; + + dReal s; + s = length * REAL(0.5); + G1Pos2[0] = vDir1[0] * s + cylpos[0]; + G1Pos2[1] = vDir1[1] * s + cylpos[1]; + G1Pos2[2] = vDir1[2] * s + cylpos[2]; + + G1Pos1[0] = vDir1[0] * -s + cylpos[0]; + G1Pos1[1] = vDir1[1] * -s + cylpos[1]; + G1Pos1[2] = vDir1[2] * -s + cylpos[2]; + + dVector3 C; + + // parallel-check + s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2]; + if(s < 0) + s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel + else + s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel + if(s < toleranz && s > (-toleranz)) + { + // discs are parallel to the plane + + // 1.compute if, and where contacts are + dVector3 P; + s = planevec[3] - dVector3Dot(planevec, G1Pos1); + dReal t; + t = planevec[3] - dVector3Dot(planevec, G1Pos2); + if(s >= t) // s == t does never happen, + { + if(s >= 0) + { + // 1. Disc + dVector3Copy(G1Pos1, P); + } + else + return GeomCount; // no contacts + } + else + { + if(t >= 0) + { + // 2. Disc + dVector3Copy(G1Pos2, P); + } + else + return GeomCount; // no contacts + } + + // 2. generate a coordinate-system on the disc + dVector3 V1, V2; + if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) + { + // not x-axis + V1[0] = vDir1[0] + REAL(1.0); // random value + V1[1] = vDir1[1]; + V1[2] = vDir1[2]; + } + else + { + // maybe x-axis + V1[0] = vDir1[0]; + V1[1] = vDir1[1] + REAL(1.0); // random value + V1[2] = vDir1[2]; + } + // V1 is now another direction than vDir1 + // Cross-product + dVector3Cross(V1, vDir1, V2); + // make unit V2 + t = dVector3Length(V2); + t = radius / t; + dVector3Scale(V2, t); + // cross again + dVector3Cross(V2, vDir1, V1); + // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' + // V1 = first axis + // V2 = second axis + + // 3. generate contactpoints + + // Potential contact 1 + dVector3Add(P, V1, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // Potential contact 2 + dVector3Subtract(P, V1, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // Potential contact 3 + dVector3Add(P, V2, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // Potential contact 4 + dVector3Subtract(P, V2, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + } + else + { + dReal t = dVector3Dot(PlaneNormal, vDir1); + C[0] = vDir1[0] * t - PlaneNormal[0]; + C[1] = vDir1[1] * t - PlaneNormal[1]; + C[2] = vDir1[2] * t - PlaneNormal[2]; + s = dVector3Length(C); + // move C onto the circle + s = radius / s; + dVector3Scale(C, s); + + // deepest point of disc 1 + dVector3Add(C, G1Pos1, contact->pos); + + // depth of the deepest point + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth >= 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // C is still computed + + // deepest point of disc 2 + dVector3Add(C, G1Pos2, contact->pos); + + // depth of the deepest point + contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; + if(contact->depth >= 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + } + return GeomCount; +} diff --git a/libs/ode-0.16.1/ode/src/collision_cylinder_sphere.cpp b/libs/ode-0.16.1/ode/src/collision_cylinder_sphere.cpp new file mode 100644 index 0000000..4a5f6ec --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_cylinder_sphere.cpp @@ -0,0 +1,277 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +/******************************************************************* + * * + * cylinder-sphere collider by Christoph Beyer (boernerb@web.de) * + * * + * In Cylinder/Sphere-collisions, there are three possibilies: * + * 1. collision with the cylinder's nappe * + * 2. collision with one of the cylinder's disc * + * 3. collision with one of the disc's border * + * * + * This collider computes two distances (s, t) and based on them, * + * it decides, which collision we have. * + * This collider always generates 1 (or 0, if we have no collison) * + * contacts. * + * It is able to "separate" cylinder and sphere in all * + * configurations, but it never pays attention to velocity. * + * So, in extrem situations, "tunneling-effect" is possible. * + * * + *******************************************************************/ + +#include <ode/collision.h> +#include <ode/rotation.h> +#include <ode/objects.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" // for dxGeom +#include "collision_util.h" + +int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (Cylinder->type == dCylinderClass); + dIASSERT (Sphere->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + //unsigned char* pContactData = (unsigned char*)contact; + int GeomCount = 0; // count of used contacts + +#ifdef dSINGLE + const dReal toleranz = REAL(0.0001); +#endif +#ifdef dDOUBLE + const dReal toleranz = REAL(0.0000001); +#endif + + // get the data from the geoms + dReal radius, length; + dGeomCylinderGetParams(Cylinder, &radius, &length); + dVector3 &cylpos = Cylinder->final_posr->pos; + //const dReal* pfRot1 = dGeomGetRotation(Cylinder); + + dReal radius2; + radius2 = dGeomSphereGetRadius(Sphere); + const dReal* SpherePos = dGeomGetPosition(Sphere); + + // G1Pos1 is the middle of the first disc + // G1Pos2 is the middle of the second disc + // vDir1 is the unit direction of the cylinderaxis + dVector3 G1Pos1, G1Pos2, vDir1; + vDir1[0] = Cylinder->final_posr->R[2]; + vDir1[1] = Cylinder->final_posr->R[6]; + vDir1[2] = Cylinder->final_posr->R[10]; + + dReal s; + s = length * REAL(0.5); // just a precomputed factor + G1Pos2[0] = vDir1[0] * s + cylpos[0]; + G1Pos2[1] = vDir1[1] * s + cylpos[1]; + G1Pos2[2] = vDir1[2] * s + cylpos[2]; + + G1Pos1[0] = vDir1[0] * -s + cylpos[0]; + G1Pos1[1] = vDir1[1] * -s + cylpos[1]; + G1Pos1[2] = vDir1[2] * -s + cylpos[2]; + + dVector3 C; + dReal t; + // Step 1: compute the two distances 's' and 't' + // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle + s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2]; + if(s < (-radius2) || s > (length + radius2) ) + { + // Sphere is too far away from the discs + // no collision + return 0; + } + + // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis + C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0]; + C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1]; + C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2]; + // t is the distance from the Sphere-middle to the cylinder-axis! + t = dVector3Length(C); + if(t > (radius + radius2) ) + { + // Sphere is too far away from the cylinder axis! + // no collision + return 0; + } + + // decide which kind of collision we have: + if(t > radius && (s < 0 || s > length) ) + { + // 3. collision + if(s <= 0) + { + contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) ); + if(contact->depth < 0) + { + // no collision! + return 0; + } + contact->pos[0] = C[0] / t * -radius + G1Pos1[0]; + contact->pos[1] = C[1] / t * -radius + G1Pos1[1]; + contact->pos[2] = C[2] / t * -radius + G1Pos1[2]; + contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth); + contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth); + contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth); + contact->g1 = Cylinder; + contact->g2 = Sphere; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + return GeomCount; + } + else + { + // now s is bigger than length here! + contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) ); + if(contact->depth < 0) + { + // no collision! + return 0; + } + contact->pos[0] = C[0] / t * -radius + G1Pos2[0]; + contact->pos[1] = C[1] / t * -radius + G1Pos2[1]; + contact->pos[2] = C[2] / t * -radius + G1Pos2[2]; + contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth); + contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth); + contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth); + contact->g1 = Cylinder; + contact->g2 = Sphere; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + return GeomCount; + } + } + else if( (radius - t) <= s && (radius - t) <= (length - s) ) + { + // 1. collsision + if(t > (radius2 + toleranz)) + { + // cylinder-axis is outside the sphere + contact->depth = (radius2 + radius) - t; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + else + { + C[0] /= t; + C[1] /= t; + C[2] /= t; + contact->pos[0] = C[0] * radius2 + SpherePos[0]; + contact->pos[1] = C[1] * radius2 + SpherePos[1]; + contact->pos[2] = C[2] * radius2 + SpherePos[2]; + contact->normal[0] = C[0]; + contact->normal[1] = C[1]; + contact->normal[2] = C[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + return GeomCount; + } + } + else + { + // cylinder-axis is outside of the sphere + contact->depth = (radius2 + radius) - t; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + else + { + contact->pos[0] = C[0] + SpherePos[0]; + contact->pos[1] = C[1] + SpherePos[1]; + contact->pos[2] = C[2] + SpherePos[2]; + contact->normal[0] = C[0] / t; + contact->normal[1] = C[1] / t; + contact->normal[2] = C[2] / t; + contact->g1 = Cylinder; + contact->g2 = Sphere; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + return GeomCount; + } + } + } + else + { + // 2. collision + if(s <= (length * REAL(0.5)) ) + { + // collsision with the first disc + contact->depth = s + radius2; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + contact->pos[0] = radius2 * vDir1[0] + SpherePos[0]; + contact->pos[1] = radius2 * vDir1[1] + SpherePos[1]; + contact->pos[2] = radius2 * vDir1[2] + SpherePos[2]; + contact->normal[0] = vDir1[0]; + contact->normal[1] = vDir1[1]; + contact->normal[2] = vDir1[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + return GeomCount; + } + else + { + // collsision with the second disc + contact->depth = (radius2 + length - s); + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0]; + contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1]; + contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2]; + contact->normal[0] = -vDir1[0]; + contact->normal[1] = -vDir1[1]; + contact->normal[2] = -vDir1[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + contact->side1 = -1; + contact->side2 = -1; + GeomCount++; + return GeomCount; + } + } + return GeomCount; +} diff --git a/libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp b/libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp new file mode 100644 index 0000000..fd22e1a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_cylinder_trimesh.cpp @@ -0,0 +1,1171 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Cylinder-trimesh collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_util.h" +#include "collision_trimesh_internal.h" +#include "util.h" + +#if dTRIMESH_ENABLED + +#define MAX_REAL dInfinity +static const int nCYLINDER_AXIS = 2; +static const int nCYLINDER_CIRCLE_SEGMENTS = 8; +static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12; + +#define OPTIMIZE_CONTACTS 1 + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +struct sCylinderTrimeshColliderData +{ + sCylinderTrimeshColliderData(int flags, int skip): m_iFlags(flags), m_iSkip(skip), m_nContacts(0), m_gLocalContacts(NULL) {} + +#ifdef OPTIMIZE_CONTACTS + void _OptimizeLocalContacts(); +#endif + void _InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh); + int _ProcessLocalContacts(dContactGeom *contact, dxGeom *Cylinder, dxTriMesh *Trimesh); + + bool _cldTestAxis(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + dVector3& vAxis, int iAxis, bool bNoFlip = false); + bool _cldTestCircleToEdgeAxis( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis); + bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + bool _cldClipCylinderEdgeToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + void _cldClipCylinderToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + void TestOneTriangleVsCylinder(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const bool bDoubleSided); + int TestCollisionForSingleTriangle(int ctContacts0, int Triint, dVector3 dv[3], + bool &bOutFinishSearching); + + // cylinder data + dMatrix3 m_mCylinderRot; + dQuaternion m_qCylinderRot; + dQuaternion m_qInvCylinderRot; + dVector3 m_vCylinderPos; + dVector3 m_vCylinderAxis; + dReal m_fCylinderRadius; + dReal m_fCylinderSize; + dVector3 m_avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS]; + + // mesh data + dQuaternion m_qTrimeshRot; + dQuaternion m_qInvTrimeshRot; + dMatrix3 m_mTrimeshRot; + dVector3 m_vTrimeshPos; + + // global collider data + dVector3 m_vBestPoint; + dReal m_fBestDepth; + dReal m_fBestCenter; + dReal m_fBestrt; + int m_iBestAxis; + dVector3 m_vContactNormal; + dVector3 m_vNormal; + dVector3 m_vE0; + dVector3 m_vE1; + dVector3 m_vE2; + + // ODE stuff + int m_iFlags; + int m_iSkip; + int m_nContacts;// = 0; + sLocalContactData* m_gLocalContacts; +}; + + +#ifdef OPTIMIZE_CONTACTS + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + dVector3Subtract(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + dVector3Subtract(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +void sCylinderTrimeshColliderData::_OptimizeLocalContacts() +{ + int nContacts = m_nContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(m_gLocalContacts[i],m_gLocalContacts[j])) + { + // If they are seem to be the same then filtered + // out the least penetrate one + if (_IsBetter(m_gLocalContacts[j],m_gLocalContacts[i])) + { + m_gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + m_gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} +#endif // OPTIMIZE_CONTACTS + +int sCylinderTrimeshColliderData::_ProcessLocalContacts(dContactGeom *contact, + dxGeom *Cylinder, dxTriMesh *Trimesh) +{ +#ifdef OPTIMIZE_CONTACTS + if (m_nContacts > 1 && !(m_iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + int iContact = 0; + dContactGeom* Contact = 0; + + int nFinalContact = 0; + + for (iContact = 0; iContact < m_nContacts; iContact ++) + { + if (1 == m_gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(m_iFlags, contact, nFinalContact, m_iSkip); + Contact->depth = m_gLocalContacts[iContact].fDepth; + dVector3Copy(m_gLocalContacts[iContact].vNormal,Contact->normal); + dVector3Copy(m_gLocalContacts[iContact].vPos,Contact->pos); + Contact->g1 = Cylinder; + Contact->g2 = Trimesh; + Contact->side1 = -1; + Contact->side2 = m_gLocalContacts[iContact].triIndex; + dVector3Inv(Contact->normal); + + nFinalContact++; + } + } + // debug + //if (nFinalContact != m_nContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",m_nContacts,m_nContacts-nFinalContact); + //} + + return nFinalContact; +} + + +bool sCylinderTrimeshColliderData::_cldTestAxis( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + dVector3& vAxis, + int iAxis, + bool bNoFlip/* = false*/) +{ + + // calculate length of separating axis vector + dReal fL = dVector3Length(vAxis); + // if not long enough + if ( fL < REAL(1e-5) ) + { + // do nothing + return true; + } + + // otherwise normalize it + vAxis[0] /= fL; + vAxis[1] /= fL; + vAxis[2] /= fL; + + dReal fdot1 = dVector3Dot(m_vCylinderAxis,vAxis); + // project capsule on vAxis + dReal frc; + + if (dFabs(fdot1) > REAL(1.0) ) + { + // fdot1 = REAL(1.0); + frc = dFabs(m_fCylinderSize* REAL(0.5)); + } + else + { + frc = dFabs((m_fCylinderSize* REAL(0.5)) * fdot1) + + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); + } + + dVector3 vV0; + dVector3Subtract(v0,m_vCylinderPos,vV0); + dVector3 vV1; + dVector3Subtract(v1,m_vCylinderPos,vV1); + dVector3 vV2; + dVector3Subtract(v2,m_vCylinderPos,vV2); + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dVector3Dot( vV0 , vAxis ); + afv[1] = dVector3Dot( vV1 , vAxis ); + afv[2] = dVector3Dot( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = -MAX_REAL; + + // for each vertex + for(int i = 0; i < 3; i++) + { + // find minimum + if (afv[i]<fMin) + { + fMin = afv[i]; + } + // find maximum + if (afv[i]>fMax) + { + fMax = afv[i]; + } + } + + // find capsule's center of interval on axis + dReal fCenter = (fMin+fMax)* REAL(0.5); + // calculate triangles halfinterval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if( dFabs(fCenter) > (frc+fTriangleRadius) ) + { + // exit, we have no intersection + return false; + } + + // calculate depth + dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) ); + + // if greater then best found so far + if ( fDepth < m_fBestDepth ) + { + // remember depth + m_fBestDepth = fDepth; + m_fBestCenter = fCenter; + m_fBestrt = frc; + dVector3Copy(vAxis,m_vContactNormal); + m_iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if ( fCenter< REAL(0.0) && !bNoFlip) + { + dVector3Inv(m_vContactNormal); + m_fBestCenter = -fCenter; + } + } + + return true; +} + +// intersection test between edge and circle +bool sCylinderTrimeshColliderData::_cldTestCircleToEdgeAxis( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) +{ + // calculate direction of edge + dVector3 vkl; + dVector3Subtract( vVx1 , vVx0 , vkl); + dNormalize3(vkl); + // starting point of edge + dVector3 vol; + dVector3Copy(vVx0,vol); + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2)<REAL(1e-5)) + { + // this can't be separating axis, because edge is parallel to circle plane + return true; + } + + // find point of intersection between edge line and circle plane + dVector3 vTemp; + dVector3Subtract(vCenterPoint,vol,vTemp); + dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1); + dVector3 vpnt;// = vol + vkl * (fdot1/fdot2); + vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2; + vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2; + vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2; + + // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt) + dVector3 vTangent; + dVector3Subtract(vCenterPoint,vpnt,vTemp); + dVector3Cross(vTemp,vCylinderAxis1,vTangent); + + // find vector orthogonal both to tangent and edge direction + dVector3 vAxis; + dVector3Cross(vTangent,vkl,vAxis); + + // use that vector as separating axis + return _cldTestAxis( v0, v1, v2, vAxis, iAxis ); +} + +// helper for less key strokes +// r = ( (v1 - v2) cross v3 ) cross v3 +inline void _CalculateAxis(const dVector3& v1, + const dVector3& v2, + const dVector3& v3, + dVector3& r) +{ + dVector3 t1; + dVector3 t2; + + dVector3Subtract(v1,v2,t1); + dVector3Cross(t1,v3,t2); + dVector3Cross(t2,v3,r); +} + +bool sCylinderTrimeshColliderData::_cldTestSeparatingAxes( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2) +{ + + // calculate edge vectors + dVector3Subtract(v1 ,v0 , m_vE0); + // m_vE1 has been calculated before -> so save some cycles here + dVector3Subtract(v0 ,v2 , m_vE2); + + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); + vCp0[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); + vCp0[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); + +#if 0 + dVector3 vCp1; + vCp1[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); + vCp1[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); + vCp1[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); +#endif + + // reset best axis + m_iBestAxis = 0; + dVector3 vAxis; + + // axis m_vNormal + //vAxis = -m_vNormal; + vAxis[0] = -m_vNormal[0]; + vAxis[1] = -m_vNormal[1]; + vAxis[2] = -m_vNormal[2]; + if (!_cldTestAxis(v0, v1, v2, vAxis, 1, true)) + { + return false; + } + + // axis CxE0 + // vAxis = ( m_vCylinderAxis cross m_vE0 ); + dVector3Cross(m_vCylinderAxis, m_vE0,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) + { + return false; + } + + // axis CxE1 + // vAxis = ( m_vCylinderAxis cross m_vE1 ); + dVector3Cross(m_vCylinderAxis, m_vE1,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) + { + return false; + } + + // axis CxE2 + // vAxis = ( m_vCylinderAxis cross m_vE2 ); + dVector3Cross(m_vCylinderAxis, m_vE2,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) + { + return false; + } + + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + //vAxis = ( ( v0-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v0 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) + { + return false; + } + + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + // vAxis = ( ( v1-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v1 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) + { + return false; + } + + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + //vAxis = ( ( v2-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v2 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 13)) + { + return false; + } + + // test cylinder axis + // vAxis = m_vCylinderAxis; + dVector3Copy(m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) + { + return false; + } + + // Test top and bottom circle ring of cylinder for separation + dVector3 vccATop; + vccATop[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); + vccATop[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); + vccATop[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); + + dVector3 vccABottom; + vccABottom[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); + vccABottom[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); + vccABottom[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); + + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v1, 15)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v1, v2, 16)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v2, 17)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v1, 18)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v1, v2, 19)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v2, 20)) + { + return false; + } + + return true; +} + +bool sCylinderTrimeshColliderData::_cldClipCylinderEdgeToTriangle( + const dVector3 &v0, const dVector3 &/*v1*/, const dVector3 &/*v2*/) +{ + // translate cylinder + dReal fTemp = dVector3Dot(m_vCylinderAxis , m_vContactNormal); + dVector3 vN2; + vN2[0] = m_vContactNormal[0] - m_vCylinderAxis[0]*fTemp; + vN2[1] = m_vContactNormal[1] - m_vCylinderAxis[1]*fTemp; + vN2[2] = m_vContactNormal[2] - m_vCylinderAxis[2]*fTemp; + + fTemp = dVector3Length(vN2); + if (fTemp < REAL(1e-5)) + { + return false; + } + + // Normalize it + vN2[0] /= fTemp; + vN2[1] /= fTemp; + vN2[2] /= fTemp; + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = m_vCylinderPos[0] + vN2[0]*m_fCylinderRadius; + vCposTrans[1] = m_vCylinderPos[1] + vN2[1]*m_fCylinderRadius; + vCposTrans[2] = m_vCylinderPos[2] + vN2[2]*m_fCylinderRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint0[1] = vCposTrans[1] + m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint0[2] = vCposTrans[2] + m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint1[1] = vCposTrans[1] - m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint1[2] = vCposTrans[2] - m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); + + // transform cylinder edge points into triangle space + vCEdgePoint0[0] -= v0[0]; + vCEdgePoint0[1] -= v0[1]; + vCEdgePoint0[2] -= v0[2]; + + vCEdgePoint1[0] -= v0[0]; + vCEdgePoint1[1] -= v0[1]; + vCEdgePoint1[2] -= v0[2]; + + dVector4 plPlane; + dVector3 vPlaneNormal; + + // triangle plane + //plPlane = Plane4f( -m_vNormal, 0); + vPlaneNormal[0] = -m_vNormal[0]; + vPlaneNormal[1] = -m_vNormal[1]; + vPlaneNormal[2] = -m_vNormal[2]; + dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 0 + //plPlane = Plane4f( ( m_vNormal cross m_vE0 ), REAL(1e-5)); + dVector3Cross(m_vNormal,m_vE0,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 1 + //dVector3 vTemp = ( m_vNormal cross m_vE1 ); + dVector3Cross(m_vNormal,m_vE1,vPlaneNormal); + fTemp = dVector3Dot(m_vE0 , vPlaneNormal) - REAL(1e-5); + //plPlane = Plane4f( vTemp, -(( m_vE0 dot vTemp )-REAL(1e-5))); + dConstructPlane(vPlaneNormal,-fTemp,plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 2 + // plPlane = Plane4f( ( m_vNormal cross m_vE2 ), REAL(1e-5)); + dVector3Cross(m_vNormal,m_vE2,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += v0[0]; + vCEdgePoint0[1] += v0[1]; + vCEdgePoint0[2] += v0[2]; + + vCEdgePoint1[0] += v0[0]; + vCEdgePoint1[1] += v0[1]; + vCEdgePoint1[2] += v0[2]; + + // calculate depths for both contact points + dVector3 vTemp; + dVector3Subtract(vCEdgePoint0,m_vCylinderPos, vTemp); + dReal fRestDepth0 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; + dVector3Subtract(vCEdgePoint1,m_vCylinderPos, vTemp); + dReal fRestDepth1 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; + + dReal fDepth0 = m_fBestDepth - (fRestDepth0); + dReal fDepth1 = m_fBestDepth - (fRestDepth1); + + // clamp depths to zero + if(fDepth0 < REAL(0.0) ) + { + fDepth0 = REAL(0.0); + } + + if(fDepth1<REAL(0.0)) + { + fDepth1 = REAL(0.0); + } + + // Generate contact 0 + { + m_gLocalContacts[m_nContacts].fDepth = fDepth0; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vCEdgePoint0,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return true; + } + + // Generate contact 1 + { + // generate contacts + m_gLocalContacts[m_nContacts].fDepth = fDepth1; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vCEdgePoint1,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + } + + return true; +} + +void sCylinderTrimeshColliderData::_cldClipCylinderToTriangle( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) +{ + int i = 0; + dVector3 avPoints[3]; + dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + + dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + + // setup array of triangle vertices + dVector3Copy(v0,avPoints[0]); + dVector3Copy(v1,avPoints[1]); + dVector3Copy(v2,avPoints[2]); + + dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; + dSetZero(vCylinderCircleNormal_Rel,4); + // check which circle from cylinder we take for clipping + if ( dVector3Dot(m_vCylinderAxis , m_vContactNormal) > REAL(0.0)) + { + // get top circle + vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + dVector3 vTemp; + dQuatInv(m_qCylinderRot , m_qInvCylinderRot); + // transform triangle points to space of cylinder circle + for(i=0; i<3; i++) + { + dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); + dQuatTransform(m_qInvCylinderRot,vTemp,avPoints[i]); + } + + int iTmpCounter1 = 0; + int iTmpCounter2 = 0; + dVector4 plPlane; + + // plane of cylinder that contains circle for intersection + //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f ); + dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); + dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane); + + // Body of base circle of Cylinder + int nCircleSegment = 0; + for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++) + { + dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane); + + if (0 == (nCircleSegment % 2)) + { + dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); + } + else + { + dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); + } + + dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + if (nCircleSegment %2) + { + for( i=0; i<iTmpCounter2; i++) + { + dQuatTransform(m_qCylinderRot,avTempArray2[i], vPoint); + vPoint[0] += vCylinderCirclePos[0]; + vPoint[1] += vCylinderCirclePos[1]; + vPoint[2] += vCylinderCirclePos[2]; + + dVector3Subtract(vPoint,m_vCylinderPos,vTemp); + ftmpdot = dFabs(dVector3Dot(vTemp, m_vContactNormal)); + fTempDepth = m_fBestrt - ftmpdot; + // Depth must be positive + if (fTempDepth > REAL(0.0)) + { + m_gLocalContacts[m_nContacts].fDepth = fTempDepth; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return;; + } + } + } + else + { + for( i=0; i<iTmpCounter1; i++) + { + dQuatTransform(m_qCylinderRot,avTempArray1[i], vPoint); + vPoint[0] += vCylinderCirclePos[0]; + vPoint[1] += vCylinderCirclePos[1]; + vPoint[2] += vCylinderCirclePos[2]; + + dVector3Subtract(vPoint,m_vCylinderPos,vTemp); + ftmpdot = dFabs(dVector3Dot(vTemp, m_vContactNormal)); + fTempDepth = m_fBestrt - ftmpdot; + // Depth must be positive + if (fTempDepth > REAL(0.0)) + { + m_gLocalContacts[m_nContacts].fDepth = fTempDepth; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return;; + } + } + } +} + +void sCylinderTrimeshColliderData::TestOneTriangleVsCylinder( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + const bool bDoubleSided) +{ + // calculate triangle normal + dVector3Subtract( v2 , v1 , m_vE1); + dVector3 vTemp; + dVector3Subtract( v0 , v1 ,vTemp); + dVector3Cross(m_vE1 , vTemp , m_vNormal ); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (!dSafeNormalize3( m_vNormal)) + { + return; + } + + // create plane from triangle + //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); + dReal plDistance = -dVector3Dot(v0, m_vNormal); + dVector4 plTrianglePlane; + dConstructPlane( m_vNormal,plDistance,plTrianglePlane); + + // calculate sphere distance to plane + dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(m_vCylinderPos , plTrianglePlane); + + // Sphere must be over positive side of triangle + if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + dVector3 vPnt1; + dVector3 vPnt2; + + if (fDistanceCylinderCenterToPlane < REAL(0.0) ) + { + // flip it + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt2); + dVector3Copy(v2 , vPnt1); + } + else + { + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt1); + dVector3Copy(v2 , vPnt2); + } + + m_fBestDepth = MAX_REAL; + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes(vPnt0, vPnt1, vPnt2) ) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( m_iBestAxis == 0 ) + { + // this should not happen (the function should have already returned in this case) + dIASSERT(false); + // do nothing + return; + } + + dReal fdot = dVector3Dot( m_vContactNormal , m_vCylinderAxis ); + + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + if (!_cldClipCylinderEdgeToTriangle(vPnt0, vPnt1, vPnt2)) + { + return; + } + } + else + { + _cldClipCylinderToTriangle(vPnt0, vPnt1, vPnt2); + } +} + +void sCylinderTrimeshColliderData::_InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh) +{ + // get cylinder information + // Rotation + const dReal* pRotCyc = dGeomGetRotation(Cylinder); + dMatrix3Copy(pRotCyc,m_mCylinderRot); + dGeomGetQuaternion(Cylinder,m_qCylinderRot); + + // Position + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(Cylinder); + dVector3Copy(*pPosCyc,m_vCylinderPos); + // Cylinder axis + dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis); + // get cylinder radius and size + dGeomCylinderGetParams(Cylinder,&m_fCylinderRadius,&m_fCylinderSize); + + // get trimesh position and orientation + const dReal* pRotTris = dGeomGetRotation(Trimesh); + dMatrix3Copy(pRotTris,m_mTrimeshRot); + dGeomGetQuaternion(Trimesh,m_qTrimeshRot); + + // Position + const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(Trimesh); + dVector3Copy(*pPosTris,m_vTrimeshPos); + + + // calculate basic angle for 8-gon + dReal fAngle = (dReal) (M_PI / nCYLINDER_CIRCLE_SEGMENTS); + // calculate angle increment + dReal fAngleIncrement = fAngle*REAL(2.0); + + // calculate plane normals + // axis dependant code + for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++) + { + m_avCylinderNormals[i][0] = -dCos(fAngle); + m_avCylinderNormals[i][1] = -dSin(fAngle); + m_avCylinderNormals[i][2] = REAL(0.0); + + fAngle += fAngleIncrement; + } + + dSetZero(m_vBestPoint,4); + // reset best depth + m_fBestCenter = REAL(0.0); +} + +int sCylinderTrimeshColliderData::TestCollisionForSingleTriangle(int ctContacts0, + int Triint, dVector3 dv[3], bool &bOutFinishSearching) +{ + // test this triangle + TestOneTriangleVsCylinder(dv[0],dv[1],dv[2], false); + + // fill-in tri index for generated contacts + for (; ctContacts0<m_nContacts; ctContacts0++) + m_gLocalContacts[ctContacts0].triIndex = Triint; + + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + bOutFinishSearching = (m_nContacts >= (m_iFlags & NUMC_MASK)); + + return ctContacts0; +} + +// OPCODE version of cylinder to mesh collider +#if dTRIMESH_OPCODE +static void dQueryCTLPotentialCollisionTriangles(OBBCollider &Collider, + sCylinderTrimeshColliderData &cData, dxGeom *Cylinder, dxTriMesh *Trimesh, + OBBCache &BoxCache) +{ + Matrix4x4 MeshMatrix; + const dVector3 vZeroVector3 = { REAL(0.0), }; + MakeMatrix(vZeroVector3, cData.m_mTrimeshRot, MeshMatrix); + + const dVector3 &vCylinderPos = cData.m_vCylinderPos; + const dMatrix3 &mCylinderRot = cData.m_mCylinderRot; + + dVector3 vCylinderOffsetPos; + dSubtractVectors3(vCylinderOffsetPos, vCylinderPos, cData.m_vTrimeshPos); + + const dReal fCylinderRadius = cData.m_fCylinderRadius, fCylinderHalfAxis = cData.m_fCylinderSize * REAL(0.5); + + OBB obbCylinder; + obbCylinder.mCenter.Set(vCylinderOffsetPos[0], vCylinderOffsetPos[1], vCylinderOffsetPos[2]); + obbCylinder.mExtents.Set( + 0 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, + 1 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, + 2 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius); + obbCylinder.mRot.Set( + mCylinderRot[0], mCylinderRot[4], mCylinderRot[8], + mCylinderRot[1], mCylinderRot[5], mCylinderRot[9], + mCylinderRot[2], mCylinderRot[6], mCylinderRot[10]); + + // TC results + if (Trimesh->getDoTC(dxTriMesh::TTC_BOX)) + { + dxTriMesh::BoxTC* BoxTC = 0; + const int iBoxCacheSize = Trimesh->m_BoxTCCache.size(); + for (int i = 0; i != iBoxCacheSize; i++) + { + if (Trimesh->m_BoxTCCache[i].Geom == Cylinder) + { + BoxTC = &Trimesh->m_BoxTCCache[i]; + break; + } + } + if (!BoxTC) + { + Trimesh->m_BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &Trimesh->m_BoxTCCache[Trimesh->m_BoxTCCache.size() - 1]; + BoxTC->Geom = Cylinder; + BoxTC->FatCoeff = REAL(1.0); + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + else + { + Collider.SetTemporalCoherence(false); + Collider.Collide(BoxCache, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } +} + +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int nContactCount = 0; + + dxGeom *Cylinder = o1; + dxTriMesh *Trimesh = (dxTriMesh *)o2; + + // Main data holder + sCylinderTrimeshColliderData cData(flags, skip); + cData._InitCylinderTrimeshData(Cylinder, Trimesh); + + const unsigned uiTLSKind = Trimesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == Cylinder->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + OBBCollider& Collider = pccColliderCache->m_OBBCollider; + + dQueryCTLPotentialCollisionTriangles(Collider, cData, Cylinder, Trimesh, pccColliderCache->m_DefaultBoxCache); + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + + if (TriCount != 0) + { + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (Trimesh->m_ArrayCallback != NULL) + { + Trimesh->m_ArrayCallback(Trimesh, Cylinder, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + int ctContacts0 = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!Trimesh->invokeCallback(Cylinder, Triint)) continue; + + + dVector3 dv[3]; + Trimesh->fetchMeshTriangle(dv, Triint, cData.m_vTrimeshPos, cData.m_mTrimeshRot); + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + if (cData.m_nContacts != 0) + { + nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh); + } + } + + return nContactCount; +} +#endif + +// GIMPACT version of cylinder to mesh collider +#if dTRIMESH_GIMPACT +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int nContactCount = 0; + + dxGeom *Cylinder = o1; + dxTriMesh *Trimesh = (dxTriMesh *)o2; + + // Main data holder + sCylinderTrimeshColliderData cData(flags, skip); + cData._InitCylinderTrimeshData(Cylinder, Trimesh); + + //*****at first , collide box aabb******// + + aabb3f test_aabb(o1->aabb[0], o1->aabb[1], o1->aabb[2], o1->aabb[3], o1->aabb[4], o1->aabb[5]); + + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_BOXQUERY_LIST(collision_result); + + gim_aabbset_box_collision(&test_aabb, &Trimesh->m_collision_trimesh.m_aabbset , &collision_result); + + if (collision_result.m_size != 0) + { + //*****Set globals for box collision******// + + int ctContacts0 = 0; + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result); + GIM_TRIMESH * ptrimesh = &Trimesh->m_collision_trimesh; + + gim_trimesh_locks_work_data(ptrimesh); + + for(unsigned int i=0;i<collision_result.m_size;i++) + { + const int Triint = boxesresult[i]; + + dVector3 dv[3]; + gim_trimesh_get_triangle_vertices(ptrimesh, Triint, dv[0], dv[1], dv[2]); + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + gim_trimesh_unlocks_work_data(ptrimesh); + + if (cData.m_nContacts != 0) + { + nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh); + } + } + + GIM_DYNARRAY_DESTROY(collision_result); + + return nContactCount; +} +#endif + +#endif // dTRIMESH_ENABLED + + diff --git a/libs/ode-0.16.1/ode/src/collision_kernel.cpp b/libs/ode-0.16.1/ode/src/collision_kernel.cpp new file mode 100644 index 0000000..527941a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_kernel.cpp @@ -0,0 +1,1247 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +core collision functions and data structures, plus part of the public API +for geometry objects + +*/ + +#include <ode/common.h> +#include <ode/rotation.h> +#include <ode/objects.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_util.h" +#include "collision_std.h" +#include "collision_transform.h" +#include "collision_trimesh_internal.h" +#include "collision_space_internal.h" +#include "odeou.h" + +#ifdef dLIBCCD_ENABLED +# include "collision_libccd.h" +#endif /* dLIBCCD_ENABLED */ + + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// helper functions for dCollide()ing a space with another geom + +// this struct records the parameters passed to dCollideSpaceGeom() + +#if dATOMICS_ENABLED +static volatile atomicptr s_cachedPosR = 0; // dxPosR * +#endif // dATOMICS_ENABLED + +static inline dxPosR* dAllocPosr() +{ + dxPosR *retPosR; + +#if dATOMICS_ENABLED + retPosR = (dxPosR *)AtomicExchangePointer(&s_cachedPosR, NULL); + + if (!retPosR) +#endif + { + retPosR = (dxPosR*) dAlloc (sizeof(dxPosR)); + } + + return retPosR; +} + +static inline void dFreePosr(dxPosR *oldPosR) +{ +#if dATOMICS_ENABLED + if (!AtomicCompareExchangePointer(&s_cachedPosR, NULL, (atomicptr)oldPosR)) +#endif + { + dFree(oldPosR, sizeof(dxPosR)); + } +} + +/*extern */void dClearPosrCache(void) +{ +#if dATOMICS_ENABLED + // No threads should be accessing ODE at this time already, + // hence variable may be read directly. + dxPosR *existingPosR = (dxPosR *)s_cachedPosR; + + if (existingPosR) + { + dFree(existingPosR, sizeof(dxPosR)); + + s_cachedPosR = 0; + } +#endif +} + +struct SpaceGeomColliderData { + int flags; // space left in contacts array + dContactGeom *contact; + int skip; +}; + + +static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2) +{ + SpaceGeomColliderData *d = (SpaceGeomColliderData*) data; + if (d->flags & NUMC_MASK) { + int n = dCollide (o1,o2,d->flags,d->contact,d->skip); + d->contact = CONTACT (d->contact,d->skip*n); + d->flags -= n; + } +} + + +static int dCollideSpaceGeom (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + SpaceGeomColliderData data; + data.flags = flags; + data.contact = contact; + data.skip = skip; + dSpaceCollide2 (o1,o2,&data,&space_geom_collider); + return (flags & NUMC_MASK) - (data.flags & NUMC_MASK); +} + +//**************************************************************************** +// dispatcher for the N^2 collider functions + +// function pointers and modes for n^2 class collider functions + +struct dColliderEntry { + dColliderFn *fn; // collider function, 0 = no function available + int reverse; // 1 = reverse o1 and o2 +}; +static dColliderEntry colliders[dGeomNumClasses][dGeomNumClasses]; +static int colliders_initialized = 0; + + +// setCollider() will refuse to write over a collider entry once it has +// been written. + +static void setCollider (int i, int j, dColliderFn *fn) +{ + if (colliders[i][j].fn == 0) { + colliders[i][j].fn = fn; + colliders[i][j].reverse = 0; + } + if (colliders[j][i].fn == 0) { + colliders[j][i].fn = fn; + colliders[j][i].reverse = 1; + } +} + + +static void setAllColliders (int i, dColliderFn *fn) +{ + for (int j=0; j<dGeomNumClasses; j++) setCollider (i,j,fn); +} + +/*extern */void dInitColliders() +{ + dIASSERT(!colliders_initialized); + colliders_initialized = 1; + + memset (colliders,0,sizeof(colliders)); + + int i,j; + + // setup space colliders + for (i=dFirstSpaceClass; i <= dLastSpaceClass; i++) { + for (j=0; j < dGeomNumClasses; j++) { + setCollider (i,j,&dCollideSpaceGeom); + } + } + + setCollider (dSphereClass,dSphereClass,&dCollideSphereSphere); + setCollider (dSphereClass,dBoxClass,&dCollideSphereBox); + setCollider (dSphereClass,dPlaneClass,&dCollideSpherePlane); + setCollider (dBoxClass,dBoxClass,&dCollideBoxBox); + setCollider (dBoxClass,dPlaneClass,&dCollideBoxPlane); + setCollider (dCapsuleClass,dSphereClass,&dCollideCapsuleSphere); + setCollider (dCapsuleClass,dBoxClass,&dCollideCapsuleBox); + setCollider (dCapsuleClass,dCapsuleClass,&dCollideCapsuleCapsule); + setCollider (dCapsuleClass,dPlaneClass,&dCollideCapsulePlane); + setCollider (dRayClass,dSphereClass,&dCollideRaySphere); + setCollider (dRayClass,dBoxClass,&dCollideRayBox); + setCollider (dRayClass,dCapsuleClass,&dCollideRayCapsule); + setCollider (dRayClass,dPlaneClass,&dCollideRayPlane); + setCollider (dRayClass,dCylinderClass,&dCollideRayCylinder); +#if dTRIMESH_ENABLED + setCollider (dTriMeshClass,dSphereClass,&dCollideSTL); + setCollider (dTriMeshClass,dBoxClass,&dCollideBTL); + setCollider (dTriMeshClass,dRayClass,&dCollideRTL); + setCollider (dTriMeshClass,dTriMeshClass,&dCollideTTL); + setCollider (dTriMeshClass,dCapsuleClass,&dCollideCCTL); + setCollider (dTriMeshClass,dPlaneClass,&dCollideTrimeshPlane); + setCollider (dCylinderClass,dTriMeshClass,&dCollideCylinderTrimesh); + setCollider (dConvexClass,dTriMeshClass,&dCollideConvexTrimesh); +#endif + +#ifdef dLIBCCD_BOX_CYL + setCollider (dBoxClass,dCylinderClass,&dCollideBoxCylinderCCD); +#else + setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox); +#endif + setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere); + setCollider (dCylinderClass,dPlaneClass,&dCollideCylinderPlane); + +#ifdef dLIBCCD_CYL_CYL + setCollider (dCylinderClass, dCylinderClass, &dCollideCylinderCylinder); +#endif +#ifdef dLIBCCD_CAP_CYL + setCollider (dCapsuleClass, dCylinderClass, &dCollideCapsuleCylinder); +#endif + + //--> Convex Collision +#ifdef dLIBCCD_CONVEX_BOX + setCollider (dConvexClass, dBoxClass, &dCollideConvexBoxCCD); +#else + setCollider (dConvexClass,dBoxClass,&dCollideConvexBox); +#endif + +#ifdef dLIBCCD_CONVEX_CAP + setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsuleCCD); +#else + setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsule); +#endif + +#ifdef dLIBCCD_CONVEX_CYL + setCollider (dConvexClass,dCylinderClass,&dCollideConvexCylinderCCD); +#endif + +#ifdef dLIBCCD_CONVEX_SPHERE + setCollider (dConvexClass,dSphereClass,&dCollideConvexSphereCCD); +#else + setCollider (dSphereClass,dConvexClass,&dCollideSphereConvex); +#endif + +#ifdef dLIBCCD_CONVEX_CONVEX + setCollider (dConvexClass,dConvexClass,&dCollideConvexConvexCCD); +#else + setCollider (dConvexClass,dConvexClass,&dCollideConvexConvex); +#endif + + setCollider (dConvexClass,dPlaneClass,&dCollideConvexPlane); + setCollider (dRayClass,dConvexClass,&dCollideRayConvex); + //<-- Convex Collision + + //--> dHeightfield Collision + setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dCapsuleClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dCylinderClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dConvexClass,&dCollideHeightfield); +#if dTRIMESH_ENABLED + setCollider (dHeightfieldClass,dTriMeshClass,&dCollideHeightfield); +#endif + //<-- dHeightfield Collision + + setAllColliders (dGeomTransformClass,&dCollideTransform); +} + +/*extern */void dFinitColliders() +{ + colliders_initialized = 0; +} + +void dSetColliderOverride (int i, int j, dColliderFn *fn) +{ + dIASSERT( colliders_initialized ); + dAASSERT( i < dGeomNumClasses ); + dAASSERT( j < dGeomNumClasses ); + + colliders[i][j].fn = fn; + colliders[i][j].reverse = 0; + colliders[j][i].fn = fn; + colliders[j][i].reverse = 1; +} + +/* +* NOTE! +* If it is necessary to add special processing mode without contact generation +* use NULL contact parameter value as indicator, not zero in flags. +*/ +int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dAASSERT(o1 && o2 && contact); + dUASSERT(colliders_initialized,"Please call ODE initialization (dInitODE() or similar) before using the library"); + dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number"); + dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number"); + // Even though comparison for greater or equal to one is used in all the + // other places, here it is more logical to check for greater than zero + // because function does not require any specific number of contact slots - + // it must be just a positive. + dUASSERT((flags & NUMC_MASK) > 0, "no contacts requested"); + + // Extra precaution for zero contact count in parameters + if ((flags & NUMC_MASK) == 0) return 0; + // no contacts if both geoms are the same + if (o1 == o2) return 0; + + // no contacts if both geoms on the same body, and the body is not 0 + if (o1->body == o2->body && o1->body) return 0; + + o1->recomputePosr(); + o2->recomputePosr(); + + dColliderEntry *ce = &colliders[o1->type][o2->type]; + int count = 0; + if (ce->fn) { + if (ce->reverse) { + count = (*ce->fn) (o2,o1,flags,contact,skip); + for (int i=0; i<count; i++) { + dContactGeom *c = CONTACT(contact,skip*i); + c->normal[0] = -c->normal[0]; + c->normal[1] = -c->normal[1]; + c->normal[2] = -c->normal[2]; + dxGeom *tmp = c->g1; + c->g1 = c->g2; + c->g2 = tmp; + int tmpint = c->side1; + c->side1 = c->side2; + c->side2 = tmpint; + } + } + else { + count = (*ce->fn) (o1,o2,flags,contact,skip); + } + } + return count; +} + +//**************************************************************************** +// dxGeom + +dxGeom::dxGeom (dSpaceID _space, int is_placeable) +{ + // setup body vars. invalid type of -1 must be changed by the constructor. + type = -1; + gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED; + if (is_placeable) gflags |= GEOM_PLACEABLE; + data = 0; + body = 0; + body_next = 0; + if (is_placeable) { + final_posr = dAllocPosr(); + dSetZero (final_posr->pos,4); + dRSetIdentity (final_posr->R); + } + else { + final_posr = 0; + } + offset_posr = 0; + + // setup space vars + next = 0; + tome = 0; + next_ex = 0; + tome_ex = 0; + parent_space = 0; + dSetZero (aabb,6); + category_bits = ~0; + collide_bits = ~0; + + // put this geom in a space if required + if (_space) dSpaceAdd (_space,this); +} + + +dxGeom::~dxGeom() +{ + if (parent_space) dSpaceRemove (parent_space,this); + if ((gflags & GEOM_PLACEABLE) && (!body || (body && offset_posr))) + dFreePosr(final_posr); + if (offset_posr) dFreePosr(offset_posr); + bodyRemove(); +} + +unsigned dxGeom::getParentSpaceTLSKind() const +{ + return parent_space ? parent_space->tls_kind : dSPACE_TLS_KIND_INIT_VALUE; +} + +int dxGeom::AABBTest (dxGeom *, dReal [6]) +{ + return 1; +} + + +void dxGeom::bodyRemove() +{ + if (body) { + // delete this geom from body list + dxGeom **last = &body->geom, *g = body->geom; + while (g) { + if (g == this) { + *last = g->body_next; + break; + } + last = &g->body_next; + g = g->body_next; + } + body = 0; + body_next = 0; + } +} + +inline void myswap(dReal& a, dReal& b) { dReal t=b; b=a; a=t; } + + +inline void matrixInvert(const dMatrix3& inMat, dMatrix3& outMat) +{ + memcpy(outMat, inMat, sizeof(dMatrix3)); + // swap _12 and _21 + myswap(outMat[0 + 4*1], outMat[1 + 4*0]); + // swap _31 and _13 + myswap(outMat[2 + 4*0], outMat[0 + 4*2]); + // swap _23 and _32 + myswap(outMat[1 + 4*2], outMat[2 + 4*1]); +} + +void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr) +{ + dMatrix3 inv_offset; + matrixInvert(offset_posr.R, inv_offset); + + dMultiply0_333(body_posr.R, final_posr.R, inv_offset); + dVector3 world_offset; + dMultiply0_331(world_offset, body_posr.R, offset_posr.pos); + body_posr.pos[0] = final_posr.pos[0] - world_offset[0]; + body_posr.pos[1] = final_posr.pos[1] - world_offset[1]; + body_posr.pos[2] = final_posr.pos[2] - world_offset[2]; +} + +void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr) +{ + dMatrix3 inv_body; + matrixInvert(body_posr.R, inv_body); + + dMultiply0_333(offset_posr.R, inv_body, world_posr.R); + dVector3 world_offset; + world_offset[0] = world_posr.pos[0] - body_posr.pos[0]; + world_offset[1] = world_posr.pos[1] - body_posr.pos[1]; + world_offset[2] = world_posr.pos[2] - body_posr.pos[2]; + dMultiply0_331(offset_posr.pos, inv_body, world_offset); +} + +void dxGeom::computePosr() +{ + // should only be recalced if we need to - ie offset from a body + dIASSERT(offset_posr); + dIASSERT(body); + + dMultiply0_331 (final_posr->pos,body->posr.R,offset_posr->pos); + final_posr->pos[0] += body->posr.pos[0]; + final_posr->pos[1] += body->posr.pos[1]; + final_posr->pos[2] += body->posr.pos[2]; + dMultiply0_333 (final_posr->R,body->posr.R,offset_posr->R); +} + +bool dxGeom::controlGeometry(int /*controlClass*/, int /*controlCode*/, void * /*dataValue*/, int *dataSize) +{ + dAASSERT(false && "Control class/code is not supported for current geom"); + + *dataSize = 0; + return false; +} + +//**************************************************************************** +// misc + +dxGeom *dGeomGetBodyNext (dxGeom *geom) +{ + return geom->body_next; +} + +//**************************************************************************** +// public API for geometry objects + +void dGeomDestroy (dxGeom *g) +{ + dAASSERT (g); + delete g; +} + + +void dGeomSetData (dxGeom *g, void *data) +{ + dAASSERT (g); + g->data = data; +} + + +void *dGeomGetData (dxGeom *g) +{ + dAASSERT (g); + return g->data; +} + + +void dGeomSetBody (dxGeom *g, dxBody *b) +{ + dAASSERT (g); + dUASSERT (b == NULL || (g->gflags & GEOM_PLACEABLE),"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + + if (b) { + if (!g->body) dFreePosr(g->final_posr); + if (g->body != b) { + if (g->offset_posr) { + dFreePosr(g->offset_posr); + g->offset_posr = 0; + } + g->final_posr = &b->posr; + g->bodyRemove(); + g->bodyAdd (b); + } + dGeomMoved (g); + } + else { + if (g->body) { + if (g->offset_posr) + { + // if we're offset, we already have our own final position, make sure its updated + g->recomputePosr(); + dFreePosr(g->offset_posr); + g->offset_posr = 0; + } + else + { + g->final_posr = dAllocPosr(); + memcpy (g->final_posr->pos,g->body->posr.pos,sizeof(dVector3)); + memcpy (g->final_posr->R,g->body->posr.R,sizeof(dMatrix3)); + } + g->bodyRemove(); + } + // dGeomMoved() should not be called if the body is being set to 0, as the + // new position of the geom is set to the old position of the body, so the + // effective position of the geom remains unchanged. + } +} + + +dBodyID dGeomGetBody (dxGeom *g) +{ + dAASSERT (g); + return g->body; +} + + +void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->offset_posr) { + // move body such that body+offset = position + dVector3 world_offset; + dMultiply0_331(world_offset, g->body->posr.R, g->offset_posr->pos); + dBodySetPosition(g->body, + x - world_offset[0], + y - world_offset[1], + z - world_offset[2]); + } + else if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetPosition (g->body,x,y,z); + } + else { + g->final_posr->pos[0] = x; + g->final_posr->pos[1] = y; + g->final_posr->pos[2] = z; + dGeomMoved (g); + } +} + + +void dGeomSetRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g && R); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->offset_posr) { + g->recomputePosr(); + // move body such that body+offset = rotation + dxPosR new_final_posr; + dxPosR new_body_posr; + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + memcpy(new_final_posr.R, R, sizeof(dMatrix3)); + getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr); + dBodySetRotation(g->body, new_body_posr.R); + dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]); + } + else if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetRotation (g->body,R); + } + else { + memcpy (g->final_posr->R,R,sizeof(dMatrix3)); + dGeomMoved (g); + } +} + + +void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat) +{ + dAASSERT (g && quat); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->offset_posr) { + g->recomputePosr(); + // move body such that body+offset = rotation + dxPosR new_final_posr; + dxPosR new_body_posr; + dQtoR (quat, new_final_posr.R); + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + + getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr); + dBodySetRotation(g->body, new_body_posr.R); + dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]); + } + if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetQuaternion (g->body,quat); + } + else { + dQtoR (quat, g->final_posr->R); + dGeomMoved (g); + } +} + + +const dReal * dGeomGetPosition (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + + return g->buildUpdatedPosition(); +} + + +void dGeomCopyPosition(dxGeom *g, dVector3 pos) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + + const dVector3 &src = g->buildUpdatedPosition(); + pos[0] = src[dV3E_X]; + pos[1] = src[dV3E_Y]; + pos[2] = src[dV3E_Z]; +} + + +const dReal * dGeomGetRotation (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + + return g->buildUpdatedRotation(); +} + + +void dGeomCopyRotation(dxGeom *g, dMatrix3 R) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + + const dMatrix3 &src = g->buildUpdatedRotation(); + R[0] = src[dM3E_XX]; + R[1] = src[dM3E_XY]; + R[2] = src[dM3E_XZ]; + R[4] = src[dM3E_YX]; + R[5] = src[dM3E_YY]; + R[6] = src[dM3E_YZ]; + R[8] = src[dM3E_ZX]; + R[9] = src[dM3E_ZY]; + R[10] = src[dM3E_ZZ]; +} + + +void dGeomGetQuaternion (dxGeom *g, dQuaternion quat) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + if (g->body && !g->offset_posr) { + const dReal * body_quat = dBodyGetQuaternion (g->body); + quat[0] = body_quat[0]; + quat[1] = body_quat[1]; + quat[2] = body_quat[2]; + quat[3] = body_quat[3]; + } + else { + g->recomputePosr(); + dRtoQ (g->final_posr->R, quat); + } +} + + +void dGeomGetAABB (dxGeom *g, dReal aabb[6]) +{ + dAASSERT (g); + dAASSERT (aabb); + g->recomputeAABB(); + memcpy (aabb,g->aabb,6 * sizeof(dReal)); +} + + +int dGeomIsSpace (dxGeom *g) +{ + dAASSERT (g); + return IS_SPACE(g); +} + + +dSpaceID dGeomGetSpace (dxGeom *g) +{ + dAASSERT (g); + return g->parent_space; +} + + +int dGeomGetClass (dxGeom *g) +{ + dAASSERT (g); + return g->type; +} + + +void dGeomSetCategoryBits (dxGeom *g, unsigned long bits) +{ + dAASSERT (g); + CHECK_NOT_LOCKED (g->parent_space); + g->category_bits = bits; +} + + +void dGeomSetCollideBits (dxGeom *g, unsigned long bits) +{ + dAASSERT (g); + CHECK_NOT_LOCKED (g->parent_space); + g->collide_bits = bits; +} + + +unsigned long dGeomGetCategoryBits (dxGeom *g) +{ + dAASSERT (g); + return g->category_bits; +} + + +unsigned long dGeomGetCollideBits (dxGeom *g) +{ + dAASSERT (g); + return g->collide_bits; +} + + +void dGeomEnable (dxGeom *g) +{ + dAASSERT (g); + g->gflags |= GEOM_ENABLED; +} + +void dGeomDisable (dxGeom *g) +{ + dAASSERT (g); + g->gflags &= ~GEOM_ENABLED; +} + +int dGeomIsEnabled (dxGeom *g) +{ + dAASSERT (g); + return (g->gflags & GEOM_ENABLED) != 0; +} + + +void dGeomGetRelPointPos (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMultiply0_331 (p,g->final_posr->R,prel); + result[0] = p[0] + g->final_posr->pos[0]; + result[1] = p[1] + g->final_posr->pos[1]; + result[2] = p[2] + g->final_posr->pos[2]; +} + + +void dGeomGetPosRelPoint (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 prel; + prel[0] = px - g->final_posr->pos[0]; + prel[1] = py - g->final_posr->pos[1]; + prel[2] = pz - g->final_posr->pos[2]; + prel[3] = 0; + dMultiply1_331 (result,g->final_posr->R,prel); +} + + +void dGeomVectorToWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMultiply0_331 (result,g->final_posr->R,p); +} + + +void dGeomVectorFromWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMultiply1_331 (result,g->final_posr->R,p); +} + + + +int dGeomLowLevelControl (dxGeom *g, int controlClass, int controlCode, void *dataValue, int *dataSize) +{ + dAASSERT (g); + dAASSERT (dataSize); + + if (!dataSize) { + return false; + } + + bool result = g->controlGeometry(controlClass, controlCode, dataValue, dataSize); + return result; +} + +//**************************************************************************** +// C interface that lets the user make new classes. this interface is a lot +// more cumbersome than C++ subclassing, which is what is used internally +// in ODE. this API is mainly to support legacy code. + +static int num_user_classes = 0; +static dGeomClass user_classes [dMaxUserClasses]; + + +struct dxUserGeom : public dxGeom { + void *user_data; + + dxUserGeom (int class_num); + ~dxUserGeom(); + void computeAABB(); + int AABBTest (dxGeom *o, dReal aabb[6]); +}; + + +dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1) +{ + type = class_num; + int size = user_classes[type-dFirstUserClass].bytes; + user_data = dAlloc (size); + memset (user_data,0,size); +} + + +dxUserGeom::~dxUserGeom() +{ + dGeomClass *c = &user_classes[type-dFirstUserClass]; + if (c->dtor) c->dtor (this); + dFree (user_data,c->bytes); +} + + +void dxUserGeom::computeAABB() +{ + user_classes[type-dFirstUserClass].aabb (this,aabb); +} + + +int dxUserGeom::AABBTest (dxGeom *o, dReal aabb[6]) +{ + dGeomClass *c = &user_classes[type-dFirstUserClass]; + if (c->aabb_test) return c->aabb_test (this,o,aabb); + else return 1; +} + + +static int dCollideUserGeomWithGeom (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + // this generic collider function is called the first time that a user class + // tries to collide against something. it will find out the correct collider + // function and then set the colliders array so that the correct function is + // called directly the next time around. + + int t1 = o1->type; // note that o1 is a user geom + int t2 = o2->type; // o2 *may* be a user geom + + // find the collider function to use. if o1 does not know how to collide with + // o2, then o2 might know how to collide with o1 (provided that it is a user + // geom). + dColliderFn *fn = user_classes[t1-dFirstUserClass].collider (t2); + int reverse = 0; + if (!fn && t2 >= dFirstUserClass && t2 <= dLastUserClass) { + fn = user_classes[t2-dFirstUserClass].collider (t1); + reverse = 1; + } + + // set the colliders array so that the correct function is called directly + // the next time around. note that fn can be 0 here if no collider was found, + // which means that dCollide() will always return 0 for this case. + colliders[t1][t2].fn = fn; + colliders[t1][t2].reverse = reverse; + colliders[t2][t1].fn = fn; + colliders[t2][t1].reverse = !reverse; + + // now call the collider function indirectly through dCollide(), so that + // contact reversing is properly handled. + return dCollide (o1,o2,flags,contact,skip); +} + + +int dCreateGeomClass (const dGeomClass *c) +{ + dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class"); + + if (num_user_classes >= dMaxUserClasses) { + dDebug (0,"too many user classes, you must increase the limit and " + "recompile ODE"); + } + user_classes[num_user_classes] = *c; + int class_number = num_user_classes + dFirstUserClass; + setAllColliders (class_number,&dCollideUserGeomWithGeom); + + num_user_classes++; + return class_number; +} + +/*extern */void dFinitUserClasses() +{ + num_user_classes = 0; +} + +void * dGeomGetClassData (dxGeom *g) +{ + dUASSERT (g && g->type >= dFirstUserClass && + g->type <= dLastUserClass,"not a custom class"); + dxUserGeom *user = (dxUserGeom*) g; + return user->user_data; +} + + +dGeomID dCreateGeom (int classnum) +{ + dUASSERT (classnum >= dFirstUserClass && + classnum <= dLastUserClass,"not a custom class"); + return new dxUserGeom (classnum); +} + + + +/* ************************************************************************ */ +/* geom offset from body */ + +void dGeomCreateOffset (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + if (g->offset_posr) + { + return; // already created + } + dIASSERT (g->final_posr == &g->body->posr); + + g->final_posr = dAllocPosr(); + g->offset_posr = dAllocPosr(); + dSetZero (g->offset_posr->pos,4); + dRSetIdentity (g->offset_posr->R); + + g->gflags |= GEOM_POSR_BAD; +} + +void dGeomSetOffsetPosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset(g); + } + g->offset_posr->pos[0] = x; + g->offset_posr->pos[1] = y; + g->offset_posr->pos[2] = z; + dGeomMoved (g); +} + +void dGeomSetOffsetRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g && R); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + memcpy (g->offset_posr->R,R,sizeof(dMatrix3)); + dGeomMoved (g); +} + +void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat) +{ + dAASSERT (g && quat); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + dQtoR (quat, g->offset_posr->R); + dGeomMoved (g); +} + +void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset(g); + } + dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos); + dGeomMoved (g); +} + +void dGeomSetOffsetWorldRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g && R); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + g->recomputePosr(); + + dxPosR new_final_posr; + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + memcpy(new_final_posr.R, R, sizeof(dMatrix3)); + + getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr); + dGeomMoved (g); +} + +void dGeomSetOffsetWorldQuaternion (dxGeom *g, const dQuaternion quat) +{ + dAASSERT (g && quat); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + + g->recomputePosr(); + + dxPosR new_final_posr; + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + dQtoR (quat, new_final_posr.R); + + getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr); + dGeomMoved (g); +} + +void dGeomClearOffset(dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + if (g->offset_posr) + { + dIASSERT(g->body); + // no longer need an offset posr + dFreePosr(g->offset_posr); + g->offset_posr = 0; + // the geom will now share the position of the body + dFreePosr(g->final_posr); + g->final_posr = &g->body->posr; + // geom has moved + g->gflags &= ~GEOM_POSR_BAD; + dGeomMoved (g); + } +} + +int dGeomIsOffset(dxGeom *g) +{ + dAASSERT (g); + return ((0 != g->offset_posr) ? 1 : 0); +} + +static const dVector3 OFFSET_POSITION_ZERO = { 0.0f, 0.0f, 0.0f, 0.0f }; + +const dReal * dGeomGetOffsetPosition (dxGeom *g) +{ + dAASSERT (g); + if (g->offset_posr) + { + return g->offset_posr->pos; + } + return OFFSET_POSITION_ZERO; +} + +void dGeomCopyOffsetPosition (dxGeom *g, dVector3 pos) +{ + dAASSERT (g); + if (g->offset_posr) + { + const dReal* src = g->offset_posr->pos; + pos[0] = src[0]; + pos[1] = src[1]; + pos[2] = src[2]; + } + else + { + pos[0] = 0; + pos[1] = 0; + pos[2] = 0; + } +} + +static const dMatrix3 OFFSET_ROTATION_ZERO = +{ + 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, +}; + +const dReal * dGeomGetOffsetRotation (dxGeom *g) +{ + dAASSERT (g); + if (g->offset_posr) + { + return g->offset_posr->R; + } + return OFFSET_ROTATION_ZERO; +} + +void dGeomCopyOffsetRotation (dxGeom *g, dMatrix3 R) +{ + dAASSERT (g); + if (g->offset_posr) + { + const dReal* src = g->offset_posr->R; + R[0] = src[0]; + R[1] = src[1]; + R[2] = src[2]; + R[4] = src[4]; + R[5] = src[5]; + R[6] = src[6]; + R[8] = src[8]; + R[9] = src[9]; + R[10] = src[10]; + } + else + { + R[0] = OFFSET_ROTATION_ZERO[0]; + R[1] = OFFSET_ROTATION_ZERO[1]; + R[2] = OFFSET_ROTATION_ZERO[2]; + R[4] = OFFSET_ROTATION_ZERO[4]; + R[5] = OFFSET_ROTATION_ZERO[5]; + R[6] = OFFSET_ROTATION_ZERO[6]; + R[8] = OFFSET_ROTATION_ZERO[8]; + R[9] = OFFSET_ROTATION_ZERO[9]; + R[10] = OFFSET_ROTATION_ZERO[10]; + } +} + +void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result) +{ + dAASSERT (g); + if (g->offset_posr) + { + dRtoQ (g->offset_posr->R, result); + } + else + { + dSetZero (result,4); + result[0] = 1; + } +} + + diff --git a/libs/ode-0.16.1/ode/src/collision_kernel.h b/libs/ode-0.16.1/ode/src/collision_kernel.h new file mode 100644 index 0000000..c982972 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_kernel.h @@ -0,0 +1,293 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +internal data structures and functions for collision detection. + +*/ + +#ifndef _ODE_COLLISION_KERNEL_H_ +#define _ODE_COLLISION_KERNEL_H_ + +#include <ode/common.h> +#include <ode/contact.h> +#include <ode/collision.h> +#include "objects.h" +#include "odetls.h" +#include "common.h" + + +//**************************************************************************** +// constants and macros + +// mask for the number-of-contacts field in the dCollide() flags parameter +#define NUMC_MASK (0xffff) + +#define IS_SPACE(geom) \ + dIN_RANGE((geom)->type, dFirstSpaceClass, dLastSpaceClass + 1) + +#define CHECK_NOT_LOCKED(space) \ + dUASSERT ((space) == NULL || (space)->lock_count == 0, \ + "Invalid operation for locked space") + + +//**************************************************************************** +// geometry object base class + + +// geom flags. +// +// GEOM_DIRTY means that the space data structures for this geom are +// potentially not up to date. NOTE THAT all space parents of a dirty geom +// are themselves dirty. this is an invariant that must be enforced. +// +// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date. +// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might +// recalculate its own AABB but does not know how to update the space data +// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY. +// the valid combinations are: +// 0 +// GEOM_DIRTY +// GEOM_DIRTY|GEOM_AABB_BAD +// GEOM_DIRTY|GEOM_AABB_BAD|GEOM_POSR_BAD + +enum { + GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown + GEOM_POSR_BAD = 2, // geom's final posr is not valid + GEOM_AABB_BAD = 4, // geom's AABB is not valid + GEOM_PLACEABLE = 8, // geom is placeable + GEOM_ENABLED = 16, // geom is enabled + GEOM_ZERO_SIZED = 32, // geom is zero sized + + GEOM_ENABLE_TEST_MASK = GEOM_ENABLED | GEOM_ZERO_SIZED, + GEOM_ENABLE_TEST_VALUE = GEOM_ENABLED, + + // Ray specific + RAY_FIRSTCONTACT = 0x10000, + RAY_BACKFACECULL = 0x20000, + RAY_CLOSEST_HIT = 0x40000 +}; + +enum dxContactMergeOptions { + DONT_MERGE_CONTACTS, + MERGE_CONTACT_NORMALS, + MERGE_CONTACTS_FULLY +}; + + +// geometry object base class. pos and R will either point to a separately +// allocated buffer (if body is 0 - pos points to the dxPosR object) or to +// the pos and R of the body (if body nonzero). +// a dGeomID is a pointer to this object. + +struct dxGeom : public dBase { + int type; // geom type number, set by subclass constructor + int gflags; // flags used by geom and space + void *data; // user-defined data pointer + dBodyID body; // dynamics body associated with this object (if any) + dxGeom *body_next; // next geom in body's linked list of associated geoms + dxPosR *final_posr; // final position of the geom in world coordinates + dxPosR *offset_posr; // offset from body in local coordinates + + // information used by spaces + dxGeom *next; // next geom in linked list of geoms + dxGeom **tome; // linked list backpointer + dxGeom *next_ex; // next geom in extra linked list of geoms (for higher level structures) + dxGeom **tome_ex; // extra linked list backpointer (for higher level structures) + dxSpace *parent_space;// the space this geom is contained in, 0 if none + dReal aabb[6]; // cached AABB for this space + unsigned long category_bits,collide_bits; + + dxGeom (dSpaceID _space, int is_placeable); + virtual ~dxGeom(); + + // Set or clear GEOM_ZERO_SIZED flag + void updateZeroSizedFlag(bool is_zero_sized) { gflags = is_zero_sized ? (gflags | GEOM_ZERO_SIZED) : (gflags & ~GEOM_ZERO_SIZED); } + // Get parent space TLS kind + unsigned getParentSpaceTLSKind() const; + + const dVector3 &buildUpdatedPosition() + { + dIASSERT(gflags & GEOM_PLACEABLE); + + recomputePosr(); + return final_posr->pos; + } + + const dMatrix3 &buildUpdatedRotation() + { + dIASSERT(gflags & GEOM_PLACEABLE); + + recomputePosr(); + return final_posr->R; + } + + // recalculate our new final position if needed + void recomputePosr() + { + if (gflags & GEOM_POSR_BAD) { + computePosr(); + gflags &= ~GEOM_POSR_BAD; + } + } + + // calculate our new final position from our offset and body + void computePosr(); + + bool checkControlValueSizeValidity(void *dataValue, int *dataSize, int iRequiresSize) { return (*dataSize == iRequiresSize && dataValue != 0) ? true : !(*dataSize = iRequiresSize); } // Here it is the intent to return true for 0 required size in any case + virtual bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize); + + virtual void computeAABB()=0; + // compute the AABB for this object and put it in aabb. this function + // always performs a fresh computation, it does not inspect the + // GEOM_AABB_BAD flag. + + virtual int AABBTest (dxGeom *o, dReal aabb[6]); + // test whether the given AABB object intersects with this object, return + // 1=yes, 0=no. this is used as an early-exit test in the space collision + // functions. the default implementation returns 1, which is the correct + // behavior if no more detailed implementation can be provided. + + // utility functions + + // compute the AABB only if it is not current. this function manipulates + // the GEOM_AABB_BAD flag. + + void recomputeAABB() { + if (gflags & GEOM_AABB_BAD) { + // our aabb functions assume final_posr is up to date + recomputePosr(); + computeAABB(); + gflags &= ~GEOM_AABB_BAD; + } + } + + inline void markAABBBad(); + + // add and remove this geom from a linked list maintained by a space. + + void spaceAdd (dxGeom **first_ptr) { + next = *first_ptr; + tome = first_ptr; + if (*first_ptr) (*first_ptr)->tome = &next; + *first_ptr = this; + } + void spaceRemove() { + if (next) next->tome = tome; + *tome = next; + } + + // add and remove this geom from a linked list maintained by a body. + + void bodyAdd (dxBody *b) { + body = b; + body_next = b->geom; + b->geom = this; + } + void bodyRemove(); +}; + +//**************************************************************************** +// the base space class +// +// the contained geoms are divided into two kinds: clean and dirty. +// the clean geoms have not moved since they were put in the list, +// and their AABBs are valid. the dirty geoms have changed position, and +// their AABBs are may not be valid. the two types are distinguished by the +// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list. + +#if dTLS_ENABLED +#define dSPACE_TLS_KIND_INIT_VALUE OTK__DEFAULT +#define dSPACE_TLS_KIND_MANUAL_VALUE OTK_MANUALCLEANUP +#else +#define dSPACE_TLS_KIND_INIT_VALUE 0 +#define dSPACE_TLS_KIND_MANUAL_VALUE 0 +#endif + +struct dxSpace : public dxGeom { + int count; // number of geoms in this space + dxGeom *first; // first geom in list + int cleanup; // cleanup mode, 1=destroy geoms on exit + int sublevel; // space sublevel (used in dSpaceCollide2). NOT TRACKED AUTOMATICALLY!!! + unsigned tls_kind; // space TLS kind to be used for global caches retrieval + + // cached state for getGeom() + int current_index; // only valid if current_geom != 0 + dxGeom *current_geom; // if 0 then there is no information + + // locking stuff. the space is locked when it is currently traversing its + // internal data structures, e.g. in collide() and collide2(). operations + // that modify the contents of the space are not permitted when the space + // is locked. + int lock_count; + + dxSpace (dSpaceID _space); + ~dxSpace(); + + void computeAABB(); + + void setCleanup (int mode) { cleanup = (mode != 0); } + int getCleanup() const { return cleanup; } + void setSublevel(int value) { sublevel = value; } + int getSublevel() const { return sublevel; } + void setManulCleanup(int value) { tls_kind = (value ? dSPACE_TLS_KIND_MANUAL_VALUE : dSPACE_TLS_KIND_INIT_VALUE); } + int getManualCleanup() const { return (tls_kind == dSPACE_TLS_KIND_MANUAL_VALUE) ? 1 : 0; } + int query (dxGeom *geom) const { dAASSERT(geom); return (geom->parent_space == this); } + int getNumGeoms() const { return count; } + + virtual dxGeom *getGeom (int i); + + virtual void add (dxGeom *); + virtual void remove (dxGeom *); + virtual void dirty (dxGeom *); + + virtual void cleanGeoms()=0; + // turn all dirty geoms into clean geoms by computing their AABBs and any + // other space data structures that are required. this should clear the + // GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms. + + virtual void collide (void *data, dNearCallback *callback)=0; + virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0; +}; + + +////////////////////////////////////////////////////////////////////////// + +/*inline */ +void dxGeom::markAABBBad() { + gflags |= (GEOM_DIRTY | GEOM_AABB_BAD); + CHECK_NOT_LOCKED(parent_space); +} + + +//**************************************************************************** +// Initialization and finalization functions + +void dInitColliders(); +void dFinitColliders(); + +void dClearPosrCache(void); +void dFinitUserClasses(); + + +#endif diff --git a/libs/ode-0.16.1/ode/src/collision_libccd.cpp b/libs/ode-0.16.1/ode/src/collision_libccd.cpp new file mode 100644 index 0000000..ba15e83 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_libccd.cpp @@ -0,0 +1,1080 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/collision.h> +#include <ccd/ccd.h> +#include "ccdcustom/vec3.h" +#include "ccdcustom/quat.h" +#include "config.h" +#include "odemath.h" +#include "collision_libccd.h" +#include "collision_trimesh_internal.h" +#include "collision_std.h" +#include "collision_util.h" +#include "error.h" + + +struct _ccd_obj_t { + ccd_vec3_t pos; + ccd_quat_t rot, rot_inv; +}; +typedef struct _ccd_obj_t ccd_obj_t; + +struct _ccd_box_t { + ccd_obj_t o; + ccd_real_t dim[3]; +}; +typedef struct _ccd_box_t ccd_box_t; + +struct _ccd_cap_t { + ccd_obj_t o; + ccd_real_t radius; + ccd_vec3_t axis; + ccd_vec3_t p1; + ccd_vec3_t p2; +}; +typedef struct _ccd_cap_t ccd_cap_t; + +struct _ccd_cyl_t { + ccd_obj_t o; + ccd_real_t radius; + ccd_vec3_t axis; + ccd_vec3_t p1; + ccd_vec3_t p2; +}; +typedef struct _ccd_cyl_t ccd_cyl_t; + +struct _ccd_sphere_t { + ccd_obj_t o; + ccd_real_t radius; +}; +typedef struct _ccd_sphere_t ccd_sphere_t; + +struct _ccd_convex_t { + ccd_obj_t o; + dxConvex *convex; +}; +typedef struct _ccd_convex_t ccd_convex_t; + +struct _ccd_triangle_t { + ccd_obj_t o; + ccd_vec3_t vertices[3]; +}; +typedef struct _ccd_triangle_t ccd_triangle_t; + +/** Transforms geom to ccd struct */ +static void ccdGeomToObj(const dGeomID g, ccd_obj_t *); +static void ccdGeomToBox(const dGeomID g, ccd_box_t *); +static void ccdGeomToCap(const dGeomID g, ccd_cap_t *); +static void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *); +static void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *); +static void ccdGeomToConvex(const dGeomID g, ccd_convex_t *); + +/** Support functions */ +static void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); + +/** Center function */ +static void ccdCenter(const void *obj, ccd_vec3_t *c); + +/** General collide function */ +static int ccdCollide(dGeomID o1, dGeomID o2, int flags, + dContactGeom *contact, int skip, + void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, + void *obj2, ccd_support_fn supp2, ccd_center_fn cen2); + +static int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip); +static bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth); +// Adds a contact between 2 cylinders +static int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts, ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip); + +static unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, + const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip, + ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount); +static bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices); +static unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip); +static void setObjPosToTriangleCenter(ccd_triangle_t *t); +static void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); + + +static +void ccdGeomToObj(const dGeomID g, ccd_obj_t *o) +{ + const dReal *ode_pos; + dQuaternion ode_rot; + + ode_pos = dGeomGetPosition(g); + dGeomGetQuaternion(g, ode_rot); + + ccdVec3Set(&o->pos, ode_pos[0], ode_pos[1], ode_pos[2]); + ccdQuatSet(&o->rot, ode_rot[1], ode_rot[2], ode_rot[3], ode_rot[0]); + + ccdQuatInvert2(&o->rot_inv, &o->rot); +} + +static +void ccdGeomToBox(const dGeomID g, ccd_box_t *box) +{ + dVector3 dim; + + ccdGeomToObj(g, (ccd_obj_t *)box); + + dGeomBoxGetLengths(g, dim); + box->dim[0] = (ccd_real_t)(dim[0] * 0.5); + box->dim[1] = (ccd_real_t)(dim[1] * 0.5); + box->dim[2] = (ccd_real_t)(dim[2] * 0.5); +} + +static +void ccdGeomToCap(const dGeomID g, ccd_cap_t *cap) +{ + dReal r, h; + ccdGeomToObj(g, (ccd_obj_t *)cap); + + dGeomCapsuleGetParams(g, &r, &h); + cap->radius = r; + ccdVec3Set(&cap->axis, 0.0, 0.0, h / 2); + ccdQuatRotVec(&cap->axis, &cap->o.rot); + ccdVec3Copy(&cap->p1, &cap->axis); + ccdVec3Copy(&cap->p2, &cap->axis); + ccdVec3Scale(&cap->p2, -1.0); + ccdVec3Add(&cap->p1, &cap->o.pos); + ccdVec3Add(&cap->p2, &cap->o.pos); +} + +static +void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *cyl) +{ + dReal r, h; + ccdGeomToObj(g, (ccd_obj_t *)cyl); + + dGeomCylinderGetParams(g, &r, &h); + cyl->radius = r; + ccdVec3Set(&cyl->axis, 0.0, 0.0, h / 2); + ccdQuatRotVec(&cyl->axis, &cyl->o.rot); + ccdVec3Copy(&cyl->p1, &cyl->axis); + ccdVec3Copy(&cyl->p2, &cyl->axis); + int cylAxisNormalizationResult = ccdVec3SafeNormalize(&cyl->axis); + dUVERIFY(cylAxisNormalizationResult == 0, "Invalid cylinder has been passed"); + ccdVec3Scale(&cyl->p2, -1.0); + ccdVec3Add(&cyl->p1, &cyl->o.pos); + ccdVec3Add(&cyl->p2, &cyl->o.pos); +} + +static +void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *s) +{ + ccdGeomToObj(g, (ccd_obj_t *)s); + s->radius = dGeomSphereGetRadius(g); +} + +static +void ccdGeomToConvex(const dGeomID g, ccd_convex_t *c) +{ + ccdGeomToObj(g, (ccd_obj_t *)c); + c->convex = (dxConvex *)g; +} + + +static +void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_box_t *o = (const ccd_box_t *)obj; + ccd_vec3_t dir; + + ccdVec3Copy(&dir, _dir); + ccdQuatRotVec(&dir, &o->o.rot_inv); + + ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0], + ccdSign(ccdVec3Y(&dir)) * o->dim[1], + ccdSign(ccdVec3Z(&dir)) * o->dim[2]); + + // transform support vertex + ccdQuatRotVec(v, &o->o.rot); + ccdVec3Add(v, &o->o.pos); +} + +static +void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_cap_t *o = (const ccd_cap_t *)obj; + + ccdVec3Copy(v, _dir); + ccdVec3Scale(v, o->radius); + + if (ccdVec3Dot(_dir, &o->axis) > 0.0){ + ccdVec3Add(v, &o->p1); + }else{ + ccdVec3Add(v, &o->p2); + } + +} + +static +void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_cyl_t *cyl = (const ccd_cyl_t *)obj; + ccd_vec3_t dir; + ccd_real_t len; + + ccd_real_t dot = ccdVec3Dot(_dir, &cyl->axis); + if (dot > 0.0){ + ccdVec3Copy(v, &cyl->p1); + } else{ + ccdVec3Copy(v, &cyl->p2); + } + // project dir onto cylinder's 'top'/'bottom' plane + ccdVec3Copy(&dir, &cyl->axis); + ccdVec3Scale(&dir, -dot); + ccdVec3Add(&dir, _dir); + len = CCD_SQRT(ccdVec3Len2(&dir)); + if (!ccdIsZero(len)) { + ccdVec3Scale(&dir, cyl->radius / len); + ccdVec3Add(v, &dir); + } +} + +static +void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_sphere_t *s = (const ccd_sphere_t *)obj; + + ccdVec3Copy(v, _dir); + ccdVec3Scale(v, s->radius); + dIASSERT(dFabs(CCD_SQRT(ccdVec3Len2(_dir)) - REAL(1.0)) < 1e-6); // ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(_dir))); + + ccdVec3Add(v, &s->o.pos); +} + +static +void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_convex_t *c = (const ccd_convex_t *)obj; + ccd_vec3_t dir, p; + ccd_real_t maxdot, dot; + sizeint i; + const dReal *curp; + + ccdVec3Copy(&dir, _dir); + ccdQuatRotVec(&dir, &c->o.rot_inv); + + maxdot = -CCD_REAL_MAX; + curp = c->convex->points; + for (i = 0; i < c->convex->pointcount; i++, curp += 3){ + ccdVec3Set(&p, curp[0], curp[1], curp[2]); + dot = ccdVec3Dot(&dir, &p); + if (dot > maxdot){ + ccdVec3Copy(v, &p); + maxdot = dot; + } + } + + + // transform support vertex + ccdQuatRotVec(v, &c->o.rot); + ccdVec3Add(v, &c->o.pos); +} + +static +void ccdCenter(const void *obj, ccd_vec3_t *c) +{ + const ccd_obj_t *o = (const ccd_obj_t *)obj; + ccdVec3Copy(c, &o->pos); +} + +static +int ccdCollide( + dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, int skip, + void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, + void *obj2, ccd_support_fn supp2, ccd_center_fn cen2) +{ + ccd_t ccd; + int res; + ccd_real_t depth; + ccd_vec3_t dir, pos; + int max_contacts = (flags & NUMC_MASK); + + if (max_contacts < 1) + return 0; + + CCD_INIT(&ccd); + ccd.support1 = supp1; + ccd.support2 = supp2; + ccd.center1 = cen1; + ccd.center2 = cen2; + ccd.max_iterations = 500; + ccd.mpr_tolerance = (ccd_real_t)1E-6; + + + if (flags & CONTACTS_UNIMPORTANT){ + if (ccdMPRIntersect(obj1, obj2, &ccd)){ + return 1; + }else{ + return 0; + } + } + + res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); + if (res == 0){ + contact->g1 = o1; + contact->g2 = o2; + + contact->side1 = contact->side2 = -1; + + contact->depth = depth; + + contact->pos[0] = ccdVec3X(&pos); + contact->pos[1] = ccdVec3Y(&pos); + contact->pos[2] = ccdVec3Z(&pos); + + ccdVec3Scale(&dir, -1.); + contact->normal[0] = ccdVec3X(&dir); + contact->normal[1] = ccdVec3Y(&dir); + contact->normal[2] = ccdVec3Z(&dir); + + return 1; + } + + return 0; +} + +/*extern */ +int dCollideBoxCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cyl_t cyl; + ccd_box_t box; + + ccdGeomToBox(o1, &box); + ccdGeomToCyl(o2, &cyl); + + return ccdCollide(o1, o2, flags, contact, skip, + &box, ccdSupportBox, ccdCenter, + &cyl, ccdSupportCyl, ccdCenter); +} + +/*extern */ +int dCollideCapsuleCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cap_t cap; + ccd_cyl_t cyl; + + ccdGeomToCap(o1, &cap); + ccdGeomToCyl(o2, &cyl); + + return ccdCollide(o1, o2, flags, contact, skip, + &cap, ccdSupportCap, ccdCenter, + &cyl, ccdSupportCyl, ccdCenter); +} + +/*extern */ +int dCollideConvexBoxCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_box_t box; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToBox(o2, &box); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &box, ccdSupportBox, ccdCenter); +} + +/*extern */ +int dCollideConvexCapsuleCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cap_t cap; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToCap(o2, &cap); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &cap, ccdSupportCap, ccdCenter); +} + +/*extern */ +int dCollideConvexSphereCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_sphere_t sphere; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToSphere(o2, &sphere); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &sphere, ccdSupportSphere, ccdCenter); +} + +/*extern */ +int dCollideConvexCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cyl_t cyl; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToCyl(o2, &cyl); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &cyl, ccdSupportCyl, ccdCenter); +} + +/*extern */ +int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_convex_t c1, c2; + + ccdGeomToConvex(o1, &c1); + ccdGeomToConvex(o2, &c2); + + return ccdCollide(o1, o2, flags, contact, skip, + &c1, ccdSupportConvex, ccdCenter, + &c2, ccdSupportConvex, ccdCenter); +} + + +/*extern */ +int dCollideCylinderCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cyl_t cyl1, cyl2; + + ccdGeomToCyl(o1, &cyl1); + ccdGeomToCyl(o2, &cyl2); + + int numContacts = collideCylCyl(o1, o2, &cyl1, &cyl2, flags, contact, skip); + if (numContacts < 0) { + numContacts = ccdCollide(o1, o2, flags, contact, skip, + &cyl1, ccdSupportCyl, ccdCenter, + &cyl2, ccdSupportCyl, ccdCenter); + } + return numContacts; +} + +static +int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip) +{ + int maxContacts = (flags & NUMC_MASK); + dAASSERT(maxContacts != 0); + + maxContacts = maxContacts > 8 ? 8 : maxContacts; + + dReal axesProd = dFabs(ccdVec3Dot(&cyl1->axis, &cyl2->axis)); + // Check if cylinders' axes are in line + if (REAL(1.0) - axesProd < 1e-3f) { + ccd_vec3_t p, proj; + dReal r1, l1; + dReal r2, l2; + dGeomCylinderGetParams(o1, &r1, &l1); + dGeomCylinderGetParams(o2, &r2, &l2); + l1 *= 0.5f; + l2 *= 0.5f; + + // Determine the cylinder with smaller radius (minCyl) and bigger radius (maxCyl) and their respective properties: radius, length + bool r1IsMin; + dReal rmin, rmax; + ccd_cyl_t *minCyl, *maxCyl; + if (r1 <= r2) { + rmin = r1; rmax = r2; + minCyl = cyl1; maxCyl = cyl2; + r1IsMin = true; + } + else { + rmin = r2; rmax = r1; + minCyl = cyl2; maxCyl = cyl1; + r1IsMin = false; + } + + dReal lSum = l1 + l2; + + ccdVec3Copy(&p, &minCyl->o.pos); + ccdVec3Sub(&p, &maxCyl->o.pos); + dReal dot = ccdVec3Dot(&p, &maxCyl->axis); + + // Maximum possible contact depth + dReal depth_v = lSum - dFabs(dot) + dSqrt(dMax(0, REAL(1.0) - axesProd * axesProd)) * rmin; + if (depth_v < 0) { + return 0; + } + + // Project the smaller cylinder's center onto the larger cylinder's plane + ccdVec3Copy(&proj, &maxCyl->axis); + ccdVec3Scale(&proj, -dot); + ccdVec3Add(&proj, &p); + dReal radiiDiff = (dReal)sqrt(ccdVec3Len2(&proj)); + dReal depth_h = r1 + r2 - radiiDiff; + + // Check the distance between cylinders' centers + if (depth_h < 0) { + return 0; + } + + // Check if "vertical" contact depth is less than "horizontal" contact depth + if (depth_v < depth_h) { + int contactCount = 0; + dReal dot2 = -ccdVec3Dot(&p, &minCyl->axis); + // lmin, lmax - distances from cylinders' centers to potential contact points relative to cylinders' axes + dReal lmax = r1IsMin ? l2 : l1; + dReal lmin = r1IsMin ? l1 : l2; + lmin = dot2 < 0 ? -lmin : lmin; + lmax = dot < 0 ? -lmax : lmax; + // Contact normal direction, relative to o1's axis + dReal normaldir = (dot < 0) != r1IsMin ? REAL(1.0) : -REAL(1.0); + + if (rmin + radiiDiff <= rmax) { + // Case 1: The smaller disc is fully contained within the larger one + // Simply generate N points on the rim of the smaller disc + dReal maxContactsRecip = (dReal)(0 < maxContacts ? (2.0 * M_PI / maxContacts) : (2.0 * M_PI)); // The 'else' value does not matter. Just try helping the optimizer. + for (int i = 0; i < maxContacts; i++) { + dReal depth; + dReal a = maxContactsRecip * i; + if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) { + contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + dIASSERT(contactCount != 0); + break; + } + } + } + return contactCount; + + } else { + // Case 2: Discs intersect + // Firstly, find intersections assuming the larger cylinder is placed at (0,0,0) + // http://math.stackexchange.com/questions/256100/how-can-i-find-the-points-at-which-two-circles-intersect + ccd_vec3_t proj2; + ccdVec3Copy(&proj2, &proj); + ccdQuatRotVec(&proj, &maxCyl->o.rot_inv); + dReal d = dSqrt(ccdVec3X(&proj) * ccdVec3X(&proj) + ccdVec3Y(&proj) * ccdVec3Y(&proj)); + dIASSERT(d != REAL(0.0)); + + dReal dRecip = REAL(1.0) / d; + dReal rmaxSquare = rmax * rmax, rminSquare = rmin * rmin, dSquare = d * d; + + dReal minA, diffA, minB, diffB; + + { + dReal l = (rmaxSquare - rminSquare + dSquare) * (REAL(0.5) * dRecip); + dReal h = dSqrt(rmaxSquare - l * l); + dReal divLbyD = l * dRecip, divHbyD = h * dRecip; + dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj); + dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj); + dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj); + dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj); + // Map the intersection points to angles + dReal ap1 = dAtan2(y1, x1); + dReal ap2 = dAtan2(y2, x2); + minA = dMin(ap1, ap2); + dReal maxA = dMax(ap1, ap2); + // If the segment connecting cylinders' centers does not intersect the arc, change the angles + dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj)); + if (a < minA || a > maxA) { + a = maxA; + maxA = (dReal)(minA + M_PI * 2.0); + minA = a; + } + diffA = maxA - minA; + } + + // Do the same for the smaller cylinder assuming it is placed at (0,0,0) now + ccdVec3Copy(&proj, &proj2); + ccdVec3Scale(&proj, -1); + ccdQuatRotVec(&proj, &minCyl->o.rot_inv); + + { + dReal l = (rminSquare - rmaxSquare + dSquare) * (REAL(0.5) * dRecip); + dReal h = dSqrt(rminSquare - l * l); + dReal divLbyD = l * dRecip, divHbyD = h * dRecip; + dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj); + dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj); + dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj); + dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj); + dReal ap1 = dAtan2(y1, x1); + dReal ap2 = dAtan2(y2, x2); + minB = dMin(ap1, ap2); + dReal maxB = dMax(ap1, ap2); + dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj)); + if (a < minB || a > maxB) { + a = maxB; + maxB = (dReal)(minB + M_PI * 2.0); + minB = a; + } + diffB = maxB - minB; + } + + // Find contact point distribution ratio based on arcs lengths + dReal ratio = diffA * rmax / (diffA * rmax + diffB * rmin); + dIASSERT(ratio <= REAL(1.0)); + dIASSERT(ratio >= REAL(0.0)); + + int nMax = (int)dFloor(ratio * maxContacts + REAL(0.5)); + int nMin = maxContacts - nMax; + dIASSERT(nMax <= maxContacts); + + // Make sure there is at least one point on the smaller radius rim + if (nMin < 1) { + nMin = 1; nMax -= 1; + } + // Otherwise transfer one point to the larger radius rim as it is going to fill the rim intersection points + else if (nMin > 1) { + nMin -= 1; nMax += 1; + } + + // Smaller disc first, skipping the overlapping points + dReal nMinRecip = 0 < nMin ? diffB / (nMin + 1) : diffB; // The 'else' value does not matter. Just try helping the optimizer. + for (int i = 1; i <= nMin; i++) { + dReal depth; + dReal a = minB + nMinRecip * i; + if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) { + contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + dIASSERT(contactCount != 0); + break; + } + } + } + + if (contactCount == 0 || (flags & CONTACTS_UNIMPORTANT) == 0) { + // Then the larger disc, + additional point as the start/end points of arcs overlap + // (or a single contact at the arc middle point if just one is required) + dReal nMaxRecip = nMax > 1 ? diffA / (nMax - 1) : diffA; // The 'else' value does not matter. Just try helping the optimizer. + dReal adjustedMinA = nMax == 1 ? minA + REAL(0.5) * diffA : minA; + + for (int i = 0; i < nMax; i++) { + dReal depth; + dReal a = adjustedMinA + nMaxRecip * i; + if (testAndPrepareDiscContactForAngle(a, rmax, lmax, lSum, maxCyl, minCyl, p, depth)) { + contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + dIASSERT(contactCount != 0); + break; + } + } + } + } + + return contactCount; + } + } + } + return -1; +} + +static +bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth) +{ + bool ret = false; + + ccd_vec3_t p2; + ccdVec3Set(&p, dCos(angle) * radius, dSin(angle) * radius, 0); + ccdQuatRotVec(&p, &priCyl->o.rot); + ccdVec3Add(&p, &priCyl->o.pos); + ccdVec3Copy(&p2, &p); + ccdVec3Sub(&p2, &secCyl->o.pos); + dReal depth = lSum - dFabs(ccdVec3Dot(&p2, &secCyl->axis)); + + if (depth >= 0) { + ccdVec3Copy(&p2, &priCyl->axis); + ccdVec3Scale(&p2, length); + ccdVec3Add(&p, &p2); + + out_depth = depth; + ret = true; + } + + return ret; +} + +static +int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts, + ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip) +{ + dIASSERT(depth >= 0); + + dContactGeom* contact = SAFECONTACT(flags, contacts, j, skip); + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + contact->normal[0] = normaldir * ccdVec3X(axis); + contact->normal[1] = normaldir * ccdVec3Y(axis); + contact->normal[2] = normaldir * ccdVec3Z(axis); + contact->depth = depth; + contact->pos[0] = ccdVec3X(p); + contact->pos[1] = ccdVec3Y(p); + contact->pos[2] = ccdVec3Z(p); + + return j + 1; +} + + +#if dTRIMESH_ENABLED + +const static float CONTACT_DEPTH_EPSILON = 0.0001f; +const static float CONTACT_POS_EPSILON = 0.0001f; +const static float CONTACT_PERTURBATION_ANGLE = 0.001f; +const static float NORMAL_PROJ_EPSILON = 0.0001f; + + +/*extern */ +unsigned dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip) +{ + ccd_convex_t c1; + ccd_triangle_t c2; + dVector3 triangle[dMTV__MAX]; + unsigned maxContacts = (flags & NUMC_MASK); + unsigned contactCount = 0; + ccdGeomToConvex(o1, &c1); + ccdGeomToObj(o2, (ccd_obj_t *)&c2); + + IFaceAngleStorageView *meshFaceAngleView = dxGeomTriMeshGetFaceAngleView(o2); + dUASSERT(meshFaceAngleView != NULL, "Please preprocess the trimesh data with dTRIDATAPREPROCESS_BUILD_FACE_ANGLES"); + + for (unsigned i = 0; i != numIndices; ++i) { + dContactGeom tempContact; + dGeomTriMeshGetTriangle(o2, indices[i], &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]); + + for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) { + ccdVec3Set(&c2.vertices[j], (ccd_real_t)triangle[j][dV3E_X], (ccd_real_t)triangle[j][dV3E_Y], (ccd_real_t)triangle[j][dV3E_Z]); + } + + setObjPosToTriangleCenter(&c2); + + if (ccdCollide(o1, o2, flags, &tempContact, skip, &c1, &ccdSupportConvex, &ccdCenter, &c2, &ccdSupportTriangle, &ccdCenter) == 1) { + tempContact.side2 = i; + + if (meshFaceAngleView == NULL || correctTriangleContactNormal(&c2, &tempContact, meshFaceAngleView, indices, numIndices)) { + contactCount = addUniqueContact(contacts, &tempContact, contactCount, maxContacts, flags, skip); + + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + break; + } + } + } + } + + if ((flags & CONTACTS_UNIMPORTANT) == 0 && contactCount == 1) { + dContactGeom *contact = SAFECONTACT(flags, contacts, 0, skip); + dGeomTriMeshGetTriangle(o2, contact->side2, &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]); + contactCount = addTrianglePerturbedContacts(o1, o2, meshFaceAngleView, indices, numIndices, flags, contacts, skip, &c1, &c2, triangle, contact, contactCount); + } + + // Normalize accumulated normals, if necessary + for (unsigned k = 0; k != contactCount; ) { + dContactGeom *contact = SAFECONTACT(flags, contacts, k, skip); + bool stayWithinThisIndex = false; + + // Only the merged contact normals need to be normalized + if (*_const_type_cast_union<bool>(&contact->normal[dV3E_PAD])) { + + if (!dxSafeNormalize3(contact->normal)) { + // If the contact normals have added up to zero, erase the contact + // Normally the time step is to be shorter so that the objects do not get into each other that deep + --contactCount; + + if (k != contactCount) { + dContactGeom *lastContact = SAFECONTACT(flags, contacts, contactCount, skip); + *contact = *lastContact; + } + + stayWithinThisIndex = true; + } + } + + if (!stayWithinThisIndex) { + ++k; + } + } + + return contactCount; +} + +static +unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, + const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip, + ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount) +{ + unsigned maxContacts = (flags & NUMC_MASK); + + dVector3 pos; + dCopyVector3(pos, contact->pos); + + dQuaternion q1[2], q2[2]; + dReal perturbationAngle = CONTACT_PERTURBATION_ANGLE; + + dVector3 upAxis; + bool upAvailable = false; + if (fabs(contact->normal[dV3E_Y]) > 0.7) { + dAssignVector3(upAxis, 0, 0, 1); + } + else { + dAssignVector3(upAxis, 0, 1, 0); + } + + dVector3 cross; + dCalcVectorCross3(cross, contact->normal, upAxis); + + if (dSafeNormalize3(cross)) { + dCalcVectorCross3(upAxis, cross, contact->normal); + + if (dSafeNormalize3(upAxis)) { + upAvailable = true; + } + } + + for (unsigned j = upAvailable ? 0 : 2; j != 2; ++j) { + dQFromAxisAndAngle(q1[j], upAxis[dV3E_X], upAxis[dV3E_Y], upAxis[dV3E_Z], perturbationAngle); + dQFromAxisAndAngle(q2[j], cross[dV3E_X], cross[dV3E_Y], cross[dV3E_Z], perturbationAngle); + perturbationAngle = -perturbationAngle; + } + + for (unsigned k = upAvailable ? 0 : 4; k != 4; ++k) { + dQuaternion qr; + dQMultiply0(qr, q1[k % 2], q2[k / 2]); + + for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) { + dVector3 p, perturbed; + dSubtractVectors3(p, triangle[j], pos); + dQuatTransform(qr, p, perturbed); + dAddVectors3(perturbed, perturbed, pos); + + ccdVec3Set(&c2->vertices[j], (ccd_real_t)perturbed[dV3E_X], (ccd_real_t)perturbed[dV3E_Y], (ccd_real_t)perturbed[dV3E_Z]); + } + + dContactGeom perturbedContact; + setObjPosToTriangleCenter(c2); + + if (ccdCollide(o1, o2, flags, &perturbedContact, skip, c1, &ccdSupportConvex, &ccdCenter, c2, &ccdSupportTriangle, &ccdCenter) == 1) { + perturbedContact.side2 = contact->side2; + + if (meshFaceAngleView == NULL || correctTriangleContactNormal(c2, &perturbedContact, meshFaceAngleView, indices, numIndices)) { + contacCount = addUniqueContact(contacts, &perturbedContact, contacCount, maxContacts, flags, skip); + } + } + } + + return contacCount; +} + +static +bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, + IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices) +{ + dIASSERT(meshFaceAngleView != NULL); + + bool anyFault = false; + + ccd_vec3_t cntOrigNormal, cntNormal; + ccdVec3Set(&cntNormal, contact->normal[0], contact->normal[1], contact->normal[2]); + ccdVec3Copy(&cntOrigNormal, &cntNormal); + + // Check if the contact point is located close to any edge - move it back and forth + // and check the resulting segment for intersection with the edge plane + ccd_vec3_t cntScaledNormal; + ccdVec3CopyScaled(&cntScaledNormal, &cntNormal, contact->depth); + + ccd_vec3_t edges[dMTV__MAX]; + ccdVec3Sub2(&edges[dMTV_THIRD], &t->vertices[0], &t->vertices[2]); + ccdVec3Sub2(&edges[dMTV_SECOND], &t->vertices[2], &t->vertices[1]); + ccdVec3Sub2(&edges[dMTV_FIRST], &t->vertices[1], &t->vertices[0]); + dSASSERT(dMTV__MAX == 3); + + bool contactGenerated = false, contactPreserved = false; + // Triangle face normal + ccd_vec3_t triNormal; + ccdVec3Cross(&triNormal, &edges[dMTV_FIRST], &edges[dMTV_SECOND]); + if (ccdVec3SafeNormalize(&triNormal) != 0) { + anyFault = true; + } + + // Check the edges to see if one of them is involved + for (unsigned testEdgeIndex = !anyFault ? dMTV__MIN : dMTV__MAX; testEdgeIndex != dMTV__MAX; ++testEdgeIndex) { + ccd_vec3_t edgeNormal, vertexToPos, v; + ccd_vec3_t &edgeAxis = edges[testEdgeIndex]; + + // Edge axis + if (ccdVec3SafeNormalize(&edgeAxis) != 0) { + // This should not happen normally as in the case on of edges is degenerated + // the triangle normal calculation would have to fail above. If for some + // reason the above calculation succeeds and this one would not, it is + // OK to break as this point as well. + anyFault = true; + break; + } + + // Edge Normal + ccdVec3Cross(&edgeNormal, &edgeAxis, &triNormal); + // ccdVec3Normalize(&edgeNormal); -- the two vectors above were already normalized and perpendicular + + // Check if the contact point is located close to any edge - move it back and forth + // and check the resulting segment for intersection with the edge plane + ccdVec3Set(&vertexToPos, contact->pos[0], contact->pos[1], contact->pos[2]); + ccdVec3Sub(&vertexToPos, &t->vertices[testEdgeIndex]); + ccdVec3Sub2(&v, &vertexToPos, &cntScaledNormal); + + if (ccdVec3Dot(&edgeNormal, &v) < 0) { + ccdVec3Add2(&v, &vertexToPos, &cntScaledNormal); + + if (ccdVec3Dot(&edgeNormal, &v) > 0) { + // This is an edge contact + + ccd_real_t x = ccdVec3Dot(&triNormal, &cntNormal); + ccd_real_t y = ccdVec3Dot(&edgeNormal, &cntNormal); + ccd_real_t contactNormalToTriangleNormalAngle = CCD_ATAN2(y, x); + + dReal angleValueAsDRead; + FaceAngleDomain angleDomain = meshFaceAngleView->retrieveFacesAngleFromStorage(angleValueAsDRead, contact->side2, (dMeshTriangleVertex)testEdgeIndex); + ccd_real_t angleValue = (ccd_real_t)angleValueAsDRead; + + ccd_real_t targetAngle; + contactGenerated = false, contactPreserved = false; // re-assign to make optimizer's task easier + + if (angleDomain != FAD_CONCAVE) { + // Convex or flat - ensure the contact normal is within the allowed range + // formed by the two triangles' normals. + if (contactNormalToTriangleNormalAngle < CCD_ZERO) { + targetAngle = CCD_ZERO; + } + else if (contactNormalToTriangleNormalAngle > angleValue) { + targetAngle = angleValue; + } + else { + contactPreserved = true; + } + } + else { + // Concave - rotate the contact normal to the face angle bisect plane + // (or to triangle normal-edge plane if negative angles are not stored) + targetAngle = angleValue != 0 ? CCD_REAL(0.5) * angleValue : CCD_ZERO; + // There is little chance the normal will initially match the correct plane, but still, a small check could save lots of calculations + if (contactNormalToTriangleNormalAngle == targetAngle) { + contactPreserved = true; + } + } + + if (!contactPreserved) { + ccd_quat_t q; + ccdQuatSetAngleAxis(&q, targetAngle - contactNormalToTriangleNormalAngle, &edgeAxis); + ccdQuatRotVec2(&cntNormal, &cntNormal, &q); + contactGenerated = true; + } + + // Calculated successfully + break; + } + } + } + + if (!anyFault && !contactPreserved) { + // No edge contact detected, set contact normal to triangle normal + const ccd_vec3_t &cntNormalToUse = !contactGenerated ? triNormal : cntNormal; + + contact->normal[dV3E_X] = ccdVec3X(&cntNormalToUse); + contact->normal[dV3E_Y] = ccdVec3Y(&cntNormalToUse); + contact->normal[dV3E_Z] = ccdVec3Z(&cntNormalToUse); + contact->depth *= CCD_FMAX(0.0, ccdVec3Dot(&cntOrigNormal, &cntNormalToUse)); + } + + bool result = !anyFault; + return result; +} + + +static +unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip) +{ + dReal minDepth = c->depth; + unsigned index = contactcount; + bool isDuplicate = false; + + dReal c_posX = c->pos[dV3E_X], c_posY = c->pos[dV3E_Y], c_posZ = c->pos[dV3E_Z]; + for (unsigned k = 0; k != contactcount; k++) { + dContactGeom* pc = SAFECONTACT(flags, contacts, k, skip); + + if (fabs(c_posX - pc->pos[dV3E_X]) < CONTACT_POS_EPSILON + && fabs(c_posY - pc->pos[dV3E_Y]) < CONTACT_POS_EPSILON + && fabs(c_posZ - pc->pos[dV3E_Z]) < CONTACT_POS_EPSILON) { + dSASSERT(dV3E__AXES_MAX - dV3E__AXES_MIN == 3); + + // Accumulate similar contacts + dAddVectors3(pc->normal, pc->normal, c->normal); + pc->depth = dMax(pc->depth, c->depth); + *_type_cast_union<bool>(&pc->normal[dV3E_PAD]) = true; // Mark the contact as a merged one + + isDuplicate = true; + break; + } + + if (contactcount == maxcontacts && pc->depth < minDepth) { + minDepth = pc->depth; + index = k; + } + } + + if (!isDuplicate && index < maxcontacts) { + dContactGeom* contact = SAFECONTACT(flags, contacts, index, skip); + contact->g1 = c->g1; + contact->g2 = c->g2; + contact->depth = c->depth; + contact->side1 = c->side1; + contact->side2 = c->side2; + dCopyVector3(contact->pos, c->pos); + dCopyVector3(contact->normal, c->normal); + *_type_cast_union<bool>(&contact->normal[dV3E_PAD]) = false; // Indicates whether the contact is merged or not + contactcount = index == contactcount ? contactcount + 1 : contactcount; + } + + return contactcount; +} + +static +void setObjPosToTriangleCenter(ccd_triangle_t *t) +{ + ccdVec3Set(&t->o.pos, 0, 0, 0); + for (int j = 0; j < 3; j++) { + ccdVec3Add(&t->o.pos, &t->vertices[j]); + } + ccdVec3Scale(&t->o.pos, 1.0f / 3.0f); +} + +static +void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_triangle_t* o = (ccd_triangle_t *) obj; + ccd_real_t maxdot, dot; + maxdot = -CCD_REAL_MAX; + for (unsigned i = 0; i != 3; i++) { + dot = ccdVec3Dot(_dir, &o->vertices[i]); + if (dot > maxdot) { + ccdVec3Copy(v, &o->vertices[i]); + maxdot = dot; + } + } +} + + +#endif // dTRIMESH_ENABLED diff --git a/libs/ode-0.16.1/ode/src/collision_libccd.h b/libs/ode-0.16.1/ode/src/collision_libccd.h new file mode 100644 index 0000000..13c67ba --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_libccd.h @@ -0,0 +1,44 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _LIBCCD_COLLISION_H_ +#define _LIBCCD_COLLISION_H_ + +int dCollideCylinderCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideBoxCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideCapsuleCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideConvexBoxCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideConvexCapsuleCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideConvexCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideConvexSphereCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +unsigned dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip); + +#endif /* _LIBCCD_COLLISION_H_ */ diff --git a/libs/ode-0.16.1/ode/src/collision_quadtreespace.cpp b/libs/ode-0.16.1/ode/src/collision_quadtreespace.cpp new file mode 100644 index 0000000..200b20f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_quadtreespace.cpp @@ -0,0 +1,609 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// QuadTreeSpace by Erwin de Vries. +// With math corrections by Oleh Derevenko. ;) + +#include <ode/common.h> +#include <ode/collision_space.h> +#include <ode/collision.h> +#include "config.h" +#include "matrix.h" +#include "collision_kernel.h" + +#include "collision_space_internal.h" + + +#define AXIS0 0 +#define AXIS1 1 +#define UP 2 + +//#define DRAWBLOCKS + +const int SPLITAXIS = 2; +const int SPLITS = SPLITAXIS * SPLITAXIS; + +#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE) + +class Block{ +public: + dReal mMinX, mMaxX; + dReal mMinZ, mMaxZ; + + dGeomID mFirst; + int mGeomCount; + + Block* mParent; + Block* mChildren; + + void Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks); + + void Collide(void* UserData, dNearCallback* Callback); + void Collide(dGeomID g1, dGeomID g2, void* UserData, dNearCallback* Callback); + + void CollideLocal(dGeomID g2, void* UserData, dNearCallback* Callback); + + void AddObject(dGeomID Object); + void DelObject(dGeomID Object); + void Traverse(dGeomID Object); + + bool Inside(const dReal* AABB); + + Block* GetBlock(const dReal* AABB); + Block* GetBlockChild(const dReal* AABB); +}; + + +#ifdef DRAWBLOCKS +#include "..\..\Include\drawstuff\\drawstuff.h" + +static void DrawBlock(Block* Block){ + dVector3 v[8]; + v[0][AXIS0] = Block->mMinX; + v[0][UP] = REAL(-1.0); + v[0][AXIS1] = Block->mMinZ; + + v[1][AXIS0] = Block->mMinX; + v[1][UP] = REAL(-1.0); + v[1][AXIS1] = Block->mMaxZ; + + v[2][AXIS0] = Block->mMaxX; + v[2][UP] = REAL(-1.0); + v[2][AXIS1] = Block->mMinZ; + + v[3][AXIS0] = Block->mMaxX; + v[3][UP] = REAL(-1.0); + v[3][AXIS1] = Block->mMaxZ; + + v[4][AXIS0] = Block->mMinX; + v[4][UP] = REAL(1.0); + v[4][AXIS1] = Block->mMinZ; + + v[5][AXIS0] = Block->mMinX; + v[5][UP] = REAL(1.0); + v[5][AXIS1] = Block->mMaxZ; + + v[6][AXIS0] = Block->mMaxX; + v[6][UP] = REAL(1.0); + v[6][AXIS1] = Block->mMinZ; + + v[7][AXIS0] = Block->mMaxX; + v[7][UP] = REAL(1.0); + v[7][AXIS1] = Block->mMaxZ; + + // Bottom + dsDrawLine(v[0], v[1]); + dsDrawLine(v[1], v[3]); + dsDrawLine(v[3], v[2]); + dsDrawLine(v[2], v[0]); + + // Top + dsDrawLine(v[4], v[5]); + dsDrawLine(v[5], v[7]); + dsDrawLine(v[7], v[6]); + dsDrawLine(v[6], v[4]); + + // Sides + dsDrawLine(v[0], v[4]); + dsDrawLine(v[1], v[5]); + dsDrawLine(v[2], v[6]); + dsDrawLine(v[3], v[7]); +} +#endif //DRAWBLOCKS + + +void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks){ + dIASSERT(MinX <= MaxX); + dIASSERT(MinZ <= MaxZ); + + mGeomCount = 0; + mFirst = 0; + + mMinX = MinX; + mMaxX = MaxX; + + mMinZ = MinZ; + mMaxZ = MaxZ; + + this->mParent = Parent; + + if (Depth > 0){ + mChildren = Blocks; + Blocks += SPLITS; + + const dReal ChildExtentX = (MaxX - MinX) / SPLITAXIS; + const dReal ChildExtentZ = (MaxZ - MinZ) / SPLITAXIS; + + const int ChildDepth = Depth - 1; + int Index = 0; + + dReal ChildRightX = MinX; + for (int i = 0; i < SPLITAXIS; i++){ + const dReal ChildLeftX = ChildRightX; + ChildRightX = (i != SPLITAXIS - 1) ? ChildLeftX + ChildExtentX : MaxX; + + dReal ChildRightZ = MinZ; + for (int j = 0; j < SPLITAXIS; j++){ + const dReal ChildLeftZ = ChildRightZ; + ChildRightZ = (j != SPLITAXIS - 1) ? ChildLeftZ + ChildExtentZ : MaxZ; + + mChildren[Index].Create(ChildLeftX, ChildRightX, ChildLeftZ, ChildRightZ, this, ChildDepth, Blocks); + ++Index; + } + } + } + else mChildren = 0; +} + +void Block::Collide(void* UserData, dNearCallback* Callback){ +#ifdef DRAWBLOCKS + DrawBlock(this); +#endif + // Collide the local list + dxGeom* g = mFirst; + while (g){ + if (GEOM_ENABLED(g)){ + Collide(g, g->next_ex, UserData, Callback); + } + g = g->next_ex; + } + + // Recurse for children + if (mChildren){ + for (int i = 0; i < SPLITS; i++){ + Block &CurrentChild = mChildren[i]; + if (CurrentChild.mGeomCount <= 1){ // Early out + continue; + } + CurrentChild.Collide(UserData, Callback); + } + } +} + +// Note: g2 is assumed to be in this Block +void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){ +#ifdef DRAWBLOCKS + DrawBlock(this); +#endif + // Collide against local list + while (g2){ + if (GEOM_ENABLED(g2)){ + collideAABBs (g1, g2, UserData, Callback); + } + g2 = g2->next_ex; + } + + // Collide against children + if (mChildren){ + for (int i = 0; i < SPLITS; i++){ + Block &CurrentChild = mChildren[i]; + // Early out for empty blocks + if (CurrentChild.mGeomCount == 0){ + continue; + } + + // Does the geom's AABB collide with the block? + // Don't do AABB tests for single geom blocks. + if (CurrentChild.mGeomCount == 1){ + // + } + else if (true){ + if (g1->aabb[AXIS0 * 2 + 0] >= CurrentChild.mMaxX || + g1->aabb[AXIS0 * 2 + 1] < CurrentChild.mMinX || + g1->aabb[AXIS1 * 2 + 0] >= CurrentChild.mMaxZ || + g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ) continue; + } + CurrentChild.Collide(g1, CurrentChild.mFirst, UserData, Callback); + } + } +} + +void Block::CollideLocal(dxGeom* g2, void* UserData, dNearCallback* Callback){ + // Collide against local list + dxGeom* g1 = mFirst; + while (g1){ + if (GEOM_ENABLED(g1)){ + collideAABBs (g1, g2, UserData, Callback); + } + g1 = g1->next_ex; + } +} + +void Block::AddObject(dGeomID Object){ + // Add the geom + Object->next_ex = mFirst; + mFirst = Object; + Object->tome_ex = (dxGeom**)this; + + // Now traverse upwards to tell that we have a geom + Block* Block = this; + do{ + Block->mGeomCount++; + Block = Block->mParent; + } + while (Block); +} + +void Block::DelObject(dGeomID Object){ + // Del the geom + dxGeom* g = mFirst; + dxGeom* Last = 0; + while (g){ + if (g == Object){ + if (Last){ + Last->next_ex = g->next_ex; + } + else mFirst = g->next_ex; + + break; + } + Last = g; + g = g->next_ex; + } + + Object->tome_ex = 0; + + // Now traverse upwards to tell that we have lost a geom + Block* Block = this; + do{ + Block->mGeomCount--; + Block = Block->mParent; + } + while (Block); +} + +void Block::Traverse(dGeomID Object){ + Block* NewBlock = GetBlock(Object->aabb); + + if (NewBlock != this){ + // Remove the geom from the old block and add it to the new block. + // This could be more optimal, but the loss should be very small. + DelObject(Object); + NewBlock->AddObject(Object); + } +} + +bool Block::Inside(const dReal* AABB){ + return AABB[AXIS0 * 2 + 0] >= mMinX && AABB[AXIS0 * 2 + 1] < mMaxX && AABB[AXIS1 * 2 + 0] >= mMinZ && AABB[AXIS1 * 2 + 1] < mMaxZ; +} + +Block* Block::GetBlock(const dReal* AABB){ + if (Inside(AABB)){ + return GetBlockChild(AABB); // Child or this will have a good block + } + else if (mParent){ + return mParent->GetBlock(AABB); // Parent has a good block + } + else return this; // We are at the root, so we have little choice +} + +Block* Block::GetBlockChild(const dReal* AABB){ + if (mChildren){ + for (int i = 0; i < SPLITS; i++){ + Block &CurrentChild = mChildren[i]; + if (CurrentChild.Inside(AABB)){ + return CurrentChild.GetBlockChild(AABB); // Child will have good block + } + } + } + return this; // This is the best block +} + +//**************************************************************************** +// quadtree space + +struct dxQuadTreeSpace : public dxSpace{ + Block* Blocks; // Blocks[0] is the root + + dArray<dxGeom*> DirtyList; + + dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth); + ~dxQuadTreeSpace(); + + dxGeom* getGeom(int i); + + void add(dxGeom* g); + void remove(dxGeom* g); + void dirty(dxGeom* g); + + void computeAABB(); + + void cleanGeoms(); + void collide(void* UserData, dNearCallback* Callback); + void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback); + + // Temp data + Block* CurrentBlock; // Only used while enumerating + int* CurrentChild; // Only used while enumerating + int CurrentLevel; // Only used while enumerating + dxGeom* CurrentObject; // Only used while enumerating + int CurrentIndex; +}; + +namespace { + + inline + sizeint numNodes(int depth) + { + // A 4-ary tree has (4^(depth+1) - 1)/3 nodes + // Note: split up into multiple constant expressions for readability + const int k = depth+1; + const sizeint fourToNthPlusOne = (sizeint)1 << (2*k); // 4^k = 2^(2k) + return (fourToNthPlusOne - 1) / 3; + } + +} + + + +dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space){ + type = dQuadTreeSpaceClass; + + sizeint BlockCount = numNodes(Depth); + + Blocks = (Block*)dAlloc(BlockCount * sizeof(Block)); + Block* Blocks = this->Blocks + 1; // This pointer gets modified! + + dReal MinX = Center[AXIS0] - Extents[AXIS0]; + dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity); + dReal MinZ = Center[AXIS1] - Extents[AXIS1]; + dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity); + this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, Blocks); + + CurrentBlock = 0; + CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int)); + CurrentLevel = 0; + CurrentObject = 0; + CurrentIndex = -1; + + // Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + +dxQuadTreeSpace::~dxQuadTreeSpace(){ + int Depth = 0; + Block* Current = &Blocks[0]; + while (Current){ + Depth++; + Current = Current->mChildren; + } + + sizeint BlockCount = numNodes(Depth); + + dFree(Blocks, BlockCount * sizeof(Block)); + dFree(CurrentChild, (Depth + 1) * sizeof(int)); +} + +dxGeom* dxQuadTreeSpace::getGeom(int Index){ + dUASSERT(Index >= 0 && Index < count, "index out of range"); + + //@@@ + dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented"); + + return 0; + + // This doesnt work +/* + if (CurrentIndex == Index){ + // Loop through all objects in the local list +CHILDRECURSE: + if (CurrentObject){ + dGeomID g = CurrentObject; + CurrentObject = CurrentObject->next_ex; + CurrentIndex++; + +#ifdef DRAWBLOCKS + DrawBlock(CurrentBlock); +#endif //DRAWBLOCKS + return g; + } + else{ + // Now lets loop through our children. Starting at index 0. + if (CurrentBlock->Children){ + CurrentChild[CurrentLevel] = 0; +PARENTRECURSE: + for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){ + if (CurrentBlock->Children[i].GeomCount == 0){ + continue; + } + CurrentBlock = &CurrentBlock->Children[i]; + CurrentObject = CurrentBlock->First; + + i++; + + CurrentLevel++; + goto CHILDRECURSE; + } + } + } + + // Now lets go back to the parent so it can continue processing its other children. + if (CurrentBlock->Parent){ + CurrentBlock = CurrentBlock->Parent; + CurrentLevel--; + goto PARENTRECURSE; + } + } + else{ + CurrentBlock = &Blocks[0]; + CurrentLevel = 0; + CurrentObject = CurrentObject; + CurrentIndex = 0; + + // Other states are already set + CurrentObject = CurrentBlock->First; + } + + + if (current_geom && current_index == Index - 1){ + //current_geom = current_geom->next_ex; // next + current_index = Index; + return current_geom; + } + else for (int i = 0; i < Index; i++){ // this will be verrrrrrry slow + getGeom(i); + } +*/ + + return 0; +} + +void dxQuadTreeSpace::add(dxGeom* g){ + CHECK_NOT_LOCKED (this); + dAASSERT(g); + dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space"); + + DirtyList.push(g); + Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block + + dxSpace::add(g); +} + +void dxQuadTreeSpace::remove(dxGeom* g){ + CHECK_NOT_LOCKED(this); + dAASSERT(g); + dUASSERT(g->parent_space == this,"object is not in this space"); + + // remove + ((Block*)g->tome_ex)->DelObject(g); + + for (int i = 0; i < DirtyList.size(); i++){ + if (DirtyList[i] == g){ + DirtyList.remove(i); + // (mg) there can be multiple instances of a dirty object on stack be sure to remove ALL and not just first, for this we decrement i + --i; + } + } + + dxSpace::remove(g); +} + +void dxQuadTreeSpace::dirty(dxGeom* g){ + DirtyList.push(g); +} + +void dxQuadTreeSpace::computeAABB(){ + // +} + +void dxQuadTreeSpace::cleanGeoms(){ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + + for (int i = 0; i < DirtyList.size(); i++){ + dxGeom* g = DirtyList[i]; + if (IS_SPACE(g)){ + ((dxSpace*)g)->cleanGeoms(); + } + + g->recomputeAABB(); + dIASSERT((g->gflags & GEOM_AABB_BAD) == 0); + + g->gflags &= ~GEOM_DIRTY; + + ((Block*)g->tome_ex)->Traverse(g); + } + DirtyList.setSize(0); + + lock_count--; +} + +void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){ + dAASSERT(Callback); + + lock_count++; + cleanGeoms(); + + Blocks[0].Collide(UserData, Callback); + + lock_count--; +} + + +struct DataCallback { + void *data; + dNearCallback *callback; +}; +// Invokes the callback with arguments swapped +static void swap_callback(void *data, dxGeom *g1, dxGeom *g2) +{ + DataCallback *dc = (DataCallback*)data; + dc->callback(dc->data, g2, g1); +} + + +void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callback){ + dAASSERT(g2 && Callback); + + lock_count++; + cleanGeoms(); + g2->recomputeAABB(); + + if (g2->parent_space == this){ + // The block the geom is in + Block* CurrentBlock = (Block*)g2->tome_ex; + + // Collide against block and its children + DataCallback dc = {UserData, Callback}; + CurrentBlock->Collide(g2, CurrentBlock->mFirst, &dc, swap_callback); + + // Collide against parents + while ((CurrentBlock = CurrentBlock->mParent)) + CurrentBlock->CollideLocal(g2, UserData, Callback); + + } + else { + DataCallback dc = {UserData, Callback}; + Blocks[0].Collide(g2, Blocks[0].mFirst, &dc, swap_callback); + } + + lock_count--; +} + +dSpaceID dQuadTreeSpaceCreate(dxSpace* space, const dVector3 Center, const dVector3 Extents, int Depth){ + return new dxQuadTreeSpace(space, Center, Extents, Depth); +} diff --git a/libs/ode-0.16.1/ode/src/collision_sapspace.cpp b/libs/ode-0.16.1/ode/src/collision_sapspace.cpp new file mode 100644 index 0000000..76258bf --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_sapspace.cpp @@ -0,0 +1,853 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Sweep and Prune adaptation/tweaks for ODE by Aras Pranckevicius. + * Additional work by David Walters + * Original code: + * OPCODE - Optimized Collision Detection + * Copyright (C) 2001 Pierre Terdiman + * Homepage: http://www.codercorner.com/Opcode.htm + * + * This version does complete radix sort, not "classical" SAP. So, we + * have no temporal coherence, but are able to handle any movement + * velocities equally well. + */ + +#include <ode/common.h> +#include <ode/collision_space.h> +#include <ode/collision.h> + +#include "config.h" +#include "matrix.h" +#include "collision_kernel.h" +#include "collision_space_internal.h" + +// Reference counting helper for radix sort global data. +//static void RadixSortRef(); +//static void RadixSortDeref(); + + +// -------------------------------------------------------------------------- +// Radix Sort Context +// -------------------------------------------------------------------------- + +struct RaixSortContext +{ +public: + RaixSortContext(): mCurrentSize(0), mCurrentUtilization(0), mRanksValid(false), mRanksBuffer(NULL), mPrimaryRanks(NULL) {} + ~RaixSortContext() { FreeRanks(); } + + // OPCODE's Radix Sorting, returns a list of indices in sorted order + const uint32* RadixSort( const float* input2, uint32 nb ); + +private: + void FreeRanks(); + void AllocateRanks(sizeint nNewSize); + + void ReallocateRanksIfNecessary(sizeint nNewSize); + +private: + void SetCurrentSize(sizeint nValue) { mCurrentSize = nValue; } + sizeint GetCurrentSize() const { return mCurrentSize; } + + void SetCurrentUtilization(sizeint nValue) { mCurrentUtilization = nValue; } + sizeint GetCurrentUtilization() const { return mCurrentUtilization; } + + uint32 *GetRanks1() const { return mPrimaryRanks; } + uint32 *GetRanks2() const { return mRanksBuffer + ((mRanksBuffer + mCurrentSize) - mPrimaryRanks); } + void SwapRanks() { mPrimaryRanks = GetRanks2(); } + + bool AreRanksValid() const { return mRanksValid; } + void InvalidateRanks() { mRanksValid = false; } + void ValidateRanks() { mRanksValid = true; } + +private: + sizeint mCurrentSize; //!< Current size of the indices list + sizeint mCurrentUtilization; //!< Current utilization of the indices list + bool mRanksValid; + uint32* mRanksBuffer; //!< Two lists allocated sequentially in a single block + uint32* mPrimaryRanks; +}; + +void RaixSortContext::AllocateRanks(sizeint nNewSize) +{ + dIASSERT(GetCurrentSize() == 0); + + mRanksBuffer = new uint32[2 * nNewSize]; + mPrimaryRanks = mRanksBuffer; + + SetCurrentSize(nNewSize); +} + +void RaixSortContext::FreeRanks() +{ + SetCurrentSize(0); + + delete[] mRanksBuffer; +} + +void RaixSortContext::ReallocateRanksIfNecessary(sizeint nNewSize) +{ + sizeint nCurUtilization = GetCurrentUtilization(); + + if (nNewSize != nCurUtilization) + { + sizeint nCurSize = GetCurrentSize(); + + if ( nNewSize > nCurSize ) + { + // Free previously used ram + FreeRanks(); + + // Get some fresh one + AllocateRanks(nNewSize); + } + + InvalidateRanks(); + SetCurrentUtilization(nNewSize); + } +} + +// -------------------------------------------------------------------------- +// SAP space code +// -------------------------------------------------------------------------- + +struct dxSAPSpace : public dxSpace +{ + // Constructor / Destructor + dxSAPSpace( dSpaceID _space, int sortaxis ); + ~dxSAPSpace(); + + // dxSpace + virtual dxGeom* getGeom(int i); + virtual void add(dxGeom* g); + virtual void remove(dxGeom* g); + virtual void dirty(dxGeom* g); + virtual void computeAABB(); + virtual void cleanGeoms(); + virtual void collide( void *data, dNearCallback *callback ); + virtual void collide2( void *data, dxGeom *geom, dNearCallback *callback ); + +private: + + //-------------------------------------------------------------------------- + // Local Declarations + //-------------------------------------------------------------------------- + + //! A generic couple structure + struct Pair + { + uint32 id0; //!< First index of the pair + uint32 id1; //!< Second index of the pair + + // Default and Value Constructor + Pair() {} + Pair( uint32 i0, uint32 i1 ) : id0( i0 ), id1( i1 ) {} + }; + + //-------------------------------------------------------------------------- + // Helpers + //-------------------------------------------------------------------------- + + /** + * Complete box pruning. + * Returns a list of overlapping pairs of boxes, each box of the pair + * belongs to the same set. + * + * @param count [in] number of boxes. + * @param geoms [in] geoms of boxes. + * @param pairs [out] array of overlapping pairs. + */ + void BoxPruning( int count, const dxGeom** geoms, dArray< Pair >& pairs ); + + + //-------------------------------------------------------------------------- + // Implementation Data + //-------------------------------------------------------------------------- + + // We have two lists (arrays of pointers) to dirty and clean + // geoms. Each geom knows it's index into the corresponding list + // (see macros above). + dArray<dxGeom*> DirtyList; // dirty geoms + dArray<dxGeom*> GeomList; // clean geoms + + // For SAP, we ultimately separate "normal" geoms and the ones that have + // infinite AABBs. No point doing SAP on infinite ones (and it doesn't handle + // infinite geoms anyway). + dArray<dxGeom*> TmpGeomList; // temporary for normal geoms + dArray<dxGeom*> TmpInfGeomList; // temporary for geoms with infinite AABBs + + // Our sorting axes. (X,Z,Y is often best). Stored *2 for minor speedup + // Axis indices into geom's aabb are: min=idx, max=idx+1 + uint32 ax0idx; + uint32 ax1idx; + uint32 ax2idx; + + // pruning position array scratch pad + // NOTE: this is float not dReal because of the OPCODE radix sorter + dArray< float > poslist; + RaixSortContext sortContext; +}; + +// Creation +dSpaceID dSweepAndPruneSpaceCreate( dxSpace* space, int axisorder ) { + return new dxSAPSpace( space, axisorder ); +} + + +//============================================================================== + +#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE) + +// HACK: We abuse 'next' and 'tome' members of dxGeom to store indices into dirty/geom lists. +#define GEOM_SET_DIRTY_IDX(g,idx) { (g)->next_ex = (dxGeom*)(sizeint)(idx); } +#define GEOM_SET_GEOM_IDX(g,idx) { (g)->tome_ex = (dxGeom**)(sizeint)(idx); } +#define GEOM_GET_DIRTY_IDX(g) ((int)(sizeint)(g)->next_ex) +#define GEOM_GET_GEOM_IDX(g) ((int)(sizeint)(g)->tome_ex) +#define GEOM_INVALID_IDX (-1) + + +/* +* A bit of repetitive work - similar to collideAABBs, but doesn't check +* if AABBs intersect (because SAP returns pairs with overlapping AABBs). +*/ +static void collideGeomsNoAABBs( dxGeom *g1, dxGeom *g2, void *data, dNearCallback *callback ) +{ + dIASSERT( (g1->gflags & GEOM_AABB_BAD)==0 ); + dIASSERT( (g2->gflags & GEOM_AABB_BAD)==0 ); + + // no contacts if both geoms on the same body, and the body is not 0 + if (g1->body == g2->body && g1->body) return; + + // test if the category and collide bitfields match + if ( ((g1->category_bits & g2->collide_bits) || + (g2->category_bits & g1->collide_bits)) == 0) { + return; + } + + dReal *bounds1 = g1->aabb; + dReal *bounds2 = g2->aabb; + + // check if either object is able to prove that it doesn't intersect the + // AABB of the other + if (g1->AABBTest (g2,bounds2) == 0) return; + if (g2->AABBTest (g1,bounds1) == 0) return; + + // the objects might actually intersect - call the space callback function + callback (data,g1,g2); +} + + +dxSAPSpace::dxSAPSpace( dSpaceID _space, int axisorder ) : dxSpace( _space ) +{ + type = dSweepAndPruneSpaceClass; + + // Init AABB to infinity + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + + ax0idx = ( ( axisorder ) & 3 ) << 1; + ax1idx = ( ( axisorder >> 2 ) & 3 ) << 1; + ax2idx = ( ( axisorder >> 4 ) & 3 ) << 1; +} + +dxSAPSpace::~dxSAPSpace() +{ + CHECK_NOT_LOCKED(this); + if ( cleanup ) { + // note that destroying each geom will call remove() + for ( ; DirtyList.size(); dGeomDestroy( DirtyList[ 0 ] ) ) {} + for ( ; GeomList.size(); dGeomDestroy( GeomList[ 0 ] ) ) {} + } + else { + // just unhook them + for ( ; DirtyList.size(); remove( DirtyList[ 0 ] ) ) {} + for ( ; GeomList.size(); remove( GeomList[ 0 ] ) ) {} + } +} + +dxGeom* dxSAPSpace::getGeom( int i ) +{ + dUASSERT( i >= 0 && i < count, "index out of range" ); + int dirtySize = DirtyList.size(); + if( i < dirtySize ) + return DirtyList[i]; + else + return GeomList[i-dirtySize]; +} + +void dxSAPSpace::add( dxGeom* g ) +{ + CHECK_NOT_LOCKED (this); + dAASSERT(g); + dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space"); + + + // add to dirty list + GEOM_SET_DIRTY_IDX( g, DirtyList.size() ); + GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX ); + DirtyList.push( g ); + + dxSpace::add(g); +} + +void dxSAPSpace::remove( dxGeom* g ) +{ + CHECK_NOT_LOCKED(this); + dAASSERT(g); + dUASSERT(g->parent_space == this,"object is not in this space"); + + // remove + int dirtyIdx = GEOM_GET_DIRTY_IDX(g); + int geomIdx = GEOM_GET_GEOM_IDX(g); + // must be in one list, not in both + dUASSERT( + (dirtyIdx==GEOM_INVALID_IDX && geomIdx>=0 && geomIdx<GeomList.size()) || + (geomIdx==GEOM_INVALID_IDX && dirtyIdx>=0 && dirtyIdx<DirtyList.size()), + "geom indices messed up" ); + if( dirtyIdx != GEOM_INVALID_IDX ) { + // we're in dirty list, remove + int dirtySize = DirtyList.size(); + if (dirtyIdx != dirtySize-1) { + dxGeom* lastG = DirtyList[dirtySize-1]; + DirtyList[dirtyIdx] = lastG; + GEOM_SET_DIRTY_IDX(lastG,dirtyIdx); + } + GEOM_SET_DIRTY_IDX(g,GEOM_INVALID_IDX); + DirtyList.setSize( dirtySize-1 ); + } else { + // we're in geom list, remove + int geomSize = GeomList.size(); + if (geomIdx != geomSize-1) { + dxGeom* lastG = GeomList[geomSize-1]; + GeomList[geomIdx] = lastG; + GEOM_SET_GEOM_IDX(lastG,geomIdx); + } + GEOM_SET_GEOM_IDX(g,GEOM_INVALID_IDX); + GeomList.setSize( geomSize-1 ); + } + + dxSpace::remove(g); +} + +void dxSAPSpace::dirty( dxGeom* g ) +{ + dAASSERT(g); + dUASSERT(g->parent_space == this, "object is not in this space"); + + // check if already dirtied + int dirtyIdx = GEOM_GET_DIRTY_IDX(g); + if( dirtyIdx != GEOM_INVALID_IDX ) + return; + + int geomIdx = GEOM_GET_GEOM_IDX(g); + dUASSERT( geomIdx>=0 && geomIdx<GeomList.size(), "geom indices messed up" ); + + // remove from geom list, place last in place of this + int geomSize = GeomList.size(); + if (geomIdx != geomSize-1) { + dxGeom* lastG = GeomList[geomSize-1]; + GeomList[geomIdx] = lastG; + GEOM_SET_GEOM_IDX(lastG,geomIdx); + } + GeomList.setSize( geomSize-1 ); + + // add to dirty list + GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX ); + GEOM_SET_DIRTY_IDX( g, DirtyList.size() ); + DirtyList.push( g ); +} + +void dxSAPSpace::computeAABB() +{ + // TODO? +} + +void dxSAPSpace::cleanGeoms() +{ + int dirtySize = DirtyList.size(); + if( !dirtySize ) + return; + + // compute the AABBs of all dirty geoms, clear the dirty flags, + // remove from dirty list, place into geom list + lock_count++; + + int geomSize = GeomList.size(); + GeomList.setSize( geomSize + dirtySize ); // ensure space in geom list + + for( int i = 0; i < dirtySize; ++i ) { + dxGeom* g = DirtyList[i]; + if( IS_SPACE(g) ) { + ((dxSpace*)g)->cleanGeoms(); + } + + g->recomputeAABB(); + dIASSERT((g->gflags & GEOM_AABB_BAD) == 0); + + g->gflags &= ~GEOM_DIRTY; + + // remove from dirty list, add to geom list + GEOM_SET_DIRTY_IDX( g, GEOM_INVALID_IDX ); + GEOM_SET_GEOM_IDX( g, geomSize + i ); + GeomList[geomSize+i] = g; + } + // clear dirty list + DirtyList.setSize( 0 ); + + lock_count--; +} + +void dxSAPSpace::collide( void *data, dNearCallback *callback ) +{ + dAASSERT (callback); + + lock_count++; + + cleanGeoms(); + + // by now all geoms are in GeomList, and DirtyList must be empty + int geom_count = GeomList.size(); + dUASSERT( geom_count == count, "geom counts messed up" ); + + // separate all ENABLED geoms into infinite AABBs and normal AABBs + TmpGeomList.setSize(0); + TmpInfGeomList.setSize(0); + int axis0max = ax0idx + 1; + for( int i = 0; i < geom_count; ++i ) { + dxGeom* g = GeomList[i]; + if( !GEOM_ENABLED(g) ) // skip disabled ones + continue; + const dReal& amax = g->aabb[axis0max]; + if( amax == dInfinity ) // HACK? probably not... + TmpInfGeomList.push( g ); + else + TmpGeomList.push( g ); + } + + // do SAP on normal AABBs + dArray< Pair > overlapBoxes; + int tmp_geom_count = TmpGeomList.size(); + if ( tmp_geom_count > 0 ) + { + // Generate a list of overlapping boxes + BoxPruning( tmp_geom_count, (const dxGeom**)TmpGeomList.data(), overlapBoxes ); + } + + // collide overlapping + int overlapCount = overlapBoxes.size(); + for( int j = 0; j < overlapCount; ++j ) + { + const Pair& pair = overlapBoxes[ j ]; + dxGeom* g1 = TmpGeomList[ pair.id0 ]; + dxGeom* g2 = TmpGeomList[ pair.id1 ]; + collideGeomsNoAABBs( g1, g2, data, callback ); + } + + int infSize = TmpInfGeomList.size(); + int normSize = TmpGeomList.size(); + int m, n; + + for ( m = 0; m < infSize; ++m ) + { + dxGeom* g1 = TmpInfGeomList[ m ]; + + // collide infinite ones + for( n = m+1; n < infSize; ++n ) { + dxGeom* g2 = TmpInfGeomList[n]; + collideGeomsNoAABBs( g1, g2, data, callback ); + } + + // collide infinite ones with normal ones + for( n = 0; n < normSize; ++n ) { + dxGeom* g2 = TmpGeomList[n]; + collideGeomsNoAABBs( g1, g2, data, callback ); + } + } + + lock_count--; +} + +void dxSAPSpace::collide2( void *data, dxGeom *geom, dNearCallback *callback ) +{ + dAASSERT (geom && callback); + + // TODO: This is just a simple N^2 implementation + + lock_count++; + + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + int geom_count = GeomList.size(); + for ( int i = 0; i < geom_count; ++i ) { + dxGeom* g = GeomList[i]; + if ( GEOM_ENABLED(g) ) + collideAABBs (g,geom,data,callback); + } + + lock_count--; +} + + +void dxSAPSpace::BoxPruning( int count, const dxGeom** geoms, dArray< Pair >& pairs ) +{ + // Size the poslist (+1 for infinity end cap) + poslist.setSize( count ); + + // 1) Build main list using the primary axis + // NOTE: uses floats instead of dReals because that's what radix sort wants + for( int i = 0; i < count; ++i ) + poslist[ i ] = (float)TmpGeomList[i]->aabb[ ax0idx ]; + + // 2) Sort the list + const uint32* Sorted = sortContext.RadixSort( poslist.data(), count ); + + // 3) Prune the list + const uint32* const LastSorted = Sorted + count; + const uint32* RunningAddress = Sorted; + + bool bExitLoop; + Pair IndexPair; + while ( Sorted < LastSorted ) + { + IndexPair.id0 = *Sorted++; + + // empty, this loop just advances RunningAddress + for (bExitLoop = false; poslist[*RunningAddress++] < poslist[IndexPair.id0]; ) + { + if (RunningAddress == LastSorted) + { + bExitLoop = true; + break; + } + } + + if ( bExitLoop || RunningAddress == LastSorted) // Not a bug!!! + { + break; + } + + const float idx0ax0max = (float)geoms[IndexPair.id0]->aabb[ax0idx+1]; // To avoid wrong decisions caused by rounding errors, cast the AABB element to float similarly as we did at the function beginning + const dReal idx0ax1max = geoms[IndexPair.id0]->aabb[ax1idx+1]; + const dReal idx0ax2max = geoms[IndexPair.id0]->aabb[ax2idx+1]; + + for (const uint32* RunningAddress2 = RunningAddress; poslist[ IndexPair.id1 = *RunningAddress2++ ] <= idx0ax0max; ) + { + const dReal* aabb0 = geoms[ IndexPair.id0 ]->aabb; + const dReal* aabb1 = geoms[ IndexPair.id1 ]->aabb; + + // Intersection? + if ( idx0ax1max >= aabb1[ax1idx] && aabb1[ax1idx+1] >= aabb0[ax1idx] + && idx0ax2max >= aabb1[ax2idx] && aabb1[ax2idx+1] >= aabb0[ax2idx] ) + { + pairs.push( IndexPair ); + } + + if (RunningAddress2 == LastSorted) + { + break; + } + } + + } // while ( RunningAddress < LastSorted && Sorted < LastSorted ) +} + + +//============================================================================== + +//------------------------------------------------------------------------------ +// Radix Sort +//------------------------------------------------------------------------------ + + + +#define CHECK_PASS_VALIDITY(pass) \ + /* Shortcut to current counters */ \ + const uint32* CurCount = &mHistogram[pass<<8]; \ + \ + /* Reset flag. The sorting pass is supposed to be performed. (default) */ \ + bool PerformPass = true; \ + \ + /* Check pass validity */ \ + \ + /* If all values have the same byte, sorting is useless. */ \ + /* It may happen when sorting bytes or words instead of dwords. */ \ + /* This routine actually sorts words faster than dwords, and bytes */ \ + /* faster than words. Standard running time (O(4*n))is reduced to O(2*n) */ \ + /* for words and O(n) for bytes. Running time for floats depends on actual values... */ \ + \ + /* Get first byte */ \ + uint8 UniqueVal = *(((const uint8*)input)+pass); \ + \ + /* Check that byte's counter */ \ + if(CurCount[UniqueVal]==nb) PerformPass=false; + +// WARNING ONLY SORTS IEEE FLOATING-POINT VALUES +const uint32* RaixSortContext::RadixSort( const float* input2, uint32 nb ) +{ + union _type_cast_union + { + _type_cast_union(const float *floats): asFloats(floats) {} + _type_cast_union(const uint32 *uints32): asUInts32(uints32) {} + + const float *asFloats; + const uint32 *asUInts32; + const uint8 *asUInts8; + }; + + const uint32* input = _type_cast_union(input2).asUInts32; + + // Resize lists if needed + ReallocateRanksIfNecessary(nb); + + // Allocate histograms & offsets on the stack + uint32 mHistogram[256*4]; + uint32* mLink[256]; + + // Create histograms (counters). Counters for all passes are created in one run. + // Pros: read input buffer once instead of four times + // Cons: mHistogram is 4Kb instead of 1Kb + // Floating-point values are always supposed to be signed values, so there's only one code path there. + // Please note the floating point comparison needed for temporal coherence! Although the resulting asm code + // is dreadful, this is surprisingly not such a performance hit - well, I suppose that's a big one on first + // generation Pentiums....We can't make comparison on integer representations because, as Chris said, it just + // wouldn't work with mixed positive/negative values.... + { + /* Clear counters/histograms */ + memset(mHistogram, 0, 256*4*sizeof(uint32)); + + /* Prepare to count */ + const uint8* p = _type_cast_union(input).asUInts8; + const uint8* pe = &p[nb*4]; + uint32* h0= &mHistogram[0]; /* Histogram for first pass (LSB) */ + uint32* h1= &mHistogram[256]; /* Histogram for second pass */ + uint32* h2= &mHistogram[512]; /* Histogram for third pass */ + uint32* h3= &mHistogram[768]; /* Histogram for last pass (MSB) */ + + bool AlreadySorted = true; /* Optimism... */ + + if (!AreRanksValid()) + { + /* Prepare for temporal coherence */ + const float* Running = input2; + float PrevVal = *Running; + + while(p!=pe) + { + /* Read input input2 in previous sorted order */ + float Val = *Running++; + /* Check whether already sorted or not */ + if(Val<PrevVal) { AlreadySorted = false; break; } /* Early out */ + /* Update for next iteration */ + PrevVal = Val; + + /* Create histograms */ + h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; + } + + /* If all input values are already sorted, we just have to return and leave the */ + /* previous list unchanged. That way the routine may take advantage of temporal */ + /* coherence, for example when used to sort transparent faces. */ + if(AlreadySorted) + { + uint32* const Ranks1 = GetRanks1(); + for(uint32 i=0;i<nb;i++) Ranks1[i] = i; + return Ranks1; + } + } + else + { + /* Prepare for temporal coherence */ + uint32* const Ranks1 = GetRanks1(); + + uint32* Indices = Ranks1; + float PrevVal = input2[*Indices]; + + while(p!=pe) + { + /* Read input input2 in previous sorted order */ + float Val = input2[*Indices++]; + /* Check whether already sorted or not */ + if(Val<PrevVal) { AlreadySorted = false; break; } /* Early out */ + /* Update for next iteration */ + PrevVal = Val; + + /* Create histograms */ + h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; + } + + /* If all input values are already sorted, we just have to return and leave the */ + /* previous list unchanged. That way the routine may take advantage of temporal */ + /* coherence, for example when used to sort transparent faces. */ + if(AlreadySorted) { return Ranks1; } + } + + /* Else there has been an early out and we must finish computing the histograms */ + while(p!=pe) + { + /* Create histograms without the previous overhead */ + h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; + } + } + + // Compute #negative values involved if needed + uint32 NbNegativeValues = 0; + + // An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128 + // last values of the last histogram. Last histogram because that's the one for the Most Significant Byte, + // responsible for the sign. 128 last values because the 128 first ones are related to positive numbers. + uint32* h3= &mHistogram[768]; + for(uint32 i=128;i<256;i++) NbNegativeValues += h3[i]; // 768 for last histogram, 128 for negative part + + // Radix sort, j is the pass number (0=LSB, 3=MSB) + for(uint32 j=0;j<4;j++) + { + // Should we care about negative values? + if(j!=3) + { + // Here we deal with positive values only + CHECK_PASS_VALIDITY(j); + + if(PerformPass) + { + uint32* const Ranks2 = GetRanks2(); + // Create offsets + mLink[0] = Ranks2; + for(uint32 i=1;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; + + // Perform Radix Sort + const uint8* InputBytes = _type_cast_union(input).asUInts8; + InputBytes += j; + if (!AreRanksValid()) + { + for(uint32 i=0;i<nb;i++) + { + *mLink[InputBytes[i<<2]]++ = i; + } + + ValidateRanks(); + } + else + { + uint32* const Ranks1 = GetRanks1(); + + uint32* Indices = Ranks1; + uint32* const IndicesEnd = Ranks1 + nb; + while(Indices!=IndicesEnd) + { + uint32 id = *Indices++; + *mLink[InputBytes[id<<2]]++ = id; + } + } + + // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. + SwapRanks(); + } + } + else + { + // This is a special case to correctly handle negative values + CHECK_PASS_VALIDITY(j); + + if(PerformPass) + { + uint32* const Ranks2 = GetRanks2(); + + // Create biased offsets, in order for negative numbers to be sorted as well + mLink[0] = Ranks2 + NbNegativeValues; // First positive number takes place after the negative ones + for(uint32 i=1;i<128;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers + + // We must reverse the sorting order for negative numbers! + mLink[255] = Ranks2; + for(uint32 i=0;i<127;i++) mLink[254-i] = mLink[255-i] + CurCount[255-i]; // Fixing the wrong order for negative values + for(uint32 i=128;i<256;i++) mLink[i] += CurCount[i]; // Fixing the wrong place for negative values + + // Perform Radix Sort + if (!AreRanksValid()) + { + for(uint32 i=0;i<nb;i++) + { + uint32 Radix = input[i]>>24; // Radix byte, same as above. AND is useless here (uint32). + // ### cmp to be killed. Not good. Later. + if(Radix<128) *mLink[Radix]++ = i; // Number is positive, same as above + else *(--mLink[Radix]) = i; // Number is negative, flip the sorting order + } + + ValidateRanks(); + } + else + { + uint32* const Ranks1 = GetRanks1(); + + for(uint32 i=0;i<nb;i++) + { + uint32 Radix = input[Ranks1[i]]>>24; // Radix byte, same as above. AND is useless here (uint32). + // ### cmp to be killed. Not good. Later. + if(Radix<128) *mLink[Radix]++ = Ranks1[i]; // Number is positive, same as above + else *(--mLink[Radix]) = Ranks1[i]; // Number is negative, flip the sorting order + } + } + // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. + SwapRanks(); + } + else + { + // The pass is useless, yet we still have to reverse the order of current list if all values are negative. + if(UniqueVal>=128) + { + if (!AreRanksValid()) + { + uint32* const Ranks2 = GetRanks2(); + // ###Possible? + for(uint32 i=0;i<nb;i++) + { + Ranks2[i] = nb-i-1; + } + + ValidateRanks(); + } + else + { + uint32* const Ranks1 = GetRanks1(); + uint32* const Ranks2 = GetRanks2(); + for(uint32 i=0;i<nb;i++) Ranks2[i] = Ranks1[nb-i-1]; + } + + // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. + SwapRanks(); + } + } + } + } + + // Return indices + uint32* const Ranks1 = GetRanks1(); + return Ranks1; +} + diff --git a/libs/ode-0.16.1/ode/src/collision_space.cpp b/libs/ode-0.16.1/ode/src/collision_space.cpp new file mode 100644 index 0000000..2ec4247 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_space.cpp @@ -0,0 +1,864 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +spaces + +*/ + +#include <vector> + +#include <ode/common.h> +#include <ode/collision_space.h> +#include <ode/collision.h> +#include "config.h" +#include "matrix.h" +#include "collision_kernel.h" +#include "collision_space_internal.h" +#include "util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags +// and moving it to the front of the space's list. all the parents of a +// dirty geom also become dirty. + +void dGeomMoved (dxGeom *geom) +{ + dAASSERT (geom); + + // if geom is offset, mark it as needing a calculate + if (geom->offset_posr) { + geom->gflags |= GEOM_POSR_BAD; + } + + // from the bottom of the space heirarchy up, process all clean geoms + // turning them into dirty geoms. + dxSpace *parent = geom->parent_space; + + while (parent && (geom->gflags & GEOM_DIRTY)==0) { + geom->markAABBBad(); + parent->dirty (geom); + geom = parent; + parent = parent->parent_space; + } + + // all the remaining dirty geoms must have their AABB_BAD flags set, to + // ensure that their AABBs get recomputed + while (geom) { + geom->markAABBBad(); + geom = geom->parent_space; + } +} + +#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE) + +//**************************************************************************** +// dxSpace + +dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0) +{ + count = 0; + first = 0; + cleanup = 1; + sublevel = 0; + tls_kind = dSPACE_TLS_KIND_INIT_VALUE; + current_index = 0; + current_geom = 0; + lock_count = 0; +} + + +dxSpace::~dxSpace() +{ + CHECK_NOT_LOCKED (this); + if (cleanup) { + // note that destroying each geom will call remove() + dxGeom *g,*n; + for (g = first; g; g=n) { + n = g->next; + dGeomDestroy (g); + } + } + else { + dxGeom *g,*n; + for (g = first; g; g=n) { + n = g->next; + remove (g); + } + } +} + + +void dxSpace::computeAABB() +{ + if (first) { + int i; + dReal a[6]; + a[0] = dInfinity; + a[1] = -dInfinity; + a[2] = dInfinity; + a[3] = -dInfinity; + a[4] = dInfinity; + a[5] = -dInfinity; + for (dxGeom *g=first; g; g=g->next) { + g->recomputeAABB(); + for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i]; + for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i]; + } + memcpy(aabb,a,6*sizeof(dReal)); + } + else { + dSetZero (aabb,6); + } +} + + +// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1 + +dxGeom *dxSpace::getGeom (int i) +{ + dUASSERT (i >= 0 && i < count,"index out of range"); + if (current_geom && current_index == i-1) { + current_geom = current_geom->next; + current_index = i; + return current_geom; + } + else { + dxGeom *g=first; + for (int j=0; j<i; j++) { + if (g) g = g->next; else return 0; + } + current_geom = g; + current_index = i; + return g; + } +} + + +void dxSpace::add (dxGeom *geom) +{ + CHECK_NOT_LOCKED (this); + dAASSERT (geom); + dUASSERT (geom->parent_space == 0 && geom->next == 0, + "geom is already in a space"); + + // add + geom->parent_space = this; + geom->spaceAdd (&first); + count++; + + // enumerator has been invalidated + current_geom = 0; + + dGeomMoved (this); +} + + +void dxSpace::remove (dxGeom *geom) +{ + CHECK_NOT_LOCKED (this); + dAASSERT (geom); + dUASSERT (geom->parent_space == this,"object is not in this space"); + + // remove + geom->spaceRemove(); + count--; + + // safeguard + geom->next = 0; + geom->tome = 0; + geom->parent_space = 0; + + // enumerator has been invalidated + current_geom = 0; + + // the bounding box of this space (and that of all the parents) may have + // changed as a consequence of the removal. + dGeomMoved (this); +} + + +void dxSpace::dirty (dxGeom *geom) +{ + geom->spaceRemove(); + geom->spaceAdd (&first); +} + +//**************************************************************************** +// simple space - reports all n^2 object intersections + +struct dxSimpleSpace : public dxSpace { + dxSimpleSpace (dSpaceID _space); + void cleanGeoms(); + void collide (void *data, dNearCallback *callback); + void collide2 (void *data, dxGeom *geom, dNearCallback *callback); +}; + + +dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space) +{ + type = dSimpleSpaceClass; +} + + +void dxSimpleSpace::cleanGeoms() +{ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) { + if (IS_SPACE(g)) { + ((dxSpace*)g)->cleanGeoms(); + } + + g->recomputeAABB(); + dIASSERT((g->gflags & GEOM_AABB_BAD) == 0); + + g->gflags &= ~GEOM_DIRTY; + } + lock_count--; +} + + +void dxSimpleSpace::collide (void *data, dNearCallback *callback) +{ + dAASSERT (callback); + + lock_count++; + cleanGeoms(); + + // intersect all bounding boxes + for (dxGeom *g1=first; g1; g1=g1->next) { + if (GEOM_ENABLED(g1)){ + for (dxGeom *g2=g1->next; g2; g2=g2->next) { + if (GEOM_ENABLED(g2)){ + collideAABBs (g1,g2,data,callback); + } + } + } + } + + lock_count--; +} + + +void dxSimpleSpace::collide2 (void *data, dxGeom *geom, + dNearCallback *callback) +{ + dAASSERT (geom && callback); + + lock_count++; + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + for (dxGeom *g=first; g; g=g->next) { + if (GEOM_ENABLED(g)){ + collideAABBs (g,geom,data,callback); + } + } + + lock_count--; +} + +//**************************************************************************** +// utility stuff for hash table space + +// kind of silly, but oh well... +#ifndef MAXINT +#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1)) +#endif + + +// prime[i] is the largest prime smaller than 2^i +#define NUM_PRIMES 31 +static const unsigned long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L, +1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L, +524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L, +67108859L,134217689L,268435399L,536870909L,1073741789L}; + + +// an axis aligned bounding box in the hash table +struct dxAABB { + int level; // the level this is stored in (cell size = 2^level) + int dbounds[6]; // AABB bounds, discretized to cell size + dxGeom *geom; // corresponding geometry object (AABB stored here) + sizeint index; // index of this AABB, starting from 0 +}; + + +// a hash table node that represents an AABB that intersects a particular cell +// at a particular level +struct Node { + Node *next; // next node in hash table collision list, 0 if none + int x,y,z; // cell position in space, discretized to cell size + dxAABB *aabb; // axis aligned bounding box that intersects this cell +}; + + +// return the `level' of an AABB. the AABB will be put into cells at this +// level - the cell size will be 2^level. the level is chosen to be the +// smallest value such that the AABB occupies no more than 8 cells, regardless +// of its placement. this means that: +// size/2 < q <= size +// where q is the maximum AABB dimension. + +static int findLevel (dReal bounds[6]) +{ + if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity || + bounds[2] <= -dInfinity || bounds[3] >= dInfinity || + bounds[4] <= -dInfinity || bounds[5] >= dInfinity) { + return MAXINT; + } + + // compute q + dReal q,q2; + q = bounds[1] - bounds[0]; // x bounds + q2 = bounds[3] - bounds[2]; // y bounds + if (q2 > q) q = q2; + q2 = bounds[5] - bounds[4]; // z bounds + if (q2 > q) q = q2; + + // find level such that 0.5 * 2^level < q <= 2^level + int level; + frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp) + return level; +} + + +// find a virtual memory address for a cell at the given level and x,y,z +// position. +// @@@ currently this is not very sophisticated, e.g. the scaling +// factors could be better designed to avoid collisions, and they should +// probably depend on the hash table physical size. + +static unsigned long getVirtualAddressBase (unsigned int level, unsigned int x, unsigned int y) +{ + return level * 1000UL + x * 100UL + y * 10UL; +} + +//**************************************************************************** +// hash space + +struct dxHashSpace : public dxSpace { + int global_minlevel; // smallest hash table level to put AABBs in + int global_maxlevel; // objects that need a level larger than this will be + // put in a "big objects" list instead of a hash table + + dxHashSpace (dSpaceID _space); + void setLevels (int minlevel, int maxlevel); + void getLevels (int *minlevel, int *maxlevel); + void cleanGeoms(); + void collide (void *data, dNearCallback *callback); + void collide2 (void *data, dxGeom *geom, dNearCallback *callback); +}; + + +dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space) +{ + type = dHashSpaceClass; + global_minlevel = -3; + global_maxlevel = 10; +} + + +void dxHashSpace::setLevels (int minlevel, int maxlevel) +{ + dAASSERT (minlevel <= maxlevel); + global_minlevel = minlevel; + global_maxlevel = maxlevel; +} + + +void dxHashSpace::getLevels (int *minlevel, int *maxlevel) +{ + if (minlevel) *minlevel = global_minlevel; + if (maxlevel) *maxlevel = global_maxlevel; +} + + +void dxHashSpace::cleanGeoms() +{ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) { + if (IS_SPACE(g)) { + ((dxSpace*)g)->cleanGeoms(); + } + + g->recomputeAABB(); + dIASSERT((g->gflags & GEOM_AABB_BAD) == 0); + + g->gflags &= ~GEOM_DIRTY; + } + lock_count--; +} + + +void dxHashSpace::collide (void *data, dNearCallback *callback) +{ + dAASSERT(this && callback); + dxGeom *geom; + int i,maxlevel; + + // 0 or 1 geoms can't collide with anything + if (count < 2) return; + + lock_count++; + cleanGeoms(); + + // create a list of auxiliary information for all geom axis aligned bounding + // boxes. set the level for all AABBs. put AABBs larger than the space's + // global_maxlevel in the big_boxes list, check everything else against + // that list at the end. for AABBs that are not too big, record the maximum + // level that we need. + + typedef std::vector<dxAABB> AABBlist; + AABBlist hash_boxes; // list of AABBs in hash table + AABBlist big_boxes; // list of AABBs too big for hash table + maxlevel = global_minlevel - 1; + for (geom = first; geom; geom=geom->next) { + if (!GEOM_ENABLED(geom)){ + continue; + } + dxAABB aabb; + aabb.geom = geom; + // compute level, but prevent cells from getting too small + int level = findLevel (geom->aabb); + if (level < global_minlevel) level = global_minlevel; + if (level <= global_maxlevel) { + aabb.level = level; + if (level > maxlevel) maxlevel = level; + // cellsize = 2^level + dReal cellSizeRecip = dRecip(ldexp(REAL(1.0), level)); // No computational errors here! + // discretize AABB position to cell size + for (i=0; i < 6; i++) { + dReal aabbBound = geom->aabb[i] * cellSizeRecip; // No computational errors so far! + dICHECK(aabbBound >= dMinIntExact && aabbBound </*=*/ dMaxIntExact); // Otherwise the scene is too large for integer types used + + aabb.dbounds[i] = (int) dFloor(aabbBound); + } + // set AABB index + aabb.index = hash_boxes.size(); + // aabb goes in main list + hash_boxes.push_back(aabb); + } + else { + // aabb is too big, put it in the big_boxes list. we don't care about + // setting level, dbounds, index, or the maxlevel + big_boxes.push_back(aabb); + } + } + + sizeint n = hash_boxes.size(); // number of AABBs in main list + + // for `n' objects, an n*n array of bits is used to record if those objects + // have been intersection-tested against each other yet. this array can + // grow large with high n, but oh well... + int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits + std::vector<uint8> tested(n * tested_rowsize); + + // create a hash table to store all AABBs. each AABB may take up to 8 cells. + // we use chaining to resolve collisions, but we use a relatively large table + // to reduce the chance of collisions. + + // compute hash table size sz to be a prime > 8*n + for (i=0; i<NUM_PRIMES; i++) { + if ((sizeint)prime[i] >= (8*n)) break; + } + if (i >= NUM_PRIMES) { + i = NUM_PRIMES-1; // probably pointless + } + + const unsigned long sz = prime[i]; + + // allocate and initialize hash table node pointers + typedef std::vector<Node*> HashTable; + HashTable table(sz); + + // add each AABB to the hash table (may need to add it to up to 8 cells) + const AABBlist::iterator hashend = hash_boxes.end(); + for (AABBlist::iterator aabb = hash_boxes.begin(); aabb != hashend; ++aabb) { + const int *dbounds = aabb->dbounds; + const int xend = dbounds[1]; + for (int xi = dbounds[0]; xi <= xend; xi++) { + const int yend = dbounds[3]; + for (int yi = dbounds[2]; yi <= yend; yi++) { + int zbegin = dbounds[4]; + unsigned long hi = (getVirtualAddressBase(aabb->level,xi,yi) + zbegin) % sz; + const int zend = dbounds[5]; + for (int zi = zbegin; zi <= zend; (hi = hi + 1U != sz ? hi + 1U : 0UL), zi++) { + // get the hash index + // add a new node to the hash table + Node *node = new Node; + node->x = xi; + node->y = yi; + node->z = zi; + node->aabb = &*aabb; + node->next = table[hi]; + table[hi] = node; + } + } + } + } + + // now that all AABBs are loaded into the hash table, we do the actual + // collision detection. for all AABBs, check for other AABBs in the + // same cells for collisions, and then check for other AABBs in all + // intersecting higher level cells. + + int db[6]; // discrete bounds at current level + for (AABBlist::iterator aabb = hash_boxes.begin(); aabb != hashend; ++aabb) { + // we are searching for collisions with aabb + for (i=0; i<6; i++) db[i] = aabb->dbounds[i]; + for (int level = aabb->level; ; ) { + dIASSERT(level <= maxlevel); + const int xend = db[1]; + for (int xi = db[0]; xi <= xend; xi++) { + const int yend = db[3]; + for (int yi = db[2]; yi <= yend; yi++) { + int zbegin = db[4]; + // get the hash index + unsigned long hi = (getVirtualAddressBase(level, xi, yi) + zbegin) % sz; + const int zend = db[5]; + for (int zi = zbegin; zi <= zend; (hi = hi + 1U != sz ? hi + 1U : 0UL), zi++) { + // search all nodes at this index + for (Node* node = table[hi]; node; node=node->next) { + // node points to an AABB that may intersect aabb + if (node->aabb == &*aabb) + continue; + if (node->aabb->level == level && + node->x == xi && node->y == yi && node->z == zi) { + // see if aabb and node->aabb have already been tested + // against each other + unsigned char mask; + if (aabb->index <= node->aabb->index) { + i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3); + mask = 1 << (node->aabb->index & 7); + } + else { + i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3); + mask = 1 << (aabb->index & 7); + } + dIASSERT (i >= 0 && (sizeint)i < (tested_rowsize*n)); + if ((tested[i] & mask)==0) { + tested[i] |= mask; + collideAABBs (aabb->geom,node->aabb->geom,data,callback); + } + } + } + } + } + } + + if (level == maxlevel) { + break; + } + ++level; + // get the discrete bounds for the next level up + for (i=0; i<6; i++) db[i] >>= 1; + } + } + + // every AABB in the normal list must now be intersected against every + // AABB in the big_boxes list. so let's hope there are not too many objects + // in the big_boxes list. + const AABBlist::iterator bigend = big_boxes.end(); + for (AABBlist::iterator aabb = hash_boxes.begin(); aabb != hashend; ++aabb) { + for (AABBlist::iterator aabb2 = big_boxes.begin(); aabb2 != bigend; ++aabb2) { + collideAABBs (aabb->geom, aabb2->geom, data, callback); + } + } + + // intersected all AABBs in the big_boxes list together + for (AABBlist::iterator aabb = big_boxes.begin(); aabb != bigend; ++aabb) { + AABBlist::iterator aabb2 = aabb; + while (++aabb2 != bigend) { + collideAABBs (aabb->geom, aabb2->geom, data, callback); + } + } + + // deallocate table + const HashTable::iterator tableend = table.end(); + for (HashTable::iterator el = table.begin(); el != tableend; ++el) + for (Node* node = *el; node; ) { + Node* next = node->next; + delete node; + node = next; + } + + lock_count--; +} + + +void dxHashSpace::collide2 (void *data, dxGeom *geom, + dNearCallback *callback) +{ + dAASSERT (geom && callback); + + // this could take advantage of the hash structure to avoid + // O(n2) complexity, but it does not yet. + + lock_count++; + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + for (dxGeom *g=first; g; g=g->next) { + if (GEOM_ENABLED(g)) collideAABBs (g,geom,data,callback); + } + + lock_count--; +} + +//**************************************************************************** +// space functions + +dxSpace *dSimpleSpaceCreate (dxSpace *space) +{ + return new dxSimpleSpace (space); +} + + +dxSpace *dHashSpaceCreate (dxSpace *space) +{ + return new dxHashSpace (space); +} + + +void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel) +{ + dAASSERT (space); + dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel"); + dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space"); + dxHashSpace *hspace = (dxHashSpace*) space; + hspace->setLevels (minlevel,maxlevel); +} + + +void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel) +{ + dAASSERT (space); + dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space"); + dxHashSpace *hspace = (dxHashSpace*) space; + hspace->getLevels (minlevel,maxlevel); +} + + +void dSpaceDestroy (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + dGeomDestroy (space); +} + + +void dSpaceSetCleanup (dxSpace *space, int mode) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->setCleanup (mode); +} + + +int dSpaceGetCleanup (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getCleanup(); +} + + +void dSpaceSetSublevel (dSpaceID space, int sublevel) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->setSublevel (sublevel); +} + + +int dSpaceGetSublevel (dSpaceID space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getSublevel(); +} + +void dSpaceSetManualCleanup (dSpaceID space, int mode) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->setManulCleanup(mode); +} + +int dSpaceGetManualCleanup (dSpaceID space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getManualCleanup(); +} + +void dSpaceAdd (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + CHECK_NOT_LOCKED (space); + space->add (g); +} + + +void dSpaceRemove (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + CHECK_NOT_LOCKED (space); + space->remove (g); +} + + +int dSpaceQuery (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->query (g); +} + +void dSpaceClean (dxSpace *space){ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + + space->cleanGeoms(); +} + +int dSpaceGetNumGeoms (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getNumGeoms(); +} + + +dGeomID dSpaceGetGeom (dxSpace *space, int i) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getGeom (i); +} + +int dSpaceGetClass (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->type; +} + + +void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback) +{ + dAASSERT (space && callback); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->collide (data,callback); +} + + +struct DataCallback { + void *data; + dNearCallback *callback; +}; +// Invokes the callback with arguments swapped +static void swap_callback(void *data, dxGeom *g1, dxGeom *g2) +{ + DataCallback *dc = (DataCallback*)data; + dc->callback(dc->data, g2, g1); +} + + +void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data, + dNearCallback *callback) +{ + dAASSERT (g1 && g2 && callback); + dxSpace *s1,*s2; + + // see if either geom is a space + if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0; + if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0; + + if (s1 && s2) { + int l1 = s1->getSublevel(); + int l2 = s2->getSublevel(); + if (l1 != l2) { + if (l1 > l2) { + s2 = 0; + } else { + s1 = 0; + } + } + } + + // handle the four space/geom cases + if (s1) { + if (s2) { + // g1 and g2 are spaces. + if (s1==s2) { + // collide a space with itself --> interior collision + s1->collide (data,callback); + } + else { + // iterate through the space that has the fewest geoms, calling + // collide2 in the other space for each one. + if (s1->count < s2->count) { + DataCallback dc = {data, callback}; + for (dxGeom *g = s1->first; g; g=g->next) { + s2->collide2 (&dc,g,swap_callback); + } + } + else { + for (dxGeom *g = s2->first; g; g=g->next) { + s1->collide2 (data,g,callback); + } + } + } + } + else { + // g1 is a space, g2 is a geom + s1->collide2 (data,g2,callback); + } + } + else { + if (s2) { + // g1 is a geom, g2 is a space + DataCallback dc = {data, callback}; + s2->collide2 (&dc,g1,swap_callback); + } + else { + // g1 and g2 are geoms + // make sure they have valid AABBs + g1->recomputeAABB(); + g2->recomputeAABB(); + collideAABBs(g1,g2, data, callback); + } + } +} diff --git a/libs/ode-0.16.1/ode/src/collision_space_internal.h b/libs/ode-0.16.1/ode/src/collision_space_internal.h new file mode 100644 index 0000000..be69b81 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_space_internal.h @@ -0,0 +1,80 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +stuff common to all spaces + +*/ + +#ifndef _ODE_COLLISION_SPACE_INTERNAL_H_ +#define _ODE_COLLISION_SPACE_INTERNAL_H_ + +#define ALLOCA(x) dALLOCA16(x) + + +// collide two geoms together. for the hash table space, this is +// called if the two AABBs inhabit the same hash table cells. +// this only calls the callback function if the AABBs actually +// intersect. if a geom has an AABB test function, that is called to +// provide a further refinement of the intersection. +// +// NOTE: this assumes that the geom AABBs are valid on entry +// and that both geoms are enabled. + +static inline void collideAABBs (dxGeom *g1, dxGeom *g2, + void *data, dNearCallback *callback) +{ + dIASSERT((g1->gflags & GEOM_AABB_BAD)==0); + dIASSERT((g2->gflags & GEOM_AABB_BAD)==0); + + // no contacts if both geoms on the same body, and the body is not 0 + if (g1->body == g2->body && g1->body) return; + + // test if the category and collide bitfields match + if ( ((g1->category_bits & g2->collide_bits) || + (g2->category_bits & g1->collide_bits)) == 0) { + return; + } + + // if the bounding boxes are disjoint then don't do anything + dReal *bounds1 = g1->aabb; + dReal *bounds2 = g2->aabb; + if (bounds1[0] > bounds2[1] || + bounds1[1] < bounds2[0] || + bounds1[2] > bounds2[3] || + bounds1[3] < bounds2[2] || + bounds1[4] > bounds2[5] || + bounds1[5] < bounds2[4]) { + return; + } + + // check if either object is able to prove that it doesn't intersect the + // AABB of the other + if (g1->AABBTest (g2,bounds2) == 0) return; + if (g2->AABBTest (g1,bounds1) == 0) return; + + // the objects might actually intersect - call the space callback function + callback (data,g1,g2); +} + +#endif diff --git a/libs/ode-0.16.1/ode/src/collision_std.h b/libs/ode-0.16.1/ode/src/collision_std.h new file mode 100644 index 0000000..710e580 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_std.h @@ -0,0 +1,238 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +the standard ODE geometry primitives. + +*/ + +#ifndef _ODE_COLLISION_STD_H_ +#define _ODE_COLLISION_STD_H_ + +#include <ode/common.h> +#include "collision_kernel.h" + + +// primitive collision functions - these have the dColliderFn interface, i.e. +// the same interface as dCollide(). the first and second geom arguments must +// have the specified types. + +int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideBoxPlane (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayCylinder (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); + +// Cylinder - Box/Sphere by (C) CroTeam +// Ported by Nguyen Binh +int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere, + int flags, dContactGeom *contact, int skip); +int dCollideCylinderPlane(dxGeom *gCylinder, dxGeom *gPlane, + int flags, dContactGeom *contact, int skip); + +//--> Convex Collision +int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +//<-- Convex Collision + +// dHeightfield +int dCollideHeightfield( dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip ); + +//**************************************************************************** +// the basic geometry objects + +struct dxSphere : public dxGeom { + dReal radius; // sphere radius + dxSphere (dSpaceID space, dReal _radius); + void computeAABB(); +}; + + +struct dxBox : public dxGeom { + dVector3 side; // side lengths (x,y,z) + dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz); + void computeAABB(); +}; + + +struct dxCapsule : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCapsule (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + + +struct dxCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + + +struct dxPlane : public dxGeom { + dReal p[4]; + dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d); + void computeAABB(); +}; + + +struct dxRay : public dxGeom { + dReal length; + dxRay (dSpaceID space, dReal _length); + void computeAABB(); +}; + +struct dxConvex : public dxGeom +{ + const dReal *planes; /*!< An array of planes in the form: + normal X, normal Y, normal Z,Distance + */ + const dReal *points; /*!< An array of points X,Y,Z */ + const unsigned int *polygons; /*! An array of indices to the points of each polygon, it should be the number of vertices followed by that amount of indices to "points" in counter clockwise order*/ + unsigned int planecount; /*!< Amount of planes in planes */ + unsigned int pointcount;/*!< Amount of points in points */ + unsigned int edgecount;/*!< Amount of edges in convex */ + dReal saabb[6];/*!< Static AABB */ + dxConvex(dSpaceID space, + const dReal *planes, + unsigned int planecount, + const dReal *points, + unsigned int pointcount, + const unsigned int *polygons); + ~dxConvex() + { + if((edgecount!=0)&&(edges!=NULL)) delete[] edges; + } + void computeAABB(); + struct edge + { + unsigned int first; + unsigned int second; + }; + edge* edges; + + /*! \brief A Support mapping function for convex shapes + \param dir [IN] direction to find the Support Point for + \return the index of the support vertex. + */ + inline unsigned int SupportIndex(dVector3 dir) + { + dVector3 rdir; + unsigned int index=0; + dMultiply1_331 (rdir,final_posr->R,dir); + dReal max = dCalcVectorDot3(points,rdir); + dReal tmp; + for (unsigned int i = 1; i < pointcount; ++i) + { + tmp = dCalcVectorDot3(points+(i*3),rdir); + if (tmp > max) + { + index=i; + max = tmp; + } + } + return index; + } + +private: + // For Internal Use Only + /*! \brief Fills the edges dynamic array based on points and polygons. + */ + void FillEdges(); +#if 0 + /* + What this does is the same as the Support function by doing some preprocessing + for optimization. Not complete yet. + */ + // Based on Eberly's Game Physics Book page 307 + struct Arc + { + // indices of polyhedron normals that form the spherical arc + int normals[2]; + // index of edge shared by polyhedron faces + int edge; + }; + struct Polygon + { + // indices of polyhedron normals that form the spherical polygon + std::vector<int> normals; + // index of extreme vertex corresponding to this polygon + int vertex; + }; + // This is for extrem feature query and not the usual level BSP structure (that comes later) + struct BSPNode + { + // Normal index (interior node), vertex index (leaf node) + int normal; + // if Dot (E,D)>=0, D gets propagated to this child + BSPNode* right; + // if Dot (E,D)<0, D gets propagated to this child + BSPNode* left; + }; + void CreateTree(); + BSPNode* CreateNode(std::vector<Arc> Arcs,std::vector<Polygon> Polygons); + void GetFacesSharedByVertex(int i, std::vector<int> f); + void GetFacesSharedByEdge(int i, int* f); + void GetFaceNormal(int i, dVector3 normal); + BSPNode* tree; +#endif +}; + + +#endif diff --git a/libs/ode-0.16.1/ode/src/collision_transform.cpp b/libs/ode-0.16.1/ode/src/collision_transform.cpp new file mode 100644 index 0000000..ece3d53 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_transform.cpp @@ -0,0 +1,234 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +geom transform + +*/ + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_transform.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// dxGeomTransform class + +struct dxGeomTransform : public dxGeom { + dxGeom *obj; // object that is being transformed + int cleanup; // 1 to destroy obj when destroyed + int infomode; // 1 to put Tx geom in dContactGeom g1 + + // cached final object transform (body tx + relative tx). this is set by + // computeAABB(), and it is valid while the AABB is valid. + dxPosR transform_posr; + + dxGeomTransform (dSpaceID space); + ~dxGeomTransform(); + void computeAABB(); + void computeFinalTx(); +}; +/* +void RunMe() +{ +printf("sizeof body = %i\n", sizeof(dxBody)); +printf("sizeof geom = %i\n", sizeof(dxGeom)); +printf("sizeof geomtransform = %i\n", sizeof(dxGeomTransform)); +printf("sizeof posr = %i\n", sizeof(dxPosR)); +} +*/ + +dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1) +{ + type = dGeomTransformClass; + obj = 0; + cleanup = 0; + infomode = 0; + dSetZero (transform_posr.pos,4); + dRSetIdentity (transform_posr.R); +} + + +dxGeomTransform::~dxGeomTransform() +{ + if (obj && cleanup) delete obj; +} + + +void dxGeomTransform::computeAABB() +{ + if (!obj) { + dSetZero (aabb,6); + return; + } + + // backup the relative pos and R pointers of the encapsulated geom object + dxPosR* posr_bak = obj->final_posr; + + // compute temporary pos and R for the encapsulated geom object + computeFinalTx(); + obj->final_posr = &transform_posr; + + // compute the AABB + obj->computeAABB(); + memcpy (aabb,obj->aabb,6*sizeof(dReal)); + + // restore the pos and R + obj->final_posr = posr_bak; +} + + +// utility function for dCollideTransform() : compute final pos and R +// for the encapsulated geom object + +void dxGeomTransform::computeFinalTx() +{ + dMultiply0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos); + transform_posr.pos[0] += final_posr->pos[0]; + transform_posr.pos[1] += final_posr->pos[1]; + transform_posr.pos[2] += final_posr->pos[2]; + dMultiply0_333 (transform_posr.R,final_posr->R,obj->final_posr->R); +} + +//**************************************************************************** +// collider function: +// this collides a transformed geom with another geom. the other geom can +// also be a transformed geom, but this case is not handled specially. + +int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dGeomTransformClass); + + dxGeomTransform *tr = (dxGeomTransform*) o1; + if (!tr->obj) return 0; + dUASSERT (tr->obj->parent_space==0, + "GeomTransform encapsulated object must not be in a space"); + dUASSERT (tr->obj->body==0, + "GeomTransform encapsulated object must not be attached " + "to a body"); + + // backup the relative pos and R pointers of the encapsulated geom object, + // and the body pointer + dxPosR *posr_bak = tr->obj->final_posr; + dxBody *bodybak = tr->obj->body; + + // compute temporary pos and R for the encapsulated geom object. + // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag, + // because computeFinalTx() will have already been called in + // dxGeomTransform::computeAABB() + + if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx(); + tr->obj->final_posr = &tr->transform_posr; + tr->obj->body = o1->body; + + // do the collision + int n = dCollide (tr->obj,o2,flags,contact,skip); + + // if required, adjust the 'g1' values in the generated contacts so that + // thay indicated the GeomTransform object instead of the encapsulated + // object. + if (tr->infomode) { + for (int i=0; i<n; i++) { + dContactGeom *c = CONTACT(contact,skip*i); + c->g1 = o1; + } + } + + // restore the pos, R and body + tr->obj->final_posr = posr_bak; + tr->obj->body = bodybak; + return n; +} + +//**************************************************************************** +// public API + +dGeomID dCreateGeomTransform (dSpaceID space) +{ + return new dxGeomTransform (space); +} + + +void dGeomTransformSetGeom (dGeomID g, dGeomID obj) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + if (tr->obj && tr->cleanup) delete tr->obj; + tr->obj = obj; +} + + +dGeomID dGeomTransformGetGeom (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->obj; +} + + +void dGeomTransformSetCleanup (dGeomID g, int mode) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + tr->cleanup = mode; +} + + +int dGeomTransformGetCleanup (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->cleanup; +} + + +void dGeomTransformSetInfo (dGeomID g, int mode) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + tr->infomode = mode; +} + + +int dGeomTransformGetInfo (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->infomode; +} + diff --git a/libs/ode-0.16.1/ode/src/collision_transform.h b/libs/ode-0.16.1/ode/src/collision_transform.h new file mode 100644 index 0000000..c3cd27c --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_transform.h @@ -0,0 +1,39 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +geom transform + +*/ + +#ifndef _ODE_COLLISION_TRANSFORM_H_ +#define _ODE_COLLISION_TRANSFORM_H_ + +#include <ode/common.h> +#include "collision_kernel.h" + + +int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + + +#endif diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_box.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_box.cpp new file mode 100644 index 0000000..521ed43 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_box.cpp @@ -0,0 +1,1380 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +/************************************************************************* + * * + * Triangle-box collider by Alen Ladavac and Vedran Klanac. * + * Ported to ODE by Oskari Nyman. * + * * + *************************************************************************/ + + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_util.h" +#include "collision_trimesh_internal.h" + +#if dTRIMESH_ENABLED + + +// largest number, double or float +#if defined(dSINGLE) +#define MAXVALUE FLT_MAX +#else +#define MAXVALUE DBL_MAX +#endif + + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) dSubtractVectors3(r, a, b) + + +// dVector3 +// a=b +#define SET(a,b) dCopyVector3(a, b) + + +// dMatrix3 +// a=b +#define SETM(a,b) dCopyMatrix4x4(a, b) + + +// dVector3 +// r=a+b +#define ADD(a,b,r) dAddVectors3(r, a, b) + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) dGetMatrixColumn3(v, m, a) + + +// dVector4, dVector3 +// distance between plane p and point v +#define POINTDISTANCE(p,v) dPointPlaneDistance(v, p) + + +// dVector4, dVector3, dReal +// construct plane from normal and d +#define CONSTRUCTPLANE(plane,normal,d) dConstructPlane(normal, d, plane) + + +// dVector3 +// length of vector a +#define LENGTHOF(a) dCalcVectorLength3(a) + + +struct sTrimeshBoxColliderData +{ + sTrimeshBoxColliderData(): m_iBestAxis(0), m_iExitAxis(0), m_ctContacts(0) {} + + void SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom, + int Flags, dContactGeom* Contacts, int Stride); + void TestCollisionForSingleTriangle(int Triint, dVector3 dv[3], bool &bOutFinishSearching); + + bool _cldTestNormal(dReal fp0, dReal fR, dVector3 vNormal, int iAxis); + bool _cldTestFace(dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD, + dVector3 vNormal, int iAxis); + bool _cldTestEdge(dReal fp0, dReal fp1, dReal fR, dReal fD, + dVector3 vNormal, int iAxis); + bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex); + bool _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex); + + void GenerateContact(int TriIndex, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth); + + // box data + dMatrix3 m_mHullBoxRot; + dVector3 m_vHullBoxPos; + dVector3 m_vBoxHalfSize; + + // mesh data + dVector3 m_vHullDstPos; + + // global collider data + dVector3 m_vBestNormal; + dReal m_fBestDepth; + int m_iBestAxis; + int m_iExitAxis; + dVector3 m_vE0, m_vE1, m_vE2, m_vN; + + // global info for contact creation + int m_iFlags; + dContactGeom *m_ContactGeoms; + int m_iStride; + dxGeom *m_Geom1; + dxGeom *m_Geom2; + int m_ctContacts; +}; + +// Test normal of mesh face as separating axis for intersection +bool sTrimeshBoxColliderData::_cldTestNormal(dReal fp0, dReal fR, dVector3 vNormal, int iAxis) +{ + // calculate overlapping interval of box and triangle + dReal fDepth = fR+fp0; + + // if we do not overlap + if ( fDepth<0 ) { + // do nothing + return false; + } + + // calculate normal's length + dReal fLength = LENGTHOF(vNormal); + // if long enough + if ( fLength > 0.0f ) { + + dReal fOneOverLength = 1.0f/fLength; + // normalize depth + fDepth = fDepth*fOneOverLength; + + // get minimum depth + if (fDepth < m_fBestDepth) { + m_vBestNormal[0] = -vNormal[0]*fOneOverLength; + m_vBestNormal[1] = -vNormal[1]*fOneOverLength; + m_vBestNormal[2] = -vNormal[2]*fOneOverLength; + m_iBestAxis = iAxis; + //dAASSERT(fDepth>=0); + m_fBestDepth = fDepth; + } + } + + return true; +} + + + + +// Test box axis as separating axis +bool sTrimeshBoxColliderData::_cldTestFace(dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD, + dVector3 vNormal, int iAxis) +{ + dReal fMin, fMax; + + // find min of triangle interval + if ( fp0 < fp1 ) { + if ( fp0 < fp2 ) { + fMin = fp0; + } else { + fMin = fp2; + } + } else { + if( fp1 < fp2 ) { + fMin = fp1; + } else { + fMin = fp2; + } + } + + // find max of triangle interval + if ( fp0 > fp1 ) { + if ( fp0 > fp2 ) { + fMax = fp0; + } else { + fMax = fp2; + } + } else { + if( fp1 > fp2 ) { + fMax = fp1; + } else { + fMax = fp2; + } + } + + // calculate minimum and maximum depth + dReal fDepthMin = fR - fMin; + dReal fDepthMax = fMax + fR; + + // if we dont't have overlapping interval + if ( fDepthMin < 0 || fDepthMax < 0 ) { + // do nothing + return false; + } + + dReal fDepth = 0; + + // if greater depth is on negative side + if ( fDepthMin > fDepthMax ) { + // use smaller depth (one from positive side) + fDepth = fDepthMax; + // flip normal direction + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + fD = -fD; + // if greater depth is on positive side + } else { + // use smaller depth (one from negative side) + fDepth = fDepthMin; + } + + // if lower depth than best found so far + if (fDepth < m_fBestDepth) { + // remember current axis as best axis + m_vBestNormal[0] = vNormal[0]; + m_vBestNormal[1] = vNormal[1]; + m_vBestNormal[2] = vNormal[2]; + m_iBestAxis = iAxis; + //dAASSERT(fDepth>=0); + m_fBestDepth = fDepth; + } + + return true; +} + +// Test cross products of box axis and triangle edges as separating axis +bool sTrimeshBoxColliderData::_cldTestEdge(dReal fp0, dReal fp1, dReal fR, dReal fD, + dVector3 vNormal, int iAxis) +{ + dReal fMin, fMax; + + // ===== Begin Patch by Francisco Leon, 2006/10/28 ===== + + // Fixed Null Normal. This prevents boxes passing + // through trimeshes at certain contact angles + + fMin = vNormal[0] * vNormal[0] + + vNormal[1] * vNormal[1] + + vNormal[2] * vNormal[2]; + + if ( fMin <= dEpsilon ) /// THIS NORMAL WOULD BE DANGEROUS + return true; + + // ===== Ending Patch by Francisco Leon ===== + + + // calculate min and max interval values + if ( fp0 < fp1 ) { + fMin = fp0; + fMax = fp1; + } else { + fMin = fp1; + fMax = fp0; + } + + // check if we overlapp + dReal fDepthMin = fR - fMin; + dReal fDepthMax = fMax + fR; + + // if we don't overlapp + if ( fDepthMin < 0 || fDepthMax < 0 ) { + // do nothing + return false; + } + + dReal fDepth; + + // if greater depth is on negative side + if ( fDepthMin > fDepthMax ) { + // use smaller depth (one from positive side) + fDepth = fDepthMax; + // flip normal direction + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + fD = -fD; + // if greater depth is on positive side + } else { + // use smaller depth (one from negative side) + fDepth = fDepthMin; + } + + // calculate normal's length + dReal fLength = LENGTHOF(vNormal); + + // if long enough + if ( fLength > 0.0f ) { + + // normalize depth + dReal fOneOverLength = 1.0f/fLength; + fDepth = fDepth*fOneOverLength; + fD*=fOneOverLength; + + // if lower depth than best found so far (favor face over edges) + if (fDepth*1.5f < m_fBestDepth) { + // remember current axis as best axis + m_vBestNormal[0] = vNormal[0]*fOneOverLength; + m_vBestNormal[1] = vNormal[1]*fOneOverLength; + m_vBestNormal[2] = vNormal[2]*fOneOverLength; + m_iBestAxis = iAxis; + //dAASSERT(fDepth>=0); + m_fBestDepth = fDepth; + } + } + + return true; +} + + +// clip polygon with plane and generate new polygon points +static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1<ctIn; i0=i1, i1++) { + + + // calculate distance of edge points to plane + dReal fDistance0 = POINTDISTANCE( plPlane ,avArrayIn[i0] ); + dReal fDistance1 = POINTDISTANCE( plPlane ,avArrayIn[i1] ); + + + // if first point is in front of plane + if( fDistance0 >= 0 ) { + // emit point + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + +} + + + + +bool sTrimeshBoxColliderData::_cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { + // reset best axis + m_iBestAxis = 0; + m_iExitAxis = -1; + m_fBestDepth = MAXVALUE; + + // calculate edges + SUBTRACT(v1,v0,m_vE0); + SUBTRACT(v2,v0,m_vE1); + SUBTRACT(m_vE1,m_vE0,m_vE2); + + // calculate poly normal + dCalcVectorCross3(m_vN,m_vE0,m_vE1); + + // calculate length of face normal + dReal fNLen = LENGTHOF(m_vN); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (!fNLen) { + return false; + } + + // extract box axes as vectors + dVector3 vA0,vA1,vA2; + GETCOL(m_mHullBoxRot,0,vA0); + GETCOL(m_mHullBoxRot,1,vA1); + GETCOL(m_mHullBoxRot,2,vA2); + + // box halfsizes + dReal fa0 = m_vBoxHalfSize[0]; + dReal fa1 = m_vBoxHalfSize[1]; + dReal fa2 = m_vBoxHalfSize[2]; + + // calculate relative position between box and triangle + dVector3 vD; + SUBTRACT(v0,m_vHullBoxPos,vD); + + dVector3 vL; + dReal fp0, fp1, fp2, fR, fD; + + // Test separating axes for intersection + // ************************************************ + // Axis 1 - Triangle Normal + SET(vL,m_vN); + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0; + fp2 = fp0; + fR=fa0*dFabs( dCalcVectorDot3(m_vN,vA0) ) + fa1 * dFabs( dCalcVectorDot3(m_vN,vA1) ) + fa2 * dFabs( dCalcVectorDot3(m_vN,vA2) ); + + if (!_cldTestNormal(fp0, fR, vL, 1)) { + m_iExitAxis=1; + return false; + } + + // ************************************************ + + // Test Faces + // ************************************************ + // Axis 2 - Box X-Axis + SET(vL,vA0); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 + dCalcVectorDot3(vA0,m_vE0); + fp2 = fp0 + dCalcVectorDot3(vA0,m_vE1); + fR = fa0; + + if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 2)) { + m_iExitAxis=2; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 3 - Box Y-Axis + SET(vL,vA1); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 + dCalcVectorDot3(vA1,m_vE0); + fp2 = fp0 + dCalcVectorDot3(vA1,m_vE1); + fR = fa1; + + if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 3)) { + m_iExitAxis=3; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 4 - Box Z-Axis + SET(vL,vA2); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 + dCalcVectorDot3(vA2,m_vE0); + fp2 = fp0 + dCalcVectorDot3(vA2,m_vE1); + fR = fa2; + + if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 4)) { + m_iExitAxis=4; + return false; + } + + // ************************************************ + + // Test Edges + // ************************************************ + // Axis 5 - Box X-Axis cross Edge0 + dCalcVectorCross3(vL,vA0,m_vE0); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0; + fp2 = fp0 + dCalcVectorDot3(vA0,m_vN); + fR = fa1 * dFabs(dCalcVectorDot3(vA2,m_vE0)) + fa2 * dFabs(dCalcVectorDot3(vA1,m_vE0)); + + if (!_cldTestEdge(fp1, fp2, fR, fD, vL, 5)) { + m_iExitAxis=5; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 6 - Box X-Axis cross Edge1 + dCalcVectorCross3(vL,vA0,m_vE1); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 - dCalcVectorDot3(vA0,m_vN); + fp2 = fp0; + fR = fa1 * dFabs(dCalcVectorDot3(vA2,m_vE1)) + fa2 * dFabs(dCalcVectorDot3(vA1,m_vE1)); + + if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 6)) { + m_iExitAxis=6; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 7 - Box X-Axis cross Edge2 + dCalcVectorCross3(vL,vA0,m_vE2); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 - dCalcVectorDot3(vA0,m_vN); + fp2 = fp0 - dCalcVectorDot3(vA0,m_vN); + fR = fa1 * dFabs(dCalcVectorDot3(vA2,m_vE2)) + fa2 * dFabs(dCalcVectorDot3(vA1,m_vE2)); + + if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 7)) { + m_iExitAxis=7; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 8 - Box Y-Axis cross Edge0 + dCalcVectorCross3(vL,vA1,m_vE0); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0; + fp2 = fp0 + dCalcVectorDot3(vA1,m_vN); + fR = fa0 * dFabs(dCalcVectorDot3(vA2,m_vE0)) + fa2 * dFabs(dCalcVectorDot3(vA0,m_vE0)); + + if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 8)) { + m_iExitAxis=8; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 9 - Box Y-Axis cross Edge1 + dCalcVectorCross3(vL,vA1,m_vE1); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 - dCalcVectorDot3(vA1,m_vN); + fp2 = fp0; + fR = fa0 * dFabs(dCalcVectorDot3(vA2,m_vE1)) + fa2 * dFabs(dCalcVectorDot3(vA0,m_vE1)); + + if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 9)) { + m_iExitAxis=9; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 10 - Box Y-Axis cross Edge2 + dCalcVectorCross3(vL,vA1,m_vE2); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 - dCalcVectorDot3(vA1,m_vN); + fp2 = fp0 - dCalcVectorDot3(vA1,m_vN); + fR = fa0 * dFabs(dCalcVectorDot3(vA2,m_vE2)) + fa2 * dFabs(dCalcVectorDot3(vA0,m_vE2)); + + if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 10)) { + m_iExitAxis=10; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 11 - Box Z-Axis cross Edge0 + dCalcVectorCross3(vL,vA2,m_vE0); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0; + fp2 = fp0 + dCalcVectorDot3(vA2,m_vN); + fR = fa0 * dFabs(dCalcVectorDot3(vA1,m_vE0)) + fa1 * dFabs(dCalcVectorDot3(vA0,m_vE0)); + + if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 11)) { + m_iExitAxis=11; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 12 - Box Z-Axis cross Edge1 + dCalcVectorCross3(vL,vA2,m_vE1); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 - dCalcVectorDot3(vA2,m_vN); + fp2 = fp0; + fR = fa0 * dFabs(dCalcVectorDot3(vA1,m_vE1)) + fa1 * dFabs(dCalcVectorDot3(vA0,m_vE1)); + + if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 12)) { + m_iExitAxis=12; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 13 - Box Z-Axis cross Edge2 + dCalcVectorCross3(vL,vA2,m_vE2); + fD = dCalcVectorDot3(vL,m_vN)/fNLen; + fp0 = dCalcVectorDot3(vL,vD); + fp1 = fp0 - dCalcVectorDot3(vA2,m_vN); + fp2 = fp0 - dCalcVectorDot3(vA2,m_vN); + fR = fa0 * dFabs(dCalcVectorDot3(vA1,m_vE2)) + fa1 * dFabs(dCalcVectorDot3(vA0,m_vE2)); + + if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 13)) { + m_iExitAxis=13; + return false; + } + + // ************************************************ + return true; +} + + + + + +// find two closest points on two lines +static bool _cldClosestPointOnTwoLines( + dVector3 vPoint1, dVector3 vLenVec1, dVector3 vPoint2, dVector3 vLenVec2, + dReal &fvalue1, dReal &fvalue2) +{ + // calculate denominator + dVector3 vp; + SUBTRACT(vPoint2,vPoint1,vp); + dReal fuaub = dCalcVectorDot3(vLenVec1,vLenVec2); + dReal fq1 = dCalcVectorDot3(vLenVec1,vp); + dReal fq2 = -dCalcVectorDot3(vLenVec2,vp); + dReal fd = 1.0f - fuaub * fuaub; + + // if denominator is positive + if (fd > 0.0f) { + // calculate points of closest approach + fd = 1.0f/fd; + fvalue1 = (fq1 + fuaub*fq2)*fd; + fvalue2 = (fuaub*fq1 + fq2)*fd; + return true; + // otherwise + } else { + // lines are parallel + fvalue1 = 0.0f; + fvalue2 = 0.0f; + return false; + } +} + + + + + +// clip and generate contacts +void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex) { + dIASSERT( !(m_iFlags & CONTACTS_UNIMPORTANT) || m_ctContacts < (m_iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results + + // if we have edge/edge intersection + if (m_iBestAxis > 4 ) { + dVector3 vub,vPb,vPa; + + SET(vPa,m_vHullBoxPos); + + // calculate point on box edge + for( int i=0; i<3; i++) { + dVector3 vRotCol; + GETCOL(m_mHullBoxRot,i,vRotCol); + dReal fSign = dCalcVectorDot3(m_vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f; + + vPa[0] += fSign * m_vBoxHalfSize[i] * vRotCol[0]; + vPa[1] += fSign * m_vBoxHalfSize[i] * vRotCol[1]; + vPa[2] += fSign * m_vBoxHalfSize[i] * vRotCol[2]; + } + + int iEdge = (m_iBestAxis-5)%3; + + // decide which edge is on triangle + if ( iEdge == 0 ) { + SET(vPb,v0); + SET(vub,m_vE0); + } else if ( iEdge == 1) { + SET(vPb,v2); + SET(vub,m_vE1); + } else { + SET(vPb,v1); + SET(vub,m_vE2); + } + + + // setup direction parameter for face edge + dNormalize3(vub); + + dReal fParam1, fParam2; + + // setup direction parameter for box edge + dVector3 vua; + int col=(m_iBestAxis-5)/3; + GETCOL(m_mHullBoxRot,col,vua); + + // find two closest points on both edges + _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 ); + vPa[0] += vua[0]*fParam1; + vPa[1] += vua[1]*fParam1; + vPa[2] += vua[2]*fParam1; + + vPb[0] += vub[0]*fParam2; + vPb[1] += vub[1]*fParam2; + vPb[2] += vub[2]*fParam2; + + // calculate collision point + dVector3 vPntTmp; + ADD(vPa,vPb,vPntTmp); + + vPntTmp[0]*=0.5f; + vPntTmp[1]*=0.5f; + vPntTmp[2]*=0.5f; + + // generate contact point between two closest points + GenerateContact(TriIndex, vPntTmp, m_vBestNormal, m_fBestDepth); + + + // if triangle is the referent face then clip box to triangle face + } else if (m_iBestAxis == 1) { + + dVector3 vNormal2; + vNormal2[0]=-m_vBestNormal[0]; + vNormal2[1]=-m_vBestNormal[1]; + vNormal2[2]=-m_vBestNormal[2]; + + + // vNr is normal in box frame, pointing from triangle to box + dMatrix3 mTransposed; + mTransposed[0*4+0]=m_mHullBoxRot[0*4+0]; + mTransposed[0*4+1]=m_mHullBoxRot[1*4+0]; + mTransposed[0*4+2]=m_mHullBoxRot[2*4+0]; + + mTransposed[1*4+0]=m_mHullBoxRot[0*4+1]; + mTransposed[1*4+1]=m_mHullBoxRot[1*4+1]; + mTransposed[1*4+2]=m_mHullBoxRot[2*4+1]; + + mTransposed[2*4+0]=m_mHullBoxRot[0*4+2]; + mTransposed[2*4+1]=m_mHullBoxRot[1*4+2]; + mTransposed[2*4+2]=m_mHullBoxRot[2*4+2]; + + dVector3 vNr; + vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2]; + vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2]; + vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2]; + + + dVector3 vAbsNormal; + vAbsNormal[0] = dFabs( vNr[0] ); + vAbsNormal[1] = dFabs( vNr[1] ); + vAbsNormal[2] = dFabs( vNr[2] ); + + // get closest face from box + int iB0, iB1, iB2; + if (vAbsNormal[1] > vAbsNormal[0]) { + if (vAbsNormal[1] > vAbsNormal[2]) { + iB1 = 0; iB0 = 1; iB2 = 2; + } else { + iB1 = 0; iB2 = 1; iB0 = 2; + } + } else { + + if (vAbsNormal[0] > vAbsNormal[2]) { + iB0 = 0; iB1 = 1; iB2 = 2; + } else { + iB1 = 0; iB2 = 1; iB0 = 2; + } + } + + // Here find center of box face we are going to project + dVector3 vCenter; + dVector3 vRotCol; + GETCOL(m_mHullBoxRot,iB0,vRotCol); + + if (vNr[iB0] > 0) { + vCenter[0] = m_vHullBoxPos[0] - v0[0] - m_vBoxHalfSize[iB0] * vRotCol[0]; + vCenter[1] = m_vHullBoxPos[1] - v0[1] - m_vBoxHalfSize[iB0] * vRotCol[1]; + vCenter[2] = m_vHullBoxPos[2] - v0[2] - m_vBoxHalfSize[iB0] * vRotCol[2]; + } else { + vCenter[0] = m_vHullBoxPos[0] - v0[0] + m_vBoxHalfSize[iB0] * vRotCol[0]; + vCenter[1] = m_vHullBoxPos[1] - v0[1] + m_vBoxHalfSize[iB0] * vRotCol[1]; + vCenter[2] = m_vHullBoxPos[2] - v0[2] + m_vBoxHalfSize[iB0] * vRotCol[2]; + } + + // Here find 4 corner points of box + dVector3 avPoints[4]; + + dVector3 vRotCol2; + GETCOL(m_mHullBoxRot,iB1,vRotCol); + GETCOL(m_mHullBoxRot,iB2,vRotCol2); + + for(int x=0;x<3;x++) { + avPoints[0][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[1][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[2][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[3][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]); + } + + // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes) + dVector3 avTempArray1[9]; + dVector3 avTempArray2[9]; + dVector4 plPlane; + + int iTempCnt1=0; + int iTempCnt2=0; + + // zeroify vectors - necessary? + for(int i=0; i<9; i++) { + avTempArray1[i][0]=0; + avTempArray1[i][1]=0; + avTempArray1[i][2]=0; + + avTempArray2[i][0]=0; + avTempArray2[i][1]=0; + avTempArray2[i][2]=0; + } + + + // Normal plane + dVector3 vTemp; + vTemp[0]=-m_vN[0]; + vTemp[1]=-m_vN[1]; + vTemp[2]=-m_vN[2]; + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p0 + dVector3 vTemp2; + SUBTRACT(v1,v0,vTemp2); + dCalcVectorCross3(vTemp,m_vN,vTemp2); + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + // Plane p1 + SUBTRACT(v2,v1,vTemp2); + dCalcVectorCross3(vTemp,m_vN,vTemp2); + dNormalize3(vTemp); + SUBTRACT(v0,v2,vTemp2); + CONSTRUCTPLANE(plPlane,vTemp,dCalcVectorDot3(vTemp2,vTemp)); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + // Plane p2 + SUBTRACT(v0,v2,vTemp2); + dCalcVectorCross3(vTemp,m_vN,vTemp2); + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + // END of clipping polygons + + // for each generated contact point + for ( int i=0; i<iTempCnt2; i++ ) { + // calculate depth + dReal fTempDepth = dCalcVectorDot3(vNormal2,avTempArray2[i]); + + // clamp depth to zero + if (fTempDepth > 0) { + fTempDepth = 0; + } + + dVector3 vPntTmp; + ADD(avTempArray2[i],v0,vPntTmp); + + GenerateContact(TriIndex, vPntTmp, m_vBestNormal, -fTempDepth); + + if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + + //dAASSERT(m_ctContacts>0); + + // if box face is the referent face, then clip triangle on box face + } else { // 2 <= if iBestAxis <= 4 + + // get normal of box face + dVector3 vNormal2; + SET(vNormal2,m_vBestNormal); + + // get indices of box axes in correct order + int iA0,iA1,iA2; + iA0 = m_iBestAxis-2; + if ( iA0 == 0 ) { + iA1 = 1; iA2 = 2; + } else if ( iA0 == 1 ) { + iA1 = 0; iA2 = 2; + } else { + iA1 = 0; iA2 = 1; + } + + dVector3 avPoints[3]; + // calculate triangle vertices in box frame + SUBTRACT(v0,m_vHullBoxPos,avPoints[0]); + SUBTRACT(v1,m_vHullBoxPos,avPoints[1]); + SUBTRACT(v2,m_vHullBoxPos,avPoints[2]); + + // CLIP Polygons + // define temp data for clipping + dVector3 avTempArray1[9]; + dVector3 avTempArray2[9]; + + int iTempCnt1, iTempCnt2; + + // zeroify vectors - necessary? + for(int i=0; i<9; i++) { + avTempArray1[i][0]=0; + avTempArray1[i][1]=0; + avTempArray1[i][2]=0; + + avTempArray2[i][0]=0; + avTempArray2[i][1]=0; + avTempArray2[i][2]=0; + } + + // clip triangle with 5 box planes (1 face plane, 4 edge planes) + + dVector4 plPlane; + + // Normal plane + dVector3 vTemp; + vTemp[0]=-vNormal2[0]; + vTemp[1]=-vNormal2[1]; + vTemp[2]=-vNormal2[2]; + CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA0]); + + _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p0 + GETCOL(m_mHullBoxRot,iA1,vTemp); + CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA1]); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // Plane p1 + GETCOL(m_mHullBoxRot,iA1,vTemp); + vTemp[0]=-vTemp[0]; + vTemp[1]=-vTemp[1]; + vTemp[2]=-vTemp[2]; + CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA1]); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + // Plane p2 + GETCOL(m_mHullBoxRot,iA2,vTemp); + CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA2]); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + // Plane p3 + GETCOL(m_mHullBoxRot,iA2,vTemp); + vTemp[0]=-vTemp[0]; + vTemp[1]=-vTemp[1]; + vTemp[2]=-vTemp[2]; + CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA2]); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + + // for each generated contact point + for ( int i=0; i<iTempCnt1; i++ ) { + // calculate depth + dReal fTempDepth = dCalcVectorDot3(vNormal2,avTempArray1[i])-m_vBoxHalfSize[iA0]; + + // clamp depth to zero + if (fTempDepth > 0) { + fTempDepth = 0; + } + + // generate contact data + dVector3 vPntTmp; + ADD(avTempArray1[i],m_vHullBoxPos,vPntTmp); + + GenerateContact(TriIndex, vPntTmp, m_vBestNormal, -fTempDepth); + + if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + + //dAASSERT(m_ctContacts>0); + } +} + +// GenerateContact - Written by Jeff Smith (jeff@burri.to) +// Generate a "unique" contact. A unique contact has a unique +// position or normal. If the potential contact has the same +// position and normal as an existing contact, but a larger +// penetration depth, this new depth is used instead +// +void sTrimeshBoxColliderData::GenerateContact(int TriIndex, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth) +{ + int TriCount = m_ctContacts; + + do + { + dContactGeom* TgtContact = NULL; + bool deeper = false; + + if (!(m_iFlags & CONTACTS_UNIMPORTANT)) + { + dReal MinDepth = dInfinity; + dContactGeom* MinContact = NULL; + + bool duplicate = false; + for (int i = 0; i < TriCount; i++) + { + dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, i, m_iStride); + + // same position? + dVector3 diff; + dSubtractVectors3(diff, in_ContactPos, Contact->pos); + + if (dCalcVectorDot3(diff, diff) < dEpsilon) + { + // same normal? + if (REAL(1.0) - dCalcVectorDot3(in_Normal, Contact->normal) < dEpsilon) + { + if (in_Depth > Contact->depth) + { + Contact->depth = in_Depth; + Contact->side1 = TriIndex; + } + + duplicate = true; + break; + } + } + + if (Contact->depth < MinDepth) + { + MinDepth = Contact->depth; + MinContact = Contact; + } + } + if (duplicate) + { + break; + } + + if (TriCount == (m_iFlags & NUMC_MASK)) + { + if (!(MinDepth < in_Depth)) + { + break; + } + + TgtContact = MinContact; + deeper = true; + } + } + else + { + dIASSERT(TriCount < (m_iFlags & NUMC_MASK)); + } + + if (!deeper) + { + // Add a new contact + TgtContact = SAFECONTACT(m_iFlags, m_ContactGeoms, TriCount, m_iStride); + TriCount++; + + TgtContact->pos[3] = 0.0; + + TgtContact->normal[3] = 0.0; + + TgtContact->g1 = m_Geom1; + TgtContact->g2 = m_Geom2; + + TgtContact->side2 = -1; + } + + TgtContact->pos[0] = in_ContactPos[0]; + TgtContact->pos[1] = in_ContactPos[1]; + TgtContact->pos[2] = in_ContactPos[2]; + + TgtContact->normal[0] = in_Normal[0]; + TgtContact->normal[1] = in_Normal[1]; + TgtContact->normal[2] = in_Normal[2]; + + TgtContact->depth = in_Depth; + + TgtContact->side1 = TriIndex; + + m_ctContacts = TriCount; + } + while (false); +} + + + + + +void sTrimeshBoxColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom, + int Flags, dContactGeom* Contacts, int Stride) +{ + // get source hull position, orientation and half size + const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom); + const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom); + + // to global + SETM(m_mHullBoxRot,mRotBox); + SET(m_vHullBoxPos,vPosBox); + + dGeomBoxGetLengths(BoxGeom, m_vBoxHalfSize); + m_vBoxHalfSize[0] *= 0.5f; + m_vBoxHalfSize[1] *= 0.5f; + m_vBoxHalfSize[2] *= 0.5f; + + // get destination hull position and orientation + const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); + + // to global + SET(m_vHullDstPos,vPosMesh); + + // global info for contact creation + m_ctContacts = 0; + m_iStride=Stride; + m_iFlags=Flags; + m_ContactGeoms=Contacts; + m_Geom1=TriMesh; + m_Geom2=BoxGeom; + + // reset stuff + m_fBestDepth = MAXVALUE; + m_vBestNormal[0]=0; + m_vBestNormal[1]=0; + m_vBestNormal[2]=0; +} + +void sTrimeshBoxColliderData::TestCollisionForSingleTriangle(int Triint, dVector3 dv[3], bool &bOutFinishSearching) +{ + bool finish = false; + + // test this triangle + if (_cldTestOneTriangle(dv[0], dv[1], dv[2], Triint)) + { + /* + NOTE by Oleh_Derevenko: + The function continues checking triangles after maximal number + of contacts is reached because it selects maximal penetration depths. + See also comments in GenerateContact() + */ + finish = ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))); + } + + bOutFinishSearching = finish; +} + +// test one mesh triangle on intersection with given box +bool sTrimeshBoxColliderData::_cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex)//, void *pvUser) +{ + // do intersection test and find best separating axis + if (!_cldTestSeparatingAxes(v0, v1, v2)) { + // if not found do nothing + return false; + } + + // if best separation axis is not found + if (m_iBestAxis == 0) { + // this should not happen (we should already exit in that case) + //dMessage (0, "best separation axis not found"); + // do nothing + return false; + } + + _cldClipping(v0, v1, v2, TriIndex); + return true; +} + + +// OPCODE version of box to mesh collider +#if dTRIMESH_OPCODE +static void dQueryBTLPotentialCollisionTriangles(OBBCollider &Collider, + const sTrimeshBoxColliderData &cData, dxTriMesh *TriMesh, dxGeom *BoxGeom, + OBBCache &BoxCache) +{ + // get destination hull position and orientation + const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh); + const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); + + Matrix4x4 MeshMatrix; + const dVector3 vZeroVector3 = { REAL(0.0), }; + MakeMatrix(vZeroVector3, mRotMesh, MeshMatrix); + + // get source hull position, orientation and half size + const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom); + const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom); + + dVector3 vOffsetPosBox; + dSubtractVectors3(vOffsetPosBox, vPosBox, vPosMesh); + + // Make OBB + OBB Box; + Box.mCenter.Set(vOffsetPosBox[0], vOffsetPosBox[1], vOffsetPosBox[2]); + Box.mExtents.Set(cData.m_vBoxHalfSize[0], cData.m_vBoxHalfSize[1], cData.m_vBoxHalfSize[2]); + Box.mRot.Set( + mRotBox[0], mRotBox[4], mRotBox[8], + mRotBox[1], mRotBox[5], mRotBox[9], + mRotBox[2], mRotBox[6], mRotBox[10]); + + // TC results + if (TriMesh->getDoTC(dxTriMesh::TTC_BOX)) { + dxTriMesh::BoxTC* BoxTC = 0; + const int iBoxCacheSize = TriMesh->m_BoxTCCache.size(); + for (int i = 0; i != iBoxCacheSize; i++){ + if (TriMesh->m_BoxTCCache[i].Geom == BoxGeom){ + BoxTC = &TriMesh->m_BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->m_BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->m_BoxTCCache[TriMesh->m_BoxTCCache.size() - 1]; + BoxTC->Geom = BoxGeom; + BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0 + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, Box, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(BoxCache, Box, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } +} + +int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (BoxGeom->type == dBoxClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + sTrimeshBoxColliderData cData; + cData.SetupInitialContext(TriMesh, BoxGeom, Flags, Contacts, Stride); + + const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == BoxGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + OBBCollider& Collider = pccColliderCache->m_OBBCollider; + + dQueryBTLPotentialCollisionTriangles(Collider, cData, TriMesh, BoxGeom, + pccColliderCache->m_DefaultBoxCache); + + if (!Collider.GetContactStatus()) { + // no collision occurred + return 0; + } + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0){ + if (TriMesh->m_ArrayCallback != null){ + TriMesh->m_ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount); + } + + // get destination hull position and orientation + const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh); + const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++){ + const int Triint = Triangles[i]; + if (!TriMesh->invokeCallback(BoxGeom, Triint)) continue; + + dVector3 dv[3]; + TriMesh->fetchMeshTriangle(dv, Triint, vPosMesh, mRotMesh); + + bool bFinishSearching; + cData.TestCollisionForSingleTriangle(Triint, dv, bFinishSearching); + + if (bFinishSearching) { + break; + } + } + } + + return cData.m_ctContacts; +} +#endif + +// GIMPACT version of box to mesh collider +#if dTRIMESH_GIMPACT +int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (BoxGeom->type == dBoxClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + g1 -> recomputeAABB(); + BoxGeom -> recomputeAABB(); + + + sTrimeshBoxColliderData cData; + cData.SetupInitialContext(TriMesh, BoxGeom, Flags, Contacts, Stride); + + //*****at first , collide box aabb******// + + GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh; + aabb3f test_aabb(BoxGeom->aabb[0], BoxGeom->aabb[1], BoxGeom->aabb[2], BoxGeom->aabb[3], BoxGeom->aabb[4], BoxGeom->aabb[5]); + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_BOXQUERY_LIST(collision_result); + + gim_aabbset_box_collision(&test_aabb, &ptrimesh->m_aabbset , &collision_result); + + if(collision_result.m_size==0) + { + GIM_DYNARRAY_DESTROY(collision_result); + return 0; + } + //*****Set globals for box collision******// + + //collide triangles + + GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result); + gim_trimesh_locks_work_data(ptrimesh); + + for(unsigned int i=0;i<collision_result.m_size;i++) + { + dVector3 dv[3]; + + int Triint = boxesresult[i]; + gim_trimesh_get_triangle_vertices(ptrimesh, Triint, dv[0], dv[1], dv[2]); + + bool bFinishSearching; + cData.TestCollisionForSingleTriangle(Triint, dv, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + gim_trimesh_unlocks_work_data(ptrimesh); + GIM_DYNARRAY_DESTROY(collision_result); + + return cData.m_ctContacts; +} +#endif + + + +#endif // dTRIMESH_ENABLED diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp new file mode 100644 index 0000000..6681cc6 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_ccylinder.cpp @@ -0,0 +1,1183 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Triangle-Capsule(Capsule) collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +// NOTES from Nguyen Binh +// 14 Apr : Seem to be robust +// There is a problem when you use original Step and set contact friction +// surface.mu = dInfinity; +// More description : +// When I dropped Capsule over the bunny ears, it seems to stuck +// there for a while. I think the cause is when you set surface.mu = dInfinity; +// the friction force is too high so it just hang the capsule there. +// So the good cure for this is to set mu = around 1.5 (in my case) +// For StepFast1, this become as solid as rock : StepFast1 just approximate +// friction force. + +// NOTES from Croteam's Alen +//As a side note... there are some extra contacts that can be generated +//on the edge between two triangles, and if the capsule penetrates deeply into +//the triangle (usually happens with large mass or low FPS), some such +//contacts can in some cases push the capsule away from the edge instead of +//away from the two triangles. This shows up as capsule slowing down a bit +//when hitting an edge while sliding along a flat tesselated grid of +//triangles. This is only if capsule is standing upwards. + +//Same thing can appear whenever a smooth object (e.g sphere) hits such an +//edge, and it needs to be solved as a special case probably. This is a +//problem we are looking forward to address soon. + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_util.h" +#include "collision_trimesh_internal.h" +#include "util.h" + + +#if dTRIMESH_ENABLED + +// OPCODE version +#if dTRIMESH_OPCODE + +// largest number, double or float +#if defined(dSINGLE) +#define MAX_REAL FLT_MAX +#define MIN_REAL (-FLT_MAX) +#else +#define MAX_REAL DBL_MAX +#define MIN_REAL (-DBL_MAX) +#endif + +// To optimize before send contacts to dynamic part +#define OPTIMIZE_CONTACTS 1 + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) dSubtractVectors3(r, a, b) + + +// dVector3 +// a=b +#define SET(a,b) dCopyVector3(a, b) + + +// dMatrix3 +// a=b +#define SETM(a,b) dCopyMatrix4x4(a, b) + + +// dVector3 +// r=a+b +#define ADD(a,b,r) dAddVectors3(r, a, b) + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) dGetMatrixColumn3(v, m, a) + + +// dVector4, dVector3 +// distance between plane p and point v +#define POINTDISTANCE(p,v) dPointPlaneDistance(v, p) + + +// dVector4, dVector3, dReal +// construct plane from normal and d +#define CONSTRUCTPLANE(plane,normal,d) dConstructPlane(normal, d, plane) + + +// dVector3 +// length of vector a +#define LENGTHOF(a) dCalcVectorLength3(a) + + +static inline dReal _length2OfVector3(dVector3 v) +{ + return dCalcVectorLengthSquare3(v); +} + + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +struct sTrimeshCapsuleColliderData +{ + sTrimeshCapsuleColliderData(): m_gLocalContacts(NULL), m_ctContacts(0) { memset(m_vN, 0, sizeof(dVector3)); } + + void SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Capsule, int flags, int skip); + int TestCollisionForSingleTriangle(int ctContacts0, int Triint, dVector3 dv[3], + uint8 flags, bool &bOutFinishSearching); + +#if OPTIMIZE_CONTACTS + void _OptimizeLocalContacts(); +#endif + int _ProcessLocalContacts(dContactGeom *contact, dxTriMesh *TriMesh, dxGeom *Capsule); + + static BOOL _cldClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane); + BOOL _cldTestAxis(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + dVector3 vAxis, int iAxis, BOOL bNoFlip = FALSE); + BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0, const dVector3 &v1, + const dVector3 &v2, uint8 flags); + void _cldTestOneTriangleVSCapsule(const dVector3 &v0, const dVector3 &v1, + const dVector3 &v2, uint8 flags); + + sLocalContactData *m_gLocalContacts; + unsigned int m_ctContacts; + + // capsule data + // real time data + dMatrix3 m_mCapsuleRotation; + dVector3 m_vCapsulePosition; + dVector3 m_vCapsuleAxis; + // static data + dReal m_vCapsuleRadius; + dReal m_fCapsuleSize; + + // mesh data + // dMatrix4 mHullDstPl; + dMatrix3 m_mTriMeshRot; + dVector3 m_vTriMeshPos; + dVector3 m_vE0, m_vE1, m_vE2; + + // global collider data + dVector3 m_vNormal; + dReal m_fBestDepth; + dReal m_fBestCenter; + dReal m_fBestrt; + int m_iBestAxis; + dVector3 m_vN; + + dVector3 m_vV0; + dVector3 m_vV1; + dVector3 m_vV2; + + // ODE contact's specific + unsigned int m_iFlags; + int m_iStride; +}; + +// Capsule lie on axis number 3 = (Z axis) +static const int nCAPSULE_AXIS = 2; + + +#if OPTIMIZE_CONTACTS + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + SUBTRACT(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + SUBTRACT(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +void sTrimeshCapsuleColliderData::_OptimizeLocalContacts() +{ + int nContacts = m_ctContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(m_gLocalContacts[i],m_gLocalContacts[j])) + { + // If they are seem to be the samed then filtered + // out the least penetrate one + if (_IsBetter(m_gLocalContacts[j],m_gLocalContacts[i])) + { + m_gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + m_gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} +#endif // OPTIMIZE_CONTACTS + +int sTrimeshCapsuleColliderData::_ProcessLocalContacts(dContactGeom *contact, + dxTriMesh *TriMesh, dxGeom *Capsule) +{ +#if OPTIMIZE_CONTACTS + if (m_ctContacts > 1 && !(m_iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + unsigned int iContact = 0; + dContactGeom* Contact = 0; + + unsigned int nFinalContact = 0; + + for (iContact = 0; iContact < m_ctContacts; iContact ++) + { + // Ensure that we haven't created too many contacts + if( nFinalContact >= (m_iFlags & NUMC_MASK)) + { + break; + } + + if (1 == m_gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(m_iFlags, contact, nFinalContact, m_iStride); + Contact->depth = m_gLocalContacts[iContact].fDepth; + SET(Contact->normal,m_gLocalContacts[iContact].vNormal); + SET(Contact->pos,m_gLocalContacts[iContact].vPos); + Contact->g1 = TriMesh; + Contact->g2 = Capsule; + Contact->side1 = m_gLocalContacts[iContact].triIndex; + Contact->side2 = -1; + + nFinalContact++; + } + } + // debug + //if (nFinalContact != m_ctContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",m_ctContacts,m_ctContacts-nFinalContact); + //} + + return nFinalContact; +} + +BOOL sTrimeshCapsuleColliderData::_cldClipEdgeToPlane( + dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) +{ + // calculate distance of edge points to plane + dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 ); + dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 ); + + // if both points are behind the plane + if ( fDistance0 < 0 && fDistance1 < 0 ) + { + // do nothing + return FALSE; + // if both points in front of the plane + } else if ( fDistance0 > 0 && fDistance1 > 0 ) + { + // accept them + return TRUE; + // if we have edge/plane intersection + } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) + { + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); + + // clamp correct edge to intersection point + if ( fDistance0 < 0 ) + { + SET(vEpnt0,vIntersectionPoint); + } else + { + SET(vEpnt1,vIntersectionPoint); + } + return TRUE; + } + return TRUE; +} + +BOOL sTrimeshCapsuleColliderData::_cldTestAxis( + const dVector3 &/*v0*/, + const dVector3 &/*v1*/, + const dVector3 &/*v2*/, + dVector3 vAxis, + int iAxis, + BOOL bNoFlip/* = FALSE*/) +{ + + // calculate length of separating axis vector + dReal fL = LENGTHOF(vAxis); + // if not long enough + // TODO : dReal epsilon please + if ( fL < REAL(1e-5) ) + { + // do nothing + //iLastOutAxis = 0; + return TRUE; + } + + // otherwise normalize it + dNormalize3(vAxis); + + // project capsule on vAxis + dReal frc = dFabs(dCalcVectorDot3(m_vCapsuleAxis,vAxis))*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius) + m_vCapsuleRadius; + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dCalcVectorDot3(m_vV0, vAxis); + afv[1] = dCalcVectorDot3(m_vV1, vAxis); + afv[2] = dCalcVectorDot3(m_vV2, vAxis); + + dReal fMin = MAX_REAL; + dReal fMax = MIN_REAL; + + // for each vertex + for(int i=0; i<3; i++) + { + // find minimum + if (afv[i]<fMin) + { + fMin = afv[i]; + } + // find maximum + if (afv[i]>fMax) + { + fMax = afv[i]; + } + } + + // find triangle's center of interval on axis + dReal fCenter = (fMin+fMax)*REAL(0.5); + // calculate triangles half interval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if (dFabs(fCenter) > ( frc + fTriangleRadius )) + { + // exit, we have no intersection + return FALSE; + } + + // calculate depth + dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius); + + // if greater then best found so far + if ( fDepth > m_fBestDepth ) + { + // remember depth + m_fBestDepth = fDepth; + m_fBestCenter = fCenter; + m_fBestrt = fTriangleRadius; + + m_vNormal[0] = vAxis[0]; + m_vNormal[1] = vAxis[1]; + m_vNormal[2] = vAxis[2]; + + m_iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if (fCenter<0 && !bNoFlip) + { + m_vNormal[0] = -m_vNormal[0]; + m_vNormal[1] = -m_vNormal[1]; + m_vNormal[2] = -m_vNormal[2]; + + m_fBestCenter = -fCenter; + } + } + + return TRUE; +} + +// helper for less key strokes +inline void _CalculateAxis(const dVector3& v1, + const dVector3& v2, + const dVector3& v3, + const dVector3& v4, + dVector3& r) +{ + dVector3 t1; + dVector3 t2; + + SUBTRACT(v1,v2,t1); + dCalcVectorCross3(t2,t1,v3); + dCalcVectorCross3(r,t2,v4); +} + +BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + uint8 flags) +{ + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = m_vCapsulePosition[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp0[1] = m_vCapsulePosition[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp0[2] = m_vCapsulePosition[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + + dVector3 vCp1; + vCp1[0] = m_vCapsulePosition[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp1[1] = m_vCapsulePosition[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCp1[2] = m_vCapsulePosition[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + + // reset best axis + m_iBestAxis = 0; + // reset best depth + m_fBestDepth = -MAX_REAL; + // reset separating axis vector + dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + + // Epsilon value for checking axis vector length + const dReal fEpsilon = 1e-6f; + + // Translate triangle to Cc cord. + SUBTRACT(v0, m_vCapsulePosition, m_vV0); + SUBTRACT(v1, m_vCapsulePosition, m_vV1); + SUBTRACT(v2, m_vCapsulePosition, m_vV2); + + // We begin to test for 19 separating axis now + // I wonder does it help if we employ the method like ISA-GJK??? + // Or at least we should do experiment and find what axis will + // be most likely to be separating axis to check it first. + + // Original + // axis m_vN + //vAxis = -m_vN; + vAxis[0] = - m_vN[0]; + vAxis[1] = - m_vN[1]; + vAxis[2] = - m_vN[2]; + if (!_cldTestAxis(v0, v1, v2, vAxis, 1, TRUE)) + { + return FALSE; + } + + if (flags & dxTriMeshData::CUF_USE_FIRST_EDGE) + { + // axis CxE0 - Edge 0 + dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE0); + //vAxis = dCalcVectorCross3( m_vCapsuleAxis cross vE0 ); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_EDGE) + { + // axis CxE1 - Edge 1 + dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE1); + //vAxis = ( m_vCapsuleAxis cross m_vE1 ); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_THIRD_EDGE) + { + // axis CxE2 - Edge 2 + //vAxis = ( m_vCapsuleAxis cross m_vE2 ); + dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE2); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_FIRST_EDGE) + { + // first capsule point + // axis ((Cp0-V0) x E0) x E0 + _CalculateAxis(vCp0,v0,m_vE0,m_vE0,vAxis); + // vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 5)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_EDGE) + { + // axis ((Cp0-V1) x E1) x E1 + _CalculateAxis(vCp0,v1,m_vE1,m_vE1,vAxis); + //vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 6)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_THIRD_EDGE) + { + // axis ((Cp0-V2) x E2) x E2 + _CalculateAxis(vCp0,v2,m_vE2,m_vE2,vAxis); + //vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 7)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_FIRST_EDGE) + { + // second capsule point + // axis ((Cp1-V0) x E0) x E0 + _CalculateAxis(vCp1,v0,m_vE0,m_vE0,vAxis); + //vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 8)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_EDGE) + { + // axis ((Cp1-V1) x E1) x E1 + _CalculateAxis(vCp1,v1,m_vE1,m_vE1,vAxis); + //vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 9)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_THIRD_EDGE) + { + // axis ((Cp1-V2) x E2) x E2 + _CalculateAxis(vCp1,v2,m_vE2,m_vE2,vAxis); + //vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 10)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_FIRST_VERTEX) + { + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + _CalculateAxis(v0,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis); + //vAxis = ( ( v0-vCp0 ) cross m_vCapsuleAxis ) cross m_vCapsuleAxis; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_VERTEX) + { + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + _CalculateAxis(v1,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis); + //vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_THIRD_VERTEX) + { + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + _CalculateAxis(v2,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis); + //vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 13)) { + return FALSE; + } + } + } + + // Test as separating axes direction vectors between each triangle + // edge and each capsule's cap center + + if (flags & dxTriMeshData::CUF_USE_FIRST_VERTEX) + { + // first triangle vertex and first capsule point + //vAxis = v0 - vCp0; + SUBTRACT(v0,vCp0,vAxis); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_VERTEX) + { + // second triangle vertex and first capsule point + //vAxis = v1 - vCp0; + SUBTRACT(v1,vCp0,vAxis); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 15)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_THIRD_VERTEX) + { + // third triangle vertex and first capsule point + //vAxis = v2 - vCp0; + SUBTRACT(v2,vCp0,vAxis); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 16)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_FIRST_VERTEX) + { + // first triangle vertex and second capsule point + //vAxis = v0 - vCp1; + SUBTRACT(v0,vCp1,vAxis); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 17)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_SECOND_VERTEX) + { + // second triangle vertex and second capsule point + //vAxis = v1 - vCp1; + SUBTRACT(v1,vCp1,vAxis); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 18)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::CUF_USE_THIRD_VERTEX) + { + // third triangle vertex and second capsule point + //vAxis = v2 - vCp1; + SUBTRACT(v2,vCp1,vAxis); + if (_length2OfVector3( vAxis ) > fEpsilon) { + if (!_cldTestAxis(v0, v1, v2, vAxis, 19)) { + return FALSE; + } + } + } + + return TRUE; +} + +// test one mesh triangle on intersection with capsule +void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + uint8 flags) +{ + // calculate edges + SUBTRACT(v1,v0,m_vE0); + SUBTRACT(v2,v1,m_vE1); + SUBTRACT(v0,v2,m_vE2); + + dVector3 _minus_vE0; + SUBTRACT(v0,v1,_minus_vE0); + + // calculate poly normal + dCalcVectorCross3(m_vN,m_vE1,_minus_vE0); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (!dSafeNormalize3(m_vN)) + { + return; + } + + // create plane from triangle + dReal plDistance = -dCalcVectorDot3(v0,m_vN); + dVector4 plTrianglePlane; + CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance); + + // calculate capsule distance to plane + dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition); + + // Capsule must be over positive side of triangle + if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0, vPnt1, vPnt2; + SET (vPnt0,v0); + + if (fDistanceCapsuleCenterToPlane < 0) + { + SET (vPnt1,v2); + SET (vPnt2,v1); + } + else + { + SET (vPnt1,v1); + SET (vPnt2,v2); + } + + // do intersection test and find best separating axis + if (!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags)) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if (m_iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + dIASSERT(FALSE); + // do nothing + return; + } + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius; + vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius; + vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); + + // transform capsule edge points into triangle space + vCEdgePoint0[0] -= vPnt0[0]; + vCEdgePoint0[1] -= vPnt0[1]; + vCEdgePoint0[2] -= vPnt0[2]; + + vCEdgePoint1[0] -= vPnt0[0]; + vCEdgePoint1[1] -= vPnt0[1]; + vCEdgePoint1[2] -= vPnt0[2]; + + dVector4 plPlane; + dVector3 _minus_vN; + _minus_vN[0] = -m_vN[0]; + _minus_vN[1] = -m_vN[1]; + _minus_vN[2] = -m_vN[2]; + // triangle plane + CONSTRUCTPLANE(plPlane,_minus_vN,0); + //plPlane = Plane4f( -m_vN, 0); + + if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + // plane with edge 0 + dVector3 vTemp; + dCalcVectorCross3(vTemp,m_vN,m_vE0); + CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); + if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCalcVectorCross3(vTemp,m_vN,m_vE1); + CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5))); + if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCalcVectorCross3(vTemp,m_vN,m_vE2); + CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); + if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { + return; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += vPnt0[0]; + vCEdgePoint0[1] += vPnt0[1]; + vCEdgePoint0[2] += vPnt0[2]; + + vCEdgePoint1[0] += vPnt0[0]; + vCEdgePoint1[1] += vPnt0[1]; + vCEdgePoint1[2] += vPnt0[2]; + + // calculate depths for both contact points + SUBTRACT(vCEdgePoint0,m_vCapsulePosition,vTemp); + dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt); + SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp); + dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt); + + // clamp depths to zero + if (fDepth0 < 0) + { + fDepth0 = 0.0f; + } + + if (fDepth1 < 0 ) + { + fDepth1 = 0.0f; + } + + // Cached contacts's data + // contact 0 + dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result + m_gLocalContacts[m_ctContacts].fDepth = fDepth0; + SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal); + SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0); + m_gLocalContacts[m_ctContacts].nFlags = 1; + m_ctContacts++; + + if (m_ctContacts < (m_iFlags & NUMC_MASK)) { + // contact 1 + m_gLocalContacts[m_ctContacts].fDepth = fDepth1; + SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal); + SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1); + m_gLocalContacts[m_ctContacts].nFlags = 1; + m_ctContacts++; + } +} + +void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Capsule, + int flags, int skip) +{ + const dMatrix3* pRot = (const dMatrix3*)dGeomGetRotation(Capsule); + memcpy(m_mCapsuleRotation, pRot, sizeof(dMatrix3)); + + const dVector3* pDst = (const dVector3*)dGeomGetPosition(Capsule); + memcpy(m_vCapsulePosition, pDst, sizeof(dVector3)); + + m_vCapsuleAxis[0] = m_mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + m_vCapsuleAxis[1] = m_mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + m_vCapsuleAxis[2] = m_mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + + // Get size of Capsule + dGeomCapsuleGetParams(Capsule, &m_vCapsuleRadius, &m_fCapsuleSize); + m_fCapsuleSize += 2*m_vCapsuleRadius; + + const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh); + memcpy(m_mTriMeshRot, pTriRot, sizeof(dMatrix3)); + + const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh); + memcpy(m_vTriMeshPos, pTriPos, sizeof(dVector3)); + + // global info for contact creation + m_iStride =skip; + m_iFlags =flags; + + // reset contact counter + m_ctContacts = 0; + + // reset best depth + m_fBestDepth = - MAX_REAL; + m_fBestCenter = 0; + m_fBestrt = 0; + + // reset collision normal + m_vNormal[0] = REAL(0.0); + m_vNormal[1] = REAL(0.0); + m_vNormal[2] = REAL(0.0); +} + +int sTrimeshCapsuleColliderData::TestCollisionForSingleTriangle(int ctContacts0, + int Triint, dVector3 dv[3], uint8 flags, bool &bOutFinishSearching) +{ + // test this triangle + _cldTestOneTriangleVSCapsule(dv[0],dv[1],dv[2], flags); + + // fill-in tri index for generated contacts + for (; ctContacts0 < (int)m_ctContacts; ctContacts0++) + m_gLocalContacts[ctContacts0].triIndex = Triint; + + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + bOutFinishSearching = (m_ctContacts >= (m_iFlags & NUMC_MASK)); + + return ctContacts0; +} + + +static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider, + const sTrimeshCapsuleColliderData &cData, dxTriMesh *TriMesh, dxGeom *Capsule, + OBBCache &BoxCache) +{ + Matrix4x4 MeshMatrix; + const dVector3 vZeroVector3 = { REAL(0.0), }; + MakeMatrix(vZeroVector3, cData.m_mTriMeshRot, MeshMatrix); + + const dVector3 &vCapsulePos = cData.m_vCapsulePosition; + const dMatrix3 &mCapsuleRot = cData.m_mCapsuleRotation; + + dVector3 vCapsuleOffsetPos; + dSubtractVectors3(vCapsuleOffsetPos, vCapsulePos, cData.m_vTriMeshPos); + + const dReal fCapsuleRadius = cData.m_vCapsuleRadius, fCapsuleHalfAxis = cData.m_fCapsuleSize * REAL(0.5); + + OBB obbCapsule; + obbCapsule.mCenter.Set(vCapsuleOffsetPos[0], vCapsuleOffsetPos[1], vCapsuleOffsetPos[2]); + obbCapsule.mExtents.Set( + 0 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius, + 1 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius, + 2 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius); + obbCapsule.mRot.Set( + mCapsuleRot[0], mCapsuleRot[4], mCapsuleRot[8], + mCapsuleRot[1], mCapsuleRot[5], mCapsuleRot[9], + mCapsuleRot[2], mCapsuleRot[6], mCapsuleRot[10]); + + // TC results + if (TriMesh->getDoTC(dxTriMesh::TTC_BOX)) { + dxTriMesh::BoxTC* BoxTC = 0; + const int iBoxCacheSize = TriMesh->m_BoxTCCache.size(); + for (int i = 0; i != iBoxCacheSize; i++){ + if (TriMesh->m_BoxTCCache[i].Geom == Capsule){ + BoxTC = &TriMesh->m_BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->m_BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->m_BoxTCCache[TriMesh->m_BoxTCCache.size() - 1]; + BoxTC->Geom = Capsule; + BoxTC->FatCoeff = 1.0f; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCapsule, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(BoxCache, obbCapsule, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } +} + +// capsule - trimesh by CroTeam +// Ported by Nguyem Binh +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int nContactCount = 0; + + dxTriMesh *TriMesh = (dxTriMesh*)o1; + dxGeom *Capsule = o2; + + sTrimeshCapsuleColliderData cData; + cData.SetupInitialContext(TriMesh, Capsule, flags, skip); + + const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == Capsule->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + OBBCollider& Collider = pccColliderCache->m_OBBCollider; + + // Will it better to use LSS here? -> confirm Pierre. + dQueryCCTLPotentialCollisionTriangles(Collider, cData, + TriMesh, Capsule, pccColliderCache->m_DefaultBoxCache); + + if (Collider.GetContactStatus()) + { + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + + if (TriCount != 0) + { + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriMesh->m_ArrayCallback != null) + { + TriMesh->m_ArrayCallback(TriMesh, Capsule, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + unsigned int ctContacts0 = cData.m_ctContacts; + + const uint8 *useFlags = TriMesh->retrieveMeshSmartUseFlags(); + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!TriMesh->invokeCallback(Capsule, Triint)) continue; + + dVector3 dv[3]; + TriMesh->fetchMeshTriangle(dv, Triint, cData.m_vTriMeshPos, cData.m_mTriMeshRot); + + uint8 flags = useFlags != NULL ? useFlags[Triint] : (uint8)dxTriMeshData::CUF__USE_ALL_COMPONENTS; + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, flags, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + if (cData.m_ctContacts != 0) + { + nContactCount = cData._ProcessLocalContacts(contact, TriMesh, Capsule); + } + } + } + + return nContactCount; +} + + +#endif + + +// GIMPACT version +#if dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "gimpact_gim_contact_accessor.h" + +#define nCAPSULE_AXIS 2 + +// capsule - trimesh By francisco leon +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)o1; + dxGeom* gCylinder = o2; + + //Get capsule params + dMatrix3 mCapsuleRotation; + dVector3 vCapsulePosition; + dVector3 vCapsuleAxis; + dReal vCapsuleRadius; + dReal fCapsuleSize; + dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder); + memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3)); + dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder); + memcpy(vCapsulePosition,pDst,sizeof(dVector3)); + //Axis + vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + // Get size of CCylinder + dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize); + fCapsuleSize*=0.5f; + //Set Capsule params + GIM_CAPSULE_DATA capsule; + + capsule.m_radius = vCapsuleRadius; + VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis); + VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1); + VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis); + VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2); + + + //Create contact list + GDYNAMIC_ARRAY trimeshcontacts; + GIM_CREATE_CONTACT_LIST(trimeshcontacts); + + //Collide trimeshe vs capsule + gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts); + + + if(trimeshcontacts.m_size == 0) + { + GIM_DYNARRAY_DESTROY(trimeshcontacts); + return 0; + } + + GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts); + unsigned contactcount = trimeshcontacts.m_size; + + dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, TriMesh, gCylinder, -1); + contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, flags, contact, skip); + + GIM_DYNARRAY_DESTROY(trimeshcontacts); + + return (int)contactcount; +} + + +#endif // dTRIMESH_GIMPACT + + +#endif // dTRIMESH_ENABLED diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_colliders.h b/libs/ode-0.16.1/ode/src/collision_trimesh_colliders.h new file mode 100644 index 0000000..9452f90 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_colliders.h @@ -0,0 +1,47 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_COLLISION_TRIMESH_COLLIDERS_H_ +#define _ODE_COLLISION_TRIMESH_COLLIDERS_H_ + + +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTrimeshPlane(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideConvexTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +ODE_PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags, + dContactGeom *contact, int skip ) +{ + // Swapped case, for code that needs it (heightfield initially) + // The other ray-geom colliders take geoms in a swapped order to the + // dCollideRTL function which is annoying when using function pointers. + return dCollideRTL( trimesh, ray, flags, contact, skip ); +} + + +#endif // _ODE_COLLISION_TRIMESH_COLLIDERS_H_ diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_disabled.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_disabled.cpp new file mode 100644 index 0000000..69203a0 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_disabled.cpp @@ -0,0 +1,302 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/collision.h> +#include "config.h" +#include "matrix.h" + + +#if !dTRIMESH_ENABLED + +#include "collision_util.h" +#include "collision_trimesh_internal.h" + + +static const dMatrix4 identity = +{ + REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), + REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), + REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), + REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ) +}; + + +typedef dxMeshBase dxDisabledTriMesh_Parent; +struct dxDisabledTriMesh: + public dxDisabledTriMesh_Parent +{ +public: + // Functions + dxDisabledTriMesh(dxSpace *Space, + dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback): + dxDisabledTriMesh_Parent(Space, NULL, Callback, ArrayCallback, RayCallback, false) + { + } + + virtual void computeAABB(); // This is an abstract method in the base class +}; + +/*virtual */ +void dxDisabledTriMesh::computeAABB() +{ + // Do nothing +} + + +////////////////////////////////////////////////////////////////////////// +// Stub functions for trimesh calls + +/*extern */ +dTriMeshDataID dGeomTriMeshDataCreate(void) +{ + return NULL; +} + +/*extern */ +void dGeomTriMeshDataDestroy(dTriMeshDataID g) +{ + // Do nothing +} + + +/*extern */ +void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) +{ + // Do nothing +} + +/*extern */ +void *dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) +{ + return NULL; +} + +/*extern */ +void *dGeomTriMeshDataGet2(dTriMeshDataID g, int data_id, sizeint *pout_size/*=NULL*/) +{ + if (pout_size != NULL) + { + *pout_size = 0; + } + + return NULL; +} + + +/*extern */ +void dGeomTriMeshSetLastTransform( dGeomID g, const dMatrix4 last_trans ) +{ + // Do nothing +} + +/*extern */ +const dReal *dGeomTriMeshGetLastTransform( dGeomID g ) +{ + return identity; +} + + +/*extern */ +dGeomID dCreateTriMesh(dSpaceID space, + dTriMeshDataID Data, + dTriCallback* Callback, + dTriArrayCallback* ArrayCallback, + dTriRayCallback* RayCallback) +{ + return new dxDisabledTriMesh(space, Callback, ArrayCallback, RayCallback); // Oleh_Derevenko: I'm not sure if a NULL can be returned here -- keep on returning an object for backward compatibility +} + + +/*extern */ +void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data) +{ + // Do nothing +} + +/*extern */ +dTriMeshDataID dGeomTriMeshGetData(dGeomID g) +{ + return NULL; +} + + +/*extern */ +void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + // Do nothing +} + +/*extern */ +void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + // Do nothing +} + +/*extern */ +void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + // Do nothing +} + +/*extern */ +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + // Do nothing +} + +/*extern */ +void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const dTriIndex* Indices, int IndexCount) +{ + // Do nothing +} + +/*extern */ +void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const dTriIndex* Indices, int IndexCount, + const int* Normals) +{ + // Do nothing +} + + +/*extern ODE_API */ +int dGeomTriMeshDataPreprocess(dTriMeshDataID g) +{ + // Do nothing + return 1; +} + +/*extern ODE_API */ +int dGeomTriMeshDataPreprocess2(dTriMeshDataID g, unsigned int buildRequestFlags, const intptr *requestExtraData/*=NULL | const intptr (*)[dTRIDATAPREPROCESS_BUILD__MAX]*/) +{ + // Do nothing + return 1; +} + +/*extern */ +void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback) +{ + // Do nothing +} + +/*extern */ +dTriCallback* dGeomTriMeshGetCallback(dGeomID g) +{ + return NULL; +} + + +/*extern */ +void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback) +{ + // Do nothing +} + +/*extern */ +dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g) +{ + return NULL; +} + + +/*extern */ +void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback) +{ + // Do nothing +} + +/*extern */ +dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g) +{ + return NULL; +} + + +/*extern */ +void dGeomTriMeshSetTriMergeCallback(dGeomID g, dTriTriMergeCallback* Callback) +{ + // Do nothing +} + +/*extern */ +dTriTriMergeCallback* dGeomTriMeshGetTriMergeCallback(dGeomID g) +{ + return NULL; +} + + +/*extern */ +void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable) +{ + // Do nothing +} + +/*extern */ +int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass) +{ + return 0; +} + + +/*extern */ +void dGeomTriMeshClearTCCache(dGeomID g) +{ + // Do nothing +} + + +/*extern */ +dTriMeshDataID dGeomTriMeshGetTriMeshDataID(dGeomID g) +{ + return NULL; +} + + +/*extern */ +int dGeomTriMeshGetTriangleCount (dGeomID g) +{ + return 0; +} + +/*extern */ +void dGeomTriMeshDataUpdate(dTriMeshDataID g) +{ + // Do nothing +} + + +#endif // !dTRIMESH_ENABLED + + diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_gimpact.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_gimpact.cpp new file mode 100644 index 0000000..d9b5ecd --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_gimpact.cpp @@ -0,0 +1,424 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2017 + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "util.h" + + +#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + +#include "collision_util.h" +#include "collision_trimesh_gimpact.h" +#include "collision_trimesh_internal_impl.h" + + +////////////////////////////////////////////////////////////////////////// +// dxTriMeshData + +bool dxTriMeshData::preprocessData(bool /*buildUseFlags*//*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/) +{ + FaceAngleStorageMethod faceAndgesRequirementToUse = faceAndgesRequirement; + + if (faceAndgesRequirement != ASM__INVALID && haveFaceAnglesBeenBuilt()) + { + dUASSERT(false, "Another request to build face angles after they had already been built"); + + faceAndgesRequirementToUse = ASM__INVALID; + } + + // If this mesh has already been preprocessed, exit + bool result = faceAndgesRequirementToUse == ASM__INVALID || retrieveTriangleCount() == 0 + || meaningfulPreprocessData(faceAndgesRequirementToUse); + return result; +} + +struct TrimeshDataVertexIndexAccessor_GIMPACT +{ + enum + { + TRIANGLEINDEX_STRIDE = dxTriMesh::TRIANGLEINDEX_STRIDE, + }; + + explicit TrimeshDataVertexIndexAccessor_GIMPACT(dxTriMeshData *meshData): + m_TriangleVertexIndices(meshData->retrieveTriangleVertexIndices()) + { + dIASSERT(meshData->retrieveTriangleStride() == TRIANGLEINDEX_STRIDE); + } + + void getTriangleVertexIndices(unsigned out_VertexIndices[dMTV__MAX], unsigned triangleIdx) const + { + const GUINT32 *triIndicesBegin = m_TriangleVertexIndices; + const unsigned triStride = TRIANGLEINDEX_STRIDE; + + const GUINT32 *triIndicesOfInterest = (const GUINT32 *)((const uint8 *)triIndicesBegin + (sizeint)triangleIdx * triStride); + std::copy(triIndicesOfInterest, triIndicesOfInterest + dMTV__MAX, out_VertexIndices); + } + + const GUINT32 *m_TriangleVertexIndices; +}; + +struct TrimeshDataTrianglePointAccessor_GIMPACT +{ + enum + { + VERTEXINSTANCE_STRIDE = dxTriMesh::VERTEXINSTANCE_STRIDE, + TRIANGLEINDEX_STRIDE = dxTriMesh::TRIANGLEINDEX_STRIDE, + }; + + TrimeshDataTrianglePointAccessor_GIMPACT(dxTriMeshData *meshData): + m_VertexInstances(meshData->retrieveVertexInstances()), + m_TriangleVertexIndices(meshData->retrieveTriangleVertexIndices()) + { + dIASSERT((unsigned)meshData->retrieveVertexStride() == (unsigned)VERTEXINSTANCE_STRIDE); + dIASSERT((unsigned)meshData->retrieveTriangleStride() == (unsigned)TRIANGLEINDEX_STRIDE); + } + + void getTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex) const + { + dxTriMeshData::retrieveTriangleVertexPoints(out_Points, triangleIndex, + &m_VertexInstances[0][0], VERTEXINSTANCE_STRIDE, m_TriangleVertexIndices, TRIANGLEINDEX_STRIDE); + } + + const vec3f *m_VertexInstances; + const GUINT32 *m_TriangleVertexIndices; +}; + +bool dxTriMeshData::meaningfulPreprocessData(FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/) +{ + const bool buildFaceAngles = true; dIASSERT(faceAndgesRequirement != ASM__INVALID); + // dIASSERT(buildFaceAngles); + dIASSERT(/*!buildFaceAngles || */!haveFaceAnglesBeenBuilt()); + + bool result = false; + + bool anglesAllocated = false; + + do + { + if (buildFaceAngles) + { + if (!allocateFaceAngles(faceAndgesRequirement)) + { + break; + } + } + + anglesAllocated = true; + + const unsigned int numTris = retrieveTriangleCount(); + const unsigned int numVertices = retrieveVertexCount(); + sizeint numEdges = (sizeint)numTris * dMTV__MAX; + dIASSERT(numVertices <= numEdges); // Edge records are going to be used for vertex data as well + + const sizeint recordsMemoryRequired = dEFFICIENT_SIZE(numEdges * sizeof(EdgeRecord)); + const sizeint verticesMemoryRequired = /*dEFFICIENT_SIZE*/(numVertices * sizeof(VertexRecord)); // Skip alignment for the last chunk + const sizeint totalTempMemoryRequired = recordsMemoryRequired + verticesMemoryRequired; + void *tempBuffer = dAlloc(totalTempMemoryRequired); + + if (tempBuffer == NULL) + { + break; + } + + EdgeRecord *edges = (EdgeRecord *)tempBuffer; + VertexRecord *vertices = (VertexRecord *)((uint8 *)tempBuffer + recordsMemoryRequired); + + TrimeshDataVertexIndexAccessor_GIMPACT indexAccessor(this); + meaningfulPreprocess_SetupEdgeRecords(edges, numEdges, indexAccessor); + + // Sort the edges, so the ones sharing the same verts are beside each other + std::sort(edges, edges + numEdges); + + TrimeshDataTrianglePointAccessor_GIMPACT pointAccessor(this); + const dReal *const externalNormals = retrieveNormals(); + IFaceAngleStorageControl *faceAngles = retrieveFaceAngles(); + meaningfulPreprocess_buildEdgeFlags(NULL, faceAngles, edges, numEdges, vertices, externalNormals, pointAccessor); + + dFree(tempBuffer, totalTempMemoryRequired); + + result = true; + } + while (false); + + if (!result) + { + if (anglesAllocated) + { + if (buildFaceAngles) + { + freeFaceAngles(); + } + } + } + + return result; +} + + +////////////////////////////////////////////////////////////////////////// +// Trimesh + +dxTriMesh::~dxTriMesh() +{ + //Terminate Trimesh + gim_trimesh_destroy(&m_collision_trimesh); + gim_terminate_buffer_managers(m_buffer_managers); +} + + +/*virtual */ +void dxTriMesh::computeAABB() +{ + //update trimesh transform + mat4f transform; + IDENTIFY_MATRIX_4X4(transform); + MakeMatrix(this, transform); + gim_trimesh_set_tranform(&m_collision_trimesh, transform); + + //Update trimesh boxes + gim_trimesh_update(&m_collision_trimesh); + + GIM_AABB_COPY( &m_collision_trimesh.m_aabbset.m_global_bound, aabb ); +} + + +void dxTriMesh::assignMeshData(dxTriMeshData *Data) +{ + // GIMPACT only supports stride 12, so we need to catch the error early. + dUASSERT( + (unsigned int)Data->retrieveVertexStride() == (unsigned)VERTEXINSTANCE_STRIDE + && (unsigned int)Data->retrieveTriangleStride() == (unsigned)TRIANGLEINDEX_STRIDE, + "Gimpact trimesh only supports a stride of 3 float/int\n" + "This means that you cannot use dGeomTriMeshDataBuildSimple() with Gimpact.\n" + "Change the stride, or use Opcode trimeshes instead.\n" + ); + + dxTriMesh_Parent::assignMeshData(Data); + + //Create trimesh + const vec3f *vertexInstances = Data->retrieveVertexInstances(); + if ( vertexInstances != NULL ) + { + const GUINT32 *triangleVertexIndices = Data->retrieveTriangleVertexIndices(); + + sizeint vertexInstanceCount = Data->retrieveVertexCount(); + sizeint triangleVertexCount = (sizeint)Data->retrieveTriangleCount() * dMTV__MAX; + + gim_trimesh_create_from_data( + m_buffer_managers, + &m_collision_trimesh, // gimpact mesh + const_cast<vec3f *>(vertexInstances), // vertices + dCAST_TO_SMALLER(GUINT32, vertexInstanceCount), // nr of verts + 0, // copy verts? + const_cast<GUINT32 *>(triangleVertexIndices), // indices + dCAST_TO_SMALLER(GUINT32, triangleVertexCount), // nr of indices + 0, // copy indices? + 1 // transformed reply + ); + } +} + + +////////////////////////////////////////////////////////////////////////// + +/*extern */ +dTriMeshDataID dGeomTriMeshDataCreate() +{ + return new dxTriMeshData(); +} + +/*extern */ +void dGeomTriMeshDataDestroy(dTriMeshDataID g) +{ + dxTriMeshData *data = g; + delete data; +} + +/*extern */ +void dGeomTriMeshDataSet(dTriMeshDataID g, int dataId, void *pDataLocation) +{ + dUASSERT(g, "The argument is not a trimesh data"); + + dxTriMeshData *data = g; + + switch (dataId) + { + case dTRIMESHDATA_FACE_NORMALS: + { + data->assignNormals((const dReal *)pDataLocation); + break; + } + + case dTRIMESHDATA_USE_FLAGS: // Not used for GIMPACT + { + break; + } + + // case dTRIMESHDATA__MAX: -- To be located by Find in Files + default: + { + dUASSERT(dataId, "invalid data type"); + break; + } + } +} + +static void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) ; + +/*extern */ +void *dGeomTriMeshDataGet(dTriMeshDataID g, int dataId) +{ + return geomTriMeshDataGet(g, dataId, NULL); +} + +/*extern */ +void *dGeomTriMeshDataGet2(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) +{ + return geomTriMeshDataGet(g, dataId, pOutDataSize); +} + +static +void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) +{ + dUASSERT(g, "The argument is not a trimesh data"); + + const dxTriMeshData *data = g; + + void *result = NULL; + + switch (dataId) + { + case dTRIMESHDATA_FACE_NORMALS: + { + if (pOutDataSize != NULL) + { + *pOutDataSize = data->calculateNormalsMemoryRequirement(); + } + + result = (void *)data->retrieveNormals(); + break; + } + + case dTRIMESHDATA_USE_FLAGS: // Not not used for GIMPACT + { + if (pOutDataSize != NULL) + { + *pOutDataSize = 0; + } + + break; + } + + // case dTRIMESHDATA__MAX: -- To be located by Find in Files + default: + { + if (pOutDataSize != NULL) + { + *pOutDataSize = 0; + } + + dUASSERT(dataId, "invalid data type"); + break; + } + } + + return result; +} + +/*extern */ +void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "The argument is not a trimesh data"); + dAASSERT(Vertices); + dAASSERT(Indices); + + dxTriMeshData *data = g; + + data->buildData(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + true); +} + +/*extern */ +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "The argument is not a trimesh data"); + dAASSERT(Vertices); + dAASSERT(Indices); + + dxTriMeshData *data = g; + + data->buildData(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + false); +} + + +////////////////////////////////////////////////////////////////////////// + +/*extern */ +dGeomID dCreateTriMesh(dSpaceID space, + dTriMeshDataID Data, + dTriCallback* Callback, + dTriArrayCallback* ArrayCallback, + dTriRayCallback* RayCallback) +{ + dxTriMesh *mesh = new dxTriMesh(space, Data, Callback, ArrayCallback, RayCallback); + return mesh; +} + + +/*extern */ +void dGeomTriMeshSetLastTransform(dGeomID g, const dMatrix4 last_trans ) +{ + dAASSERT(g); + dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh"); + + //stub +} + +/*extern */ +const dReal *dGeomTriMeshGetLastTransform(dGeomID g) +{ + dAASSERT(g); + dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh"); + + return NULL; // stub +} + + +#endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_gimpact.h b/libs/ode-0.16.1/ode/src/collision_trimesh_gimpact.h new file mode 100644 index 0000000..b928e97 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_gimpact.h @@ -0,0 +1,278 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. +// Modified for FreeSOLID Compatibility by Rodrigo Hernandez +// Trimesh caches separation by Oleh Derevenko +// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2019 + + +#ifndef _ODE_COLLISION_TRIMESH_GIMPACT_H_ +#define _ODE_COLLISION_TRIMESH_GIMPACT_H_ + + +#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + + +//**************************************************************************** +// dxTriMesh class + + +#include "collision_kernel.h" +#include "collision_trimesh_colliders.h" +#include "collision_util.h" +#include <ode/collision_trimesh.h> + +#include "collision_trimesh_internal.h" +#include <GIMPACT/gimpact.h> + + +struct TrimeshCollidersCache // Required for compatibility with OPCODE +{ +}; + + +typedef dxTriDataBase dxTriMeshData_Parent; +struct dxTriMeshData: + public dxTriMeshData_Parent +{ +public: + dxTriMeshData(): + dxTriMeshData_Parent() + { + } + + ~dxTriMeshData() { /* Do nothing */ } + + using dxTriMeshData_Parent::buildData; + + /* Setup the UseFlags array and/or build face angles*/ + bool preprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/); + +private: + bool meaningfulPreprocessData(FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/); + +public: + /* For when app changes the vertices */ + void updateData() { /* Do nothing */ } + +public: + const vec3f *retrieveVertexInstances() const { return (const vec3f *)dxTriMeshData_Parent::retrieveVertexInstances(); } + const GUINT32 *retrieveTriangleVertexIndices() const { return (const GUINT32 *)dxTriMeshData_Parent::retrieveTriangleVertexIndices(); } + +public: + void assignNormals(const dReal *normals) { dxTriMeshData_Parent::assignNormals(normals); } + const dReal *retrieveNormals() const { return (const dReal *)dxTriMeshData_Parent::retrieveNormals(); } + sizeint calculateNormalsMemoryRequirement() const { return retrieveTriangleCount() * (sizeof(dReal) * dSA__MAX); } +}; + + + +#ifdef dDOUBLE +// To use GIMPACT with doubles, we need to patch a couple of the GIMPACT functions to +// convert arguments to floats before sending them in + + +/// Convert an gimpact vec3f to a ODE dVector3d: dVector3[i] = vec3f[i] +#define dVECTOR3_VEC3F_COPY(b,a) { \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + (b)[2] = (a)[2]; \ + (b)[3] = 0; \ +} + +static inline +void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32 triangle_index, dVector3 v1, dVector3 v2, dVector3 v3) +{ + vec3f src1, src2, src3; + GREAL *psrc1 = v1 != NULL ? src1 : NULL; + GREAL *psrc2 = v2 != NULL ? src2 : NULL; + GREAL *psrc3 = v3 != NULL ? src3 : NULL; + gim_trimesh_get_triangle_vertices(trimesh, triangle_index, psrc1, psrc2, psrc3); + + if (v1 != NULL) + { + dVECTOR3_VEC3F_COPY(v1, src1); + } + + if (v2 != NULL) + { + dVECTOR3_VEC3F_COPY(v2, src2); + } + + if (v3 != NULL) + { + dVECTOR3_VEC3F_COPY(v3, src3); + } +} + +// Anything calling gim_trimesh_get_triangle_vertices from within ODE +// should be patched through to the dDOUBLE version above + +#define gim_trimesh_get_triangle_vertices gim_trimesh_get_triangle_verticesODE + +static inline +int gim_trimesh_ray_closest_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, dReal tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) +{ + vec3f dir_vec3f = { (GREAL)dir[ 0 ], (GREAL)dir[ 1 ], (GREAL)dir[ 2 ] }; + vec3f origin_vec3f = { (GREAL)origin[ 0 ], (GREAL)origin[ 1 ], (GREAL)origin[ 2 ] }; + + return gim_trimesh_ray_closest_collision( mesh, origin_vec3f, dir_vec3f, (GREAL)tmax, contact ); +} + +static inline +int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, const dVector3 origin, const dVector3 dir, dReal tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) +{ + vec3f dir_vec3f = { (GREAL)dir[ 0 ], (GREAL)dir[ 1 ], (GREAL)dir[ 2 ] }; + vec3f origin_vec3f = { (GREAL)origin[ 0 ], (GREAL)origin[ 1 ], (GREAL)origin[ 2 ] }; + + return gim_trimesh_ray_collision( mesh, origin_vec3f, dir_vec3f, (GREAL)tmax, contact ); +} + +static inline +void gim_trimesh_sphere_collisionODE( GIM_TRIMESH *mesh, const dVector3 Position, dReal Radius, GDYNAMIC_ARRAY *contact ) +{ + vec3f pos_vec3f = { (GREAL)Position[ 0 ], (GREAL)Position[ 1 ], (GREAL)Position[ 2 ] }; + gim_trimesh_sphere_collision( mesh, pos_vec3f, (GREAL)Radius, contact ); +} + +static inline +void gim_trimesh_plane_collisionODE( GIM_TRIMESH *mesh, const dVector4 plane, GDYNAMIC_ARRAY *contact ) +{ + vec4f plane_vec4f = { (GREAL)plane[ 0 ], (GREAL)plane[ 1 ], (GREAL)plane[ 2 ], (GREAL)plane[ 3 ] }; \ + gim_trimesh_plane_collision( mesh, plane_vec4f, contact ); \ +} + +#define GIM_AABB_COPY( src, dst ) { \ + (dst)[ 0 ]= (src) -> minX; \ + (dst)[ 1 ]= (src) -> maxX; \ + (dst)[ 2 ]= (src) -> minY; \ + (dst)[ 3 ]= (src) -> maxY; \ + (dst)[ 4 ]= (src) -> minZ; \ + (dst)[ 5 ]= (src) -> maxZ; \ +} + + +#else // #ifdef !dDOUBLE + +// With single precision, we can pass native ODE vectors directly to GIMPACT + +#define gim_trimesh_ray_closest_collisionODE gim_trimesh_ray_closest_collision +#define gim_trimesh_ray_collisionODE gim_trimesh_ray_collision +#define gim_trimesh_sphere_collisionODE gim_trimesh_sphere_collision +#define gim_trimesh_plane_collisionODE gim_trimesh_plane_collision + +#define GIM_AABB_COPY( src, dst ) memcpy( dst, src, 6 * sizeof( GREAL ) ) + + +#endif // #ifdef !dDOUBLE + + +typedef dxMeshBase dxTriMesh_Parent; +struct dxTriMesh: + public dxTriMesh_Parent +{ +public: + // Functions + dxTriMesh(dxSpace *Space, dxTriMeshData *Data, + dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback): + dxTriMesh_Parent(Space, NULL, Callback, ArrayCallback, RayCallback, true) // TC has speed/space 'issues' that don't make it a clear win by default on spheres/boxes. + { + gim_init_buffer_managers(m_buffer_managers); + assignMeshData(Data); + } + + ~dxTriMesh(); + + void clearTCCache() { /* do nothing */ } + + virtual void computeAABB(); + +public: + dxTriMeshData *retrieveMeshData() const { return getMeshData(); } + + unsigned getMeshTriangleCount() const { return gim_trimesh_get_triangle_count(const_cast<GIM_TRIMESH *>(&m_collision_trimesh)); } + + void fetchMeshTransformedTriangle(dVector3 *const pout_triangle[3], unsigned index) + { + gim_trimesh_locks_work_data(&m_collision_trimesh); + gim_trimesh_get_triangle_vertices(&m_collision_trimesh, (GUINT32)index, *pout_triangle[0], *pout_triangle[1], *pout_triangle[2]); + gim_trimesh_unlocks_work_data(&m_collision_trimesh); + } + + void fetchMeshTransformedTriangle(dVector3 out_triangle[3], unsigned index) + { + gim_trimesh_locks_work_data(&m_collision_trimesh); + gim_trimesh_get_triangle_vertices(&m_collision_trimesh, (GUINT32)index, out_triangle[0], out_triangle[1], out_triangle[2]); + gim_trimesh_unlocks_work_data(&m_collision_trimesh); + } + +private: + dxTriMeshData *getMeshData() const { return static_cast<dxTriMeshData *>(dxTriMesh_Parent::getMeshData()); } + +public: + enum + { + VERTEXINSTANCE_STRIDE = sizeof(vec3f), + TRIANGLEINDEX_STRIDE = sizeof(GUINT32) * dMTV__MAX, + }; + + void assignMeshData(dxTriMeshData *Data); + +public: + GIM_TRIMESH m_collision_trimesh; + GBUFFER_MANAGER_DATA m_buffer_managers[G_BUFFER_MANAGER__MAX]; +}; + + +static inline +void MakeMatrix(const dVector3 position, const dMatrix3 rotation, mat4f m) +{ + m[0][0] = (GREAL)rotation[dM3E_XX]; + m[0][1] = (GREAL)rotation[dM3E_XY]; + m[0][2] = (GREAL)rotation[dM3E_XZ]; + + m[1][0] = (GREAL)rotation[dM3E_YX]; + m[1][1] = (GREAL)rotation[dM3E_YY]; + m[1][2] = (GREAL)rotation[dM3E_YZ]; + + m[2][0] = (GREAL)rotation[dM3E_ZX]; + m[2][1] = (GREAL)rotation[dM3E_ZY]; + m[2][2] = (GREAL)rotation[dM3E_ZZ]; + + m[0][3] = (GREAL)position[dV3E_X]; + m[1][3] = (GREAL)position[dV3E_Y]; + m[2][3] = (GREAL)position[dV3E_Z]; +} + +static inline +void MakeMatrix(dxGeom *g, mat4f m) +{ + const dVector3 &position = g->buildUpdatedPosition(); + const dMatrix3 &rotation = g->buildUpdatedRotation(); + MakeMatrix(position, rotation, m); +} + + +#endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + +#endif //_ODE_COLLISION_TRIMESH_GIMPACT_H_ diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_internal.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_internal.cpp new file mode 100644 index 0000000..b96e25f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_internal.cpp @@ -0,0 +1,804 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2019 + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + + +#if dTRIMESH_ENABLED + +#include "collision_trimesh_internal.h" +#include "odeou.h" + +#include <algorithm> + + + +////////////////////////////////////////////////////////////////////////// + +enum EdgeStorageSignInclusion +{ + SSI__MIN, + + SSI_SIGNED_STORED = SSI__MIN, + SSI_POSITIVE_STORED, + + SSI__MAX, +}; + +template<typename TStorageType, EdgeStorageSignInclusion t_SignInclusion> +class FaceAngleStorageCodec; + +template<typename TStorageType> +class FaceAngleStorageCodec<TStorageType, SSI_SIGNED_STORED> +{ +public: + typedef typename _make_signed<TStorageType>::type storage_type; + enum + { + STORAGE_TYPE_MAX = (typename _make_unsigned<TStorageType>::type)(~(typename _make_unsigned<TStorageType>::type)0) >> 1, + }; + + static bool areNegativeAnglesCoded() + { + return true; + } + + static storage_type encodeForStorage(dReal angleValue) + { + unsigned angleAsInt = (unsigned)dFloor(dFabs(angleValue) * (dReal)(STORAGE_TYPE_MAX / M_PI)); + unsigned limitedAngleAsInt = dMACRO_MIN(angleAsInt, STORAGE_TYPE_MAX); + storage_type result = angleValue < REAL(0.0) ? -(storage_type)limitedAngleAsInt : (storage_type)limitedAngleAsInt; + return result; + } + + static FaceAngleDomain classifyStorageValue(storage_type storedValue) + { + dSASSERT(EAD__MAX == 3); + + return storedValue < 0 ? FAD_CONCAVE : (storedValue == 0 ? FAD_FLAT : FAD_CONVEX); + } + + static bool isAngleDomainStored(FaceAngleDomain domainValue) + { + return !dTMPL_IN_RANGE(domainValue, FAD__SIGNSTORED_IMPLICITVALUE_MIN, FAD__SIGNSTORED_IMPLICITVALUE_MAX); + } + + static dReal decodeStorageValue(storage_type storedValue) + { + return storedValue * (dReal)(M_PI / STORAGE_TYPE_MAX); + } +}; + +template<typename TStorageType> +class FaceAngleStorageCodec<TStorageType, SSI_POSITIVE_STORED> +{ +public: + typedef typename _make_unsigned<TStorageType>::type storage_type; + enum + { + STORAGE_TYPE_MIN = 0, + STORAGE_TYPE_MAX = (storage_type)(~(storage_type)0), + }; + + static bool areNegativeAnglesCoded() + { + return false; + } + + static storage_type encodeForStorage(dReal angleValue) + { + storage_type result = STORAGE_TYPE_MIN; + + if (angleValue >= REAL(0.0)) + { + unsigned angleAsInt = (unsigned)dFloor(angleValue * (dReal)(((STORAGE_TYPE_MAX - STORAGE_TYPE_MIN - 1) / M_PI))); + result = (STORAGE_TYPE_MIN + 1) + dMACRO_MIN(angleAsInt, STORAGE_TYPE_MAX - STORAGE_TYPE_MIN - 1); + } + + return result; + } + + static FaceAngleDomain classifyStorageValue(storage_type storedValue) + { + dSASSERT(EAD__MAX == 3); + + return storedValue < STORAGE_TYPE_MIN + 1 ? FAD_CONCAVE : (storedValue == STORAGE_TYPE_MIN + 1 ? FAD_FLAT : FAD_CONVEX); + } + + static bool isAngleDomainStored(FaceAngleDomain domainValue) + { + return dTMPL_IN_RANGE(domainValue, FAD__BYTEPOS_STORED_MIN, FAD__BYTEPOS_STORED_MAX); + } + + static dReal decodeStorageValue(storage_type storedValue) + { + dIASSERT(storedValue >= (STORAGE_TYPE_MIN + 1)); + + return (storedValue - (STORAGE_TYPE_MIN + 1)) * (dReal)(M_PI / (STORAGE_TYPE_MAX - STORAGE_TYPE_MIN - 1)); + } +}; + +template<class TStorageCodec> +class FaceAnglesWrapper: + public IFaceAngleStorageControl, + public IFaceAngleStorageView +{ +protected: + FaceAnglesWrapper(unsigned triangleCount) { setAllocatedTriangleCount(triangleCount); } + +public: + virtual ~FaceAnglesWrapper(); + + static IFaceAngleStorageControl *allocateInstance(unsigned triangleCount, IFaceAngleStorageView *&out_storageView); + + static bool calculateInstanceSizeRequired(sizeint &out_sizeRequired, unsigned triangleCount); + +private: + void freeInstance(); + +private: + typedef typename TStorageCodec::storage_type storage_type; + typedef storage_type TriangleFaceAngles[dMTV__MAX]; + + struct StorageRecord + { + StorageRecord(): m_triangleCount(0) {} + + unsigned m_triangleCount; + TriangleFaceAngles m_triangleFaceAngles[1]; + }; + + static sizeint calculateStorageSizeForTriangleCount(unsigned triangleCount) + { + const unsigned baseIncludedTriangleCount = dSTATIC_ARRAY_SIZE(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles); + const sizeint singleTriangleSize = membersize(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles[0]); + return sizeof(FaceAnglesWrapper<TStorageCodec>) + (triangleCount > baseIncludedTriangleCount ? (triangleCount - baseIncludedTriangleCount) * singleTriangleSize : 0U); + } + + static sizeint calculateTriangleCountForStorageSize(sizeint storageSize) + { + dIASSERT(storageSize >= sizeof(FaceAnglesWrapper<TStorageCodec>)); + + const unsigned baseIncludedTriangleCount = dSTATIC_ARRAY_SIZE(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles); + const sizeint singleTriangleSize = membersize(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles[0]); + return (storageSize - sizeof(FaceAnglesWrapper<TStorageCodec>)) / singleTriangleSize + baseIncludedTriangleCount; + } + +private: // IFaceAngleStorageControl + virtual void disposeStorage(); + + virtual bool areNegativeAnglesStored() const; + + virtual void assignFacesAngleIntoStorage(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue); + +private: // IFaceAngleStorageView + virtual FaceAngleDomain retrieveFacesAngleFromStorage(dReal &out_angleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex); + +public: + void setFaceAngle(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue) + { + dIASSERT(dTMPL_IN_RANGE(triangleIndex, 0, getAllocatedTriangleCount())); + dIASSERT(dTMPL_IN_RANGE(vertexIndex, dMTV__MIN, dMTV__MAX)); + + m_record.m_triangleFaceAngles[triangleIndex][vertexIndex] = TStorageCodec::encodeForStorage(dAngleValue); + } + + FaceAngleDomain getFaceAngle(dReal &out_angleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex) const + { + dIASSERT(dTMPL_IN_RANGE(triangleIndex, 0, getAllocatedTriangleCount())); + dIASSERT(dTMPL_IN_RANGE(vertexIndex, dMTV__MIN, dMTV__MAX)); + + storage_type storedValue = m_record.m_triangleFaceAngles[triangleIndex][vertexIndex]; + FaceAngleDomain resultDomain = TStorageCodec::classifyStorageValue(storedValue); + + out_angleValue = TStorageCodec::isAngleDomainStored(resultDomain) ? TStorageCodec::decodeStorageValue(storedValue) : REAL(0.0); + return resultDomain; + } + +private: + unsigned getAllocatedTriangleCount() const { return m_record.m_triangleCount; } + void setAllocatedTriangleCount(unsigned triangleCount) { m_record.m_triangleCount = triangleCount; } + +private: + StorageRecord m_record; +}; + + +template<class TStorageCodec> +FaceAnglesWrapper<TStorageCodec>::~FaceAnglesWrapper() +{ +} + + +template<class TStorageCodec> +/*static */ +IFaceAngleStorageControl *FaceAnglesWrapper<TStorageCodec>::allocateInstance(unsigned triangleCount, IFaceAngleStorageView *&out_storageView) +{ + FaceAnglesWrapper<TStorageCodec> *result = NULL; + + do + { + sizeint sizeRequired; + if (!FaceAnglesWrapper<TStorageCodec>::calculateInstanceSizeRequired(sizeRequired, triangleCount)) + { + break; + } + + void *bufferPointer = dAlloc(sizeRequired); + if (bufferPointer == NULL) + { + break; + } + + result = (FaceAnglesWrapper<TStorageCodec> *)bufferPointer; + new(result) FaceAnglesWrapper<TStorageCodec>(triangleCount); + + out_storageView = result; + } + while (false); + + return result; +} + +template<class TStorageCodec> +/*static */ +bool FaceAnglesWrapper<TStorageCodec>::calculateInstanceSizeRequired(sizeint &out_sizeRequired, unsigned triangleCount) +{ + bool result = false; + + do + { + sizeint triangleMaximumCount = calculateTriangleCountForStorageSize(SIZE_MAX); + dIASSERT(triangleCount <= triangleMaximumCount); + + if (triangleCount > triangleMaximumCount) // Check for overflow + { + break; + } + + out_sizeRequired = calculateStorageSizeForTriangleCount(triangleCount); // Trailing alignment is going to be added by memory manager automatically + result = true; + } + while (false); + + return result; +} + +template<class TStorageCodec> +void FaceAnglesWrapper<TStorageCodec>::freeInstance() +{ + unsigned triangleCount = getAllocatedTriangleCount(); + + this->FaceAnglesWrapper<TStorageCodec>::~FaceAnglesWrapper(); + + sizeint memoryBlockSize = calculateStorageSizeForTriangleCount(triangleCount); + dFree(this, memoryBlockSize); +} + + +template<class TStorageCodec> +/*virtual */ +void FaceAnglesWrapper<TStorageCodec>::disposeStorage() +{ + freeInstance(); +} + +template<class TStorageCodec> +/*virtual */ +bool FaceAnglesWrapper<TStorageCodec>::areNegativeAnglesStored() const +{ + return TStorageCodec::areNegativeAnglesCoded(); +} + +template<class TStorageCodec> +/*virtual */ +void FaceAnglesWrapper<TStorageCodec>::assignFacesAngleIntoStorage(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue) +{ + setFaceAngle(triangleIndex, vertexIndex, dAngleValue); +} + +template<class TStorageCodec> +/*virtual */ +FaceAngleDomain FaceAnglesWrapper<TStorageCodec>::retrieveFacesAngleFromStorage(dReal &out_angleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex) +{ + return getFaceAngle(out_angleValue, triangleIndex, vertexIndex); +} + + +typedef IFaceAngleStorageControl *(FAngleStorageAllocProc)(unsigned triangleCount, IFaceAngleStorageView *&out_storageView); + +BEGIN_NAMESPACE_OU(); +template<> +FAngleStorageAllocProc *const CEnumUnsortedElementArray<FaceAngleStorageMethod, ASM__MAX, FAngleStorageAllocProc *, 0x161211AD>::m_aetElementArray[] = +{ + &FaceAnglesWrapper<FaceAngleStorageCodec<uint8, SSI_SIGNED_STORED> >::allocateInstance, // ASM_BYTE_SIGNED, + &FaceAnglesWrapper<FaceAngleStorageCodec<uint8, SSI_POSITIVE_STORED> >::allocateInstance, // ASM_BYTE_POSITIVE, + &FaceAnglesWrapper<FaceAngleStorageCodec<uint16, SSI_SIGNED_STORED> >::allocateInstance, // ASM_WORD_SIGNED, +}; +END_NAMESPACE_OU(); +static const CEnumUnsortedElementArray<FaceAngleStorageMethod, ASM__MAX, FAngleStorageAllocProc *, 0x161211AD> g_AngleStorageAllocProcs; + + +////////////////////////////////////////////////////////////////////////// + +dxTriDataBase::~dxTriDataBase() +{ + freeFaceAngles(); +} + + +void dxTriDataBase::buildData(const void *vertices, int vertexStride, unsigned vertexCount, + const void *indices, unsigned indexCount, int triStride, + const void *normals, + bool single) +{ + dIASSERT(vertices); + dIASSERT(indices); + dIASSERT(vertexStride); + dIASSERT(triStride); + dIASSERT(indexCount); + dIASSERT(indexCount % dMTV__MAX == 0); + + m_vertices = vertices; + m_vertexStride = vertexStride; + m_vertexCount = vertexCount; + m_indices = indices; + m_triangleCount = indexCount / dMTV__MAX; + m_triStride = triStride; + m_single = single; + + m_normals = normals; +} + + +bool dxTriDataBase::allocateFaceAngles(FaceAngleStorageMethod storageMethod) +{ + bool result = false; + + dIASSERT(m_faceAngles == NULL); + + IFaceAngleStorageView *storageView; + + unsigned triangleCount = m_triangleCount; + + FAngleStorageAllocProc *allocProc = g_AngleStorageAllocProcs.Encode(storageMethod); + IFaceAngleStorageControl *storageInstance = allocProc(triangleCount, storageView); + + if (storageInstance != NULL) + { + m_faceAngles = storageInstance; + m_faceAngleView = storageView; + result = true; + } + + return result; +} + +void dxTriDataBase::freeFaceAngles() +{ + if (m_faceAngles != NULL) + { + m_faceAngles->disposeStorage(); + m_faceAngles = NULL; + m_faceAngleView = NULL; + } +} + + +void dxTriDataBase::EdgeRecord::setupEdge(dMeshTriangleVertex edgeIdx, int triIdx, const unsigned vertexIndices[dMTV__MAX]) +{ + if (edgeIdx < dMTV_SECOND) + { + dIASSERT(edgeIdx == dMTV_FIRST); + + m_edgeFlags = dxTriMeshData::CUF_USE_FIRST_EDGE; + m_vert1Flags = dxTriMeshData::CUF_USE_FIRST_VERTEX; + m_vert2Flags = dxTriMeshData::CUF_USE_SECOND_VERTEX; + m_vertIdx1 = vertexIndices[dMTV_FIRST]; + m_vertIdx2 = vertexIndices[dMTV_SECOND]; + } + else if (edgeIdx == dMTV_SECOND) + { + m_edgeFlags = dxTriMeshData::CUF_USE_SECOND_EDGE; + m_vert1Flags = dxTriMeshData::CUF_USE_SECOND_VERTEX; + m_vert2Flags = dxTriMeshData::CUF_USE_THIRD_VERTEX; + m_vertIdx1 = vertexIndices[dMTV_SECOND]; + m_vertIdx2 = vertexIndices[dMTV_THIRD]; + } + else + { + dIASSERT(edgeIdx == dMTV_THIRD); + + m_edgeFlags = dxTriMeshData::CUF_USE_THIRD_EDGE; + m_vert1Flags = dxTriMeshData::CUF_USE_THIRD_VERTEX; + m_vert2Flags = dxTriMeshData::CUF_USE_FIRST_VERTEX; + m_vertIdx1 = vertexIndices[dMTV_THIRD]; + m_vertIdx2 = vertexIndices[dMTV_FIRST]; + } + + // Make sure vertex index 1 is less than index 2 (for easier sorting) + if (m_vertIdx1 > m_vertIdx2) + { + dxSwap(m_vertIdx1, m_vertIdx2); + dxSwap(m_vert1Flags, m_vert2Flags); + } + + m_triIdx = triIdx; + m_absVertexFlags = 0; +} + + +BEGIN_NAMESPACE_OU(); +template<> +const dMeshTriangleVertex CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161116DC>::m_aetElementArray[] = +{ + dMTV_FIRST, // kVert0 / kVert_Base + dMTV_SECOND, // kVert1 / kVert_Base + dMTV__MAX, + dMTV_THIRD, // kVert2 / kVert_Base +}; +END_NAMESPACE_OU(); +/*extern */const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161116DC> g_VertFlagOppositeIndices; + +BEGIN_NAMESPACE_OU(); +template<> +const dMeshTriangleVertex CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161225E9>::m_aetElementArray[] = +{ + dMTV_SECOND, // kVert0 / kVert_Base + dMTV_THIRD, // kVert1 / kVert_Base + dMTV__MAX, + dMTV_FIRST, // kVert2 / kVert_Base +}; +END_NAMESPACE_OU(); +/*extern */const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161225E9> g_VertFlagEdgeStartIndices; + + +////////////////////////////////////////////////////////////////////////// + +/*extern ODE_API */ +void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const dTriIndex* Indices, int IndexCount, + const int *Normals) +{ +#ifdef dSINGLE + dGeomTriMeshDataBuildSingle1(g, + Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(dTriIndex), + Normals); +#else + dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(dTriIndex), + Normals); +#endif +} + + +/*extern ODE_API */ +void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, (const void *)NULL); +} + +/*extern ODE_API */ +void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, NULL); +} + +/*extern ODE_API */ +void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const dTriIndex* Indices, int IndexCount) +{ + dGeomTriMeshDataBuildSimple1(g, + Vertices, VertexCount, Indices, IndexCount, + (int *)NULL); +} + + +/*extern ODE_API */ +int dGeomTriMeshDataPreprocess(dTriMeshDataID g) +{ + unsigned buildRequestFlags = (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES); + return dGeomTriMeshDataPreprocess2(g, buildRequestFlags, NULL); +} + + +BEGIN_NAMESPACE_OU(); +template<> +const FaceAngleStorageMethod CEnumUnsortedElementArray<unsigned, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX, FaceAngleStorageMethod, 0x17010902>::m_aetElementArray[] = +{ + ASM_BYTE_POSITIVE, // dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA_BYTE_POSITIVE, + ASM_BYTE_SIGNED, // dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA_BYTE_ALL, + ASM_WORD_SIGNED, // dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA_WORD_ALL, +}; +END_NAMESPACE_OU(); +static const CEnumUnsortedElementArray<unsigned, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX, FaceAngleStorageMethod, 0x17010902> g_TriMeshDataPreprocess_FaceAndlesExtraDataAngleStorageMethods; + +/*extern ODE_API */ +int dGeomTriMeshDataPreprocess2(dTriMeshDataID g, unsigned int buildRequestFlags, const intptr *requestExtraData/*=NULL | const intptr (*)[dTRIDATAPREPROCESS_BUILD__MAX]*/) +{ + dUASSERT(g, "The argument is not a trimesh data"); + dAASSERT((buildRequestFlags & (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES)) == 0 || requestExtraData == NULL || dIN_RANGE(requestExtraData[dTRIDATAPREPROCESS_BUILD_FACE_ANGLES], dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MIN, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX)); + + dxTriMeshData *data = g; + + bool buildUseFlags = (buildRequestFlags & (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES)) != 0; + FaceAngleStorageMethod faceAnglesRequirement = (buildRequestFlags & (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES)) != 0 + ? g_TriMeshDataPreprocess_FaceAndlesExtraDataAngleStorageMethods.Encode(requestExtraData != NULL && dIN_RANGE(requestExtraData[dTRIDATAPREPROCESS_BUILD_FACE_ANGLES], dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MIN, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX) ? (unsigned)requestExtraData[dTRIDATAPREPROCESS_BUILD_FACE_ANGLES] : dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__DEFAULT) + : ASM__INVALID; + return data->preprocessData(buildUseFlags, faceAnglesRequirement); +} + +/*extern ODE_API */ +void dGeomTriMeshDataUpdate(dTriMeshDataID g) +{ + dUASSERT(g, "The argument is not a trimesh data"); + + dxTriMeshData *data = g; + data->updateData(); +} + + +////////////////////////////////////////////////////////////////////////// + +/*extern ODE_API */ +void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + mesh->assignCallback(Callback); +} + +/*extern ODE_API */ +dTriCallback* dGeomTriMeshGetCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + return mesh->retrieveCallback(); +} + +/*extern ODE_API */ +void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + mesh->assignArrayCallback(ArrayCallback); +} + +/*extern ODE_API */ +dTriArrayCallback *dGeomTriMeshGetArrayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + return mesh->retrieveArrayCallback(); +} + +/*extern ODE_API */ +void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + mesh->assignRayCallback(Callback); +} + +/*extern ODE_API */ +dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + return mesh->retrieveRayCallback(); +} + +/*extern ODE_API */ +void dGeomTriMeshSetTriMergeCallback(dGeomID g, dTriTriMergeCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + mesh->assignTriMergeCallback(Callback); +} + +/*extern ODE_API */ +dTriTriMergeCallback *dGeomTriMeshGetTriMergeCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + return mesh->retrieveTriMergeCallback(); +} + +/*extern ODE_API */ +void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + mesh->assignMeshData(Data); +} + +/*extern ODE_API */ +dTriMeshDataID dGeomTriMeshGetData(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + return mesh->retrieveMeshData(); +} + + +BEGIN_NAMESPACE_OU(); +template<> +const int CEnumSortedElementArray<dxTriMesh::TRIMESHTC, dxTriMesh::TTC__MAX, int, 0x161003D5>::m_aetElementArray[] = +{ + dSphereClass, // TTC_SPHERE, + dBoxClass, // TTC_BOX, + dCapsuleClass, // TTC_CAPSULE, +}; +END_NAMESPACE_OU(); +static const CEnumSortedElementArray<dxTriMesh::TRIMESHTC, dxTriMesh::TTC__MAX, int, 0x161003D5> g_asiMeshTCGeomClasses; + +/*extern ODE_API */ +void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + + dxTriMesh::TRIMESHTC tc = g_asiMeshTCGeomClasses.Decode(geomClass); + + if (g_asiMeshTCGeomClasses.IsValidDecode(tc)) + { + mesh->assignDoTC(tc, enable != 0); + } +} + +/*extern ODE_API */ +int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + + dxTriMesh::TRIMESHTC tc = g_asiMeshTCGeomClasses.Decode(geomClass); + + bool result = g_asiMeshTCGeomClasses.IsValidDecode(tc) + && mesh->retrieveDoTC(tc); + return result; +} + + +/*extern ODE_API */ +dTriMeshDataID dGeomTriMeshGetTriMeshDataID(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + return mesh->retrieveMeshData(); +} + + +/*extern ODE_API */ +void dGeomTriMeshClearTCCache(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + mesh->clearTCCache(); +} + + +/*extern ODE_API */ +int dGeomTriMeshGetTriangleCount(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + const dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + unsigned result = mesh->getMeshTriangleCount(); + return result; +} + + +/*extern ODE_API */ +void dGeomTriMeshGetTriangle(dGeomID g, int index, dVector3 *v0/*=NULL*/, dVector3 *v1/*=NULL*/, dVector3 *v2/*=NULL*/) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + dUASSERT(v0 != NULL || v1 != NULL || v2 != NULL, "A meaningless call"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + + dVector3 *pv[3] = { v0, v1, v2 }; + mesh->fetchMeshTransformedTriangle(pv, index); +} + +/*extern ODE_API */ +void dGeomTriMeshGetPoint(dGeomID g, int index, dReal u, dReal v, dVector3 Out) +{ + dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + + dVector3 dv[3]; + mesh->fetchMeshTransformedTriangle(dv, index); + + GetPointFromBarycentric(dv, u, v, Out); +} + + +/*extern */ +IFaceAngleStorageView *dxGeomTriMeshGetFaceAngleView(dxGeom *triMeshGeom) +{ + dUASSERT(triMeshGeom && triMeshGeom->type == dTriMeshClass, "The argument is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(triMeshGeom); + return mesh->retrieveFaceAngleView(); +} + + +#endif // #if dTRIMESH_ENABLED + + +////////////////////////////////////////////////////////////////////////// +// Deprecated functions + +/*extern */ +void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char **buf, int *bufLen) +{ + sizeint dataSizeStorage; + void *dataPointer = dGeomTriMeshDataGet2(g, dTRIMESHDATA_USE_FLAGS, (bufLen != NULL ? &dataSizeStorage : NULL)); + + if (bufLen != NULL) + { + *bufLen = (int)dataSizeStorage; + } + + if (buf != NULL) + { + *buf = (unsigned char *)dataPointer; + } +} + +/*extern */ +void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf) +{ + dGeomTriMeshDataSet(g, dTRIMESHDATA_USE_FLAGS, (void *)buf); +} + diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_internal.h b/libs/ode-0.16.1/ode/src/collision_trimesh_internal.h new file mode 100644 index 0000000..477b770 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_internal.h @@ -0,0 +1,399 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. +// Modified for FreeSOLID Compatibility by Rodrigo Hernandez +// TriMesh caches separation by Oleh Derevenko +// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2019 + + +#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_ +#define _ODE_COLLISION_TRIMESH_INTERNAL_H_ + + +//**************************************************************************** +// dxTriMesh class + + +#include "collision_kernel.h" +#include "collision_trimesh_colliders.h" +#include "collision_util.h" +#include <ode/collision_trimesh.h> + +#if dTLS_ENABLED +#include "odetls.h" +#endif + + +struct TrimeshCollidersCache; +struct dxTriMeshData; + + +static inline +TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind) +{ +#if dTLS_ENABLED + EODETLSKIND tkTLSKind = (EODETLSKIND)uiTLSKind; + return COdeTls::GetTrimeshCollidersCache(tkTLSKind); +#else // dTLS_ENABLED + (void)uiTLSKind; // unused + extern TrimeshCollidersCache g_ccTrimeshCollidersCache; + return &g_ccTrimeshCollidersCache; +#endif // dTLS_ENABLED +} + + +enum FaceAngleStorageMethod +{ + ASM__MIN, + + ASM_BYTE_SIGNED = ASM__MIN, + ASM_BYTE_POSITIVE, + ASM_WORD_SIGNED, + + ASM__MAX, + + ASM__INVALID = ASM__MAX, +}; + +enum FaceAngleDomain +{ + FAD__MIN, + + FAD_CONCAVE = FAD__MIN, + + FAD__SIGNSTORED_IMPLICITVALUE_MIN, + + FAD_FLAT = FAD__SIGNSTORED_IMPLICITVALUE_MIN, + + FAD__SIGNSTORED_IMPLICITVALUE_MAX, + + FAD__BYTEPOS_STORED_MIN = FAD__SIGNSTORED_IMPLICITVALUE_MAX, + + FAD_CONVEX = FAD__BYTEPOS_STORED_MIN, + + FAD__BYTEPOS_STORED_MAX, + + EAD__MAX = FAD__BYTEPOS_STORED_MAX, +}; + +class IFaceAngleStorageControl +{ +public: + virtual void disposeStorage() = 0; + + virtual bool areNegativeAnglesStored() const = 0; + + // This is to store angles between neighbor triangle normals as positive value for convex and negative for concave edges + virtual void assignFacesAngleIntoStorage(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue) = 0; +}; + +class IFaceAngleStorageView +{ +public: + virtual FaceAngleDomain retrieveFacesAngleFromStorage(dReal &out_AngleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex) = 0; +}; + + +typedef dBase dxTriDataBase_Parent; +struct dxTriDataBase: + public dxTriDataBase_Parent +{ +public: + dxTriDataBase(): + dxTriDataBase_Parent(), + m_vertices(NULL), + m_vertexStride(0), + m_vertexCount(0), + m_indices(NULL), + m_triangleCount(0), + m_triStride(0), + m_single(false), + m_normals(NULL), + m_faceAngles(NULL), + m_faceAngleView(NULL) + { +#if !dTRIMESH_ENABLED + dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work"); +#endif + } + + ~dxTriDataBase(); + + void buildData(const void *Vertices, int VertexStide, unsigned VertexCount, + const void *Indices, unsigned IndexCount, int TriStride, + const void *Normals, + bool Single); + + +public: + unsigned retrieveVertexCount() const { return m_vertexCount; } + int retrieveVertexStride() const { return m_vertexStride; } + + unsigned retrieveTriangleCount() const { return m_triangleCount; } + int retrieveTriangleStride() const { return m_triStride; } + +protected: + const void *retrieveVertexInstances() const { return m_vertices; } + const void *retrieveTriangleVertexIndices() const { return m_indices; } + bool isSingle() const { return m_single; } + +public: + template<typename tcoordfloat, typename tindexint> + static void retrieveTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex, + const tcoordfloat *vertexInstances, int vertexStride, const tindexint *triangleVertexIndices, int triangleStride); + +public: + void assignNormals(const void *normals) { m_normals = normals; } + const void *retrieveNormals() const { return m_normals; } + + IFaceAngleStorageControl *retrieveFaceAngles() const { return m_faceAngles; } + IFaceAngleStorageView *retrieveFaceAngleView() const { return m_faceAngleView; } + +protected: + bool allocateFaceAngles(FaceAngleStorageMethod storageMethod); + void freeFaceAngles(); + + bool haveFaceAnglesBeenBuilt() const { return m_faceAngles != NULL; } + +public: + enum MeshComponentUseFlags + { + CUF__USE_EDGES_MIN = 0x01, + CUF_USE_FIRST_EDGE = CUF__USE_EDGES_MIN << dMTV_FIRST, + CUF_USE_SECOND_EDGE = CUF__USE_EDGES_MIN << dMTV_SECOND, + CUF_USE_THIRD_EDGE = CUF__USE_EDGES_MIN << dMTV_THIRD, + CUF__USE_EDGES_MAX = CUF__USE_EDGES_MIN << dMTV__MAX, + CUF__USE_ALL_EDGES = CUF_USE_FIRST_EDGE | CUF_USE_SECOND_EDGE | CUF_USE_THIRD_EDGE, + + CUF__USE_VERTICES_MIN = CUF__USE_EDGES_MAX, + CUF_USE_FIRST_VERTEX = CUF__USE_VERTICES_MIN << dMTV_FIRST, + CUF_USE_SECOND_VERTEX = CUF__USE_VERTICES_MIN << dMTV_SECOND, + CUF_USE_THIRD_VERTEX = CUF__USE_VERTICES_MIN << dMTV_THIRD, + CUF__USE_VERTICES_LAST = CUF__USE_VERTICES_MIN << (dMTV__MAX - 1), + CUF__USE_VERTICES_MAX = CUF__USE_VERTICES_MIN << dMTV__MAX, + CUF__USE_ALL_VERTICES = CUF_USE_FIRST_VERTEX | CUF_USE_SECOND_VERTEX | CUF_USE_THIRD_VERTEX, + + CUF__USE_ALL_COMPONENTS = CUF__USE_ALL_VERTICES | CUF__USE_ALL_EDGES, + }; + + // Make sure that the flags match the values declared in public interface + dSASSERT((unsigned)CUF_USE_FIRST_EDGE == dMESHDATAUSE_EDGE1); + dSASSERT((unsigned)CUF_USE_SECOND_EDGE == dMESHDATAUSE_EDGE2); + dSASSERT((unsigned)CUF_USE_THIRD_EDGE == dMESHDATAUSE_EDGE3); + dSASSERT((unsigned)CUF_USE_FIRST_VERTEX == dMESHDATAUSE_VERTEX1); + dSASSERT((unsigned)CUF_USE_SECOND_VERTEX == dMESHDATAUSE_VERTEX2); + dSASSERT((unsigned)CUF_USE_THIRD_VERTEX == dMESHDATAUSE_VERTEX3); + +protected: + struct EdgeRecord + { + public: + void setupEdge(dMeshTriangleVertex edgeIdx, int triIdx, const unsigned vertexIndices[dMTV__MAX]); + + // Get the vertex opposite this edge in the triangle + dMeshTriangleVertex getOppositeVertexIndex() const + { + extern const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161116DC> g_VertFlagOppositeIndices; + + dMeshTriangleVertex oppositeIndex = g_VertFlagOppositeIndices.Encode(((m_vert1Flags | m_vert2Flags) ^ CUF__USE_ALL_VERTICES) / CUF__USE_VERTICES_MIN - 1); + dIASSERT(dIN_RANGE(oppositeIndex, dMTV__MIN, dMTV__MAX)); + + return oppositeIndex; + } + + dMeshTriangleVertex getEdgeStartVertexIndex() const + { + extern const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161225E9> g_VertFlagEdgeStartIndices; + + dMeshTriangleVertex startIndex = g_VertFlagEdgeStartIndices.Encode(((m_vert1Flags | m_vert2Flags) ^ CUF__USE_ALL_VERTICES) / CUF__USE_VERTICES_MIN - 1); + dIASSERT(dIN_RANGE(startIndex, dMTV__MIN, dMTV__MAX)); + + return startIndex; + } + + public: + bool operator <(const EdgeRecord &anotherEdge) const { return m_vertIdx1 < anotherEdge.m_vertIdx1 || (m_vertIdx1 == anotherEdge.m_vertIdx1 && m_vertIdx2 < anotherEdge.m_vertIdx2); } + + public: + enum + { + AVF_VERTEX_USED = 0x01, + AVF_VERTEX_HAS_CONCAVE_EDGE = 0x02, + }; + + public: + unsigned m_vertIdx1; // Index into vertex array for this edges vertices + unsigned m_vertIdx2; + unsigned m_triIdx; // Index into triangle array for triangle this edge belongs to + + uint8 m_edgeFlags; + uint8 m_vert1Flags; + uint8 m_vert2Flags; + uint8 m_absVertexFlags; + }; + + struct VertexRecord + { + unsigned m_UsedFromEdgeIndex; + }; + + template<class TMeshDataAccessor> + static void meaningfulPreprocess_SetupEdgeRecords(EdgeRecord *edges, sizeint numEdges, const TMeshDataAccessor &dataAccessor); + template<class TMeshDataAccessor> + static void meaningfulPreprocess_buildEdgeFlags(uint8 *useFlags/*=NULL*/, IFaceAngleStorageControl *faceAngles/*=NULL*/, + EdgeRecord *edges, sizeint numEdges, VertexRecord *vertices, + const dReal *externalNormals, const TMeshDataAccessor &dataAccessor); + static void buildBoundaryEdgeAngle(IFaceAngleStorageControl *faceAngles, EdgeRecord *currEdge); + template<class TMeshDataAccessor> + static void buildConcaveEdgeAngle(IFaceAngleStorageControl *faceAngles, bool negativeAnglesStored, + EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct, + const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment, + const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, + const TMeshDataAccessor &dataAccessor); + template<class TMeshDataAccessor> + static + void buildConvexEdgeAngle(IFaceAngleStorageControl *faceAngles, + EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct, + const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment, + const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, + const TMeshDataAccessor &dataAccessor); + template<class TMeshDataAccessor> + static dReal calculateEdgeAngleValidated(unsigned firstVertexStartIndex, + EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct, + const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment, + const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, + const TMeshDataAccessor &dataAccessor); + +private: + const void *m_vertices; + int m_vertexStride; + unsigned m_vertexCount; + const void *m_indices; + unsigned m_triangleCount; + int m_triStride; + bool m_single; + +private: + const void *m_normals; + IFaceAngleStorageControl *m_faceAngles; + IFaceAngleStorageView *m_faceAngleView; +}; + + +typedef dxGeom dxMeshBase_Parent; +struct dxMeshBase: + public dxMeshBase_Parent +{ +public: + dxMeshBase(dxSpace *Space, dxTriDataBase *Data, + dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback, + bool doTCs=false): + dxMeshBase_Parent(Space, 1), + m_Callback(Callback), + m_ArrayCallback(ArrayCallback), + m_RayCallback(RayCallback), + m_TriMergeCallback(NULL), + m_Data(Data) + { + std::fill(m_DoTCs, m_DoTCs + dARRAY_SIZE(m_DoTCs), doTCs); + type = dTriMeshClass; + } + + bool invokeCallback(dxGeom *Object, int TriIndex) + { + return m_Callback == NULL || m_Callback(this, Object, TriIndex) != 0; + } + +public: + enum TRIMESHTC + { + TTC__MIN, + + TTC_SPHERE = TTC__MIN, + TTC_BOX, + TTC_CAPSULE, + + TTC__MAX, + }; + +public: + void assignCallback(dTriCallback *value) { m_Callback = value; } + dTriCallback *retrieveCallback() const { return m_Callback; } + + void assignArrayCallback(dTriArrayCallback *value) { m_ArrayCallback = value; } + dTriArrayCallback *retrieveArrayCallback() const { return m_ArrayCallback; } + + void assignRayCallback(dTriRayCallback *value) { m_RayCallback = value; } + dTriRayCallback *retrieveRayCallback() const { return m_RayCallback; } + + void assignTriMergeCallback(dTriTriMergeCallback *value) { m_TriMergeCallback = value; } + dTriTriMergeCallback *retrieveTriMergeCallback() const { return m_TriMergeCallback; } + + void assignMeshData(dxTriDataBase *instance) + { + setMeshData(instance); + // I changed my data -- I know nothing about my own AABB anymore. + markAABBBad(); + } + dxTriDataBase *retrieveMeshData() const { return getMeshData(); } + + IFaceAngleStorageControl *retrieveFaceAngleStorage() const { return m_Data->retrieveFaceAngles(); } + IFaceAngleStorageView *retrieveFaceAngleView() const { return m_Data->retrieveFaceAngleView(); } + + void assignDoTC(TRIMESHTC tc, bool value) { setDoTC(tc, value); } + bool retrieveDoTC(TRIMESHTC tc) const { return getDoTC(tc); } + +public: + void setDoTC(TRIMESHTC tc, bool value) { dIASSERT(dIN_RANGE(tc, TTC__MIN, TTC__MAX)); m_DoTCs[tc] = value; } + bool getDoTC(TRIMESHTC tc) const { dIASSERT(dIN_RANGE(tc, TTC__MIN, TTC__MAX)); return m_DoTCs[tc]; } + +private: + void setMeshData(dxTriDataBase *Data) { m_Data = Data; } + +protected: + dxTriDataBase *getMeshData() const { return m_Data; } + +public: + // Callbacks + dTriCallback *m_Callback; + dTriArrayCallback *m_ArrayCallback; + dTriRayCallback *m_RayCallback; + dTriTriMergeCallback *m_TriMergeCallback; + +private: + // Data types + dxTriDataBase *m_Data; + +public: + bool m_DoTCs[TTC__MAX]; +}; + + +IFaceAngleStorageView *dxGeomTriMeshGetFaceAngleView(dxGeom *triMeshGeom); + + +#include "collision_trimesh_gimpact.h" +#include "collision_trimesh_opcode.h" + + +#endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_ diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_internal_impl.h b/libs/ode-0.16.1/ode/src/collision_trimesh_internal_impl.h new file mode 100644 index 0000000..be41ff5 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_internal_impl.h @@ -0,0 +1,463 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh base template method implementations by Oleh Derevenko (C) 2016-2019 + + +#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_ +#define _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_ + + +#include "collision_trimesh_internal.h" + + +#if dTRIMESH_ENABLED + + +template<typename tcoordfloat, typename tindexint> +/*static */ +void dxTriDataBase::retrieveTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex, + const tcoordfloat *vertexInstances, int vertexStride, const tindexint *triangleVertexIndices, int triangleStride) +{ + const tindexint *triangleIndicesOfInterest = (const tindexint *)((uint8 *)triangleVertexIndices + (sizeint)triangleIndex * triangleStride); + for (unsigned trianglePoint = dMTV__MIN; trianglePoint != dMTV__MAX; ++trianglePoint) + { + unsigned vertexIndex = triangleIndicesOfInterest[trianglePoint]; + tcoordfloat *pointVertex = (tcoordfloat *)((uint8 *)vertexInstances + (sizeint)vertexIndex * vertexStride); + dAssignVector3(out_Points[trianglePoint], (dReal)pointVertex[dSA_X], (dReal)pointVertex[dSA_Y], (dReal)pointVertex[dSA_Z]); + dSASSERT(dSA_X == 0); + dSASSERT(dSA_Y == 1); + dSASSERT(dSA_Z == 2); + } +} + + +template<class TMeshDataAccessor> +/*static */ +void dxTriDataBase::meaningfulPreprocess_SetupEdgeRecords(EdgeRecord *edges, sizeint numEdges, const TMeshDataAccessor &dataAccessor) +{ + unsigned vertexIndices[dMTV__MAX]; + // Make a list of every edge in the mesh + unsigned triangleIdx = 0; + for (sizeint edgeIdx = 0; edgeIdx != numEdges; ++triangleIdx, edgeIdx += dMTV__MAX) + { + dataAccessor.getTriangleVertexIndices(vertexIndices, triangleIdx); + edges[edgeIdx + dMTV_FIRST].setupEdge(dMTV_FIRST, triangleIdx, vertexIndices); + edges[edgeIdx + dMTV_SECOND].setupEdge(dMTV_SECOND, triangleIdx, vertexIndices); + edges[edgeIdx + dMTV_THIRD].setupEdge(dMTV_THIRD, triangleIdx, vertexIndices); + } +} + +template<class TMeshDataAccessor> +/*static */ +void dxTriDataBase::meaningfulPreprocess_buildEdgeFlags(uint8 *useFlags/*=NULL*/, IFaceAngleStorageControl *faceAngles/*=NULL*/, + EdgeRecord *edges, sizeint numEdges, VertexRecord *vertices, + const dReal *externalNormals/*=NULL*/, const TMeshDataAccessor &dataAccessor) +{ + dIASSERT(useFlags != NULL || faceAngles != NULL); + dIASSERT(numEdges != 0); + + const bool negativeAnglesStored = faceAngles != NULL && faceAngles->areNegativeAnglesStored(); + + // Go through the sorted list of edges and flag all the edges and vertices that we need to use + EdgeRecord *const lastEdge = edges + (numEdges - 1); + for (EdgeRecord *currEdge = edges; ; ++currEdge) + { + // Handle the last edge separately to have an optimizer friendly loop + if (currEdge >= lastEdge) + { + // This is a boundary edge + if (currEdge == lastEdge) + { + if (faceAngles != NULL) + { + buildBoundaryEdgeAngle(faceAngles, currEdge); + } + + if (useFlags != NULL) + { + // For the last element EdgeRecord::kAbsVertexUsed assignment can be skipped as noone is going to need it any more + useFlags[currEdge[0].m_triIdx] |= ((edges[currEdge[0].m_vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0 ? currEdge[0].m_vert1Flags : 0) + | ((edges[currEdge[0].m_vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0 ? currEdge[0].m_vert2Flags : 0) + | currEdge[0].m_edgeFlags; + } + } + + break; + } + + unsigned vertIdx1 = currEdge[0].m_vertIdx1; + unsigned vertIdx2 = currEdge[0].m_vertIdx2; + + if (vertIdx2 == currEdge[1].m_vertIdx2 // Check second vertex first as it is more likely to change taking the sorting rules into account + && vertIdx1 == currEdge[1].m_vertIdx1) + { + // We let the dot threshold for concavity get slightly negative to allow for rounding errors + const float kConcaveThreshold = 0.000001f; + + const dVector3 *pSecondTriangleEdgeToUse = NULL, *pFirstTriangleToUse = NULL; + dVector3 secondTriangleMatchingEdge; + dVector3 firstTriangle[dMTV__MAX]; + dVector3 secondOppositeVertexSegment, triangleNormal; + dReal lengthSquareProduct, secondOppositeSegmentLengthSquare; + + // Calculate orthogonal vector from the matching edge of the second triangle to its opposite point + { + dVector3 secondTriangle[dMTV__MAX]; + dataAccessor.getTriangleVertexPoints(secondTriangle, currEdge[1].m_triIdx); + + // Get the vertex opposite this edge in the second triangle + dMeshTriangleVertex secondOppositeVertex = currEdge[1].getOppositeVertexIndex(); + dMeshTriangleVertex secondEdgeStart = secondOppositeVertex + 1 != dMTV__MAX ? (dMeshTriangleVertex)(secondOppositeVertex + 1) : dMTV__MIN; + dMeshTriangleVertex secondEdgeEnd = (dMeshTriangleVertex)(dMTV_FIRST + dMTV_SECOND + dMTV_THIRD - secondEdgeStart - secondOppositeVertex); + + dSubtractVectors3(secondTriangleMatchingEdge, secondTriangle[secondEdgeEnd], secondTriangle[secondEdgeStart]); + + if (dSafeNormalize3(secondTriangleMatchingEdge)) + { + pSecondTriangleEdgeToUse = &secondTriangleMatchingEdge; + + dVector3 secondTriangleOppositeEdge; + dSubtractVectors3(secondTriangleOppositeEdge, secondTriangle[secondOppositeVertex], secondTriangle[secondEdgeStart]); + dReal dProjectionLength = dCalcVectorDot3(secondTriangleOppositeEdge, secondTriangleMatchingEdge); + dAddVectorScaledVector3(secondOppositeVertexSegment, secondTriangleOppositeEdge, secondTriangleMatchingEdge, -dProjectionLength); + } + else + { + dSubtractVectors3(secondOppositeVertexSegment, secondTriangle[secondOppositeVertex], secondTriangle[secondEdgeStart]); + } + + secondOppositeSegmentLengthSquare = dCalcVectorLengthSquare3(secondOppositeVertexSegment); + } + + // Either calculate the normal from triangle vertices... + if (externalNormals == NULL) + { + // Get the normal of the first triangle + dataAccessor.getTriangleVertexPoints(firstTriangle, currEdge[0].m_triIdx); + pFirstTriangleToUse = &firstTriangle[dMTV__MIN]; + + dVector3 firstEdge, secondEdge; + dSubtractVectors3(secondEdge, firstTriangle[dMTV_THIRD], firstTriangle[dMTV_SECOND]); + dSubtractVectors3(firstEdge, firstTriangle[dMTV_FIRST], firstTriangle[dMTV_SECOND]); + dCalcVectorCross3(triangleNormal, secondEdge, firstEdge); + dReal normalLengthSuqare = dCalcVectorLengthSquare3(triangleNormal); + lengthSquareProduct = secondOppositeSegmentLengthSquare * normalLengthSuqare; + } + // ...or use the externally supplied normals + else + { + const dReal *pTriangleExternalNormal = externalNormals + currEdge[0].m_triIdx * dSA__MAX; + dAssignVector3(triangleNormal, pTriangleExternalNormal[dSA_X], pTriangleExternalNormal[dSA_Y], pTriangleExternalNormal[dSA_Z]); + // normalLengthSuqare = REAL(1.0); + dUASSERT(dFabs(dCalcVectorLengthSquare3(triangleNormal) - REAL(1.0)) < REAL(0.25) * kConcaveThreshold * kConcaveThreshold, "Mesh triangle normals must be normalized"); + + lengthSquareProduct = secondOppositeSegmentLengthSquare/* * normalLengthSuqare*/; + } + + dReal normalSegmentDot = dCalcVectorDot3(triangleNormal, secondOppositeVertexSegment); + + // This is a concave edge, leave it for the next pass + // OD: This is the "dot >= kConcaveThresh" check, but since the vectros were not normalized to save on roots and divisions, + // the check against zero is performed first and then the dot product is squared and compared against the threshold multiplied by lengths' squares + // OD: Originally, there was dot > -kConcaveThresh check, but this does not seem to be a good idea + // as it can mark all edges on potentially large (nearly) flat surfaces concave. + if (normalSegmentDot > REAL(0.0) && normalSegmentDot * normalSegmentDot >= kConcaveThreshold * kConcaveThreshold * lengthSquareProduct) + { + if (faceAngles != NULL) + { + buildConcaveEdgeAngle(faceAngles, negativeAnglesStored, currEdge, normalSegmentDot, lengthSquareProduct, + triangleNormal, secondOppositeVertexSegment, + pSecondTriangleEdgeToUse, pFirstTriangleToUse, dataAccessor); + } + + if (useFlags != NULL) + { + // Mark the vertices of a concave edge to prevent their use + unsigned absVertexFlags1 = edges[vertIdx1].m_absVertexFlags; + edges[vertIdx1].m_absVertexFlags |= absVertexFlags1 | EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED; + + if ((absVertexFlags1 & (EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED)) == EdgeRecord::AVF_VERTEX_USED) + { + // If the vertex was already used from other triangles but then discovered + // to have a concave edge, unmark the previous use + unsigned usedFromEdgeIndex = vertices[vertIdx1].m_UsedFromEdgeIndex; + const EdgeRecord *usedFromEdge = edges + usedFromEdgeIndex; + unsigned usedInTriangleIndex = usedFromEdge->m_triIdx; + uint8 usedVertFlags = usedFromEdge->m_vertIdx1 == vertIdx1 ? usedFromEdge->m_vert1Flags : usedFromEdge->m_vert2Flags; + useFlags[usedInTriangleIndex] ^= usedVertFlags; + dIASSERT((useFlags[usedInTriangleIndex] & usedVertFlags) == 0); + } + + unsigned absVertexFlags2 = edges[vertIdx2].m_absVertexFlags; + edges[vertIdx2].m_absVertexFlags = absVertexFlags2 | EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED; + + if ((absVertexFlags2 & (EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED)) == EdgeRecord::AVF_VERTEX_USED) + { + // Similarly unmark the possible previous use of the edge's second vertex + unsigned usedFromEdgeIndex = vertices[vertIdx2].m_UsedFromEdgeIndex; + const EdgeRecord *usedFromEdge = edges + usedFromEdgeIndex; + unsigned usedInTriangleIndex = usedFromEdge->m_triIdx; + uint8 usedVertFlags = usedFromEdge->m_vertIdx1 == vertIdx2 ? usedFromEdge->m_vert1Flags : usedFromEdge->m_vert2Flags; + useFlags[usedInTriangleIndex] ^= usedVertFlags; + dIASSERT((useFlags[usedInTriangleIndex] & usedVertFlags) == 0); + } + } + } + // If this is a convex edge, mark its vertices and edge as used + else + { + if (faceAngles != NULL) + { + buildConvexEdgeAngle(faceAngles, currEdge, normalSegmentDot, lengthSquareProduct, + triangleNormal, secondOppositeVertexSegment, + pSecondTriangleEdgeToUse, pFirstTriangleToUse, dataAccessor); + } + + if (useFlags != NULL) + { + EdgeRecord *edgeToUse = currEdge; + unsigned triIdx = edgeToUse[0].m_triIdx; + unsigned triIdx1 = edgeToUse[1].m_triIdx; + + unsigned triUseFlags = useFlags[triIdx]; + unsigned triUseFlags1 = useFlags[triIdx1]; + + // Choose to add flags to the bitmask that already has more edges + // (to group flags in selected triangles rather than scattering them evenly) + if ((triUseFlags1 & CUF__USE_ALL_EDGES) > (triUseFlags & CUF__USE_ALL_EDGES)) + { + triIdx = triIdx1; + triUseFlags = triUseFlags1; + edgeToUse = edgeToUse + 1; + } + + if ((edges[vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0) + { + // Only add each vertex once and set a mark to prevent further additions + edges[vertIdx1].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED; + // Also remember the index the vertex flags are going to be applied to + // to allow easily clear the vertex from the use flags if any concave edges are found to connect to it + vertices[vertIdx1].m_UsedFromEdgeIndex = (unsigned)(edgeToUse - edges); + triUseFlags |= edgeToUse[0].m_vert1Flags; + } + + // Same processing for the second vertex... + if ((edges[vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0) + { + edges[vertIdx2].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED; + vertices[vertIdx2].m_UsedFromEdgeIndex = (unsigned)(edgeToUse - edges); + triUseFlags |= edgeToUse[0].m_vert2Flags; + } + + // And finally store the use flags adding the edge flags in + useFlags[triIdx] = triUseFlags | edgeToUse[0].m_edgeFlags; + } + } + + // Skip the second edge + ++currEdge; + } + // This is a boundary edge + else + { + if (faceAngles != NULL) + { + buildBoundaryEdgeAngle(faceAngles, currEdge); + } + + if (useFlags != NULL) + { + unsigned triIdx = currEdge[0].m_triIdx; + unsigned triUseExtraFlags = 0; + + if ((edges[vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0) + { + edges[vertIdx1].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED; + vertices[vertIdx1].m_UsedFromEdgeIndex = (unsigned)(currEdge - edges); + triUseExtraFlags |= currEdge[0].m_vert1Flags; + } + + if ((edges[vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0) + { + edges[vertIdx2].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED; + vertices[vertIdx2].m_UsedFromEdgeIndex = (unsigned)(currEdge - edges); + triUseExtraFlags |= currEdge[0].m_vert2Flags; + } + + useFlags[triIdx] |= triUseExtraFlags | currEdge[0].m_edgeFlags; + } + } + } +} + +/*static */ +void dxTriDataBase::buildBoundaryEdgeAngle(IFaceAngleStorageControl *faceAngles, + EdgeRecord *currEdge) +{ + const dReal faceAngle = REAL(0.0); + + dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex(); + faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle); + // -- For boundary edges, only the first element is valid + // dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex(); + // faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_TriIdx, secondVertexStartIndex, faceAngle); +} + +template<class TMeshDataAccessor> +/*static */ +void dxTriDataBase::buildConcaveEdgeAngle(IFaceAngleStorageControl *faceAngles, bool negativeAnglesStored, + EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct, + const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment, + const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, + const TMeshDataAccessor &dataAccessor) +{ + dReal faceAngle; + dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex(); + + // Check if concave angles are stored at all + if (negativeAnglesStored) + { + // The length square product can become zero due to precision loss + // when both the normal and the opposite edge vectors are very small. + if (lengthSquareProduct != REAL(0.0)) + { + faceAngle = -calculateEdgeAngleValidated(firstVertexStartIndex, + currEdge, normalSegmentDot, lengthSquareProduct, triangleNormal, secondOppositeVertexSegment, + pSecondTriangleMatchingEdge, pFirstTriangle, dataAccessor); + } + else + { + faceAngle = REAL(0.0); + } + } + else + { + // If concave angles ate not stored, set an arbitrary negative value + faceAngle = -(dReal)M_PI; + } + + faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle); + dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex(); + faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_triIdx, secondVertexStartIndex, faceAngle); +} + +template<class TMeshDataAccessor> +/*static */ +void dxTriDataBase::buildConvexEdgeAngle(IFaceAngleStorageControl *faceAngles, + EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct, + const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment, + const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, + const TMeshDataAccessor &dataAccessor) +{ + dReal faceAngle; + dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex(); + + // The length square product can become zero due to precision loss + // when both the normal and the opposite edge vectors are very small. + if (normalSegmentDot < REAL(0.0) && lengthSquareProduct != REAL(0.0)) + { + faceAngle = calculateEdgeAngleValidated(firstVertexStartIndex, + currEdge, -normalSegmentDot, lengthSquareProduct, triangleNormal, secondOppositeVertexSegment, + pSecondTriangleMatchingEdge, pFirstTriangle, dataAccessor); + } + else + { + faceAngle = REAL(0.0); + } + + faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle); + dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex(); + faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_triIdx, secondVertexStartIndex, faceAngle); +} + +template<class TMeshDataAccessor> +/*static */ +dReal dxTriDataBase::calculateEdgeAngleValidated(unsigned firstVertexStartIndex, + EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct, + const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment, + const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, + const TMeshDataAccessor &dataAccessor) +{ + dIASSERT(lengthSquareProduct >= REAL(0.0)); + + dReal result; + dReal angleCosine = normalSegmentDot / dSqrt(lengthSquareProduct); + + if (angleCosine < REAL(1.0)) + { + dVector3 normalSecondOppositeSegmentCross; + dCalcVectorCross3(normalSecondOppositeSegmentCross, triangleNormal, secondOppositeVertexSegment); + + dReal secondTriangleEdgeDirectionCheck; + + if (pSecondTriangleMatchingEdge != NULL) + { + // Check the cross product against the second triangle edge, if possible... + secondTriangleEdgeDirectionCheck = dCalcVectorDot3(normalSecondOppositeSegmentCross, *pSecondTriangleMatchingEdge); + } + else + { + // ...if not, calculate the supposed direction of the second triangle's edge + // as negative of first triangle edge. For that cross-multiply the precomputed + // first triangle normal by vector from the degenerate edge to its opposite vertex. + + // Retrieve the first triangle points if necessary + dVector3 firstTriangleStorage[dMTV__MAX]; + const dVector3 *pFirstTriangleToUse = pFirstTriangle; + + if (pFirstTriangle == NULL) + { + dataAccessor.getTriangleVertexPoints(firstTriangleStorage, currEdge[0].m_triIdx); + pFirstTriangleToUse = &firstTriangleStorage[dMTV__MIN]; + } + + // Calculate the opposite vector + unsigned firstTriangleOppositeIndex = firstVertexStartIndex != dMTV__MIN ? firstVertexStartIndex - 1 : dMTV__MAX - 1; + + dVector3 firstOppositeVertexSegment; + dSubtractVectors3(firstOppositeVertexSegment, pFirstTriangleToUse[firstTriangleOppositeIndex], pFirstTriangleToUse[firstVertexStartIndex]); + + dVector3 normalFirstOppositeSegmentCross; + dCalcVectorCross3(normalFirstOppositeSegmentCross, triangleNormal, firstOppositeVertexSegment); + + // And finally calculate the dot product to compare vector directions + secondTriangleEdgeDirectionCheck = dCalcVectorDot3(normalSecondOppositeSegmentCross, normalFirstOppositeSegmentCross); + } + + // Negative product means the angle absolute value is less than M_PI_2, positive - greater. + result = secondTriangleEdgeDirectionCheck < REAL(0.0) ? dAsin(angleCosine) : (dReal)M_PI_2 + dAcos(angleCosine); + } + else + { + result = (dReal)M_PI_2; + dIASSERT(angleCosine - REAL(1.0) < 1e-4); // The computational error can not be too high because the dot product had been verified to be greater than the concave threshold above + } + + return result; +} + + +#endif // #if dTRIMESH_ENABLED + + +#endif // #ifndef _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_ diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_opcode.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_opcode.cpp new file mode 100644 index 0000000..53d8b0f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_opcode.cpp @@ -0,0 +1,767 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. +// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2019 + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + + +#if dTRIMESH_ENABLED && dTRIMESH_OPCODE + +#include "collision_util.h" +#include "collision_trimesh_opcode.h" +#include "collision_trimesh_internal_impl.h" +#include <algorithm> + + +////////////////////////////////////////////////////////////////////////// +// TrimeshCollidersCache + +void TrimeshCollidersCache::initOPCODECaches() +{ + m_RayCollider.SetDestination(&m_Faces); + + /* -- not used + _PlanesCollider.SetTemporalCoherence(true); + */ + + m_SphereCollider.SetTemporalCoherence(true); + m_SphereCollider.SetPrimitiveTests(false); + + m_OBBCollider.SetTemporalCoherence(true); + + // no first-contact test (i.e. return full contact info) + m_AABBTreeCollider.SetFirstContact( false ); + // temporal coherence only works with "first contact" tests + m_AABBTreeCollider.SetTemporalCoherence(false); + // Perform full BV-BV tests (true) or SAT-lite tests (false) + m_AABBTreeCollider.SetFullBoxBoxTest( true ); + // Perform full Primitive-BV tests (true) or SAT-lite tests (false) + m_AABBTreeCollider.SetFullPrimBoxTest( true ); + const char* msg; + if ((msg =m_AABBTreeCollider.ValidateSettings())) + { + dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__); + } + + /* -- not used + _LSSCollider.SetTemporalCoherence(false); + _LSSCollider.SetPrimitiveTests(false); + _LSSCollider.SetFirstContact(false); + */ +} + +void TrimeshCollidersCache::clearOPCODECaches() +{ + m_Faces.Empty(); + m_DefaultSphereCache.TouchedPrimitives.Empty(); + m_DefaultBoxCache.TouchedPrimitives.Empty(); + m_DefaultCapsuleCache.TouchedPrimitives.Empty(); +} + + +////////////////////////////////////////////////////////////////////////// +// Trimesh data + +dxTriMeshData::~dxTriMeshData() +{ + if ( m_InternalUseFlags != NULL ) + { + sizeint flagsMemoryRequired = calculateUseFlagsMemoryRequirement(); + dFree(m_InternalUseFlags, flagsMemoryRequired); + } +} + +void dxTriMeshData::buildData(const Point *Vertices, int VertexStide, unsigned VertexCount, + const IndexedTriangle *Indices, unsigned IndexCount, int TriStride, + const dReal *in_Normals, + bool Single) +{ + dxTriMeshData_Parent::buildData(Vertices, VertexStide, VertexCount, Indices, IndexCount, TriStride, in_Normals, Single); + dAASSERT(IndexCount % dMTV__MAX == 0); + + m_Mesh.SetNbTriangles(IndexCount / dMTV__MAX); + m_Mesh.SetNbVertices(VertexCount); + m_Mesh.SetPointers(Indices, Vertices); + m_Mesh.SetStrides(TriStride, VertexStide); + m_Mesh.SetSingle(Single); + + // Build tree + // recommended in Opcode User Manual + //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER; + // used in ODE, why? + //Settings.mRules = SPLIT_BEST_AXIS; + // best compromise? + BuildSettings Settings(SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER); + + OPCODECREATE TreeBuilder(&m_Mesh, Settings, true, false); + + m_BVTree.Build(TreeBuilder); + + // compute model space AABB + dVector3 AABBMax, AABBMin; + calculateDataAABB(AABBMax, AABBMin); + + dAddVectors3(m_AABBCenter, AABBMin, AABBMax); + dScaleVector3(m_AABBCenter, REAL(0.5)); + + dSubtractVectors3(m_AABBExtents, AABBMax, m_AABBCenter); + + // user data (not used by OPCODE) + dIASSERT(m_InternalUseFlags == NULL); +} + + +void dxTriMeshData::calculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin) +{ + if (isSingle()) + { + templateCalculateDataAABB<float>(AABBMax, AABBMin); + } + else + { + templateCalculateDataAABB<double>(AABBMax, AABBMin); + } +} + +template<typename treal> +void dxTriMeshData::templateCalculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin) +{ + dIASSERT(isSingle() == (sizeof(treal) == sizeof(float))); + + const Point *vertices = retrieveVertexInstances(); + const int vertexStide = retrieveVertexStride(); + const unsigned vertexCount = retrieveVertexCount(); + + AABBMax[dV3E_X] = AABBMax[dV3E_Y] = AABBMax[dV3E_Z] = -dInfinity; + AABBMin[dV3E_X] = AABBMin[dV3E_Y] = AABBMin[dV3E_Z] = dInfinity; + dSASSERT(dV3E__AXES_COUNT == 3); + + const uint8 *verts = (const uint8 *)vertices; + for( unsigned i = 0; i < vertexCount; ++i ) + { + const treal *v = (const treal *)verts; + if( v[dSA_X] > AABBMax[dV3E_X] ) AABBMax[dV3E_X] = (dReal)v[dSA_X]; + if( v[dSA_X] < AABBMin[dV3E_X] ) AABBMin[dV3E_X] = (dReal)v[dSA_X]; + if( v[dSA_Y] > AABBMax[dV3E_Y] ) AABBMax[dV3E_Y] = (dReal)v[dSA_Y]; + if( v[dSA_Y] < AABBMin[dV3E_Y] ) AABBMin[dV3E_Y] = (dReal)v[dSA_Y]; + if( v[dSA_Z] > AABBMax[dV3E_Z] ) AABBMax[dV3E_Z] = (dReal)v[dSA_Z]; + if( v[dSA_Z] < AABBMin[dV3E_Z] ) AABBMin[dV3E_Z] = (dReal)v[dSA_Z]; + verts += vertexStide; + } +} + + +bool dxTriMeshData::preprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/) +{ + bool buildUseFlagsToUse = buildUseFlags; + FaceAngleStorageMethod faceAndgesRequirementToUse = faceAndgesRequirement; + + if (buildUseFlags && haveUseFlagsBeenBuilt()) + { + dUASSERT(false, "Another request to build edge/vertex use flags after they had already been built"); + + buildUseFlagsToUse = false; + } + + if (faceAndgesRequirement != ASM__INVALID && haveFaceAnglesBeenBuilt()) + { + dUASSERT(false, "Another request to build face angles after they had already been built"); + + faceAndgesRequirementToUse = ASM__INVALID; + } + + // If this mesh has already been preprocessed, exit + bool result = (!buildUseFlagsToUse && faceAndgesRequirementToUse == ASM__INVALID) || m_Mesh.GetNbTriangles() == 0 + || meaningfulPreprocessData(buildUseFlagsToUse, faceAndgesRequirementToUse); + return result; +} + +struct TrimeshDataVertexIndexAccessor_OPCODE +{ + TrimeshDataVertexIndexAccessor_OPCODE(const IndexedTriangle *triIndicesBegin, unsigned triStride): + m_TriIndicesBegin(triIndicesBegin), + m_TriStride(triStride) + { + } + + void getTriangleVertexIndices(unsigned out_VertexIndices[dMTV__MAX], unsigned triangleIdx) const + { + const IndexedTriangle *triIndicesBegin = m_TriIndicesBegin; + const unsigned triStride = m_TriStride; + + const IndexedTriangle *triIndicesOfInterest = (const IndexedTriangle *)((const uint8 *)triIndicesBegin + triangleIdx * (sizeint)triStride); + std::copy(triIndicesOfInterest->mVRef, triIndicesOfInterest->mVRef + dMTV__MAX, out_VertexIndices); + dSASSERT(dMTV__MAX == dARRAY_SIZE(triIndicesOfInterest->mVRef)); + dSASSERT(dMTV_FIRST == 0); + dSASSERT(dMTV_SECOND == 1); + dSASSERT(dMTV_THIRD == 2); + dSASSERT(dMTV__MAX == 3); + } + + + const IndexedTriangle *m_TriIndicesBegin; + unsigned m_TriStride; +}; + +struct TrimeshDataTrianglePointAccessor_OPCODE +{ + TrimeshDataTrianglePointAccessor_OPCODE(const MeshInterface &mesh): + m_Mesh(mesh) + { + } + + void getTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex) const + { + VertexPointers vpTriangle; + ConversionArea vc; + m_Mesh.GetTriangle(vpTriangle, triangleIndex, vc); + + for (unsigned pointIndex = 0; pointIndex != 3; ++pointIndex) + { + dAssignVector3(out_Points[pointIndex], vpTriangle.Vertex[pointIndex]->x, vpTriangle.Vertex[pointIndex]->y, vpTriangle.Vertex[pointIndex]->z); + } + dSASSERT(dMTV_FIRST == 0); + dSASSERT(dMTV_SECOND == 1); + dSASSERT(dMTV_THIRD == 2); + dSASSERT(dMTV__MAX == 3); + } + + const MeshInterface &m_Mesh; +}; + +bool dxTriMeshData::meaningfulPreprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/) +{ + const bool buildFaceAngles = faceAndgesRequirement != ASM__INVALID; + dIASSERT(buildUseFlags || buildFaceAngles); + dIASSERT(!buildUseFlags || !haveUseFlagsBeenBuilt()); + dIASSERT(!buildFaceAngles || !haveFaceAnglesBeenBuilt()); + + bool result = false; + + uint8 *useFlags = NULL; + sizeint flagsMemoryRequired = 0; + bool flagsAllocated = false, anglesAllocated = false; + + do + { + if (buildUseFlags) + { + flagsMemoryRequired = calculateUseFlagsMemoryRequirement(); + useFlags = (uint8 *)dAlloc(flagsMemoryRequired); + + if (useFlags == NULL) + { + break; + } + } + + flagsAllocated = true; + + if (buildFaceAngles) + { + if (!allocateFaceAngles(faceAndgesRequirement)) + { + break; + } + } + + anglesAllocated = true; + + const unsigned int numTris = m_Mesh.GetNbTriangles(); + const unsigned int numVertices = m_Mesh.GetNbVertices(); + sizeint numEdges = (sizeint)numTris * dMTV__MAX; + dIASSERT(numVertices <= numEdges); // Edge records are going to be used for vertex data as well + + const sizeint recordsMemoryRequired = dEFFICIENT_SIZE(numEdges * sizeof(EdgeRecord)); + const sizeint verticesMemoryRequired = /*dEFFICIENT_SIZE*/(numVertices * sizeof(VertexRecord)); // Skip alignment for the last chunk + const sizeint totalTempMemoryRequired = recordsMemoryRequired + verticesMemoryRequired; + void *tempBuffer = dAlloc(totalTempMemoryRequired); + + if (tempBuffer == NULL) + { + break; + } + + EdgeRecord *edges = (EdgeRecord *)tempBuffer; + VertexRecord *vertices = (VertexRecord *)((uint8 *)tempBuffer + recordsMemoryRequired); + + // Delay zero-filling until all the allocations succeed + if (useFlags != NULL) + { + memset(useFlags, 0, flagsMemoryRequired); + } + + const IndexedTriangle *triIndicesBegin = m_Mesh.GetTris(); + unsigned triStride = m_Mesh.GetTriStride(); + TrimeshDataVertexIndexAccessor_OPCODE indexAccessor(triIndicesBegin, triStride); + meaningfulPreprocess_SetupEdgeRecords(edges, numEdges, indexAccessor); + + // Sort the edges, so the ones sharing the same verts are beside each other + std::sort(edges, edges + numEdges); + + TrimeshDataTrianglePointAccessor_OPCODE pointAccessor(m_Mesh); + const dReal *const externalNormals = retrieveNormals(); + IFaceAngleStorageControl *faceAngles = retrieveFaceAngles(); + meaningfulPreprocess_buildEdgeFlags(useFlags, faceAngles, edges, numEdges, vertices, externalNormals, pointAccessor); + + dFree(tempBuffer, totalTempMemoryRequired); + + if (buildUseFlags) + { + m_InternalUseFlags = useFlags; + } + + result = true; + } + while (false); + + if (!result) + { + if (flagsAllocated) + { + if (anglesAllocated) + { + if (buildFaceAngles) + { + freeFaceAngles(); + } + } + + if (buildUseFlags) + { + dFree(useFlags, flagsMemoryRequired); + } + } + } + + return result; +} + + +void dxTriMeshData::updateData() +{ + m_BVTree.Refit(); +} + + + +////////////////////////////////////////////////////////////////////////// +// dxTriMesh + +dxTriMesh::~dxTriMesh() +{ + // +} + +void dxTriMesh::clearTCCache() +{ + /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches - + but the destructor isn't called when doing this, so we would leak. + So, call the previous caches' containers' destructors by hand first. */ + int i, n; + + n = m_SphereTCCache.size(); + for( i = 0; i != n; ++i ) + { + m_SphereTCCache[i].~SphereTC(); + } + m_SphereTCCache.setSize(0); + + n = m_BoxTCCache.size(); + for( i = 0; i != n; ++i ) + { + m_BoxTCCache[i].~BoxTC(); + } + m_BoxTCCache.setSize(0); + + n = m_CapsuleTCCache.size(); + for( i = 0; i != n; ++i ) + { + m_CapsuleTCCache[i].~CapsuleTC(); + } + m_CapsuleTCCache.setSize(0); +} + + +bool dxTriMesh::controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize) +{ + if (controlClass == dGeomColliderControlClass) + { + if (controlCode == dGeomCommonAnyControlCode) + { + return checkControlValueSizeValidity(dataValue, dataSize, 0); + } + else if (controlCode == dGeomColliderSetMergeSphereContactsControlCode) + { + return checkControlValueSizeValidity(dataValue, dataSize, sizeof(int)) + && controlGeometry_SetMergeSphereContacts(*(int *)dataValue); + } + else if (controlCode == dGeomColliderGetMergeSphereContactsControlCode) + { + return checkControlValueSizeValidity(dataValue, dataSize, sizeof(int)) + && controlGeometry_GetMergeSphereContacts(*(int *)dataValue); + } + } + + return dxTriMesh_Parent::controlGeometry(controlClass, controlCode, dataValue, dataSize); +} + +bool dxTriMesh::controlGeometry_SetMergeSphereContacts(int dataValue) +{ + if (dataValue == dGeomColliderMergeContactsValue__Default) + { + m_SphereContactsMergeOption = (dxContactMergeOptions)MERGE_NORMALS__SPHERE_DEFAULT; + } + else if (dataValue == dGeomColliderMergeContactsValue_None) + { + m_SphereContactsMergeOption = DONT_MERGE_CONTACTS; + } + else if (dataValue == dGeomColliderMergeContactsValue_Normals) + { + m_SphereContactsMergeOption = MERGE_CONTACT_NORMALS; + } + else if (dataValue == dGeomColliderMergeContactsValue_Full) + { + m_SphereContactsMergeOption = MERGE_CONTACTS_FULLY; + } + else + { + dAASSERT(false && "Invalid contact merge control value"); + return false; + } + + return true; +} + +bool dxTriMesh::controlGeometry_GetMergeSphereContacts(int &returnValue) +{ + if (m_SphereContactsMergeOption == DONT_MERGE_CONTACTS) { + returnValue = dGeomColliderMergeContactsValue_None; + } + else if (m_SphereContactsMergeOption == MERGE_CONTACT_NORMALS) { + returnValue = dGeomColliderMergeContactsValue_Normals; + } + else if (m_SphereContactsMergeOption == MERGE_CONTACTS_FULLY) { + returnValue = dGeomColliderMergeContactsValue_Full; + } + else { + dIASSERT(false && "Internal error: unexpected contact merge option field value"); + return false; + } + + return true; +} + + +/*virtual */ +void dxTriMesh::computeAABB() +{ + const dxTriMeshData *meshData = getMeshData(); + dVector3 c; + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dMultiply0_331( c, R, meshData->m_AABBCenter ); + + dReal xrange = dFabs(R[0] * meshData->m_AABBExtents[0]) + + dFabs(R[1] * meshData->m_AABBExtents[1]) + + dFabs(R[2] * meshData->m_AABBExtents[2]); + dReal yrange = dFabs(R[4] * meshData->m_AABBExtents[0]) + + dFabs(R[5] * meshData->m_AABBExtents[1]) + + dFabs(R[6] * meshData->m_AABBExtents[2]); + dReal zrange = dFabs(R[8] * meshData->m_AABBExtents[0]) + + dFabs(R[9] * meshData->m_AABBExtents[1]) + + dFabs(R[10] * meshData->m_AABBExtents[2]); + + aabb[0] = c[0] + pos[0] - xrange; + aabb[1] = c[0] + pos[0] + xrange; + aabb[2] = c[1] + pos[1] - yrange; + aabb[3] = c[1] + pos[1] + yrange; + aabb[4] = c[2] + pos[2] - zrange; + aabb[5] = c[2] + pos[2] + zrange; +} + + +void dxTriMesh::fetchMeshTransformedTriangle(dVector3 *const pout_triangle[3], unsigned index) +{ + const dVector3 &position = buildUpdatedPosition(); + const dMatrix3 &rotation = buildUpdatedRotation(); + fetchMeshTriangle(pout_triangle, index, position, rotation); +} + +void dxTriMesh::fetchMeshTransformedTriangle(dVector3 out_triangle[3], unsigned index) +{ + const dVector3 &position = buildUpdatedPosition(); + const dMatrix3 &rotation = buildUpdatedRotation(); + fetchMeshTriangle(out_triangle, index, position, rotation); +} + +void dxTriMesh::fetchMeshTriangle(dVector3 *const pout_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const +{ + dIASSERT(dIN_RANGE(index, 0, getMeshTriangleCount())); + + VertexPointers VP; + ConversionArea VC; + + const dxTriMeshData *meshData = getMeshData(); + meshData->m_Mesh.GetTriangle(VP, index, VC); + + for (unsigned i = 0; i != 3; ++i) + { + if (pout_triangle[i] != NULL) + { + dVector3 v; + v[dV3E_X] = VP.Vertex[i]->x; + v[dV3E_Y] = VP.Vertex[i]->y; + v[dV3E_Z] = VP.Vertex[i]->z; + + dVector3 &out_triangle = *(pout_triangle[i]); + dMultiply0_331(out_triangle, rotation, v); + dAddVectors3(out_triangle, out_triangle, position); + out_triangle[dV3E_PAD] = REAL(0.0); + } + } +} + +void dxTriMesh::fetchMeshTriangle(dVector3 out_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const +{ + dIASSERT(dIN_RANGE(index, 0, getMeshTriangleCount())); + + VertexPointers VP; + ConversionArea VC; + + const dxTriMeshData *meshData = getMeshData(); + meshData->m_Mesh.GetTriangle(VP, index, VC); + + for (unsigned i = 0; i != 3; ++i) + { + dVector3 v; + v[dV3E_X] = VP.Vertex[i]->x; + v[dV3E_Y] = VP.Vertex[i]->y; + v[dV3E_Z] = VP.Vertex[i]->z; + + dMultiply0_331(out_triangle[i], rotation, v); + dAddVectors3(out_triangle[i], out_triangle[i], position); + out_triangle[i][dV3E_PAD] = REAL(0.0); + } +} + + +////////////////////////////////////////////////////////////////////////// + +/*extern */ +dTriMeshDataID dGeomTriMeshDataCreate() +{ + return new dxTriMeshData(); +} + +/*extern */ +void dGeomTriMeshDataDestroy(dTriMeshDataID g) +{ + dxTriMeshData *mesh = g; + delete mesh; +} + + +/*extern */ +void dGeomTriMeshDataSet(dTriMeshDataID g, int dataId, void *pDataLocation) +{ + dUASSERT(g, "The argument is not a trimesh data"); + + dxTriMeshData *data = g; + + switch (dataId) + { + case dTRIMESHDATA_FACE_NORMALS: + { + data->assignNormals((const dReal *)pDataLocation); + break; + } + + case dTRIMESHDATA_USE_FLAGS: + { + data->assignExternalUseFlagsBuffer((uint8 *)pDataLocation); + break; + } + + // case dTRIMESHDATA__MAX: -- To be located by Find in Files + default: + { + dUASSERT(dataId, "invalid data type"); + break; + } + } +} + +static void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize); + +/*extern */ +void *dGeomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) +{ + return geomTriMeshDataGet(g, dataId, NULL); +} + +/*extern */ +void *dGeomTriMeshDataGet2(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) +{ + return geomTriMeshDataGet(g, dataId, pOutDataSize); +} + +static +void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) +{ + dUASSERT(g, "The argument is not a trimesh data"); + + const dxTriMeshData *data = g; + + void *result = NULL; + + switch (dataId) + { + case dTRIMESHDATA_FACE_NORMALS: + { + if (pOutDataSize != NULL) + { + *pOutDataSize = data->calculateNormalsMemoryRequirement(); + } + + result = (void *)data->retrieveNormals(); + break; + } + + case dTRIMESHDATA_USE_FLAGS: + { + if (pOutDataSize != NULL) + { + *pOutDataSize = data->calculateUseFlagsMemoryRequirement(); + } + + result = const_cast<uint8 *>(data->smartRetrieveUseFlags()); + break; + } + + // case dTRIMESHDATA__MAX: -- To be located by Find in Files + default: + { + if (pOutDataSize != NULL) + { + *pOutDataSize = 0; + } + + dUASSERT(dataId, "invalid data type"); + break; + } + } + + return result; +} + + +/*extern */ +void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "The argument is not a trimesh data"); + + dxTriMeshData *data = g; + data->buildData((const Point *)Vertices, VertexStride, VertexCount, + (const IndexedTriangle *)Indices, IndexCount, TriStride, + (const dReal *)Normals, + true); +} + +/*extern */ +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "The argument is not a trimesh data"); + + g->buildData((const Point *)Vertices, VertexStride, VertexCount, + (const IndexedTriangle *)Indices, IndexCount, TriStride, + (const dReal *)Normals, + false); +} + + +////////////////////////////////////////////////////////////////////////// + +/*extern */ +dGeomID dCreateTriMesh(dSpaceID space, + dTriMeshDataID Data, + dTriCallback* Callback, + dTriArrayCallback* ArrayCallback, + dTriRayCallback* RayCallback) +{ + dxTriMesh *mesh = new dxTriMesh(space, Data, Callback, ArrayCallback, RayCallback); + return mesh; +} + + +/*extern */ +void dGeomTriMeshSetLastTransform(dGeomID g, const dMatrix4 last_trans ) +{ + dAASSERT(g); + dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + mesh->assignLastTransform(last_trans); +} + +/*extern */ +const dReal *dGeomTriMeshGetLastTransform(dGeomID g) +{ + dAASSERT(g); + dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh"); + + dxTriMesh *mesh = static_cast<dxTriMesh *>(g); + return mesh->retrieveLastTransform(); +} + + +////////////////////////////////////////////////////////////////////////// + +// Cleanup for allocations when shutting down ODE +/*extern */ +void opcode_collider_cleanup() +{ +#if !dTLS_ENABLED + + // Clear TC caches + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(0); + pccColliderCache->clearOPCODECaches(); + +#endif // dTLS_ENABLED +} + + +#endif // dTRIMESH_ENABLED && dTRIMESH_OPCODE + diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_opcode.h b/libs/ode-0.16.1/ode/src/collision_trimesh_opcode.h new file mode 100644 index 0000000..fdce2f1 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_opcode.h @@ -0,0 +1,333 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. +// Modified for FreeSOLID Compatibility by Rodrigo Hernandez +// Trimesh caches separation by Oleh Derevenko +// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2019 + + +#ifndef _ODE_COLLISION_TRIMESH_OPCODE_H_ +#define _ODE_COLLISION_TRIMESH_OPCODE_H_ + + +#if dTRIMESH_ENABLED && dTRIMESH_OPCODE + +//**************************************************************************** +// dxTriMesh class + + +#include "collision_kernel.h" +#include "collision_trimesh_colliders.h" +#include "collision_util.h" +#include <ode/collision_trimesh.h> + +#include "collision_trimesh_internal.h" + +#define BAN_OPCODE_AUTOLINK +#include "Opcode.h" +using namespace Opcode; + + +#if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + +// New trimesh collider hash table types +enum +{ + MAXCONTACT_X_NODE = 4, + CONTACTS_HASHSIZE = 256 +}; + +struct CONTACT_KEY +{ + dContactGeom * m_contact; + unsigned int m_key; +}; + +struct CONTACT_KEY_HASH_NODE +{ + CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE]; + int m_keycount; +}; + +struct CONTACT_KEY_HASH_TABLE +{ +public: + CONTACT_KEY_HASH_NODE &operator[](unsigned int index) { return m_storage[index]; } + +private: + CONTACT_KEY_HASH_NODE m_storage[CONTACTS_HASHSIZE]; +}; + +#endif // !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + + +struct VertexUseCache +{ +public: + VertexUseCache(): m_VertexUseBits(NULL), m_VertexUseElements(0) {} + ~VertexUseCache() { freeVertexUSEDFlags(); } + + bool resizeAndResetVertexUSEDFlags(unsigned VertexCount) + { + bool Result = false; + sizeint VertexNewElements = (VertexCount + 7) / 8; + if (VertexNewElements <= m_VertexUseElements || reallocVertexUSEDFlags(VertexNewElements)) { + memset(m_VertexUseBits, 0, VertexNewElements); + Result = true; + } + return Result; + } + + bool getVertexUSEDFlag(unsigned VertexIndex) const { return (m_VertexUseBits[VertexIndex / 8] & (1 << (VertexIndex % 8))) != 0; } + void setVertexUSEDFlag(unsigned VertexIndex) { m_VertexUseBits[VertexIndex / 8] |= (1 << (VertexIndex % 8)); } + +private: + bool reallocVertexUSEDFlags(sizeint VertexNewElements) + { + bool Result = false; + uint8 *VertexNewBits = (uint8 *)dRealloc(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0]), VertexNewElements * sizeof(m_VertexUseBits[0])); + if (VertexNewBits) { + m_VertexUseBits = VertexNewBits; + m_VertexUseElements = VertexNewElements; + Result = true; + } + return Result; + } + + void freeVertexUSEDFlags() + { + dFree(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0])); + m_VertexUseBits = NULL; + m_VertexUseElements = 0; + } + +private: + uint8 *m_VertexUseBits; + sizeint m_VertexUseElements; +}; + + +struct TrimeshCollidersCache +{ + TrimeshCollidersCache() + { + initOPCODECaches(); + } + + void initOPCODECaches(); + void clearOPCODECaches(); + + // Collider caches + BVTCache ColCache; + +#if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + CONTACT_KEY_HASH_TABLE m_hashcontactset; +#endif + + // Colliders + /* -- not used -- also uncomment in InitOPCODECaches() + PlanesCollider _PlanesCollider; -- not used + */ + SphereCollider m_SphereCollider; + OBBCollider m_OBBCollider; + RayCollider m_RayCollider; + AABBTreeCollider m_AABBTreeCollider; + /* -- not used -- also uncomment in InitOPCODECaches() + LSSCollider _LSSCollider; + */ + // Trimesh caches + CollisionFaces m_Faces; + SphereCache m_DefaultSphereCache; + OBBCache m_DefaultBoxCache; + LSSCache m_DefaultCapsuleCache; + + // Trimesh-plane collision vertex use cache + VertexUseCache m_VertexUses; +}; + + +typedef dxTriDataBase dxTriMeshData_Parent; +struct dxTriMeshData: + public dxTriMeshData_Parent +{ +public: + dxTriMeshData(): + dxTriMeshData_Parent(), + m_ExternalUseFlags(NULL), + m_InternalUseFlags(NULL) + { + } + + ~dxTriMeshData(); + + void buildData(const Point *Vertices, int VertexStide, unsigned VertexCount, + const IndexedTriangle *Indices, unsigned IndexCount, int TriStride, + const dReal *in_Normals, + bool Single); + +private: + void calculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin); + template<typename treal> + void templateCalculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin); + +public: + /* Setup the UseFlags array and/or build face angles*/ + bool preprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/); + +private: + bool meaningfulPreprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/); + +public: + /* For when app changes the vertices */ + void updateData(); + +public: + const Point *retrieveVertexInstances() const { return (const Point *)dxTriMeshData_Parent::retrieveVertexInstances(); } + +public: + void assignNormals(const dReal *normals) { dxTriMeshData_Parent::assignNormals(normals); } + const dReal *retrieveNormals() const { return (const dReal *)dxTriMeshData_Parent::retrieveNormals(); } + sizeint calculateNormalsMemoryRequirement() const { return retrieveTriangleCount() * (sizeof(dReal) * dSA__MAX); } + +public: + void assignExternalUseFlagsBuffer(uint8 *buffer) { m_ExternalUseFlags = buffer != m_InternalUseFlags ? buffer : NULL; } + const uint8 *smartRetrieveUseFlags() const { return m_ExternalUseFlags != NULL ? m_ExternalUseFlags : m_InternalUseFlags; } + bool haveUseFlagsBeenBuilt() const { return m_InternalUseFlags != NULL; } + sizeint calculateUseFlagsMemoryRequirement() const { return m_Mesh.GetNbTriangles() * sizeof(m_InternalUseFlags[0]); } + +public: + Model m_BVTree; + MeshInterface m_Mesh; + + /* aabb in model space */ + dVector3 m_AABBCenter; + dVector3 m_AABBExtents; + + // data for use in collision resolution + uint8 *m_ExternalUseFlags; + uint8 *m_InternalUseFlags; + +}; + + +typedef dxMeshBase dxTriMesh_Parent; +struct dxTriMesh: + public dxTriMesh_Parent +{ +public: + // Functions + dxTriMesh(dxSpace *Space, dxTriMeshData *Data, + dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback): + dxTriMesh_Parent(Space, Data, Callback, ArrayCallback, RayCallback, false) + { + m_SphereContactsMergeOption = (dxContactMergeOptions)MERGE_NORMALS__SPHERE_DEFAULT; + + dZeroMatrix4(m_last_trans); + } + + ~dxTriMesh(); + + void clearTCCache(); + + bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize); + + virtual void computeAABB(); + +public: + dxTriMeshData *retrieveMeshData() const { return getMeshData(); } + const dReal *retrieveMeshNormals() const { return getMeshData()->retrieveNormals(); } + Model &retrieveMeshBVTreeRef() const { return getMeshData()->m_BVTree; } + const uint8 *retrieveMeshSmartUseFlags() const { return getMeshData()->smartRetrieveUseFlags(); } + + unsigned getMeshTriangleCount() const { return getMeshData()->m_Mesh.GetNbTriangles(); } + void fetchMeshTransformedTriangle(dVector3 *const pout_triangle[3], unsigned index)/* const*/; + void fetchMeshTransformedTriangle(dVector3 out_triangle[3], unsigned index)/* const*/; + void fetchMeshTriangle(dVector3 *const pout_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const; + void fetchMeshTriangle(dVector3 out_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const; + +public: + void assignLastTransform(const dMatrix4 last_trans) { dCopyMatrix4x4(m_last_trans, last_trans); } + const dReal *retrieveLastTransform() const { return m_last_trans; } + +private: + enum + { + MERGE_NORMALS__SPHERE_DEFAULT = DONT_MERGE_CONTACTS + }; + + bool controlGeometry_SetMergeSphereContacts(int dataValue); + bool controlGeometry_GetMergeSphereContacts(int &returnValue); + +private: + dxTriMeshData *getMeshData() const { return static_cast<dxTriMeshData *>(dxTriMesh_Parent::getMeshData()); } + +public: + // Some constants + // Temporal coherence + struct SphereTC : public SphereCache{ + dxGeom* Geom; + }; + + struct BoxTC : public OBBCache{ + dxGeom* Geom; + }; + + struct CapsuleTC : public LSSCache{ + dxGeom* Geom; + }; + +public: + // Contact merging option + dxContactMergeOptions m_SphereContactsMergeOption; + // Instance data for last transform. + dMatrix4 m_last_trans; + + dArray<SphereTC> m_SphereTCCache; + dArray<BoxTC> m_BoxTCCache; + dArray<CapsuleTC> m_CapsuleTCCache; +}; + + +static inline +Matrix4x4 &MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4 &Out) +{ + return Out.Set( + Rotation[0], Rotation[4], Rotation[8], 0.0f, + Rotation[1], Rotation[5], Rotation[9], 0.0f, + Rotation[2], Rotation[6], Rotation[10],0.0f, + Position[0], Position[1], Position[2], 1.0f); +} + +static inline +Matrix4x4 &MakeMatrix(dxGeom* g, Matrix4x4 &Out) +{ + const dVector3 &position = g->buildUpdatedPosition(); + const dMatrix3 &rotation = g->buildUpdatedRotation(); + return MakeMatrix(position, rotation, Out); +} + + +#endif // #if dTRIMESH_ENABLED && dTRIMESH_OPCODE + + +#endif //_ODE_COLLISION_TRIMESH_OPCODE_H_ diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp new file mode 100644 index 0000000..5c3c67a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp @@ -0,0 +1,226 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh - Plane collider by David Walters, July 2006 + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#include "collision_std.h" +#include "collision_trimesh_internal.h" + + +#if dTRIMESH_OPCODE + +int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dTriMeshClass ); + dIASSERT( o2->type == dPlaneClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Alias pointers to the plane and trimesh + dxTriMesh* trimesh = (dxTriMesh*)( o1 ); + dxPlane* plane = (dxPlane*)( o2 ); + + int contact_count = 0; + + // Cache the maximum contact count. + const int contact_max = ( flags & NUMC_MASK ); + + // Cache trimesh position and rotation. + const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh ); + const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh ); + + // + // For all triangles. + // + + VertexPointersEx VPE; + VertexPointers &VP = VPE.vp; + ConversionArea VC; + dReal alpha; + dVector3 vertex; + +#if !defined(dSINGLE) || 1 + dVector3 int_vertex; // Intermediate vertex for double precision mode. +#endif // dSINGLE + + const unsigned uiTLSKind = trimesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == plane->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + VertexUseCache &vertex_use_cache = pccColliderCache->m_VertexUses; + + // Reallocate vertex use cache if necessary + const dxTriMeshData *meshData = trimesh->retrieveMeshData(); + const int vertex_count = meshData->m_Mesh.GetNbVertices(); + const bool cache_status = vertex_use_cache.resizeAndResetVertexUSEDFlags(vertex_count); + + // Cache the triangle count. + const int tri_count = meshData->m_Mesh.GetNbTriangles(); + + // For each triangle + for ( int t = 0; t < tri_count; ++t ) + { + // Get triangle, which should also use callback. + bool ex_avail = meshData->m_Mesh.GetExTriangle( VPE, t, VC); + + // For each vertex. + for ( int v = 0; v < 3; ++v ) + { + // point already used ? + if (cache_status && ex_avail) + { + unsigned VIndex = VPE.Index[v]; + if (vertex_use_cache.getVertexUSEDFlag(VIndex)) + continue; + // mark this point as used + vertex_use_cache.setVertexUSEDFlag(VIndex); + } + + // + // Get Vertex + // + +#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation + + dMultiply0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) ); + +#else // dDOUBLE || 1 + + // OPCODE data is in single precision format. + int_vertex[ 0 ] = VP.Vertex[ v ]->x; + int_vertex[ 1 ] = VP.Vertex[ v ]->y; + int_vertex[ 2 ] = VP.Vertex[ v ]->z; + + dMultiply0_331( vertex, trimesh_R, int_vertex ); + +#endif // dSINGLE/dDOUBLE + + vertex[ 0 ] += trimesh_pos[ 0 ]; + vertex[ 1 ] += trimesh_pos[ 1 ]; + vertex[ 2 ] += trimesh_pos[ 2 ]; + + + // + // Collision? + // + + // If alpha < 0 then point is if front of plane. i.e. no contact + // If alpha = 0 then the point is on the plane + alpha = plane->p[ 3 ] - dCalcVectorDot3( plane->p, vertex ); + + // If alpha > 0 the point is behind the plane. CONTACT! + if ( alpha > 0 ) + { + // Alias the contact + dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip ); + + contact->pos[ 0 ] = vertex[ 0 ]; + contact->pos[ 1 ] = vertex[ 1 ]; + contact->pos[ 2 ] = vertex[ 2 ]; + + contact->normal[ 0 ] = plane->p[ 0 ]; + contact->normal[ 1 ] = plane->p[ 1 ]; + contact->normal[ 2 ] = plane->p[ 2 ]; + + contact->depth = alpha; + contact->g1 = trimesh; + contact->g2 = plane; + contact->side1 = t; + contact->side2 = -1; + + ++contact_count; + + // All contact slots are full? + if ( contact_count >= contact_max ) + return contact_count; // <=== STOP HERE + } + } + } + + // Return contact count. + return contact_count; +} + + +#endif // dTRIMESH_OPCODE + + +#if dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "gimpact_plane_contact_accessor.h" + + +int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dTriMeshClass ); + dIASSERT( o2->type == dPlaneClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Alias pointers to the plane and trimesh + dxTriMesh* trimesh = (dxTriMesh*)( o1 ); + dVector4 plane; + dGeomPlaneGetParams(o2, plane); + + o1 -> recomputeAABB(); + o2 -> recomputeAABB(); + + //Find collision + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result); + + gim_trimesh_plane_collisionODE(&trimesh->m_collision_trimesh,plane,&collision_result); + + if(collision_result.m_size == 0 ) + { + GIM_DYNARRAY_DESTROY(collision_result); + return 0; + } + + + vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f, collision_result); + unsigned int contactcount = collision_result.m_size; + + dxPlaneContactAccessor contactaccessor(planecontact_results, plane, o1, o2); + contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, flags, contacts, skip); + + GIM_DYNARRAY_DESTROY(collision_result); + + return (int)contactcount; +} + + +#endif // dTRIMESH_GIMPACT + + +#endif // dTRIMESH_ENABLED + diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp new file mode 100644 index 0000000..866758a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp @@ -0,0 +1,207 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#include "collision_trimesh_internal.h" + +#if dTRIMESH_OPCODE +int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (RayGeom->type == dRayClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + RayCollider& Collider = pccColliderCache->m_RayCollider; + + dReal Length = dGeomRayGetLength(RayGeom); + + int FirstContact = dGeomRayGetFirstContact(RayGeom); + int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + + Collider.SetFirstContact(FirstContact != 0); + Collider.SetClosestHit(ClosestHit != 0); + Collider.SetCulling(BackfaceCull != 0); + Collider.SetMaxDist(Length); + + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + Matrix4x4 MeshMatrix; + const dVector3 ZeroVector3 = { REAL(0.0), }; + MakeMatrix(ZeroVector3, TLRotation, MeshMatrix); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + dVector3 OffsetOrigin; + dSubtractVectors3(OffsetOrigin, Origin, TLPosition); + + /* Make Ray */ + Ray WorldRay; + WorldRay.mOrig.Set(OffsetOrigin[0], OffsetOrigin[1], OffsetOrigin[2]); + WorldRay.mDir.Set(Direction[0], Direction[1], Direction[2]); + + /* Intersect */ + int TriCount = 0; + if (Collider.Collide(WorldRay, TriMesh->retrieveMeshBVTreeRef(), &MeshMatrix)) { + TriCount = pccColliderCache->m_Faces.GetNbFaces(); + } + + if (TriCount == 0) { + return 0; + } + + const CollisionFace* Faces = pccColliderCache->m_Faces.GetFaces(); + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++) { + if (TriMesh->m_RayCallback == null || + TriMesh->m_RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, + Faces[i].mU, Faces[i].mV)) { + const int& TriIndex = Faces[i].mFaceID; + if (!TriMesh->invokeCallback(RayGeom, TriIndex)) { + continue; + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + dVector3 dv[3]; + TriMesh->fetchMeshTriangle(dv, TriIndex, TLPosition, TLRotation); + + dVector3 vu; + vu[0] = dv[1][0] - dv[0][0]; + vu[1] = dv[1][1] - dv[0][1]; + vu[2] = dv[1][2] - dv[0][2]; + vu[3] = REAL(0.0); + + dVector3 vv; + vv[0] = dv[2][0] - dv[0][0]; + vv[1] = dv[2][1] - dv[0][1]; + vv[2] = dv[2][2] - dv[0][2]; + vv[3] = REAL(0.0); + + dCalcVectorCross3(Contact->normal, vv, vu); // Reversed + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (dSafeNormalize3(Contact->normal)) + { + // No sense to save on single type conversion in algorithm of this size. + // If there would be a custom typedef for distance type it could be used + // instead of dReal. However using float directly is the loss of abstraction + // and possible loss of precision in future. + /*float*/ dReal T = Faces[i].mDistance; + Contact->pos[0] = Origin[0] + (Direction[0] * T); + Contact->pos[1] = Origin[1] + (Direction[1] * T); + Contact->pos[2] = Origin[2] + (Direction[2] * T); + Contact->pos[3] = REAL(0.0); + + Contact->depth = T; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + Contact->side1 = TriIndex; + Contact->side2 = -1; + + OutTriCount++; + + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + if (OutTriCount >= (Flags & NUMC_MASK)) { + break; + } + } + } + } + return OutTriCount; +} +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (RayGeom->type == dRayClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + dReal Length = dGeomRayGetLength(RayGeom); + int FirstContact = dGeomRayGetFirstContact(RayGeom); + int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + char intersect=0; + GIM_TRIANGLE_RAY_CONTACT_DATA contact_data; + + if(ClosestHit) + { + intersect = gim_trimesh_ray_closest_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + else + { + intersect = gim_trimesh_ray_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + + if(intersect == 0) + { + return 0; + } + + + if(!TriMesh->m_RayCallback || + TriMesh->m_RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v)) + { + dContactGeom* Contact = &( Contacts[ 0 ] ); + VEC_COPY(Contact->pos,contact_data.m_point); + VEC_COPY(Contact->normal,contact_data.m_normal); + Contact->depth = contact_data.tparam; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + Contact->side1 = contact_data.m_face_id; + Contact->side2 = -1; + return 1; + } + + return 0; +} +#endif // dTRIMESH_GIMPACT + +#endif // dTRIMESH_ENABLED diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_sphere.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_sphere.cpp new file mode 100644 index 0000000..8076411 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_sphere.cpp @@ -0,0 +1,596 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_util.h" + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + + +#if dTRIMESH_ENABLED + +#include "collision_trimesh_internal.h" + + +#if dTRIMESH_OPCODE + +// Ripped from Opcode 1.1. +static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){ + + // now onto the bulk of the collision... + + dVector3 Diff; + Diff[0] = Origin[0] - Center[0]; + Diff[1] = Origin[1] - Center[1]; + Diff[2] = Origin[2] - Center[2]; + Diff[3] = Origin[3] - Center[3]; + + dReal A00 = dCalcVectorDot3(Edge0, Edge0); + dReal A01 = dCalcVectorDot3(Edge0, Edge1); + dReal A11 = dCalcVectorDot3(Edge1, Edge1); + + dReal B0 = dCalcVectorDot3(Diff, Edge0); + dReal B1 = dCalcVectorDot3(Diff, Edge1); + + dReal C = dCalcVectorDot3(Diff, Diff); + + dReal Det = dFabs(A00 * A11 - A01 * A01); + u = A01 * B1 - A11 * B0; + v = A01 * B0 - A00 * B1; + + dReal DistSq; + + if (u + v <= Det){ + if(u < REAL(0.0)){ + if(v < REAL(0.0)){ // region 4 + if(B0 < REAL(0.0)){ + v = REAL(0.0); + if (-B0 >= A00){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + else{ + u = REAL(0.0); + if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else if(-B1 >= A11){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else{ // region 3 + u = REAL(0.0); + if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else if(-B1 >= A11){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else if(v < REAL(0.0)){ // region 5 + v = REAL(0.0); + if (B0 >= REAL(0.0)){ + u = REAL(0.0); + DistSq = C; + } + else if (-B0 >= A00){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + else{ // region 0 + // minimum at interior point + if (Det == REAL(0.0)){ + u = REAL(0.0); + v = REAL(0.0); + DistSq = FLT_MAX; + } + else{ + dReal InvDet = REAL(1.0) / Det; + u *= InvDet; + v *= InvDet; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + } + else{ + dReal Tmp0, Tmp1, Numer, Denom; + + if(u < REAL(0.0)){ // region 2 + Tmp0 = A01 + B0; + Tmp1 = A11 + B1; + if (Tmp1 > Tmp0){ + Numer = Tmp1 - Tmp0; + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + u = REAL(1.0); + v = REAL(0.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = Numer / Denom; + v = REAL(1.0) - u; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + else{ + u = REAL(0.0); + if(Tmp1 <= REAL(0.0)){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else if(v < REAL(0.0)){ // region 6 + Tmp0 = A01 + B1; + Tmp1 = A00 + B0; + if (Tmp1 > Tmp0){ + Numer = Tmp1 - Tmp0; + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + v = REAL(1.0); + u = REAL(0.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = Numer / Denom; + u = REAL(1.0) - v; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + else{ + v = REAL(0.0); + if (Tmp1 <= REAL(0.0)){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else if(B0 >= REAL(0.0)){ + u = REAL(0.0); + DistSq = C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + } + else{ // region 1 + Numer = A11 + B1 - A01 - B0; + if (Numer <= REAL(0.0)){ + u = REAL(0.0); + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + u = REAL(1.0); + v = REAL(0.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = Numer / Denom; + v = REAL(1.0) - u; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + } + } + + Dist = dSqrt(dFabs(DistSq)); + + if (Dist <= Radius){ + Dist = Radius - Dist; + return true; + } + else return false; +} + +int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (SphereGeom->type == dSphereClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + SphereCollider& Collider = pccColliderCache->m_SphereCollider; + + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + Matrix4x4 MeshMatrix; + const dVector3 ZeroVector3 = { REAL(0.0), }; + MakeMatrix(ZeroVector3, TLRotation, MeshMatrix); + + const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom); + dReal Radius = dGeomSphereGetRadius(SphereGeom); + + dVector3 OffsetPosition; + dSubtractVectors3(OffsetPosition, Position, TLPosition); + + // Sphere + Sphere Sphere; + Sphere.mCenter.Set(OffsetPosition[0], OffsetPosition[1], OffsetPosition[2]); + Sphere.mRadius = Radius; + + + // TC results + if (TriMesh->getDoTC(dxTriMesh::TTC_SPHERE)) { + dxTriMesh::SphereTC* sphereTC = 0; + const int sphereCacheSize = TriMesh->m_SphereTCCache.size(); + for (int i = 0; i != sphereCacheSize; i++){ + if (TriMesh->m_SphereTCCache[i].Geom == SphereGeom){ + sphereTC = &TriMesh->m_SphereTCCache[i]; + break; + } + } + + if (!sphereTC) { + TriMesh->m_SphereTCCache.push(dxTriMesh::SphereTC()); + + sphereTC = &TriMesh->m_SphereTCCache[TriMesh->m_SphereTCCache.size() - 1]; + sphereTC->Geom = SphereGeom; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*sphereTC, Sphere, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(pccColliderCache->m_DefaultSphereCache, Sphere, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + + if (! Collider.GetContactStatus()) { + // no collision occurred + return 0; + } + + // get results + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0){ + if (TriMesh->m_ArrayCallback != null){ + TriMesh->m_ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount); + } + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++){ + if (OutTriCount == (Flags & NUMC_MASK)){ + break; + } + + const int TriIndex = Triangles[i]; + + dVector3 dv[3]; + if (!TriMesh->invokeCallback(SphereGeom, TriIndex)) + continue; + + TriMesh->fetchMeshTriangle(dv, TriIndex, TLPosition, TLRotation); + + dVector3& v0 = dv[0]; + dVector3& v1 = dv[1]; + dVector3& v2 = dv[2]; + + dVector3 vu; + vu[0] = v1[0] - v0[0]; + vu[1] = v1[1] - v0[1]; + vu[2] = v1[2] - v0[2]; + vu[3] = REAL(0.0); + + dVector3 vv; + vv[0] = v2[0] - v0[0]; + vv[1] = v2[1] - v0[1]; + vv[2] = v2[2] - v0[2]; + vv[3] = REAL(0.0); + + // Get plane coefficients + dVector4 Plane; + dCalcVectorCross3(Plane, vu, vv); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (!dSafeNormalize3(Plane)) { + continue; + } + + /* If the center of the sphere is within the positive halfspace of the + * triangle's plane, allow a contact to be generated. + * If the center of the sphere made it into the positive halfspace of a + * back-facing triangle, then the physics update and/or velocity needs + * to be adjusted (penetration has occured anyway). + */ + + dReal side = dCalcVectorDot3(Plane,Position) - dCalcVectorDot3(Plane, v0); + + if(side < REAL(0.0)) { + continue; + } + + dReal Depth; + dReal u, v; + if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){ + continue; // Sphere doesn't hit triangle + } + + if (Depth < REAL(0.0)){ + continue; // Negative depth does not produce a contact + } + + dVector3 ContactPos; + + dReal w = REAL(1.0) - u - v; + ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v); + ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v); + ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v); + + // Depth returned from GetContactData is depth along + // contact point - sphere center direction + // we'll project it to contact normal + dVector3 dir; + dir[0] = Position[0]-ContactPos[0]; + dir[1] = Position[1]-ContactPos[1]; + dir[2] = Position[2]-ContactPos[2]; + dReal dirProj = dCalcVectorDot3(dir, Plane) / dSqrt(dCalcVectorDot3(dir, dir)); + + // Since Depth already had a requirement to be non-negative, + // negative direction projections should not be allowed as well, + // as otherwise the multiplication will result in negative contact depth. + if (dirProj < REAL(0.0)){ + continue; // Zero contact depth could be ignored + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + Contact->pos[0] = ContactPos[0]; + Contact->pos[1] = ContactPos[1]; + Contact->pos[2] = ContactPos[2]; + Contact->pos[3] = REAL(0.0); + + // Using normal as plane (reversed) + Contact->normal[0] = -Plane[0]; + Contact->normal[1] = -Plane[1]; + Contact->normal[2] = -Plane[2]; + Contact->normal[3] = REAL(0.0); + + Contact->depth = Depth * dirProj; + //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance + + // We need to set these unconditionally, as the merging may fail! - Bram + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + Contact->side2 = -1; + + Contact->side1 = TriIndex; + + OutTriCount++; + } + if (OutTriCount > 0){ + if (TriMesh->m_SphereContactsMergeOption == MERGE_CONTACTS_FULLY) { + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride); + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + Contact->side2 = -1; + + if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){ + dVector3 pos; + pos[0] = Contact->pos[0]; + pos[1] = Contact->pos[1]; + pos[2] = Contact->pos[2]; + + dVector3 normal; + normal[0] = Contact->normal[0] * Contact->depth; + normal[1] = Contact->normal[1] * Contact->depth; + normal[2] = Contact->normal[2] * Contact->depth; + normal[3] = REAL(0.0); + + int TriIndex = Contact->side1; + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride); + + pos[0] += TempContact->pos[0]; + pos[1] += TempContact->pos[1]; + pos[2] += TempContact->pos[2]; + + normal[0] += TempContact->normal[0] * TempContact->depth; + normal[1] += TempContact->normal[1] * TempContact->depth; + normal[2] += TempContact->normal[2] * TempContact->depth; + + TriIndex = (TriMesh->m_TriMergeCallback) ? TriMesh->m_TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1; + } + + Contact->side1 = TriIndex; + + Contact->pos[0] = pos[0] / OutTriCount; + Contact->pos[1] = pos[1] / OutTriCount; + Contact->pos[2] = pos[2] / OutTriCount; + + if ( !dSafeNormalize3(normal) ) + return OutTriCount; // Cannot merge in this pathological case + + // Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection. + // That is why we need to correct this by increasing the depth for each intersection. + // The maximum of the adjusted depths is our newly merged depth value - Bram. + + dReal mergedDepth = REAL(0.0); + dReal minEffectiveness = REAL(0.5); + for ( int i = 0; i < OutTriCount; ++i ) + { + dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride); + dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal); + if ( effectiveness < dEpsilon ) + return OutTriCount; // Cannot merge this pathological case + // Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal. + effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness; + dReal adjusted = TempContact->depth / effectiveness; + mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth; + } + Contact->depth = mergedDepth; + Contact->normal[0] = normal[0]; + Contact->normal[1] = normal[1]; + Contact->normal[2] = normal[2]; + Contact->normal[3] = normal[3]; + } + + return 1; + } + else if (TriMesh->m_SphereContactsMergeOption == MERGE_CONTACT_NORMALS) { + if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){ + dVector3 Normal; + + dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride); + Normal[0] = FirstContact->normal[0] * FirstContact->depth; + Normal[1] = FirstContact->normal[1] * FirstContact->depth; + Normal[2] = FirstContact->normal[2] * FirstContact->depth; + Normal[3] = FirstContact->normal[3] * FirstContact->depth; + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + Normal[0] += Contact->normal[0] * Contact->depth; + Normal[1] += Contact->normal[1] * Contact->depth; + Normal[2] += Contact->normal[2] * Contact->depth; + Normal[3] += Contact->normal[3] * Contact->depth; + } + + dNormalize3(Normal); + + for (int i = 0; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + Contact->normal[0] = Normal[0]; + Contact->normal[1] = Normal[1]; + Contact->normal[2] = Normal[2]; + Contact->normal[3] = Normal[3]; + } + } + + return OutTriCount; + } + else { + dIASSERT(TriMesh->m_SphereContactsMergeOption == DONT_MERGE_CONTACTS); + return OutTriCount; + } + } + else return 0; + } + else return 0; +} + + +#endif // dTRIMESH_OPCODE + + +#if dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "gimpact_gim_contact_accessor.h" + + +int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (SphereGeom->type == dSphereClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + dVector3& Position = *(dVector3*)dGeomGetPosition(SphereGeom); + dReal Radius = dGeomSphereGetRadius(SphereGeom); + //Create contact list + GDYNAMIC_ARRAY trimeshcontacts; + GIM_CREATE_CONTACT_LIST(trimeshcontacts); + + g1 -> recomputeAABB(); + SphereGeom -> recomputeAABB(); + + //Collide trimeshes + gim_trimesh_sphere_collisionODE(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts); + + if(trimeshcontacts.m_size == 0) + { + GIM_DYNARRAY_DESTROY(trimeshcontacts); + return 0; + } + + GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts); + unsigned contactcount = trimeshcontacts.m_size; + + dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, g1, SphereGeom, -1); + contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, Flags, Contacts, Stride); + + GIM_DYNARRAY_DESTROY(trimeshcontacts); + + return (int)contactcount; +} + + +#endif // dTRIMESH_GIMPACT + + +#endif // dTRIMESH_ENABLED diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp new file mode 100644 index 0000000..27c90bc --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp @@ -0,0 +1,1367 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// OPCODE TriMesh/TriMesh collision code +// Written at 2006-10-28 by Francisco León (http://gimpact.sourceforge.net) + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#include "collision_trimesh_internal.h" + + +#if !dTLS_ENABLED +// Have collider cache instance unconditionally of OPCODE or GIMPACT selection +/*extern */TrimeshCollidersCache g_ccTrimeshCollidersCache; +#endif + + +#if dTRIMESH_OPCODE + +// New Implementation +#if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + +#define SMALL_ELT REAL(2.5e-4) +#define EXPANDED_ELT_THRESH REAL(1.0e-3) +#define DISTANCE_EPSILON REAL(1.0e-8) +#define VELOCITY_EPSILON REAL(1.0e-5) +#define TINY_PENETRATION REAL(5.0e-6) + +struct LineContactSet +{ + enum + { + MAX_POINTS = 8 + }; + + dVector3 Points[MAX_POINTS]; + int Count; +}; + + +// static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); -- not used +static inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B); +//static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ); +//static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3); +static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& ); + + +///returns the penetration depth +static dReal MostDeepPoints( + LineContactSet & points, + const dVector3 plane_normal, + dReal plane_dist, + LineContactSet & deep_points); + +static bool TriTriContacts(const dVector3 tr1[3], + const dVector3 tr2[3], + int TriIndex1, int TriIndex2, + dxGeom* g1, dxGeom* g2, int Flags, + CONTACT_KEY_HASH_TABLE &hashcontactset, + dContactGeom* Contacts, int Stride, + int &contactcount); + + +/* some math macros */ +#define IS_ZERO(v) (!(v)[0] && !(v)[1] && !(v)[2]) + +#define CROSS(dest,v1,v2) dCalcVectorCross3(dest, v1, v2) + +#define DOT(v1,v2) dCalcVectorDot3(v1, v2) + +#define SUB(dest,v1,v2) dSubtractVectors3(dest, v1, v2) + +#define ADD(dest,v1,v2) dAddVectors3(dest, v1, v2) + +#define MULT(dest,v,factor) dCopyScaledVector3(dest, v, factor) + +#define SET(dest,src) dCopyVector3(dest, src) + +#define SMULT(p,q,s) dCopyScaledVector3(p, q, s) + +#define COMBO(combo,p,t,q) dAddVectorScaledVector3(combo, p, q, t) + +#define LENGTH(x) dCalcVectorLength3(x) + +#define DEPTH(d, p, q, n) d = dCalcPointDepth3(q, p, n) + + +static inline +void SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2, + dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2, + dVector3 n, dVector3 n1, dVector3 n2) +{ + if (pen_v == v1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } +} + +///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE/////////////////////////////// +////* Written by Francisco León (http://gimpact.sourceforge.net) */// +#define CONTACT_DIFF_EPSILON REAL(0.00001) +#if defined(dDOUBLE) +#define CONTACT_NORMAL_ZERO REAL(0.0000001) +#else // if defined(dSINGLE) +#define CONTACT_NORMAL_ZERO REAL(0.00001) +#endif +#define CONTACT_POS_HASH_QUOTIENT REAL(10000.0) +#define dSQRT3 REAL(1.7320508075688773) + +static +void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact) +{ + key.m_contact = contact; + + unsigned int hash=0; + + int i = 0; + + while (true) + { + dReal coord = contact->pos[i]; + coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT); + + const int sz = sizeof(coord) / sizeof(unsigned); + dIASSERT(sizeof(coord) % sizeof(unsigned) == 0); + + unsigned hash_v[ sz ]; + memcpy(hash_v, &coord, sizeof(coord)); + + unsigned int hash_input = hash_v[0]; + for (int i=1; i<sz; ++i) + hash_input ^= hash_v[i]; + + hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 ); + hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 ); + hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 ); + hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 ); + + if (++i == 3) + { + break; + } + + hash = (hash << 11) | (hash >> 21); + } + + key.m_key = hash; +} + + +static inline +unsigned int MakeContactIndex(unsigned int key) +{ + dIASSERT(CONTACTS_HASHSIZE == 256); + + unsigned int index = key ^ (key >> 16); + index = (index ^ (index >> 8)) & 0xFF; + + return index; +} + +static +dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node) +{ + for(int i=0;i<node->m_keycount;i++) + { + if(node->m_keyarray[i].m_key == contactkey->m_key) + { + dContactGeom *contactfound = node->m_keyarray[i].m_contact; + if (dCalcPointsDistance3(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/) + { + return contactfound; + } + } + } + + if (node->m_keycount < MAXCONTACT_X_NODE) + { + node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact; + node->m_keyarray[node->m_keycount].m_key = contactkey->m_key; + node->m_keycount++; + } + else + { + dDEBUGMSG("Trimesh-trimesh contach hash table bucket overflow - close contacts might not be culled"); + } + + return contactkey->m_contact; +} + +static +void RemoveNewContactFromNode(const CONTACT_KEY * contactkey, CONTACT_KEY_HASH_NODE * node) +{ + dIASSERT(node->m_keycount > 0); + + if (node->m_keyarray[node->m_keycount - 1].m_contact == contactkey->m_contact) + { + node->m_keycount -= 1; + } + else + { + dIASSERT(node->m_keycount == MAXCONTACT_X_NODE); + } +} + +static +void RemoveArbitraryContactFromNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node) +{ + dIASSERT(node->m_keycount > 0); + + int keyindex, lastkeyindex = node->m_keycount - 1; + + // Do not check the last contact + for (keyindex = 0; keyindex < lastkeyindex; keyindex++) + { + if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact) + { + node->m_keyarray[keyindex] = node->m_keyarray[lastkeyindex]; + break; + } + } + + dIASSERT(keyindex < lastkeyindex || + node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match + + node->m_keycount = lastkeyindex; +} + +static +void UpdateArbitraryContactInNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node, + dContactGeom *pwithcontact) +{ + dIASSERT(node->m_keycount > 0); + + int keyindex, lastkeyindex = node->m_keycount - 1; + + // Do not check the last contact + for (keyindex = 0; keyindex < lastkeyindex; keyindex++) + { + if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact) + { + break; + } + } + + dIASSERT(keyindex < lastkeyindex || + node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match + + node->m_keyarray[keyindex].m_contact = pwithcontact; +} + +static +void ClearContactSet(CONTACT_KEY_HASH_TABLE &hashcontactset) +{ + memset(&hashcontactset, 0, sizeof(CONTACT_KEY_HASH_TABLE)); +} + +//return true if found +static +dContactGeom *InsertContactInSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &newkey) +{ + unsigned int index = MakeContactIndex(newkey.m_key); + + return AddContactToNode(&newkey, &hashcontactset[index]); +} + +static +void RemoveNewContactFromSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey) +{ + unsigned int index = MakeContactIndex(contactkey.m_key); + + RemoveNewContactFromNode(&contactkey, &hashcontactset[index]); +} + +static +void RemoveArbitraryContactFromSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey) +{ + unsigned int index = MakeContactIndex(contactkey.m_key); + + RemoveArbitraryContactFromNode(&contactkey, &hashcontactset[index]); +} + +static +void UpdateArbitraryContactInSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey, + dContactGeom *pwithcontact) +{ + unsigned int index = MakeContactIndex(contactkey.m_key); + + UpdateArbitraryContactInNode(&contactkey, &hashcontactset[index], pwithcontact); +} + +static +bool AllocNewContact( + const dVector3 newpoint, dContactGeom *& out_pcontact, + int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset, + dContactGeom* Contacts, int Stride, int &contactcount) +{ + bool allocated_new = false; + + dContactGeom dLocalContact; + + dContactGeom * pcontact = contactcount != (Flags & NUMC_MASK) ? + SAFECONTACT(Flags, Contacts, contactcount, Stride) : &dLocalContact; + + pcontact->pos[0] = newpoint[0]; + pcontact->pos[1] = newpoint[1]; + pcontact->pos[2] = newpoint[2]; + pcontact->pos[3] = 1.0f; + + CONTACT_KEY newkey; + UpdateContactKey(newkey, pcontact); + + dContactGeom *pcontactfound = InsertContactInSet(hashcontactset, newkey); + if (pcontactfound == pcontact) + { + if (pcontactfound != &dLocalContact) + { + contactcount++; + } + else + { + RemoveNewContactFromSet(hashcontactset, newkey); + pcontactfound = NULL; + } + + allocated_new = true; + } + + out_pcontact = pcontactfound; + return allocated_new; +} + +static +void FreeExistingContact(dContactGeom *pcontact, + int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset, + dContactGeom *Contacts, int Stride, int &contactcount) +{ + CONTACT_KEY contactKey; + UpdateContactKey(contactKey, pcontact); + + RemoveArbitraryContactFromSet(hashcontactset, contactKey); + + int lastContactIndex = contactcount - 1; + dContactGeom *plastContact = SAFECONTACT(Flags, Contacts, lastContactIndex, Stride); + + if (pcontact != plastContact) + { + *pcontact = *plastContact; + + CONTACT_KEY lastContactKey; + UpdateContactKey(lastContactKey, plastContact); + + UpdateArbitraryContactInSet(hashcontactset, lastContactKey, pcontact); + } + + contactcount = lastContactIndex; +} + + +static +dContactGeom * PushNewContact( dxGeom* g1, dxGeom* g2, int TriIndex1, int TriIndex2, + const dVector3 point, + dVector3 normal, + dReal depth, + int Flags, + CONTACT_KEY_HASH_TABLE &hashcontactset, + dContactGeom* Contacts, int Stride, + int &contactcount) +{ + dIASSERT(dFabs(dVector3Length((const dVector3 &)(*normal)) - REAL(1.0)) < REAL(1e-6)); // This assumption is used in the code + + dContactGeom * pcontact; + + if (!AllocNewContact(point, pcontact, Flags, hashcontactset, Contacts, Stride, contactcount)) + { + const dReal depthDifference = depth - pcontact->depth; + + if (depthDifference > CONTACT_DIFF_EPSILON) + { + pcontact->normal[0] = normal[0]; + pcontact->normal[1] = normal[1]; + pcontact->normal[2] = normal[2]; + pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging + pcontact->depth = depth; + + pcontact->g1 = g1; + pcontact->g2 = g2; + pcontact->side1 = TriIndex1; + pcontact->side2 = TriIndex2; + } + else if (depthDifference >= -CONTACT_DIFF_EPSILON) ///average + { + if(pcontact->g1 == g2) + { + MULT(normal,normal, REAL(-1.0)); + int tempInt = TriIndex1; TriIndex1 = TriIndex2; TriIndex2 = tempInt; + // This should be discarded by optimizer as g1 and g2 are + // not used any more but it's preferable to keep this line for + // the sake of consistency in variable values. + dxGeom *tempGeom = g1; g1 = g2; g2 = tempGeom; + } + + const dReal oldLen = pcontact->normal[3]; + COMBO(pcontact->normal, normal, oldLen, pcontact->normal); + + const dReal len = LENGTH(pcontact->normal); + if (len > CONTACT_NORMAL_ZERO) + { + MULT(pcontact->normal, pcontact->normal, REAL(1.0) / len); + pcontact->normal[3] = len; + + pcontact->side1 = ((dxTriMesh *)pcontact->g1)->m_TriMergeCallback ? ((dxTriMesh *)pcontact->g1)->m_TriMergeCallback((dxTriMesh *)pcontact->g1, pcontact->side1, TriIndex1) : -1; + pcontact->side2 = ((dxTriMesh *)pcontact->g2)->m_TriMergeCallback ? ((dxTriMesh *)pcontact->g2)->m_TriMergeCallback((dxTriMesh *)pcontact->g2, pcontact->side2, TriIndex2) : -1; + } + else + { + FreeExistingContact(pcontact, Flags, hashcontactset, Contacts, Stride, contactcount); + } + } + } + // Contact can be not available if buffer is full + else if (pcontact) + { + pcontact->normal[0] = normal[0]; + pcontact->normal[1] = normal[1]; + pcontact->normal[2] = normal[2]; + pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging + pcontact->depth = depth; + pcontact->g1 = g1; + pcontact->g2 = g2; + pcontact->side1 = TriIndex1; + pcontact->side2 = TriIndex2; + } + + return pcontact; +} + +//////////////////////////////////////////////////////////////////////////////////////////// + + + + +/*extern */ +int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (g2->type == dTriMeshClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh1 = (dxTriMesh*) g1; + dxTriMesh* TriMesh2 = (dxTriMesh*) g2; + + //dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals; + //dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals; + + const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1); + // TLRotation1 = column-major order + const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1); + + const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2); + // TLRotation2 = column-major order + const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2); + + const unsigned uiTLSKind = TriMesh1->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == TriMesh2->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + AABBTreeCollider& Collider = pccColliderCache->m_AABBTreeCollider; + BVTCache &ColCache = pccColliderCache->ColCache; + CONTACT_KEY_HASH_TABLE &hashcontactset = pccColliderCache->m_hashcontactset; + + ColCache.Model0 = &TriMesh1->retrieveMeshBVTreeRef(); + ColCache.Model1 = &TriMesh2->retrieveMeshBVTreeRef(); + + ////Prepare contact list + ClearContactSet(hashcontactset); + + // Collision query + Matrix4x4 amatrix, bmatrix; + dVector3 TLOffsetPosition1 = { REAL(0.0), }; + dVector3 TLOffsetPosition2; + dSubtractVectors3(TLOffsetPosition2, TLPosition2, TLPosition1); + MakeMatrix(TLOffsetPosition1, TLRotation1, amatrix); + MakeMatrix(TLOffsetPosition2, TLRotation2, bmatrix); + bool IsOk = Collider.Collide(ColCache, &amatrix, &bmatrix); + + + if (IsOk) { + // Get collision status => if true, objects overlap + if ( Collider.GetContactStatus() ) { + // Number of colliding pairs and list of pairs + int TriCount = Collider.GetNbPairs(); + const Pair* CollidingPairs = Collider.GetPairs(); + + if (TriCount > 0) { + // step through the pairs, adding contacts + int id1, id2; + int OutTriCount = 0; + dVector3 v1[3], v2[3]; + + for (int i = 0; i < TriCount; i++) + { + id1 = CollidingPairs[i].id0; + id2 = CollidingPairs[i].id1; + + // grab the colliding triangles + static_cast<dxTriMesh *>(g1)->fetchMeshTriangle(v1, id1, TLPosition1, TLRotation1); + static_cast<dxTriMesh *>(g2)->fetchMeshTriangle(v2, id2, TLPosition2, TLRotation2); + + // Since we'll be doing matrix transformations, we need to + // make sure that all vertices have four elements + for (int j=0; j<3; j++) { + v1[j][3] = 1.0; + v2[j][3] = 1.0; + } + + TriTriContacts(v1,v2, id1,id2, + g1, g2, Flags, hashcontactset, + Contacts,Stride,OutTriCount); + + // Continue loop even after contacts are full + // as existing contacts' normals/depths might be updated + // Break only if contacts are not important + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) + { + break; + } + } + + // Return the number of contacts + return OutTriCount; + + } + } + } + + + // There was some kind of failure during the Collide call or + // there are no faces overlapping + return 0; +} + + +/* -- not used +static void +GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data) +{ + dVector3 Out[3]; + + FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out); + + for (int i = 0; i < 3; i++) + triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]); +} +*/ + +// +// +// +#define B11 B[0] +#define B12 B[1] +#define B13 B[2] +#define B14 B[3] +#define B21 B[4] +#define B22 B[5] +#define B23 B[6] +#define B24 B[7] +#define B31 B[8] +#define B32 B[9] +#define B33 B[10] +#define B34 B[11] +#define B41 B[12] +#define B42 B[13] +#define B43 B[14] +#define B44 B[15] + +#define Binv11 Binv[0] +#define Binv12 Binv[1] +#define Binv13 Binv[2] +#define Binv14 Binv[3] +#define Binv21 Binv[4] +#define Binv22 Binv[5] +#define Binv23 Binv[6] +#define Binv24 Binv[7] +#define Binv31 Binv[8] +#define Binv32 Binv[9] +#define Binv33 Binv[10] +#define Binv34 Binv[11] +#define Binv41 Binv[12] +#define Binv42 Binv[13] +#define Binv43 Binv[14] +#define Binv44 Binv[15] + +static inline +void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B) +{ + B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0]; + B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1]; + B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2]; + + B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0; +} + +#if 0 +static void +dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ) +{ + dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43) + -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42) + +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42) + +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41) + -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41) + +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41); + + dAASSERT (det != 0.0); + + det = 1.0 / det; + + Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32))); + Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12))); + Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22))); + Binv14 = 0.0f; + Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33))); + Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13))); + Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23))); + Binv24 = 0.0f; + Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31))); + Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11))); + Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21))); + Binv34 = 0.0f; + Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42))); + Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42))); + Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22))); + Binv44 = 1.0f; +} +#endif + + +// Find the intersectiojn point between a coplanar line segement, +// defined by X1 and X2, and a ray defined by X3 and direction N. +// +// This forumla for this calculation is: +// (c x b) . (a x b) +// Q = x1 + a ------------------- +// | a x b | ^2 +// +// where a = x2 - x1 +// b = x4 - x3 +// c = x3 - x1 +// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt +// and x4 is (CoplanarPt - n) +#if 0 +static int +IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n, + dVector3 out_pt) +{ + dVector3 a, b, c, x4; + + ADD(x4, x3, n); // x4 = x3 + n + + SUB(a, x2, x1); // a = x2 - x1 + SUB(b, x4, x3); + SUB(c, x3, x1); + + dVector3 tmp1, tmp2; + CROSS(tmp1, c, b); + CROSS(tmp2, a, b); + + dReal num, denom; + num = dCalcVectorDot3(tmp1, tmp2); + denom = LENGTH( tmp2 ); + + dReal s; + s = num /(denom*denom); + + for (int i=0; i<3; i++) + out_pt[i] = x1[i] + a[i]*s; + + // Test if this intersection is "behind" x3, w.r.t. n + SUB(a, x3, out_pt); + if (dCalcVectorDot3(a, n) > 0.0) + return 0; + + // Test if this intersection point is outside the edge limits, + // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside + // else outside + SUB(a, out_pt, x1); + SUB(b, out_pt, x2); + if (dCalcVectorDot3(a,b) < 0.0) + return 1; + else + return 0; +} +#endif + + +void PlaneClipSegment( const dVector3 s1, const dVector3 s2, + const dVector3 N, dReal C, dVector3 clipped) +{ + dReal dis1,dis2; + dis1 = DOT(s1,N)-C; + SUB(clipped,s2,s1); + dis2 = DOT(clipped,N); + MULT(clipped,clipped,-dis1/dis2); + ADD(clipped,clipped,s1); + clipped[3] = 1.0f; +} + +/* ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by +CONTACTS, with a plane (described by N and C distance from origin). +Note: the input vertices are assumed to be in invcounterclockwise order. +changed by Francisco Leon (http://gimpact.sourceforge.net) */ +static void +ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C, + LineContactSet& Contacts ) +{ + int i, vi, prevclassif=32000, classif; + /* + classif 0 : back, 1 : front + */ + + dReal d; + dVector3 clipped[8]; + int clippedcount =0; + + if(Contacts.Count==0) + { + return; + } + for(i=0;i<=Contacts.Count;i++) + { + vi = i%Contacts.Count; + + d = DOT(N,Contacts.Points[vi]) - C; + ////classify point + if(d>REAL(1.0e-8)) classif = 1; + else classif = 0; + + if(classif == 0)//back + { + if(i>0) + { + if(prevclassif==1)///in front + { + + ///add clipped point + if(clippedcount<8) + { + PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi], + N,C,clipped[clippedcount]); + clippedcount++; + } + } + } + ///add point + if(clippedcount<8&&i<Contacts.Count) + { + clipped[clippedcount][0] = Contacts.Points[vi][0]; + clipped[clippedcount][1] = Contacts.Points[vi][1]; + clipped[clippedcount][2] = Contacts.Points[vi][2]; + clipped[clippedcount][3] = 1.0f; + clippedcount++; + } + } + else + { + + if(i>0) + { + if(prevclassif==0) + { + ///add point + if(clippedcount<8) + { + PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi], + N,C,clipped[clippedcount]); + clippedcount++; + } + } + } + } + + prevclassif = classif; + } + + if(clippedcount==0) + { + Contacts.Count = 0; + return; + } + Contacts.Count = clippedcount; + memcpy( Contacts.Points, clipped, clippedcount * sizeof(dVector3) ); + return; +} + + +bool BuildPlane(const dVector3 s0, const dVector3 s1,const dVector3 s2, + dVector3 Normal, dReal & Dist) +{ + dVector3 e0,e1; + SUB(e0,s1,s0); + SUB(e1,s2,s0); + + CROSS(Normal,e0,e1); + + if (!dSafeNormalize3(Normal)) + { + return false; + } + + Dist = DOT(Normal,s0); + return true; + +} + +// bool BuildEdgesDir(const dVector3 s0, const dVector3 s1, +// const dVector3 t0, const dVector3 t1, +// dVector3 crossdir) +// { +// dVector3 e0,e1; +// +// SUB(e0,s1,s0); +// SUB(e1,t1,t0); +// CROSS(crossdir,e0,e1); +// +// if (!dSafeNormalize3(crossdir)) +// { +// return false; +// } +// return true; +// } + + + +bool BuildEdgePlane( + const dVector3 s0, const dVector3 s1, + const dVector3 normal, + dVector3 plane_normal, + dReal & plane_dist) +{ + dVector3 e0; + + SUB(e0,s1,s0); + CROSS(plane_normal,e0,normal); + if (!dSafeNormalize3(plane_normal)) + { + return false; + } + plane_dist = DOT(plane_normal,s0); + return true; +} + + + + +/* +Positive penetration +Negative number: they are separated +*/ +dReal IntervalPenetration(dReal &vmin1,dReal &vmax1, + dReal &vmin2,dReal &vmax2) +{ + if(vmax1<=vmin2) + { + return -(vmin2-vmax1); + } + else + { + if(vmax2<=vmin1) + { + return -(vmin1-vmax2); + } + else + { + if(vmax1<=vmax2) + { + return vmax1-vmin2; + } + + return vmax2-vmin1; + } + + } + return 0; +} + +void FindInterval( + const dVector3 * vertices, int verticecount, + dVector3 dir,dReal &vmin,dReal &vmax) +{ + + dReal dist; + int i; + vmin = DOT(vertices[0],dir); + vmax = vmin; + for(i=1;i<verticecount;i++) + { + dist = DOT(vertices[i],dir); + if(vmin>dist) vmin=dist; + else if(vmax<dist) vmax=dist; + } +} + +///returns the penetration depth +dReal MostDeepPoints( + LineContactSet & points, + const dVector3 plane_normal, + dReal plane_dist, + LineContactSet & deep_points) +{ + int i; + int max_candidates[8]; + dReal maxdeep=-dInfinity; + dReal dist; + + deep_points.Count = 0; + for(i=0;i<points.Count;i++) + { + dist = DOT(plane_normal,points.Points[i]) - plane_dist; + dist *= -1.0f; + if(dist>maxdeep) + { + maxdeep = dist; + deep_points.Count=1; + max_candidates[deep_points.Count-1] = i; + } + else if(dist+REAL(0.000001)>=maxdeep) + { + deep_points.Count++; + max_candidates[deep_points.Count-1] = i; + } + } + + for(i=0;i<deep_points.Count;i++) + { + SET(deep_points.Points[i],points.Points[max_candidates[i]]); + } + return maxdeep; + +} + +void ClipPointsByTri( + const dVector3 * points, int pointcount, + const dVector3 tri[3], + const dVector3 triplanenormal, + dReal triplanedist, + LineContactSet & clipped_points, + bool triplane_clips) +{ + ///build edges planes + int i; + dVector4 plane; + + clipped_points.Count = pointcount; + memcpy(&clipped_points.Points[0],&points[0],pointcount*sizeof(dVector3)); + for(i=0;i<3;i++) + { + if (BuildEdgePlane( + tri[i],tri[(i+1)%3],triplanenormal, + plane,plane[3])) + { + ClipConvexPolygonAgainstPlane( + plane, + plane[3], + clipped_points); + } + } + + if(triplane_clips) + { + ClipConvexPolygonAgainstPlane( + triplanenormal, + triplanedist, + clipped_points); + } +} + + +///returns the penetration depth +dReal FindTriangleTriangleCollision( + const dVector3 tri1[3], + const dVector3 tri2[3], + dVector3 separating_normal, + LineContactSet & deep_points) +{ + dReal maxdeep=dInfinity; + dReal dist; + int mostdir=0, /*mostface=0,*/ currdir=0; + // dReal vmin1,vmax1,vmin2,vmax2; + // dVector3 crossdir, pt1,pt2; + dVector4 tri1plane,tri2plane; + separating_normal[3] = 0.0f; + bool bl; + LineContactSet clipped_points1,clipped_points2; + LineContactSet deep_points1,deep_points2; + // It is necessary to initialize the count because both conditional statements + // might be skipped leading to uninitialized count being used for memcpy in if(mostdir==0) + deep_points1.Count = 0; + + ////find interval face1 + + bl = BuildPlane(tri1[0],tri1[1],tri1[2], + tri1plane,tri1plane[3]); + clipped_points1.Count = 0; + + if(bl) + { + ClipPointsByTri( + tri2, 3, + tri1, + tri1plane, + tri1plane[3], + clipped_points1,false); + + + + maxdeep = MostDeepPoints( + clipped_points1, + tri1plane, + tri1plane[3], + deep_points1); + SET(separating_normal,tri1plane); + + } + currdir++; + + ////find interval face2 + + bl = BuildPlane(tri2[0],tri2[1],tri2[2], + tri2plane,tri2plane[3]); + + + clipped_points2.Count = 0; + if(bl) + { + ClipPointsByTri( + tri1, 3, + tri2, + tri2plane, + tri2plane[3], + clipped_points2,false); + + + + dist = MostDeepPoints( + clipped_points2, + tri2plane, + tri2plane[3], + deep_points2); + + + + if(dist<maxdeep) + { + maxdeep = dist; + mostdir = currdir; + //mostface = 1; + SET(separating_normal,tri2plane); + } + } + currdir++; + + + ///find edge edge distances + ///test each edge plane + + /*for(i=0;i<3;i++) + { + + + for(j=0;j<3;j++) + { + + + bl = BuildEdgesDir( + tri1[i],tri1[(i+1)%3], + tri2[j],tri2[(j+1)%3], + crossdir); + + ////find plane distance + + if(bl) + { + FindInterval(tri1,3,crossdir,vmin1,vmax1); + FindInterval(tri2,3,crossdir,vmin2,vmax2); + + dist = IntervalPenetration( + vmin1, + vmax1, + vmin2, + vmax2); + if(dist<maxdeep) + { + maxdeep = dist; + mostdir = currdir; + SET(separating_normal,crossdir); + } + } + currdir++; + } + }*/ + + + ////check most dir for contacts + if(mostdir==0) + { + ///find most deep points + deep_points.Count = deep_points1.Count; + memcpy( + &deep_points.Points[0], + &deep_points1.Points[0], + deep_points1.Count*sizeof(dVector3)); + + ///invert normal for point to tri1 + MULT(separating_normal,separating_normal,-1.0f); + } + else if(mostdir==1) + { + deep_points.Count = deep_points2.Count; + memcpy( + &deep_points.Points[0], + &deep_points2.Points[0], + deep_points2.Count*sizeof(dVector3)); + + } + /*else + {///edge separation + mostdir -= 2; + + //edge 2 + j = mostdir%3; + //edge 1 + i = mostdir/3; + + ///find edge closest points + dClosestLineSegmentPoints( + tri1[i],tri1[(i+1)%3], + tri2[j],tri2[(j+1)%3], + pt1,pt2); + ///find correct direction + + SUB(crossdir,pt2,pt1); + + vmin1 = LENGTH(crossdir); + if(vmin1<REAL(0.000001)) + { + + if(mostface==0) + { + vmin1 = DOT(separating_normal,tri1plane); + if(vmin1>0.0) + { + MULT(separating_normal,separating_normal,-1.0f); + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + else + { + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + + } + else + { + vmin1 = DOT(separating_normal,tri2plane); + if(vmin1<0.0) + { + MULT(separating_normal,separating_normal,-1.0f); + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + else + { + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + + } + + + + + } + else + { + MULT(separating_normal,crossdir,1.0f/vmin1); + + vmin1 = DOT(separating_normal,tri1plane); + if(vmin1>0.0) + { + MULT(separating_normal,separating_normal,-1.0f); + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + else + { + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + + + } + + + }*/ + return maxdeep; +} + + + +///SUPPORT UP TO 8 CONTACTS +bool TriTriContacts(const dVector3 tr1[3], + const dVector3 tr2[3], + int TriIndex1, int TriIndex2, + dxGeom* g1, dxGeom* g2, int Flags, + CONTACT_KEY_HASH_TABLE &hashcontactset, + dContactGeom* Contacts, int Stride, + int &contactcount) +{ + + + dVector4 normal; + dReal depth; + ///Test Tri Vs Tri + // dContactGeom* pcontact; + int ccount = 0; + LineContactSet contactpoints; + contactpoints.Count = 0; + + + + ///find best direction + + depth = FindTriangleTriangleCollision( + tr1, + tr2, + normal, + contactpoints); + + + + if(depth<0.0f) return false; + + ccount = 0; + while (ccount<contactpoints.Count) + { + PushNewContact( g1, g2, TriIndex1, TriIndex2, + contactpoints.Points[ccount], + normal, depth, Flags, hashcontactset, + Contacts,Stride,contactcount); + + // Continue loop even after contacts are full + // as existing contacts' normals/depths might be updated + // Break only if contacts are not important + if ((contactcount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) + { + break; + } + + ccount++; + } + return true; +} + + +#endif // !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + + +#endif // dTRIMESH_OPCODE + + +////////////////////////////////////////////////////////////////////////// + +#if dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "gimpact_gim_contact_accessor.h" + + +// +// GIMPACT TRIMESH-TRIMESH COLLIDER +// + +/*extern */ +int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (g2->type == dTriMeshClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + int result = 0; + + dxTriMesh *triMesh1 = static_cast<dxTriMesh *>(g1); + dxTriMesh *triMesh2 = static_cast<dxTriMesh *>(g2); + //Create contact list + GDYNAMIC_ARRAY trimeshContacts; + GIM_CREATE_CONTACT_LIST(trimeshContacts); + + triMesh1->recomputeAABB(); + triMesh2->recomputeAABB(); + + //Collide trimeshes + gim_trimesh_trimesh_collision(&triMesh1->m_collision_trimesh, &triMesh2->m_collision_trimesh, &trimeshContacts); + + unsigned contactCount = trimeshContacts.m_size; + + if (contactCount != 0) + { + GIM_CONTACT *pTriMeshContacts = GIM_DYNARRAY_POINTER(GIM_CONTACT, trimeshContacts); + + dxGIMCContactAccessor contactAccessor(pTriMeshContacts, g1, g2); + unsigned culledContactCount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactAccessor, contactCount, Flags, Contacts, Stride); + + result = culledContactCount; + } + + GIM_DYNARRAY_DESTROY(trimeshContacts); + + return result; +} + + +#endif // dTRIMESH_GIMPACT + +#endif // dTRIMESH_ENABLED + diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp new file mode 100644 index 0000000..23d04a1 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp @@ -0,0 +1,2071 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// OPCODE TriMesh/TriMesh collision code by Jeff Smith (c) 2004 + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#include "collision_trimesh_internal.h" + + +#if dTRIMESH_OPCODE + +// Classic Implementation +#if dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + +#define SMALL_ELT REAL(2.5e-4) +#define EXPANDED_ELT_THRESH REAL(1.0e-3) +#define DISTANCE_EPSILON REAL(1.0e-8) +#define VELOCITY_EPSILON REAL(1.0e-5) +#define TINY_PENETRATION REAL(5.0e-6) + +struct LineContactSet +{ + enum + { + MAX_POINTS = 8 + }; + + dVector3 Points[MAX_POINTS]; + int Count; +}; + + +// static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); -- not used +static void GenerateContact(int, dContactGeom*, int, dxTriMesh*, dxTriMesh*, + int TriIndex1, int TriIndex2, + const dVector3, const dVector3, dReal, int&); +static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, + dReal isectpt1[3],dReal isectpt2[3]); +inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B); +static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ); +//static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3); +static bool FindTriSolidIntrsection(const dVector3 Tri[3], + const dVector4 Planes[6], int numSides, + LineContactSet& ClippedPolygon ); +static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& ); +static bool SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, + dVector3 in_n, dVector3* in_col_v, dReal &out_depth); +static int ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point); +static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir, + const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, + dReal *t,dReal *u,dReal *v); + + + + +/* some math macros */ +#define IS_ZERO(v) (!(v)[0] && !(v)[1] && !(v)[2]) + +#define CROSS(dest,v1,v2) dCalcVectorCross3(dest, v1, v2) + +#define DOT(v1,v2) dCalcVectorDot3(v1, v2) + +#define SUB(dest,v1,v2) dSubtractVectors3(dest, v1, v2) + +#define ADD(dest,v1,v2) dAddVectors3(dest, v1, v2) + +#define MULT(dest,v,factor) dCopyScaledVector3(dest, v, factor) + +#define SET(dest,src) dCopyVector3(dest, src) + +#define SMULT(p,q,s) dCopyScaledVector3(p, q, s) + +#define LENGTH(x) dCalcVectorLength3(x) + +#define DEPTH(d, p, q, n) d = dCalcPointDepth3(q, p, n) + + +inline void +SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2, + dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2, + dVector3 n, dVector3 n1, dVector3 n2) +{ + if (pen_v == v1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } +} + + + + +int +dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (g2->type == dTriMeshClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh1 = (dxTriMesh*) g1; + dxTriMesh* TriMesh2 = (dxTriMesh*) g2; + + const dReal* TriNormals1 = TriMesh1->retrieveMeshNormals(); + const dReal* TriNormals2 = TriMesh2->retrieveMeshNormals(); + + const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1); + // TLRotation1 = column-major order + const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1); + + const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2); + // TLRotation2 = column-major order + const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2); + + const unsigned uiTLSKind = TriMesh1->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == TriMesh2->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + AABBTreeCollider& Collider = pccColliderCache->m_AABBTreeCollider; + BVTCache &ColCache = pccColliderCache->ColCache; + + ColCache.Model0 = &TriMesh1->retrieveMeshBVTreeRef(); + ColCache.Model1 = &TriMesh2->retrieveMeshBVTreeRef(); + + // Collision query + Matrix4x4 amatrix, bmatrix; + dVector3 TLOffsetPosition1 = { REAL(0.0), }; + dVector3 TLOffsetPosition2; + dSubtractVectors3(TLOffsetPosition2, TLPosition2, TLPosition1); + MakeMatrix(TLOffsetPosition1, TLRotation1, amatrix); + MakeMatrix(TLOffsetPosition2, TLRotation2, bmatrix); + BOOL IsOk = Collider.Collide(ColCache, &amatrix, &bmatrix); + + + // Make "double" versions of these matrices, if appropriate + dMatrix4 A, B; + dMakeMatrix4(TLPosition1, TLRotation1, A); + dMakeMatrix4(TLPosition2, TLRotation2, B); + + + if (IsOk) { + // Get collision status => if true, objects overlap + if ( Collider.GetContactStatus() ) { + // Number of colliding pairs and list of pairs + int TriCount = Collider.GetNbPairs(); + const Pair* CollidingPairs = Collider.GetPairs(); + + if (TriCount > 0) { + // step through the pairs, adding contacts + int id1, id2; + int OutTriCount = 0; + dVector3 v1[3], v2[3], CoplanarPt; + dVector3 e1, e2, e3, n1, n2, n, ContactNormal; + dReal depth; + dVector3 orig_pos, old_pos1, old_pos2, elt1, elt2, elt_sum; + dVector3 elt_f1[3], elt_f2[3]; + dReal contact_elt_length = SMALL_ELT; + LineContactSet firstClippedTri, secondClippedTri; + dVector3 *firstClippedElt = new dVector3[LineContactSet::MAX_POINTS]; + dVector3 *secondClippedElt = new dVector3[LineContactSet::MAX_POINTS]; + + + // only do these expensive inversions once + dMatrix4 InvMatrix1, InvMatrix2; + dInvertMatrix4(A, InvMatrix1); + dInvertMatrix4(B, InvMatrix2); + + + for (int i = 0; i < TriCount; i++) { + + id1 = CollidingPairs[i].id0; + id2 = CollidingPairs[i].id1; + + // grab the colliding triangles + static_cast<dxTriMesh *>(g1)->fetchMeshTriangle(v1, id1, TLPosition1, TLRotation1); + static_cast<dxTriMesh *>(g2)->fetchMeshTriangle(v2, id2, TLPosition2, TLRotation2); + + // Since we'll be doing matrix transformations, we need to + // make sure that all vertices have four elements + for (int j=0; j<3; j++) { + v1[j][3] = 1.0; + v2[j][3] = 1.0; + } + + + int IsCoplanar = 0; + dReal IsectPt1[3], IsectPt2[3]; + + // Sometimes OPCODE makes mistakes, so we look at the return + // value for TriTriIntersectWithIsectLine. A retcode of "0" + // means no intersection took place + if ( TriTriIntersectWithIsectLine( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], + &IsCoplanar, + IsectPt1, IsectPt2) ) { + + // Compute the normals of the colliding faces + // + if (TriNormals1 == NULL) { + SUB( e1, v1[1], v1[0] ); + SUB( e2, v1[2], v1[0] ); + CROSS( n1, e1, e2 ); + dNormalize3(n1); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e1[0] = TriNormals1[id1*3]; + e1[1] = TriNormals1[id1*3 + 1]; + e1[2] = TriNormals1[id1*3 + 2]; + e1[3] = 0.0; + + //dMultiply1(n1, TLRotation1, e1, 3, 3, 1); + dMultiply0(n1, TLRotation1, e1, 3, 3, 1); + n1[3] = 1.0; + } + + if (TriNormals2 == NULL) { + SUB( e1, v2[1], v2[0] ); + SUB( e2, v2[2], v2[0] ); + CROSS( n2, e1, e2); + dNormalize3(n2); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e2[0] = TriNormals2[id2*3]; + e2[1] = TriNormals2[id2*3 + 1]; + e2[2] = TriNormals2[id2*3 + 2]; + e2[3] = 0.0; + + //dMultiply1(n2, TLRotation2, e2, 3, 3, 1); + dMultiply0(n2, TLRotation2, e2, 3, 3, 1); + n2[3] = 1.0; + } + + + if (IsCoplanar) { + // We can reach this case if the faces are coplanar, OR + // if they don't actually intersect. (OPCODE can make + // mistakes) + if (dFabs(dCalcVectorDot3(n1, n2)) > REAL(0.999)) { + // If the faces are coplanar, we declare that the point of + // contact is at the average location of the vertices of + // both faces + dVector3 ContactPt; + for (int j=0; j<3; j++) { + ContactPt[j] = 0.0; + for (int k=0; k<3; k++) + ContactPt[j] += v1[k][j] + v2[k][j]; + ContactPt[j] /= 6.0; + } + ContactPt[3] = 1.0; + + // and the contact normal is the normal of face 2 + // (could be face 1, because they are the same) + SET(n, n2); + + // and the penetration depth is the co-normal + // distance between any two vertices A and B, + // i.e. d = DOT(n, (A-B)) + DEPTH(depth, v1[1], v2[1], n); + if (depth < 0) + depth *= -1.0; + + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + ContactPt, n, depth, OutTriCount); + } + } + else { + // Otherwise (in non-co-planar cases), we create a coplanar + // point -- the middle of the line of intersection -- that + // will be used for various computations down the road + for (int j=0; j<3; j++) + CoplanarPt[j] = ( (IsectPt1[j] + IsectPt2[j]) / REAL(2.0) ); + CoplanarPt[3] = 1.0; + + // Find the ELT of the coplanar point + // + dMultiply1(orig_pos, InvMatrix1, CoplanarPt, 4, 4, 1); + dMultiply1(old_pos1, ((dxTriMesh*)g1)->m_last_trans, orig_pos, 4, 4, 1); + SUB(elt1, CoplanarPt, old_pos1); + + dMultiply1(orig_pos, InvMatrix2, CoplanarPt, 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->m_last_trans, orig_pos, 4, 4, 1); + SUB(elt2, CoplanarPt, old_pos2); + + SUB(elt_sum, elt1, elt2); // net motion of the coplanar point + dReal elt_sum_len = LENGTH(elt_sum); // Could be calculated on demand but there is no good place... + + + // Calculate how much the vertices of each face moved in the + // direction of the opposite face's normal + // + dReal total_dp1, total_dp2; + total_dp1 = 0.0; + total_dp2 = 0.0; + + for (int ii=0; ii<3; ii++) { + // find the estimated linear translation (ELT) of the vertices + // on face 1, wrt to the center of face 2. + + // un-transform this vertex by the current transform + dMultiply1(orig_pos, InvMatrix1, v1[ii], 4, 4, 1 ); + + // re-transform this vertex by last_trans (to get its old + // position) + dMultiply1(old_pos1, ((dxTriMesh*)g1)->m_last_trans, orig_pos, 4, 4, 1); + + // Then subtract this position from our current one to find + // the elapsed linear translation (ELT) + for (int k=0; k<3; k++) { + elt_f1[ii][k] = (v1[ii][k] - old_pos1[k]) - elt2[k]; + } + + // Take the dot product of the ELT for each vertex (wrt the + // center of face2) + total_dp1 += dFabs( dCalcVectorDot3(elt_f1[ii], n2) ); + } + + for (int ii=0; ii<3; ii++) { + // find the estimated linear translation (ELT) of the vertices + // on face 2, wrt to the center of face 1. + dMultiply1(orig_pos, InvMatrix2, v2[ii], 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->m_last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + elt_f2[ii][k] = (v2[ii][k] - old_pos2[k]) - elt1[k]; + } + + // Take the dot product of the ELT for each vertex (wrt the + // center of face2) and add them + total_dp2 += dFabs( dCalcVectorDot3(elt_f2[ii], n1) ); + } + + + //////// + // Estimate the penetration depth. + // + dReal dp; + BOOL badPen = true; + dVector3 *pen_v; // the "penetrating vertices" + dVector3 *pen_elt; // the elt_f of the penetrating face + dVector3 *col_v; // the "collision vertices" (the penetrated face) + + SMULT(n2, n2, -1.0); // SF PATCH #1335183 + depth = 0.0; + if ((total_dp1 > DISTANCE_EPSILON) || (total_dp2 > DISTANCE_EPSILON)) { + //////// + // Find the collision normal, by finding the face + // that is pointed "most" in the direction of travel + // of the two triangles + // + if (total_dp2 > total_dp1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } + } + else { + // the total_dp is very small, so let's fall back + // to a different test + if (LENGTH(elt2) > LENGTH(elt1)) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } + } + + + for (int j=0; j<3; j++) { + if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + pen_v[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + if (badPen) { + // try the other normal + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + for (int j=0; j<3; j++) + if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + pen_v[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + + + //////////////////////////////////////// + // + // If we haven't found a good penetration, then we're probably straddling + // the edge of one of the objects, or the penetraing face is big + // enough that all of its vertices are outside the bounds of the + // penetrated face. + // In these cases, we do a more expensive test. We clip the penetrating + // triangle with a solid defined by the penetrated triangle, and repeat + // the tests above on this new polygon + if (badPen) { + + // Switch pen_v and n back again + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + + // Find the three sides (no top or bottom) of the solid defined by + // the edges of the penetrated triangle. + + // The dVector4 "plane" structures contain the following information: + // [0]-[2]: The normal of the face, pointing INWARDS (i.e. + // the inverse normal + // [3]: The distance between the face and the center of the + // solid, along the normal + dVector4 SolidPlanes[3]; + dVector3 tmp1; + dVector3 sn; + + for (int j=0; j<3; j++) { + e1[j] = col_v[1][j] - col_v[0][j]; + e2[j] = col_v[0][j] - col_v[2][j]; + e3[j] = col_v[2][j] - col_v[1][j]; + } + + // side 1 + CROSS(sn, e1, n); + dNormalize3(sn); + SMULT( SolidPlanes[0], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[0][3] = dCalcVectorDot3(tmp1, SolidPlanes[0]); + + + // side 2 + CROSS(sn, e2, n); + dNormalize3(sn); + SMULT( SolidPlanes[1], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[2]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[1][3] = dCalcVectorDot3(tmp1, SolidPlanes[1]); + + + // side 3 + CROSS(sn, e3, n); + dNormalize3(sn); + SMULT( SolidPlanes[2], sn, -1.0 ); + + ADD(tmp1, col_v[2], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[2][3] = dCalcVectorDot3(tmp1, SolidPlanes[2]); + + + FindTriSolidIntrsection(pen_v, SolidPlanes, 3, firstClippedTri); + + for (int j=0; j<firstClippedTri.Count; j++) { + firstClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults + + DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], n); + + // if the penetration depth (calculated above) is more than the contact + // point's ELT, then we've chosen the wrong face and should switch faces + if (pen_v == v1) { + dMultiply1(orig_pos, InvMatrix1, firstClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos1, ((dxTriMesh*)g1)->m_last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; + } + } + else { + dMultiply1(orig_pos, InvMatrix2, firstClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->m_last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; + } + } + + if (dp >= 0.0) { + contact_elt_length = dFabs(dCalcVectorDot3(firstClippedElt[j], n)); + + depth = dp; + if (depth == 0.0) + depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + firstClippedTri.Points[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + } + } + + if (badPen) { + // Switch pen_v and n (again!) + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + + // Find the three sides (no top or bottom) of the solid created by + // the penetrated triangle. + // The dVector4 "plane" structures contain the following information: + // [0]-[2]: The normal of the face, pointing INWARDS (i.e. + // the inverse normal + // [3]: The distance between the face and the center of the + // solid, along the normal + dVector4 SolidPlanes[3]; + dVector3 tmp1; + + dVector3 sn; + for (int j=0; j<3; j++) { + e1[j] = col_v[1][j] - col_v[0][j]; + e2[j] = col_v[0][j] - col_v[2][j]; + e3[j] = col_v[2][j] - col_v[1][j]; + } + + // side 1 + CROSS(sn, e1, n); + dNormalize3(sn); + SMULT( SolidPlanes[0], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[0][3] = dCalcVectorDot3(tmp1, SolidPlanes[0]); + + + // side 2 + CROSS(sn, e2, n); + dNormalize3(sn); + SMULT( SolidPlanes[1], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[2]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[1][3] = dCalcVectorDot3(tmp1, SolidPlanes[1]); + + + // side 3 + CROSS(sn, e3, n); + dNormalize3(sn); + SMULT( SolidPlanes[2], sn, -1.0 ); + + ADD(tmp1, col_v[2], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[2][3] = dCalcVectorDot3(tmp1, SolidPlanes[2]); + + FindTriSolidIntrsection(pen_v, SolidPlanes, 3, secondClippedTri); + + for (int j=0; j<secondClippedTri.Count; j++) { + secondClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults + + DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], n); + + if (pen_v == v1) { + dMultiply1(orig_pos, InvMatrix1, secondClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos1, ((dxTriMesh*)g1)->m_last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; + } + } + else { + dMultiply1(orig_pos, InvMatrix2, secondClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->m_last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; + } + } + + + if (dp >= 0.0) { + contact_elt_length = dFabs(dCalcVectorDot3(secondClippedElt[j],n)); + + depth = dp; + if (depth == 0.0) + depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + secondClippedTri.Points[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + + } + } + + + + ///////////////// + // All conventional tests have failed at this point, so now we deal with + // cases on a more "heuristic" basis + // + + if (badPen) { + // Switch pen_v and n (for the fourth time, so they're + // what my original guess said they were) + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + if (dFabs(dCalcVectorDot3(n1, n2)) < REAL(0.01)) { + // If we reach this point, we have (close to) perpindicular + // faces, either resting on each other or sliding in a + // direction orthogonal to both surface normals. + if (elt_sum_len < DISTANCE_EPSILON) { + depth = dFabs(dCalcVectorDot3(n, elt_sum)); + + if (depth > REAL(1e-12)) { + dNormalize3(n); + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + CoplanarPt, n, depth, OutTriCount); + badPen = false; + } + else { + // If the two faces are (nearly) perfectly at rest with + // respect to each other, then we ignore the contact, + // allowing the objects to slip a little in the hopes + // that next frame, they'll give us something to work + // with. + badPen = false; + } + } + else { + // The faces are perpindicular, but moving significantly + // This can be sliding, or an unusual edge-straddling + // penetration. + dVector3 cn; + + CROSS(cn, n1, n2); + dNormalize3(cn); + SET(n, cn); + + // The shallowest ineterpenetration of the faces + // is the depth + dVector3 ContactPt; + dVector3 dvTmp; + dReal rTmp; + depth = dInfinity; + for (int j=0; j<3; j++) { + for (int k=0; k<3; k++) { + SUB(dvTmp, col_v[k], pen_v[j]); + + rTmp = dCalcVectorDot3(dvTmp, n); + if ( dFabs(rTmp) < dFabs(depth) ) { + depth = rTmp; + SET( ContactPt, pen_v[j] ); + contact_elt_length = dFabs(dCalcVectorDot3(pen_elt[j], n)); + } + } + } + if (depth < 0.0) { + SMULT(n, n, -1.0); + depth *= -1.0; + } + + if ((depth > 0.0) && (depth <= contact_elt_length)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + ContactPt, n, depth, OutTriCount); + badPen = false; + } + + } + } + } + + + if (badPen && elt_sum_len != 0.0) { + // Use as the normal the direction of travel, rather than any particular + // face normal + // + dVector3 esn; + + if (pen_v == v1) { + SMULT(esn, elt_sum, -1.0); + } + else { + SET(esn, elt_sum); + } + dNormalize3(esn); + + + // The shallowest ineterpenetration of the faces + // is the depth + dVector3 ContactPt; + depth = dInfinity; + for (int j=0; j<3; j++) { + for (int k=0; k<3; k++) { + DEPTH(dp, col_v[k], pen_v[j], esn); + if ( (ExamineContactPoint(col_v, esn, pen_v[j])) && + ( dFabs(dp) < dFabs(depth)) ) { + depth = dp; + SET( ContactPt, pen_v[j] ); + contact_elt_length = dFabs(dCalcVectorDot3(pen_elt[j], esn)); + } + } + } + + if ((depth > 0.0) && (depth <= contact_elt_length)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + ContactPt, esn, depth, OutTriCount); + badPen = false; + } + } + + + if (badPen && elt_sum_len != 0.0) { + // If the direction of motion is perpindicular to both normals + if ( (dFabs(dCalcVectorDot3(n1, elt_sum)) < REAL(0.01)) && (dFabs(dCalcVectorDot3(n2, elt_sum)) < REAL(0.01)) ) { + dVector3 esn; + if (pen_v == v1) { + SMULT(esn, elt_sum, -1.0); + } + else { + SET(esn, elt_sum); + } + + dNormalize3(esn); + + + // Look at the clipped points again, checking them against this + // new normal + for (int j=0; j<firstClippedTri.Count; j++) { + DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], esn); + + if (dp >= 0.0) { + contact_elt_length = dFabs(dCalcVectorDot3(firstClippedElt[j], esn)); + + depth = dp; + //if (depth == 0.0) + //depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + firstClippedTri.Points[j], esn, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + } + + if (badPen) { + // If this test failed, try it with the second set of clipped faces + for (int j=0; j<secondClippedTri.Count; j++) { + DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], esn); + + if (dp >= 0.0) { + contact_elt_length = dFabs(dCalcVectorDot3(secondClippedElt[j], esn)); + + depth = dp; + //if (depth == 0.0) + //depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + secondClippedTri.Points[j], esn, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + } + } + } + } + + + + if (badPen) { + // if we have very little motion, we're dealing with resting contact + // and shouldn't reference the ELTs at all + // + if (elt_sum_len < VELOCITY_EPSILON) { + + // instead of a "contact_elt_length" threshhold, we'll use an + // arbitrary, small one + for (int j=0; j<3; j++) { + DEPTH(dp, CoplanarPt, pen_v[j], n); + + if (dp == 0.0) + dp = TINY_PENETRATION; + + if ( (dp > 0.0) && (dp <= SMALL_ELT)) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + pen_v[j], n, dp, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + + if (badPen) { + // try the other normal + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + for (int j=0; j<3; j++) { + DEPTH(dp, CoplanarPt, pen_v[j], n); + + if (dp == 0.0) + dp = TINY_PENETRATION; + + if ( (dp > 0.0) && (dp <= SMALL_ELT)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + pen_v[j], n, dp, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + } + + + + } + } + + if (badPen) { + // find the nearest existing contact, and replicate it's + // normal and depth + // + dContactGeom* Contact; + dVector3 pos_diff; + dReal min_dist, dist; + + min_dist = dInfinity; + depth = 0.0; + for (int j=0; j<OutTriCount; j++) { + Contact = SAFECONTACT(Flags, Contacts, j, Stride); + + SUB(pos_diff, Contact->pos, CoplanarPt); + + dist = dCalcVectorDot3(pos_diff, pos_diff); + if (dist < min_dist) { + min_dist = dist; + depth = Contact->depth; + SMULT(ContactNormal, Contact->normal, -1.0); + } + } + + if (depth > 0.0) { + // Add a tiny contact at the coplanar point + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + CoplanarPt, ContactNormal, depth, OutTriCount); + badPen = false; + } + } + + + if (badPen) { + // Add a tiny contact at the coplanar point + if (-dCalcVectorDot3(elt_sum, n1) > -dCalcVectorDot3(elt_sum, n2)) { + SET(ContactNormal, n1); + } + else { + SET(ContactNormal, n2); + } + + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2, + CoplanarPt, ContactNormal, TINY_PENETRATION, OutTriCount); + badPen = false; + } + + + } // not coplanar (main loop) + } // TriTriIntersectWithIsectLine + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + + // Free memory + delete[] firstClippedElt; + delete[] secondClippedElt; + + // Return the number of contacts + return OutTriCount; + } + } + } + + + // There was some kind of failure during the Collide call or + // there are no faces overlapping + return 0; +} + + +/* -- not used +static void +GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data) +{ +dVector3 Out[3]; + +FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out); + +for (int i = 0; i < 3; i++) +triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]); +} +*/ + +// +// +// +#define B11 B[0] +#define B12 B[1] +#define B13 B[2] +#define B14 B[3] +#define B21 B[4] +#define B22 B[5] +#define B23 B[6] +#define B24 B[7] +#define B31 B[8] +#define B32 B[9] +#define B33 B[10] +#define B34 B[11] +#define B41 B[12] +#define B42 B[13] +#define B43 B[14] +#define B44 B[15] + +#define Binv11 Binv[0] +#define Binv12 Binv[1] +#define Binv13 Binv[2] +#define Binv14 Binv[3] +#define Binv21 Binv[4] +#define Binv22 Binv[5] +#define Binv23 Binv[6] +#define Binv24 Binv[7] +#define Binv31 Binv[8] +#define Binv32 Binv[9] +#define Binv33 Binv[10] +#define Binv34 Binv[11] +#define Binv41 Binv[12] +#define Binv42 Binv[13] +#define Binv43 Binv[14] +#define Binv44 Binv[15] + +inline void +dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B) +{ + B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0]; + B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1]; + B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2]; + + B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0; +} + + +static void +dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ) +{ + dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43) + -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42) + +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42) + +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41) + -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41) + +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41); + + dAASSERT (det != 0.0); + + det = 1.0 / det; + + Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32))); + Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12))); + Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22))); + Binv14 = 0.0f; + Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33))); + Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13))); + Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23))); + Binv24 = 0.0f; + Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31))); + Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11))); + Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21))); + Binv34 = 0.0f; + Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42))); + Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42))); + Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22))); + Binv44 = 1.0f; +} + + + +///////////////////////////////////////////////// +// +// Triangle/Triangle intersection utilities +// +// From the article "A Fast Triangle-Triangle Intersection Test", +// Journal of Graphics Tools, 2(2), 1997 +// +// Some of this functionality is duplicated in OPCODE (see +// OPC_TriTriOverlap.h) but we have replicated it here so we don't +// have to mess with the internals of OPCODE, as well as so we can +// further optimize some of the functions. +// +// This version computes the line of intersection as well (if they +// are not coplanar): +// int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], +// dReal U0[3],dReal U1[3],dReal U2[3], +// int *coplanar, +// dReal isectpt1[3],dReal isectpt2[3]); +// +// parameters: vertices of triangle 1: V0,V1,V2 +// vertices of triangle 2: U0,U1,U2 +// +// result : returns 1 if the triangles intersect, otherwise 0 +// "coplanar" returns whether the tris are coplanar +// isectpt1, isectpt2 are the endpoints of the line of +// intersection +// + + + +/* if USE_EPSILON_TEST is true then we do a check: + if |dv|<EPSILON then dv=0.0; + else no check is done (which is less robust) +*/ +#define USE_EPSILON_TEST TRUE +#define EPSILON REAL(0.000001) + + +/* sort so that a<=b */ +#define SORT(a,b) \ + if(a>b) \ + { \ + dReal c; \ + c=a; \ + a=b; \ + b=c; \ + } + +#define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \ + isect0=VV0+(VV1-VV0)*D0/(D0-D1); \ + isect1=VV0+(VV2-VV0)*D0/(D0-D2); + + +#define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \ + if(D0D1>0.0f) \ + { \ + /* here we know that D0D2<=0.0 */ \ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ + ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ + } \ + else if(D0D2>0.0f) \ + { \ + /* here we know that d0d1<=0.0 */ \ + ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \ + } \ + else if(D1!=0.0f) \ + { \ + ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ + } \ + else if(D2!=0.0f) \ + { \ + ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } + + + +/* this edge to edge test is based on Franlin Antonio's gem: +"Faster Line Segment Intersection", in Graphics Gems III, +pp. 199-202 */ +#define EDGE_EDGE_TEST(V0,U0,U1) \ + Bx=U0[i0]-U1[i0]; \ + By=U0[i1]-U1[i1]; \ + Cx=V0[i0]-U0[i0]; \ + Cy=V0[i1]-U0[i1]; \ + f=Ay*Bx-Ax*By; \ + d=By*Cx-Bx*Cy; \ + if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \ + { \ + e=Ax*Cy-Ay*Cx; \ + if(f>0) \ + { \ + if(e>=0 && e<=f) return 1; \ + } \ + else \ + { \ + if(e<=0 && e>=f) return 1; \ + } \ +} + +#define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \ +{ \ + dReal Ax,Ay,Bx,By,Cx,Cy,e,d,f; \ + Ax=V1[i0]-V0[i0]; \ + Ay=V1[i1]-V0[i1]; \ + /* test edge U0,U1 against V0,V1 */ \ + EDGE_EDGE_TEST(V0,U0,U1); \ + /* test edge U1,U2 against V0,V1 */ \ + EDGE_EDGE_TEST(V0,U1,U2); \ + /* test edge U2,U1 against V0,V1 */ \ + EDGE_EDGE_TEST(V0,U2,U0); \ +} + +#define POINT_IN_TRI(V0,U0,U1,U2) \ +{ \ + dReal a,b,c,d0,d1,d2; \ + /* is T1 completly inside T2? */ \ + /* check if V0 is inside tri(U0,U1,U2) */ \ + a=U1[i1]-U0[i1]; \ + b=-(U1[i0]-U0[i0]); \ + c=-a*U0[i0]-b*U0[i1]; \ + d0=a*V0[i0]+b*V0[i1]+c; \ + \ + a=U2[i1]-U1[i1]; \ + b=-(U2[i0]-U1[i0]); \ + c=-a*U1[i0]-b*U1[i1]; \ + d1=a*V0[i0]+b*V0[i1]+c; \ + \ + a=U0[i1]-U2[i1]; \ + b=-(U0[i0]-U2[i0]); \ + c=-a*U2[i0]-b*U2[i1]; \ + d2=a*V0[i0]+b*V0[i1]+c; \ + if(d0*d1>0.0) \ + { \ + if(d0*d2>0.0) return 1; \ + } \ +} + +int coplanar_tri_tri(dReal N[3],dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3]) +{ + dReal A[3]; + short i0,i1; + /* first project onto an axis-aligned plane, that maximizes the area */ + /* of the triangles, compute indices: i0,i1. */ + A[0]= dFabs(N[0]); + A[1]= dFabs(N[1]); + A[2]= dFabs(N[2]); + if(A[0]>A[1]) + { + if(A[0]>A[2]) + { + i0=1; /* A[0] is greatest */ + i1=2; + } + else + { + i0=0; /* A[2] is greatest */ + i1=1; + } + } + else /* A[0]<=A[1] */ + { + if(A[2]>A[1]) + { + i0=0; /* A[2] is greatest */ + i1=1; + } + else + { + i0=0; /* A[1] is greatest */ + i1=2; + } + } + + /* test all edges of triangle 1 against the edges of triangle 2 */ + EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2); + EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2); + EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2); + + /* finally, test if tri1 is totally contained in tri2 or vice versa */ + POINT_IN_TRI(V0,U0,U1,U2); + POINT_IN_TRI(U0,V0,V1,V2); + + return 0; +} + + + +#define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \ +{ \ + if(D0D1>0.0f) \ + { \ + /* here we know that D0D2<=0.0 */ \ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ + A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \ + } \ + else if(D0D2>0.0f)\ + { \ + /* here we know that d0d1<=0.0 */ \ + A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \ + } \ + else if(D1!=0.0f) \ + { \ + A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \ + } \ + else if(D2!=0.0f) \ + { \ + A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } \ +} + + + + +/* sort so that a<=b */ +#define SORT2(a,b,smallest) \ + if(a>b) \ + { \ + dReal c; \ + c=a; \ + a=b; \ + b=c; \ + smallest=1; \ + } \ + else smallest=0; + + +inline void isect2(dReal VTX0[3],dReal VTX1[3],dReal VTX2[3],dReal VV0,dReal VV1,dReal VV2, + dReal D0,dReal D1,dReal D2,dReal *isect0,dReal *isect1,dReal isectpoint0[3],dReal isectpoint1[3]) +{ + dReal tmp=D0/(D0-D1); + dReal diff[3]; + *isect0=VV0+(VV1-VV0)*tmp; + SUB(diff,VTX1,VTX0); + MULT(diff,diff,tmp); + ADD(isectpoint0,diff,VTX0); + tmp=D0/(D0-D2); + *isect1=VV0+(VV2-VV0)*tmp; + SUB(diff,VTX2,VTX0); + MULT(diff,diff,tmp); + ADD(isectpoint1,VTX0,diff); +} + + +#if 0 +#define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \ + tmp=D0/(D0-D1); \ + isect0=VV0+(VV1-VV0)*tmp; \ + SUB(diff,VTX1,VTX0); \ + MULT(diff,diff,tmp); \ + ADD(isectpoint0,diff,VTX0); \ + tmp=D0/(D0-D2); + /*isect1=VV0+(VV2-VV0)*tmp; \ */ + /*SUB(diff,VTX2,VTX0); \ */ + /*MULT(diff,diff,tmp); \ */ + /*ADD(isectpoint1,VTX0,diff); */ +#endif + +inline int compute_intervals_isectline(dReal VERT0[3],dReal VERT1[3],dReal VERT2[3], + dReal VV0,dReal VV1,dReal VV2,dReal D0,dReal D1,dReal D2, + dReal D0D1,dReal D0D2,dReal *isect0,dReal *isect1, + dReal isectpoint0[3],dReal isectpoint1[3]) +{ + if(D0D1>0.0f) + { + /* here we know that D0D2<=0.0 */ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D0D2>0.0f) + { + /* here we know that d0d1<=0.0 */ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D1*D2>0.0f || D0!=0.0f) + { + /* here we know that d0d1<=0.0 or that D0!=0.0 */ + isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D1!=0.0f) + { + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D2!=0.0f) + { + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); + } + else + { + /* triangles are coplanar */ + return 1; + } + return 0; +} + +#define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \ + if(D0D1>0.0f) \ + { \ + /* here we know that D0D2<=0.0 */ \ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ + } +#if 0 + else if(D0D2>0.0f) \ + { \ + /* here we know that d0d1<=0.0 */ \ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D1!=0.0f) \ + { \ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D2!=0.0f) \ + { \ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + coplanar=1; \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } +#endif + + + +static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, + dReal isectpt1[3],dReal isectpt2[3]) +{ + dReal E1[3],E2[3]; + dReal N1[3],N2[3],d1,d2; + dReal du0,du1,du2,dv0,dv1,dv2; + dReal D[3]; + dReal isect1[2]={0,0}, isect2[2]={0,0}; + dReal isectpointA1[3],isectpointA2[3]; + dReal isectpointB1[3]={0,0,0},isectpointB2[3]={0,0,0}; + dReal du0du1,du0du2,dv0dv1,dv0dv2; + short index; + dReal vp0,vp1,vp2; + dReal up0,up1,up2; + dReal b,c,max; + int smallest1,smallest2; + + /* compute plane equation of triangle(V0,V1,V2) */ + SUB(E1,V1,V0); + SUB(E2,V2,V0); + CROSS(N1,E1,E2); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + // + // Oleh_Derevenko: + // I'm not quite sure if this routine will fail/assert for zero normal + // (it's too large and complex to be fully analyzed). + // However in such a large code block three extra float comparisons + // will not have any noticeable influence on performance. + if (IS_ZERO(N1)) + return 0; + + d1=-DOT(N1,V0); + /* plane equation 1: N1.X+d1=0 */ + + /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/ + du0=DOT(N1,U0)+d1; + du1=DOT(N1,U1)+d1; + du2=DOT(N1,U2)+d1; + + /* coplanarity robustness check */ +#if USE_EPSILON_TEST==TRUE + if(dFabs(du0)<EPSILON) du0=0.0; + if(dFabs(du1)<EPSILON) du1=0.0; + if(dFabs(du2)<EPSILON) du2=0.0; +#endif + du0du1=du0*du1; + du0du2=du0*du2; + + if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */ + return 0; /* no intersection occurs */ + + /* compute plane of triangle (U0,U1,U2) */ + SUB(E1,U1,U0); + SUB(E2,U2,U0); + CROSS(N2,E1,E2); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + // + // Oleh_Derevenko: + // I'm not quite sure if this routine will fail/assert for zero normal + // (it's too large and complex to be fully analyzed). + // However in such a large code block three extra float comparisons + // will not have any noticeable influence on performance. + if (IS_ZERO(N2)) + return 0; + + d2=-DOT(N2,U0); + /* plane equation 2: N2.X+d2=0 */ + + /* put V0,V1,V2 into plane equation 2 */ + dv0=DOT(N2,V0)+d2; + dv1=DOT(N2,V1)+d2; + dv2=DOT(N2,V2)+d2; + +#if USE_EPSILON_TEST==TRUE + if(dFabs(dv0)<EPSILON) dv0=0.0; + if(dFabs(dv1)<EPSILON) dv1=0.0; + if(dFabs(dv2)<EPSILON) dv2=0.0; +#endif + + dv0dv1=dv0*dv1; + dv0dv2=dv0*dv2; + + if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */ + return 0; /* no intersection occurs */ + + /* compute direction of intersection line */ + CROSS(D,N1,N2); + + /* compute and index to the largest component of D */ + max= dFabs(D[0]); + index=0; + b= dFabs(D[1]); + c= dFabs(D[2]); + if(b>max) max=b,index=1; + if(c>max) max=c,index=2; + + /* this is the simplified projection onto L*/ + vp0=V0[index]; + vp1=V1[index]; + vp2=V2[index]; + + up0=U0[index]; + up1=U1[index]; + up2=U2[index]; + + /* compute interval for triangle 1 */ + *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2, + dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2); + if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); + + + /* compute interval for triangle 2 */ + compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2, + du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2); + + SORT2(isect1[0],isect1[1],smallest1); + SORT2(isect2[0],isect2[1],smallest2); + + if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0; + + /* at this point, we know that the triangles intersect */ + + if(isect2[0]<isect1[0]) + { + if(smallest1==0) { SET(isectpt1,isectpointA1); } + else { SET(isectpt1,isectpointA2); } + + if(isect2[1]<isect1[1]) + { + if(smallest2==0) { SET(isectpt2,isectpointB2); } + else { SET(isectpt2,isectpointB1); } + } + else + { + if(smallest1==0) { SET(isectpt2,isectpointA2); } + else { SET(isectpt2,isectpointA1); } + } + } + else + { + if(smallest2==0) { SET(isectpt1,isectpointB1); } + else { SET(isectpt1,isectpointB2); } + + if(isect2[1]>isect1[1]) + { + if(smallest1==0) { SET(isectpt2,isectpointA2); } + else { SET(isectpt2,isectpointA1); } + } + else + { + if(smallest2==0) { SET(isectpt2,isectpointB2); } + else { SET(isectpt2,isectpointB1); } + } + } + return 1; +} + + + + + +// Find the intersectiojn point between a coplanar line segement, +// defined by X1 and X2, and a ray defined by X3 and direction N. +// +// This forumla for this calculation is: +// (c x b) . (a x b) +// Q = x1 + a ------------------- +// | a x b | ^2 +// +// where a = x2 - x1 +// b = x4 - x3 +// c = x3 - x1 +// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt +// and x4 is (CoplanarPt - n) +#if 0 // not used anywhere +static int + IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n, + dVector3 out_pt) +{ + dVector3 a, b, c, x4; + + ADD(x4, x3, n); // x4 = x3 + n + + SUB(a, x2, x1); // a = x2 - x1 + SUB(b, x4, x3); + SUB(c, x3, x1); + + dVector3 tmp1, tmp2; + CROSS(tmp1, c, b); + CROSS(tmp2, a, b); + + dReal num, denom; + num = dCalcVectorDot3(tmp1, tmp2); + denom = LENGTH( tmp2 ); + + dReal s; + s = num /(denom*denom); + + for (int i=0; i<3; i++) + out_pt[i] = x1[i] + a[i]*s; + + // Test if this intersection is "behind" x3, w.r.t. n + SUB(a, x3, out_pt); + if (dCalcVectorDot3(a, n) > 0.0) + return 0; + + // Test if this intersection point is outside the edge limits, + // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside + // else outside + SUB(a, out_pt, x1); + SUB(b, out_pt, x2); + if (dCalcVectorDot3(a,b) < 0.0) + return 1; + else + return 0; +} +#endif + +// FindTriSolidIntersection - Clips the input trinagle TRI with the +// sides of a convex bounding solid, described by PLANES, returning +// the (convex) clipped polygon in CLIPPEDPOLYGON +// +static bool + FindTriSolidIntrsection(const dVector3 Tri[3], + const dVector4 Planes[6], int numSides, + LineContactSet& ClippedPolygon ) +{ + // Set up the LineContactSet structure + for (int k=0; k<3; k++) { + SET(ClippedPolygon.Points[k], Tri[k]); + } + ClippedPolygon.Count = 3; + + // Clip wrt the sides + for ( int i = 0; i < numSides; i++ ) + ClipConvexPolygonAgainstPlane( Planes[i], Planes[i][3], ClippedPolygon ); + + return (ClippedPolygon.Count > 0); +} + + + + +// ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by +// CONTACTS, with a plane (described by N and C). Note: the input +// vertices are assumed to be in counterclockwise order. +// +// This code is taken from The Nebula Device: +// http://nebuladevice.sourceforge.net/cgi-bin/twiki/view/Nebula/WebHome +// and is licensed under the following license: +// http://nebuladevice.sourceforge.net/doc/source/license.txt +// +static void ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C, LineContactSet& Contacts ) +{ + // test on which side of line are the vertices + int Positive = 0, Negative = 0, PIndex = -1; + int Quantity = Contacts.Count; + + dReal Test[8]; + for ( int i = 0; i < Contacts.Count; i++ ) { + // An epsilon is used here because it is possible for the dot product + // and C to be exactly equal to each other (in theory), but differ + // slightly because of floating point problems. Thus, add a little + // to the test number to push actually equal numbers over the edge + // towards the positive. This should probably be somehow a relative + // tolerance, and I don't think multiplying by the constant is the best + // way to do this. + Test[i] = dCalcVectorDot3(N, Contacts.Points[i]) - C + dFabs(C)*REAL(1e-08); + + if (Test[i] >= REAL(0.0)) { + Positive++; + if (PIndex < 0) { + PIndex = i; + } + } + else Negative++; + } + + if (Positive > 0) { + if (Negative > 0) { + // plane transversely intersects polygon + dVector3 CV[8]; + int CQuantity = 0, Cur, Prv; + dReal T; + + if (PIndex > 0) { + // first clip vertex on line + Cur = PIndex; + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // vertices on positive side of line + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + + // last clip vertex on line + if (Cur < Quantity) { + Prv = Cur - 1; + } + else { + Cur = 0; + Prv = Quantity - 1; + } + + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + } + else { + // iPIndex is 0 + // vertices on positive side of line + Cur = 0; + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + + // last clip vertex on line + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // skip vertices on negative side + while (Cur < Quantity && Test[Cur] < REAL(0.0)) { + Cur++; + } + + // first clip vertex on line + if (Cur < Quantity) { + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // vertices on positive side of line + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + } + else { + // iCur = 0 + Prv = Quantity - 1; + T = Test[0] / (Test[0] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[0][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[0][0]); + CV[CQuantity][1] = Contacts.Points[0][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[0][1]); + CV[CQuantity][2] = Contacts.Points[0][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[0][2]); + CV[CQuantity][3] = Contacts.Points[0][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[0][3]); + CQuantity++; + } + } + Quantity = CQuantity; + memcpy( Contacts.Points, CV, CQuantity * sizeof(dVector3) ); + } + // else polygon fully on positive side of plane, nothing to do + Contacts.Count = Quantity; + } + else { + Contacts.Count = 0; // This should not happen, but for safety + } + +} + + + +// Determine if a potential collision point is +// +// +static int +ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point) +{ + // Cast a ray from in_point, along the collison normal. Does it intersect the + // collision face. + dReal t, u, v; + + if (!RayTriangleIntersect(in_point, in_n, v_col[0], v_col[1], v_col[2], + &t, &u, &v)) + return 0; + else + return 1; +} + + + +// RayTriangleIntersect - If an intersection is found, t contains the +// distance along the ray (dir) and u/v contain u/v coordinates into +// the triangle. Returns 0 if no hit is found +// From "Real-Time Rendering," page 305 +// +static int +RayTriangleIntersect(const dVector3 orig, const dVector3 dir, + const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, + dReal *t,dReal *u,dReal *v) + +{ + dReal edge1[3], edge2[3], tvec[3], pvec[3], qvec[3]; + dReal det,inv_det; + + // find vectors for two edges sharing vert0 + SUB(edge1, vert1, vert0); + SUB(edge2, vert2, vert0); + + // begin calculating determinant - also used to calculate U parameter + CROSS(pvec, dir, edge2); + + // if determinant is near zero, ray lies in plane of triangle + det = DOT(edge1, pvec); + + if ((det > REAL(-0.001)) && (det < REAL(0.001))) + return 0; + inv_det = 1.0 / det; + + // calculate distance from vert0 to ray origin + SUB(tvec, orig, vert0); + + // calculate U parameter and test bounds + *u = DOT(tvec, pvec) * inv_det; + if ((*u < 0.0) || (*u > 1.0)) + return 0; + + // prepare to test V parameter + CROSS(qvec, tvec, edge1); + + // calculate V parameter and test bounds + *v = DOT(dir, qvec) * inv_det; + if ((*v < 0.0) || ((*u + *v) > 1.0)) + return 0; + + // calculate t, ray intersects triangle + *t = DOT(edge2, qvec) * inv_det; + + return 1; +} + + + +static bool +SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, + dVector3 in_n, dVector3* in_col_v, dReal &out_depth) +{ + dReal dp = 0.0; + dReal contact_elt_length; + + DEPTH(dp, in_CoplanarPt, in_v, in_n); + + if (dp >= 0.0) { + // if the penetration depth (calculated above) is more than + // the contact point's ELT, then we've chosen the wrong face + // and should switch faces + contact_elt_length = dFabs(dCalcVectorDot3(in_elt, in_n)); + + if (dp == 0.0) + dp = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (dp < EXPANDED_ELT_THRESH)) + dp = contact_elt_length; + + if ( (dp > 0.0) && (dp <= contact_elt_length)) { + // Add a contact + + if ( ExamineContactPoint(in_col_v, in_n, in_v) ) { + out_depth = dp; + return true; + } + } + } + + return false; +} + + + + +// Generate a "unique" contact. A unique contact has a unique +// position or normal. If the potential contact has the same +// position and normal as an existing contact, but a larger +// penetration depth, this new depth is used instead +// +static void +GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, + dxTriMesh* in_TriMesh1, dxTriMesh* in_TriMesh2, + int TriIndex1, int TriIndex2, + const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, + int& OutTriCount) +{ + /* + NOTE by Oleh_Derevenko: + This function is called after maximal number of contacts has already been + collected because it has a side effect of replacing penetration depth of + existing contact with larger penetration depth of another matching normal contact. + If this logic is not necessary any more, you can bail out on reach of contact + number maximum immediately in dCollideTTL(). You will also need to correct + conditional statements after invocations of GenerateContact() in dCollideTTL(). + */ + dIASSERT(in_Depth >= 0.0); + //if (in_Depth < 0.0) -- the function is always called with depth >= 0 + // return; + + do + { + dContactGeom* Contact; + dVector3 diff; + + if (!(in_Flags & CONTACTS_UNIMPORTANT)) + { + bool duplicate = false; + + for (int i=0; i<OutTriCount; i++) + { + Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride); + + // same position? + SUB(diff, in_ContactPos, Contact->pos); + if (dCalcVectorDot3(diff, diff) < dEpsilon) + { + // same normal? + if (REAL(1.0) - dFabs(dCalcVectorDot3(in_Normal, Contact->normal)) < dEpsilon) + { + if (in_Depth > Contact->depth) { + Contact->depth = in_Depth; + SMULT( Contact->normal, in_Normal, -1.0); + Contact->normal[3] = 0.0; + } + duplicate = true; + /* + NOTE by Oleh_Derevenko: + There may be a case when two normals are close to each other but no duplicate + while third normal is detected to be duplicate for both of them. + This is the only reason I can think of, there is no "break" statement. + Perhaps author considered it to be logical that the third normal would + replace the depth in both of initial contacts. + However, I consider it a questionable practice which should not + be applied without deep understanding of underlaying physics. + Even more, is this situation with close normal triplet acceptable at all? + Should not be two initial contacts reduced to one (replaced with the latter)? + If you know the answers for these questions, you may want to change this code. + See the same statement in GenerateContact() of collision_trimesh_box.cpp + */ + } + } + } + + if (duplicate || OutTriCount == (in_Flags & NUMC_MASK)) + { + break; + } + } + else + { + dIASSERT(OutTriCount < (in_Flags & NUMC_MASK)); + } + + // Add a new contact + Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride); + + SET( Contact->pos, in_ContactPos ); + Contact->pos[3] = 0.0; + + SMULT( Contact->normal, in_Normal, -1.0); + Contact->normal[3] = 0.0; + + Contact->depth = in_Depth; + + Contact->g1 = in_TriMesh1; + Contact->g2 = in_TriMesh2; + + Contact->side1 = TriIndex1; + Contact->side2 = TriIndex2; + + OutTriCount++; + } + while (false); +} + + +#endif // dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + + +#endif // dTRIMESH_OPCODE + + +#endif // dTRIMESH_ENABLED diff --git a/libs/ode-0.16.1/ode/src/collision_util.cpp b/libs/ode-0.16.1/ode/src/collision_util.cpp new file mode 100644 index 0000000..39ac87e --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_util.cpp @@ -0,0 +1,613 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +some useful collision utility stuff. this includes some API utility +functions that are defined in the public header files. + +*/ + +#include <ode/common.h> +#include <ode/collision.h> +#include "config.h" +#include "odemath.h" +#include "collision_util.h" + +//**************************************************************************** + +int dCollideSpheres (dVector3 p1, dReal r1, + dVector3 p2, dReal r2, dContactGeom *c) +{ + // printf ("d=%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n", + // d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2); + + dReal d = dCalcPointsDistance3(p1,p2); + if (d > (r1 + r2)) return 0; + if (d <= 0) { + c->pos[0] = p1[0]; + c->pos[1] = p1[1]; + c->pos[2] = p1[2]; + c->normal[0] = 1; + c->normal[1] = 0; + c->normal[2] = 0; + c->depth = r1 + r2; + } + else { + dReal d1 = dRecip (d); + c->normal[0] = (p1[0]-p2[0])*d1; + c->normal[1] = (p1[1]-p2[1])*d1; + c->normal[2] = (p1[2]-p2[2])*d1; + dReal k = REAL(0.5) * (r2 - r1 - d); + c->pos[0] = p1[0] + c->normal[0]*k; + c->pos[1] = p1[1] + c->normal[1]*k; + c->pos[2] = p1[2] + c->normal[2]*k; + c->depth = r1 + r2 - d; + } + return 1; +} + + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta) +{ + dVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + dReal uaub = dCalcVectorDot3(ua,ub); + dReal q1 = dCalcVectorDot3(ua,p); + dReal q2 = -dCalcVectorDot3(ub,p); + dReal d = 1-uaub*uaub; + if (d <= REAL(0.0001)) { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else { + d = dRecip(d); + *alpha = (q1 + uaub*q2)*d; + *beta = (uaub*q1 + q2)*d; + } +} + + +// given two line segments A and B with endpoints a1-a2 and b1-b2, return the +// points on A and B that are closest to each other (in cp1 and cp2). +// in the case of parallel lines where there are multiple solutions, a +// solution involving the endpoint of at least one line will be returned. +// this will work correctly for zero length lines, e.g. if a1==a2 and/or +// b1==b2. +// +// the algorithm works by applying the voronoi clipping rule to the features +// of the line segments. the three features of each line segment are the two +// endpoints and the line between them. the voronoi clipping rule states that, +// for feature X on line A and feature Y on line B, the closest points PA and +// PB between X and Y are globally the closest points if PA is in V(Y) and +// PB is in V(X), where V(X) is the voronoi region of X. + +void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2, + const dVector3 b1, const dVector3 b2, + dVector3 cp1, dVector3 cp2) +{ + dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n; + dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det; + +#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2]; +#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; + + // check vertex-vertex features + + SET3 (a1a2,a2,-,a1); + SET3 (b1b2,b2,-,b1); + SET3 (a1b1,b1,-,a1); + da1 = dCalcVectorDot3(a1a2,a1b1); + db1 = dCalcVectorDot3(b1b2,a1b1); + if (da1 <= 0 && db1 >= 0) { + SET2 (cp1,a1); + SET2 (cp2,b1); + return; + } + + SET3 (a1b2,b2,-,a1); + da2 = dCalcVectorDot3(a1a2,a1b2); + db2 = dCalcVectorDot3(b1b2,a1b2); + if (da2 <= 0 && db2 <= 0) { + SET2 (cp1,a1); + SET2 (cp2,b2); + return; + } + + SET3 (a2b1,b1,-,a2); + da3 = dCalcVectorDot3(a1a2,a2b1); + db3 = dCalcVectorDot3(b1b2,a2b1); + if (da3 >= 0 && db3 >= 0) { + SET2 (cp1,a2); + SET2 (cp2,b1); + return; + } + + SET3 (a2b2,b2,-,a2); + da4 = dCalcVectorDot3(a1a2,a2b2); + db4 = dCalcVectorDot3(b1b2,a2b2); + if (da4 >= 0 && db4 <= 0) { + SET2 (cp1,a2); + SET2 (cp2,b2); + return; + } + + // check edge-vertex features. + // if one or both of the lines has zero length, we will never get to here, + // so we do not have to worry about the following divisions by zero. + + la = dCalcVectorDot3(a1a2,a1a2); + if (da1 >= 0 && da3 <= 0) { + k = da1 / la; + SET3 (n,a1b1,-,k*a1a2); + if (dCalcVectorDot3(b1b2,n) >= 0) { + SET3 (cp1,a1,+,k*a1a2); + SET2 (cp2,b1); + return; + } + } + + if (da2 >= 0 && da4 <= 0) { + k = da2 / la; + SET3 (n,a1b2,-,k*a1a2); + if (dCalcVectorDot3(b1b2,n) <= 0) { + SET3 (cp1,a1,+,k*a1a2); + SET2 (cp2,b2); + return; + } + } + + lb = dCalcVectorDot3(b1b2,b1b2); + if (db1 <= 0 && db2 >= 0) { + k = -db1 / lb; + SET3 (n,-a1b1,-,k*b1b2); + if (dCalcVectorDot3(a1a2,n) >= 0) { + SET2 (cp1,a1); + SET3 (cp2,b1,+,k*b1b2); + return; + } + } + + if (db3 <= 0 && db4 >= 0) { + k = -db3 / lb; + SET3 (n,-a2b1,-,k*b1b2); + if (dCalcVectorDot3(a1a2,n) <= 0) { + SET2 (cp1,a2); + SET3 (cp2,b1,+,k*b1b2); + return; + } + } + + // it must be edge-edge + + k = dCalcVectorDot3(a1a2,b1b2); + det = la*lb - k*k; + if (det <= 0) { + // this should never happen, but just in case... + SET2(cp1,a1); + SET2(cp2,b1); + return; + } + det = dRecip (det); + dReal alpha = (lb*da1 - k*db1) * det; + dReal beta = ( k*da1 - la*db1) * det; + SET3 (cp1,a1,+,alpha*a1a2); + SET3 (cp2,b1,+,beta*b1b2); + +# undef SET2 +# undef SET3 +} + + +// a simple root finding algorithm is used to find the value of 't' that +// satisfies: +// d|D(t)|^2/dt = 0 +// where: +// |D(t)| = |p(t)-b(t)| +// where p(t) is a point on the line parameterized by t: +// p(t) = p1 + t*(p2-p1) +// and b(t) is that same point clipped to the boundary of the box. in box- +// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components +// each of which looks like this: +// +// t_lo / +// ______/ -->t +// / t_hi +// / +// +// t_lo and t_hi are the t values where the line passes through the planes +// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt +// in a piecewise fashion from t=0 to t=1, stopping at the point where +// d|D(t)|^2/dt crosses from negative to positive. + +void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, + const dVector3 c, const dMatrix3 R, + const dVector3 side, + dVector3 lret, dVector3 bret) +{ + int i; + + // compute the start and delta of the line p1-p2 relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,v; + tmp[0] = p1[0] - c[0]; + tmp[1] = p1[1] - c[1]; + tmp[2] = p1[2] - c[2]; + dMultiply1_331 (s,R,tmp); + tmp[0] = p2[0] - p1[0]; + tmp[1] = p2[1] - p1[1]; + tmp[2] = p2[2] - p1[2]; + dMultiply1_331 (v,R,tmp); + + // mirror the line so that v has all components >= 0 + dVector3 sign; + for (i=0; i<3; i++) { + if (v[i] < 0) { + s[i] = -s[i]; + v[i] = -v[i]; + sign[i] = -1; + } + else sign[i] = 1; + } + + // compute v^2 + dVector3 v2; + v2[0] = v[0]*v[0]; + v2[1] = v[1]*v[1]; + v2[2] = v[2]*v[2]; + + // compute the half-sides of the box + dReal h[3]; + h[0] = REAL(0.5) * side[0]; + h[1] = REAL(0.5) * side[1]; + h[2] = REAL(0.5) * side[2]; + + // region is -1,0,+1 depending on which side of the box planes each + // coordinate is on. tanchor is the next t value at which there is a + // transition, or the last one if there are no more. + int region[3]; + dReal tanchor[3]; + + // Denormals are a problem, because we divide by v[i], and then + // multiply that by 0. Alas, infinity times 0 is infinity (!) + // We also use v2[i], which is v[i] squared. Here's how the epsilons + // are chosen: + // float epsilon = 1.175494e-038 (smallest non-denormal number) + // double epsilon = 2.225074e-308 (smallest non-denormal number) + // For single precision, choose an epsilon such that v[i] squared is + // not a denormal; this is for performance. + // For double precision, choose an epsilon such that v[i] is not a + // denormal; this is for correctness. (Jon Watte on mailinglist) + +#if defined( dSINGLE ) + const dReal tanchor_eps = REAL(1e-19); +#else + const dReal tanchor_eps = REAL(1e-307); +#endif + + // find the region and tanchor values for p1 + for (i=0; i<3; i++) { + if (v[i] > tanchor_eps) { + if (s[i] < -h[i]) { + region[i] = -1; + tanchor[i] = (-h[i]-s[i])/v[i]; + } + else { + region[i] = (s[i] > h[i]); + tanchor[i] = (h[i]-s[i])/v[i]; + } + } + else { + region[i] = 0; + tanchor[i] = 2; // this will never be a valid tanchor + } + } + + // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point + dReal t=0; + dReal dd2dt = 0; + for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i]; + if (dd2dt >= 0) goto got_answer; + + do { + // find the point on the line that is at the next clip plane boundary + dReal next_t = 1; + for (i=0; i<3; i++) { + if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t) + next_t = tanchor[i]; + } + + // compute d|d|^2/dt for the next t + dReal next_dd2dt = 0; + for (i=0; i<3; i++) { + next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]); + } + + // if the sign of d|d|^2/dt has changed, solution = the crossover point + if (next_dd2dt >= 0) { + dReal m = (next_dd2dt-dd2dt)/(next_t - t); + t -= dd2dt/m; + goto got_answer; + } + + // advance to the next anchor point / region + for (i=0; i<3; i++) { + if (tanchor[i] == next_t) { + tanchor[i] = (h[i]-s[i])/v[i]; + region[i]++; + } + } + t = next_t; + dd2dt = next_dd2dt; + } + while (t < 1); + t = 1; + +got_answer: + + // compute closest point on the line + for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1 + + // compute closest point on the box + for (i=0; i<3; i++) { + tmp[i] = sign[i] * (s[i] + t*v[i]); + if (tmp[i] < -h[i]) tmp[i] = -h[i]; + else if (tmp[i] > h[i]) tmp[i] = h[i]; + } + dMultiply0_331 (s,R,tmp); + for (i=0; i<3; i++) bret[i] = s[i] + c[i]; +} + + +// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect +// or 0 if not. + +int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2) +{ + // two boxes are disjoint if (and only if) there is a separating axis + // perpendicular to a face from one box or perpendicular to an edge from + // either box. the following tests are derived from: + // "OBB Tree: A Hierarchical Structure for Rapid Interference Detection", + // S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996. + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2. + // Qij is abs(Rij) + dVector3 p,pp; + dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMultiply1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5); + B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5); + + // for the following tests, excluding computation of Rij, in the worst case, + // 15 compares, 60 adds, 81 multiplies, and 24 absolutes. + // notation: R1=[u1 u2 u3], R2=[v1 v2 v3] + + // separating axis = u1,u2,u3 + R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2); + Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); + if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0; + R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2); + Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); + if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0; + R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2); + Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); + if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0; + + // separating axis = v1,v2,v3 + if (dFabs(dCalcVectorDot3_41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0; + if (dFabs(dCalcVectorDot3_41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0; + if (dFabs(dCalcVectorDot3_41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0; + + // separating axis = u1 x (v1,v2,v3) + if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0; + if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0; + if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0; + + // separating axis = u2 x (v1,v2,v3) + if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0; + if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0; + if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0; + + // separating axis = u3 x (v1,v2,v3) + if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0; + if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0; + if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0; + + return 1; +} + +//**************************************************************************** +// other utility functions + +/*ODE_API */void dInfiniteAABB (dxGeom *geom, dReal aabb[6]) +{ + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + + +//**************************************************************************** +// Helpers for Croteam's collider - by Nguyen Binh + +int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) +{ + // calculate distance of edge points to plane + dReal fDistance0 = dPointPlaneDistance( vEpnt0 ,plPlane ); + dReal fDistance1 = dPointPlaneDistance( vEpnt1 ,plPlane ); + + // if both points are behind the plane + if ( fDistance0 < 0 && fDistance1 < 0 ) + { + // do nothing + return 0; + // if both points in front of the plane + } + else if ( fDistance0 > 0 && fDistance1 > 0 ) + { + // accept them + return 1; + // if we have edge/plane intersection + } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); + + // clamp correct edge to intersection point + if ( fDistance0 < 0 ) + { + dVector3Copy(vIntersectionPoint,vEpnt0); + } else + { + dVector3Copy(vIntersectionPoint,vEpnt1); + } + return 1; + } + return 1; +} + +// clip polygon with plane and generate new polygon points +void dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1<ctIn; i0=i1, i1++) { + + + // calculate distance of edge points to plane + dReal fDistance0 = dPointPlaneDistance( avArrayIn[i0],plPlane ); + dReal fDistance1 = dPointPlaneDistance( avArrayIn[i1],plPlane ); + + // if first point is in front of plane + if( fDistance0 >= 0 ) { + // emit point + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - + (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - + (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - + (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + +} + +void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ,dReal fRadius) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1<ctIn; i0=i1, i1++) + { + // calculate distance of edge points to plane + dReal fDistance0 = dPointPlaneDistance( avArrayIn[i0],plPlane ); + dReal fDistance1 = dPointPlaneDistance( avArrayIn[i1],plPlane ); + + // if first point is in front of plane + if( fDistance0 >= 0 ) + { + // emit point + if (dVector3LengthSquare(avArrayIn[i0]) <= fRadius*fRadius) + { + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - + (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - + (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - + (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + if (dVector3LengthSquare(avArrayIn[i0]) <= fRadius*fRadius) + { + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + } +} + diff --git a/libs/ode-0.16.1/ode/src/collision_util.h b/libs/ode-0.16.1/ode/src/collision_util.h new file mode 100644 index 0000000..57e116a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_util.h @@ -0,0 +1,358 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +some useful collision utility stuff. + +*/ + +#ifndef _ODE_COLLISION_UTIL_H_ +#define _ODE_COLLISION_UTIL_H_ + +#include <ode/common.h> +#include <ode/contact.h> +#include <ode/rotation.h> +#include "odemath.h" + + +// given a pointer `p' to a dContactGeom, return the dContactGeom at +// p + skip bytes. +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) + +#if 1 +#include "collision_kernel.h" +// Fetches a contact +static inline +dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ + dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK)); + return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); +} +#endif + + +// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and +// return 1, else return 0. + +int dCollideSpheres (dVector3 p1, dReal r1, + dVector3 p2, dReal r2, dContactGeom *c); + + +// given two lines +// qa = pa + alpha* ua +// qb = pb + beta * ub +// where pa,pb are two points, ua,ub are two unit length vectors, and alpha, +// beta go from [-inf,inf], return alpha and beta such that qa and qb are +// as close as possible + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta); + + +// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length +// vector 'side'), compute the points of closest approach between the box +// and the line. return these points in 'lret' (the point on the line) and +// 'bret' (the point on the box). if the line actually penetrates the box +// then the solution is not unique, but only one solution will be returned. +// in this case the solution points will coincide. + +void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, + const dVector3 c, const dMatrix3 R, + const dVector3 side, + dVector3 lret, dVector3 bret); + +// 20 Apr 2004 +// Start code by Nguyen Binh +int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane); +// clip polygon with plane and generate new polygon points +void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ); + +void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius); + +// Some vector math +static inline +void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c) +{ + dSubtractVectors3(c, a, b); +} + +static inline +void dVector3Scale(dVector3& a,dReal nScale) +{ + dScaleVector3(a, nScale); +} + +static inline +void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c) +{ + dAddVectors3(c, a, b); +} + +static inline +void dVector3Copy(const dVector3& a,dVector3& c) +{ + dCopyVector3(c, a); +} + +static inline +void dVector4Copy(const dVector4& a,dVector4& c) +{ + dCopyVector4(c, a); +} + +static inline +void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c) +{ + dCalcVectorCross3(c, a, b); +} + +static inline +dReal dVector3Length(const dVector3& a) +{ + return dCalcVectorLength3(a); +} + +static inline +dReal dVector3LengthSquare(const dVector3& a) +{ + return dCalcVectorLengthSquare3(a); +} + +static inline +dReal dVector3Dot(const dVector3& a,const dVector3& b) +{ + return dCalcVectorDot3(a, b); +} + +static inline +void dVector3Inv(dVector3& a) +{ + dNegateVector3(a); +} + +static inline +void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v) +{ + dGetMatrixColumn3(v, m, col); +} + +static inline +void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) +{ + dCalcVectorCross3_114(r, v, m + col); +} + +static inline +void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) +{ + dCalcVectorCross3_141(r, m + col, v); +} + +static inline +void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r) +{ + dMultiply0_331(r, m, v); +} + +static inline +dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane) +{ + return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]); +} + +static inline +void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane) +{ + plane[0] = normal[0]; + plane[1] = normal[1]; + plane[2] = normal[2]; + plane[3] = distance; +} + +static inline +void dMatrix3Copy(const dReal* source,dMatrix3& dest) +{ + dCopyMatrix4x3(dest, source); +} + +static inline +dReal dMatrix3Det( const dMatrix3& mat ) +{ + dReal det; + + det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) + - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) + + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); + + return( det ); +} + + +static inline +void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst ) +{ + dReal det = dMatrix3Det( ma ); + + if ( dFabs( det ) < REAL(0.0005) ) + { + dRSetIdentity( dst ); + return; + } + + double detRecip = REAL(1.0) / det; + + dst[0] = (dReal)(( ma[5]*ma[10] - ma[6]*ma[9] ) * detRecip); + dst[1] = (dReal)(( ma[9]*ma[2] - ma[1]*ma[10] ) * detRecip); + dst[2] = (dReal)(( ma[1]*ma[6] - ma[5]*ma[2] ) * detRecip); + + dst[4] = (dReal)(( ma[6]*ma[8] - ma[4]*ma[10] ) * detRecip); + dst[5] = (dReal)(( ma[0]*ma[10] - ma[8]*ma[2] ) * detRecip); + dst[6] = (dReal)(( ma[4]*ma[2] - ma[0]*ma[6] ) * detRecip); + + dst[8] = (dReal)(( ma[4]*ma[9] - ma[8]*ma[5] ) * detRecip); + dst[9] = (dReal)(( ma[8]*ma[1] - ma[0]*ma[9] ) * detRecip); + dst[10] = (dReal)(( ma[0]*ma[5] - ma[1]*ma[4] ) * detRecip); +} + +static inline +void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) +{ + + // Nguyen Binh : this code seem to be the fastest. + dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3]; + dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1]; + dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2]; + dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3]; + + dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1; + dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2; + dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0; + + /* + // nVidia SDK implementation + dVector3 uv, uuv; + dVector3 qvec; + qvec[0] = quat[1]; + qvec[1] = quat[2]; + qvec[2] = quat[3]; + + dVector3Cross(qvec,source,uv); + dVector3Cross(qvec,uv,uuv); + + dVector3Scale(uv,REAL(2.0)*quat[0]); + dVector3Scale(uuv,REAL(2.0)); + + dest[0] = source[0] + uv[0] + uuv[0]; + dest[1] = source[1] + uv[1] + uuv[1]; + dest[2] = source[2] + uv[2] + uuv[2]; + */ +} + +static inline +void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) +{ + + dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]; + + if (norm > REAL(0.0)) + { + dQuaternion invQuat; + invQuat[0] = quat[0] / norm; + invQuat[1] = -quat[1] / norm; + invQuat[2] = -quat[2] / norm; + invQuat[3] = -quat[3] / norm; + + dQuatTransform(invQuat,source,dest); + + } + else + { + // Singular -> return identity + dVector3Copy(source,dest); + } +} + +static inline +void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ) +{ + rY = asin(mRot[0 * 4 + 2]); + if (rY < M_PI /2) + { + if (rY > -M_PI /2) + { + rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]); + rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]); + } + else + { + // not unique + rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]); + rZ = REAL(0.0); + } + } + else + { + // not unique + rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]); + rZ = REAL(0.0); + } +} + +static inline +void dQuatInv(const dQuaternion& source, dQuaternion& dest) +{ + dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3]; + + if (norm > 0.0f) + { + dReal neg_norm_recip = -REAL(1.0) / norm; + dest[0] = -source[0] * neg_norm_recip; + dest[1] = source[1] * neg_norm_recip; + dest[2] = source[2] * neg_norm_recip; + dest[3] = source[3] * neg_norm_recip; + } + else + { + // Singular -> return identity + dest[0] = REAL(1.0); + dest[1] = REAL(0.0); + dest[2] = REAL(0.0); + dest[3] = REAL(0.0); + } +} + +// Finds barycentric +static inline +void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){ + dReal w = REAL(1.0) - u - v; + + Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v); + Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v); + Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v); + Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v); +} + + + + +#endif diff --git a/libs/ode-0.16.1/ode/src/common.h b/libs/ode-0.16.1/ode/src/common.h new file mode 100644 index 0000000..0d67c2e --- /dev/null +++ b/libs/ode-0.16.1/ode/src/common.h @@ -0,0 +1,351 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_PRIVATE_COMMON_H_ +#define _ODE_PRIVATE_COMMON_H_ + + +#include "typedefs.h" +#include "error.h" +#include <ode/memory.h> +#include <algorithm> + + +#ifndef SIZE_MAX +#define SIZE_MAX ((sizeint)(-1)) +#endif + +#define dMACRO_MAX(a, b) ((a) > (b) ? (a) : (b)) +#define dMACRO_MIN(a, b) ((a) < (b) ? (a) : (b)) + + +#ifdef dSINGLE
+#define dEpsilon FLT_EPSILON
+#else
+#define dEpsilon DBL_EPSILON
+#endif
+ + +#ifdef dSINGLE
+ +#if !defined(FLT_MANT_DIG) +#define FLT_MANT_DIG 24 +#endif +
+#define dMaxExact ((float)((1UL << FLT_MANT_DIG) - 1))
+#define dMinExact ((float)(-dMaxExact))
+
+
+#else // #ifndef dSINGLE
+ +#if !defined(DBL_MANT_DIG) +#define DBL_MANT_DIG 53 +#endif + +#define dMaxExact (double)((1ULL << DBL_MANT_DIG) - 1)
+#define dMinExact ((double)(-dMaxExact))
+
+
+#endif // #ifndef dSINGLE
+
+
+#define dMaxIntExact dMACRO_MIN(dMaxExact, (dReal)INT_MAX)
+#define dMinIntExact dMACRO_MAX(dMinExact, (dReal)INT_MIN)
+
+ +#ifndef offsetof +#define offsetof(s, m) ((sizeint)&(((s *)8)->m) - (sizeint)8) +#endif +#ifndef membersize +#define membersize(s, m) (sizeof(((s *)8)->m)) +#endif +#ifndef endoffsetof +#define endoffsetof(s, m) ((sizeint)((sizeint)&(((s *)8)->m) - (sizeint)8) + sizeof(((s *)8)->m)) +#endif + + +/* the efficient alignment. most platforms align data structures to some + * number of bytes, but this is not always the most efficient alignment. + * for example, many x86 compilers align to 4 bytes, but on a pentium it + * is important to align doubles to 8 byte boundaries (for speed), and + * the 4 floats in a SIMD register to 16 byte boundaries. many other + * platforms have similar behavior. setting a larger alignment can waste + * a (very) small amount of memory. NOTE: this number must be a power of + * two. this is set to 16 by default. + */ +#ifndef EFFICIENT_ALIGNMENT +#define EFFICIENT_ALIGNMENT 16 +#endif + +#define dALIGN_SIZE(buf_size, alignment) (((buf_size) + (alignment - 1)) & (int)(~(alignment - 1))) // Casting the mask to int ensures sign-extension to larger integer sizes +#define dALIGN_PTR(buf_ptr, alignment) ((void *)(((uintptr)(buf_ptr) + ((alignment) - 1)) & (int)(~(alignment - 1)))) // Casting the mask to int ensures sign-extension to larger integer sizes + +/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */ +#define dEFFICIENT_SIZE(x) dALIGN_SIZE(x, EFFICIENT_ALIGNMENT) +#define dEFFICIENT_PTR(p) dALIGN_PTR(p, EFFICIENT_ALIGNMENT) +#define dOFFSET_EFFICIENTLY(p, b) ((void *)((uintptr)(p) + dEFFICIENT_SIZE(b))) + +#define dOVERALIGNED_SIZE(size, alignment) dEFFICIENT_SIZE((size) + ((alignment) - EFFICIENT_ALIGNMENT)) +#define dOVERALIGNED_PTR(buf_ptr, alignment) dALIGN_PTR(buf_ptr, alignment) +#define dOFFSET_OVERALIGNEDLY(buf_ptr, size, alignment) ((void *)((uintptr)(buf_ptr) + dOVERALIGNED_SIZE(size, alignment))) + + + +#define dDERIVE_SIZE_UNION_PADDING_ELEMENTS(DataSize, ElementType) (((DataSize) + sizeof(ElementType) - 1) / sizeof(ElementType)) +#define dDERIVE_TYPE_UNION_PADDING_ELEMENTS(DataType, ElementType) dDERIVE_SIZE_UNION_PADDING_ELEMENTS(sizeof(DataType), ElementType) +#define dDERIVE_SIZE_EXTRA_PADDING_ELEMENTS(DataSize, AlignmentSize, ElementType) (((dALIGN_SIZE(DataSize, dMACRO_MAX(AlignmentSize, sizeof(ElementType))) - (DataSize)) / sizeof(ElementType)) + + + +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ +#define dALLOCA16(n) \ + dEFFICIENT_PTR(alloca((n)+(EFFICIENT_ALIGNMENT))) + + +class dxAlignedAllocation +{ +public: + dxAlignedAllocation(): m_userAreaPointer(NULL), m_bufferAllocated(NULL), m_sizeUsed(0) {} + ~dxAlignedAllocation() { freeAllocation(); } + + void *allocAligned(sizeint sizeRequired, unsigned alignmentRequired) + { + dIASSERT((alignmentRequired & (alignmentRequired - 1)) == 0); + dIASSERT(alignmentRequired <= SIZE_MAX - sizeRequired); + + sizeint sizeToUse = sizeRequired + alignmentRequired; + void *bufferPointer = dAlloc(sizeToUse); + void *userAreaPointer = bufferPointer != NULL && alignmentRequired != 0 ? dALIGN_PTR(bufferPointer, alignmentRequired) : bufferPointer; + assignData(userAreaPointer, bufferPointer, sizeToUse); + + return userAreaPointer; + } + + void *getUserAreaPointer() const { return m_userAreaPointer; } + sizeint getUserAreaSize() const { return m_sizeUsed - ((uint8 *)m_userAreaPointer - (uint8 *)m_bufferAllocated); } + + void freeAllocation() + { + sizeint sizeUsed; + void *bufferPointer = extractData(sizeUsed); + + if (bufferPointer != NULL) + { + dFree(bufferPointer, sizeUsed); + } + } + +private: + void assignData(void *userAreaPointer, void *bufferAllocated, sizeint sizeUsed) + { + dIASSERT(m_userAreaPointer == NULL); + dIASSERT(m_bufferAllocated == NULL); + dIASSERT(m_sizeUsed == 0); + + m_userAreaPointer = userAreaPointer; + m_bufferAllocated = bufferAllocated; + m_sizeUsed = sizeUsed; + } + + void *extractData(sizeint &out_sizeUsed) + { + void *bufferPointer = m_bufferAllocated; + + if (bufferPointer != NULL) + { + out_sizeUsed = m_sizeUsed; + + m_userAreaPointer = NULL; + m_bufferAllocated = NULL; + m_sizeUsed = 0; + } + + return bufferPointer; + } + +private: + void *m_userAreaPointer; + void *m_bufferAllocated; + sizeint m_sizeUsed; +}; + + +template<typename DstType, typename SrcType> +inline +bool _cast_to_smaller(DstType &dtOutResult, const SrcType &stArgument) +{ + return (SrcType)(dtOutResult = (DstType)stArgument) == stArgument; +} + +#if defined(__GNUC__) + +#define dCAST_TO_SMALLER(TargetType, SourceValue) ({ TargetType ttCastSmallerValue; dIVERIFY(_cast_to_smaller(ttCastSmallerValue, SourceValue)); ttCastSmallerValue; }) + + +#else // #if !defined(__GNUC__) + +#define dCAST_TO_SMALLER(TargetType, SourceValue) templateCAST_TO_SMALLER<TargetType>(SourceValue) + +template <typename TTargetType, typename TSourceType> +inline TTargetType templateCAST_TO_SMALLER(const TSourceType &stSourceValue) +{ + TTargetType ttCastSmallerValue; + dIVERIFY(_cast_to_smaller(ttCastSmallerValue, stSourceValue)); + return ttCastSmallerValue; +} + + +#endif // #if !defined(__GNUC__) + + +template<typename value_type> +inline +void dxSwap(value_type &one, value_type &another) +{ + std::swap(one, another); +} + +template<typename value_type, typename lo_type, typename hi_type> +inline +value_type dxClamp(const value_type &value, const lo_type &lo, const hi_type &hi) +{ + return value < lo ? (value_type)lo : value > hi ? (value_type)hi : value; +} + + +template <typename Type> +union _const_type_cast_union +{ + explicit _const_type_cast_union(const void *psvCharBuffer): m_psvCharBuffer(psvCharBuffer) {} + + operator const Type *() const { return m_pstTypedPointer; } + const Type &operator *() const { return *m_pstTypedPointer; } + const Type *operator ->() const { return m_pstTypedPointer; } + const Type &operator [](diffint diElementIndex) const { return m_pstTypedPointer[diElementIndex]; } + const Type &operator [](sizeint siElementIndex) const { return m_pstTypedPointer[siElementIndex]; } + + const void *m_psvCharBuffer; + const Type *m_pstTypedPointer; +}; + +template <typename Type> +union _type_cast_union +{ + explicit _type_cast_union(void *psvCharBuffer): m_psvCharBuffer(psvCharBuffer) {} + + operator Type *() const { return m_pstTypedPointer; } + Type &operator *() const { return *m_pstTypedPointer; } + Type *operator ->() const { return m_pstTypedPointer; } + Type &operator [](diffint diElementIndex) const { return m_pstTypedPointer[diElementIndex]; } + Type &operator [](sizeint siElementIndex) const { return m_pstTypedPointer[siElementIndex]; } + + void *m_psvCharBuffer; + Type *m_pstTypedPointer; +}; + + +template<sizeint tsiTypeSize> +struct _sized_signed; + +template<> +struct _sized_signed<sizeof(uint8)> +{ + typedef int8 type; +}; + +template<> +struct _sized_signed<sizeof(uint16)> +{ + typedef int16 type; +}; + +template<> +struct _sized_signed<sizeof(uint32)> +{ + typedef int32 type; +}; + +template<> +struct _sized_signed<sizeof(uint64)> +{ + typedef int64 type; +}; + +template<typename tintergraltype> +struct _make_signed +{ + typedef typename _sized_signed<sizeof(tintergraltype)>::type type; +}; + + +template<sizeint tsiTypeSize> +struct _sized_unsigned; + +template<> +struct _sized_unsigned<sizeof(int8)> +{ + typedef uint8 type; +}; + +template<> +struct _sized_unsigned<sizeof(int16)> +{ + typedef uint16 type; +}; + +template<> +struct _sized_unsigned<sizeof(int32)> +{ + typedef uint32 type; +}; + +template<> +struct _sized_unsigned<sizeof(int64)> +{ + typedef uint64 type; +}; + +template<typename tintergraltype> +struct _make_unsigned +{ + typedef typename _sized_unsigned<sizeof(tintergraltype)>::type type; +}; + + +// template<typename tvalueint, typename tminint, typename tmaxint> +// inline +// bool dxInRange(tvalueint viValue, tminint miMin, tmaxint miMax) +// { +// return (typename _sized_unsigned<dMACRO_MAX(sizeof(tvalueint), sizeof(tminint))>::type)(viValue - miMin) < (typename _sized_unsigned<dMACRO_MAX(sizeof(tmaxint), sizeof(tminint))>::type)(miMax - miMin); +// } +// #define dIN_RANGE(aval, amin, amax) dxInRange(aval, amin, amax) + +#define dIN_RANGE(aval, amin, amax) ((_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)((_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(aval) - (_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(amin)) < (_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)((_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amax) - (_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amin))) +#define dTMPL_IN_RANGE(aval, amin, amax) ((typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)((typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(aval) - (typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(amin)) < (typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)((typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amax) - (typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amin))) +#define dCLAMP(aval, alo, ahi) dxClamp(aval, alo, ahi) +#define dARRAY_SIZE(aarr) (sizeof(aarr) / sizeof((aarr)[0])) +#define dSTATIC_ARRAY_SIZE(aclass, aarr) dARRAY_SIZE(((aclass *)sizeof(void *))->aarr) + + +#endif diff --git a/libs/ode-0.16.1/ode/src/config.h.in b/libs/ode-0.16.1/ode/src/config.h.in new file mode 100644 index 0000000..e6c0256 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/config.h.in @@ -0,0 +1,329 @@ +/* ode/src/config.h.in. Generated from configure.ac by autoheader. */ + + +#ifndef ODE_CONFIG_H +#define ODE_CONFIG_H + + +/* Define if building universal (internal helper macro) */ +#undef AC_APPLE_UNIVERSAL_BUILD + +/* Define to one of `_getb67', `GETB67', `getb67' for Cray-2 and Cray-YMP + systems. This function is required for `alloca.c' support on those systems. + */ +#undef CRAY_STACKSEG_END + +/* Define to 1 if using `alloca.c'. */ +#undef C_ALLOCA + +/* Define to 1 if you have `alloca', as a function or macro. */ +#undef HAVE_ALLOCA + +/* Define to 1 if you have <alloca.h> and it should be used (not on Ultrix). + */ +#undef HAVE_ALLOCA_H + +/* Use the Apple OpenGL framework. */ +#undef HAVE_APPLE_OPENGL_FRAMEWORK + +/* Define to 1 if you have the `atan2f' function. */ +#undef HAVE_ATAN2F + +/* Define to 1 if you have the `clock_gettime' function. */ +#undef HAVE_CLOCK_GETTIME + +/* Define to 1 if you have the `copysign' function. */ +#undef HAVE_COPYSIGN + +/* Define to 1 if you have the `copysignf' function. */ +#undef HAVE_COPYSIGNF + +/* Define to 1 if you have the `cosf' function. */ +#undef HAVE_COSF + +/* Define to 1 if you have the <dlfcn.h> header file. */ +#undef HAVE_DLFCN_H + +/* Define to 1 if you have the `fabsf' function. */ +#undef HAVE_FABSF + +/* Define to 1 if you have the <float.h> header file. */ +#undef HAVE_FLOAT_H + +/* Define to 1 if you have the `floor' function. */ +#undef HAVE_FLOOR + +/* Define to 1 if you have the `fmodf' function. */ +#undef HAVE_FMODF + +/* Define to 1 if you have the `gettimeofday' function. */ +#undef HAVE_GETTIMEOFDAY + +/* Define to 1 if you have the <GL/glext.h> header file. */ +#undef HAVE_GL_GLEXT_H + +/* Define to 1 if you have the <GL/glu.h> header file. */ +#undef HAVE_GL_GLU_H + +/* Define to 1 if you have the <GL/gl.h> header file. */ +#undef HAVE_GL_GL_H + +/* Define to 1 if you have the <inttypes.h> header file. */ +#undef HAVE_INTTYPES_H + +/* Define to 1 if you have the `isnan' function. */ +#undef HAVE_ISNAN + +/* Define to 1 if you have the `isnanf' function. */ +#undef HAVE_ISNANF + +/* Define to 1 if you have the `m' library (-lm). */ +#undef HAVE_LIBM + +/* Define to 1 if you have the `rt' library (-lrt). */ +#undef HAVE_LIBRT + +/* Define to 1 if you have the `sunmath' library (-lsunmath). */ +#undef HAVE_LIBSUNMATH + +/* Define to 1 if you have the <limits.h> header file. */ +#undef HAVE_LIMITS_H + +/* Define to 1 if you have the <malloc.h> header file. */ +#undef HAVE_MALLOC_H + +/* Define to 1 if you have the <math.h> header file. */ +#undef HAVE_MATH_H + +/* Define to 1 if you have the `memmove' function. */ +#undef HAVE_MEMMOVE + +/* Define to 1 if you have the <memory.h> header file. */ +#undef HAVE_MEMORY_H + +/* Define to 1 if you have the `memset' function. */ +#undef HAVE_MEMSET + +/* Define to 1 if you have the `no_pthread_condattr_setclock' function. */ +#undef HAVE_NO_PTHREAD_CONDATTR_SETCLOCK + +/* Define to 1 if libc includes obstacks. */ +#undef HAVE_OBSTACK + +/* Define to 1 if you have the `pthread_attr_setinheritsched' function. */ +#undef HAVE_PTHREAD_ATTR_SETINHERITSCHED + +/* Define to 1 if you have the `pthread_attr_setstacklazy' function. */ +#undef HAVE_PTHREAD_ATTR_SETSTACKLAZY + +/* Define to 1 if you have the `pthread_condattr_setclock' function. */ +#undef HAVE_PTHREAD_CONDATTR_SETCLOCK + +/* Define to 1 if you have the `sinf' function. */ +#undef HAVE_SINF + +/* Define to 1 if you have the `snprintf' function. */ +#undef HAVE_SNPRINTF + +/* Define to 1 if you have the `sqrt' function. */ +#undef HAVE_SQRT + +/* Define to 1 if you have the `sqrtf' function. */ +#undef HAVE_SQRTF + +/* Define to 1 if you have the <stdarg.h> header file. */ +#undef HAVE_STDARG_H + +/* Define to 1 if stdbool.h conforms to C99. */ +#undef HAVE_STDBOOL_H + +/* Define to 1 if you have the <stddef.h> header file. */ +#undef HAVE_STDDEF_H + +/* Define to 1 if you have the <stdint.h> header file. */ +#undef HAVE_STDINT_H + +/* Define to 1 if you have the <stdio.h> header file. */ +#undef HAVE_STDIO_H + +/* Define to 1 if you have the <stdlib.h> header file. */ +#undef HAVE_STDLIB_H + +/* Define to 1 if you have the `strchr' function. */ +#undef HAVE_STRCHR + +/* Define to 1 if you have the <strings.h> header file. */ +#undef HAVE_STRINGS_H + +/* Define to 1 if you have the <string.h> header file. */ +#undef HAVE_STRING_H + +/* Define to 1 if you have the `strstr' function. */ +#undef HAVE_STRSTR + +/* Define to 1 if you have the <sys/stat.h> header file. */ +#undef HAVE_SYS_STAT_H + +/* Define to 1 if you have the <sys/time.h> header file. */ +#undef HAVE_SYS_TIME_H + +/* Define to 1 if you have the <sys/types.h> header file. */ +#undef HAVE_SYS_TYPES_H + +/* Define to 1 if you have the <time.h> header file. */ +#undef HAVE_TIME_H + +/* Define to 1 if you have the <unistd.h> header file. */ +#undef HAVE_UNISTD_H + +/* Define to 1 if you have the `vsnprintf' function. */ +#undef HAVE_VSNPRINTF + +/* Define to 1 if the system has the type `_Bool'. */ +#undef HAVE__BOOL + +/* Define to 1 if you have the `_isnan' function. */ +#undef HAVE__ISNAN + +/* Define to 1 if you have the `_isnanf' function. */ +#undef HAVE__ISNANF + +/* Define to 1 if you have the `__isnan' function. */ +#undef HAVE___ISNAN + +/* Define to 1 if you have the `__isnanf' function. */ +#undef HAVE___ISNANF + +/* Define to the sub-directory where libtool stores uninstalled libraries. */ +#undef LT_OBJDIR + +/* Mac OS X version setting for OU Library */ +#undef MAC_OS_X_VERSION + +/* Name of package */ +#undef PACKAGE + +/* Define to the address where bug reports for this package should be sent. */ +#undef PACKAGE_BUGREPORT + +/* Define to the full name of this package. */ +#undef PACKAGE_NAME + +/* Define to the full name and version of this package. */ +#undef PACKAGE_STRING + +/* Define to the one symbol short name of this package. */ +#undef PACKAGE_TARNAME + +/* Define to the home page for this package. */ +#undef PACKAGE_URL + +/* Define to the version of this package. */ +#undef PACKAGE_VERSION + +/* compiling for a pentium on a gcc-based platform? */ +#undef PENTIUM + +/* If using the C implementation of alloca, define if you know the + direction of stack growth for your system; otherwise it will be + automatically deduced at runtime. + STACK_DIRECTION > 0 => grows toward higher addresses + STACK_DIRECTION < 0 => grows toward lower addresses + STACK_DIRECTION = 0 => direction of growth unknown */ +#undef STACK_DIRECTION + +/* Define to 1 if you have the ANSI C header files. */ +#undef STDC_HEADERS + +/* Version number of package */ +#undef VERSION + +/* Define WORDS_BIGENDIAN to 1 if your processor stores words with the most + significant byte first (like Motorola and SPARC, unlike Intel). */ +#if defined AC_APPLE_UNIVERSAL_BUILD +# if defined __BIG_ENDIAN__ +# define WORDS_BIGENDIAN 1 +# endif +#else +# ifndef WORDS_BIGENDIAN +# undef WORDS_BIGENDIAN +# endif +#endif + +/* compiling for a X86_64 system on a gcc-based platform? */ +#undef X86_64_SYSTEM + +/* OU features enabled */ +#undef _OU_FEATURE_SET + +/* libou namespace for ODE */ +#undef _OU_NAMESPACE + +/* Target OS setting for OU Library */ +#undef _OU_TARGET_OS + +/* Define for Solaris 2.5.1 so the uint32_t typedef from <sys/synch.h>, + <pthread.h>, or <semaphore.h> is not used. If the typedef were allowed, the + #define below would cause a syntax error. */ +#undef _UINT32_T + +/* Atomic API of OU is enabled */ +#undef dATOMICS_ENABLED + +/* Built-in multithreaded threading implementation is included */ +#undef dBUILTIN_THREADING_IMPL_ENABLED + +/* Generic OU features are enabled */ +#undef dOU_ENABLED + +/* Threading interface is disabled */ +#undef dTHREADING_INTF_DISABLED + +/* Thread Local Storage API of OU is enabled */ +#undef dTLS_ENABLED + +/* Use the old trimesh-trimesh collider */ +#undef dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + +/* Define to `__inline__' or `__inline' if that's what the C compiler + calls it, or to nothing if 'inline' is not supported under any name. */ +#ifndef __cplusplus +#undef inline +#endif + +/* Define to the type of a signed integer type of width exactly 32 bits if + such a type exists and the standard includes do not define it. */ +#undef int32_t + +/* Define to `unsigned int' if <sys/types.h> does not define. */ +#undef size_t + +/* Define to the type of an unsigned integer type of width exactly 32 bits if + such a type exists and the standard includes do not define it. */ +#undef uint32_t + +/* Define to empty if the keyword `volatile' does not work. Warning: valid + code using `volatile' can become incorrect without. Disable with care. */ +#undef volatile + + + +#ifdef HAVE_ALLOCA_H +#include <alloca.h> +#endif +#ifdef HAVE_MALLOC_H +#include <malloc.h> +#endif +#ifdef HAVE_STDINT_H +#include <stdint.h> +#endif +#ifdef HAVE_INTTYPES_H +#include <inttypes.h> +#endif + + +#include "typedefs.h" + + +#endif /* #define ODE_CONFIG_H */ + diff --git a/libs/ode-0.16.1/ode/src/convex.cpp b/libs/ode-0.16.1/ode/src/convex.cpp new file mode 100644 index 0000000..7a17941 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/convex.cpp @@ -0,0 +1,1621 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ +/* +Code for Convex Collision Detection +By Rodrigo Hernandez +*/ +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +#if 1 +#define dMIN(A,B) ((A)>(B) ? (B) : (A)) +#define dMAX(A,B) ((A)>(B) ? (A) : (B)) +#else +#define dMIN(A,B) std::min(A,B) +#define dMAX(A,B) std::max(A,B) +#endif + +//**************************************************************************** +// Convex public API +dxConvex::dxConvex (dSpaceID space, + const dReal *_planes, + unsigned int _planecount, + const dReal *_points, + unsigned int _pointcount, + const unsigned int *_polygons) : +dxGeom (space,1) +{ + dAASSERT (_planes != NULL); + dAASSERT (_points != NULL); + dAASSERT (_polygons != NULL); + //fprintf(stdout,"dxConvex Constructor planes %X\n",_planes); + type = dConvexClass; + planes = _planes; + planecount = _planecount; + // we need points as well + points = _points; + pointcount = _pointcount; + polygons=_polygons; + edges = NULL; + FillEdges(); +#ifndef dNODEBUG + // Check for properly build polygons by calculating the determinant + // of the 3x3 matrix composed of the first 3 points in the polygon. + const unsigned int *points_in_poly=polygons; + const unsigned int *index=polygons+1; + + for(unsigned int i=0;i<planecount;++i) + { + dAASSERT (*points_in_poly > 2 ); + if(( + points[(index[0]*3)+0]*points[(index[1]*3)+1]*points[(index[2]*3)+2] + + points[(index[0]*3)+1]*points[(index[1]*3)+2]*points[(index[2]*3)+0] + + points[(index[0]*3)+2]*points[(index[1]*3)+0]*points[(index[2]*3)+1] - + points[(index[0]*3)+2]*points[(index[1]*3)+1]*points[(index[2]*3)+0] - + points[(index[0]*3)+1]*points[(index[1]*3)+0]*points[(index[2]*3)+2] - + points[(index[0]*3)+0]*points[(index[1]*3)+2]*points[(index[2]*3)+1])<0) + { + fprintf(stdout,"WARNING: Polygon %d is not defined counterclockwise\n",i); + } + points_in_poly+=(*points_in_poly+1); + index=points_in_poly+1; + if(planes[(i*4)+3]<0) fprintf(stdout,"WARNING: Plane %d does not contain the origin\n",i); + } +#endif + + //CreateTree(); +} + + +void dxConvex::computeAABB() +{ + // this can, and should be optimized + dVector3 point; + dMultiply0_331 (point,final_posr->R,points); + aabb[0] = point[0]+final_posr->pos[0]; + aabb[1] = point[0]+final_posr->pos[0]; + aabb[2] = point[1]+final_posr->pos[1]; + aabb[3] = point[1]+final_posr->pos[1]; + aabb[4] = point[2]+final_posr->pos[2]; + aabb[5] = point[2]+final_posr->pos[2]; + for(unsigned int i=3;i<(pointcount*3);i+=3) + { + dMultiply0_331 (point,final_posr->R,&points[i]); + aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]); + aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]); + aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]); + aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]); + aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]); + aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]); + } +} + +/*! \brief Populates the edges set, should be called only once whenever the polygon array gets updated */ +void dxConvex::FillEdges() +{ + const unsigned int *points_in_poly=polygons; + const unsigned int *index=polygons+1; + if (edges!=NULL) delete[] edges; + edgecount = 0; + edge e; + bool isinset; + for(unsigned int i=0;i<planecount;++i) + { + for(unsigned int j=0;j<*points_in_poly;++j) + { + e.first = dMIN(index[j],index[(j+1)%*points_in_poly]); + e.second = dMAX(index[j],index[(j+1)%*points_in_poly]); + isinset=false; + for(unsigned int k=0;k<edgecount;++k) + { + if((edges[k].first==e.first)&&(edges[k].second==e.second)) + { + isinset=true; + break; + } + } + if(!isinset) + { + edge* tmp = new edge[edgecount+1]; + if(edgecount!=0) + { + memcpy(tmp,edges,(edgecount)*sizeof(edge)); + delete[] edges; + } + tmp[edgecount].first=e.first; + tmp[edgecount].second=e.second; + edges = tmp; + ++edgecount; + } + } + points_in_poly+=(*points_in_poly+1); + index=points_in_poly+1; + } +} +#if 0 +dxConvex::BSPNode* dxConvex::CreateNode(std::vector<Arc> Arcs,std::vector<Polygon> Polygons) +{ +#if 0 + dVector3 ea,eb,e; + dVector3Copy(points+((edges.begin()+Arcs[0].edge)first*3),ea); + dMultiply0_331(e1b,cvx1.final_posr->R,cvx1.points+(i->second*3)); + + dVector3Copy(points[edges[Arcs[0].edge] +#endif + return NULL; +} + +void dxConvex::CreateTree() +{ + std::vector<Arc> A; + A.reserve(edgecount); + for(unsigned int i=0;i<edgecount;++i) + { + this->GetFacesSharedByEdge(i,A[i].normals); + A[i].edge = i; + } + std::vector<Polygon> S; + S.reserve(pointcount); + for(unsigned int i=0;i<pointcount;++i) + { + this->GetFacesSharedByVertex(i,S[i].normals); + S[i].vertex=i; + } + this->tree = CreateNode(A,S); +} + +void dxConvex::GetFacesSharedByVertex(int i, std::vector<int> f) +{ +} +void dxConvex::GetFacesSharedByEdge(int i, int* f) +{ +} +void dxConvex::GetFaceNormal(int i, dVector3 normal) +{ +} +#endif + +dGeomID dCreateConvex (dSpaceID space,const dReal *_planes,unsigned int _planecount, + const dReal *_points, + unsigned int _pointcount, + const unsigned int *_polygons) +{ + //fprintf(stdout,"dxConvex dCreateConvex\n"); + return new dxConvex(space,_planes, _planecount, + _points, + _pointcount, + _polygons); +} + +void dGeomSetConvex (dGeomID g,const dReal *_planes,unsigned int _planecount, + const dReal *_points, + unsigned int _pointcount, + const unsigned int *_polygons) +{ + //fprintf(stdout,"dxConvex dGeomSetConvex\n"); + dUASSERT (g && g->type == dConvexClass,"argument not a convex shape"); + dxConvex *s = (dxConvex*) g; + s->planes = _planes; + s->planecount = _planecount; + s->points = _points; + s->pointcount = _pointcount; + s->polygons=_polygons; +} + +//**************************************************************************** +// Helper Inlines +// + +/*! \brief Returns Whether or not the segment ab intersects plane p + \param a origin of the segment + \param b segment destination + \param p plane to test for intersection + \param t returns the time "t" in the segment ray that gives us the intersecting + point + \param q returns the intersection point + \return true if there is an intersection, otherwise false. +*/ +bool IntersectSegmentPlane(dVector3 a, + dVector3 b, + dVector4 p, + dReal &t, + dVector3 q) +{ + // Compute the t value for the directed line ab intersecting the plane + dVector3 ab; + ab[0]= b[0] - a[0]; + ab[1]= b[1] - a[1]; + ab[2]= b[2] - a[2]; + + t = (p[3] - dCalcVectorDot3(p,a)) / dCalcVectorDot3(p,ab); + + // If t in [0..1] compute and return intersection point + if (t >= 0.0 && t <= 1.0) + { + q[0] = a[0] + t * ab[0]; + q[1] = a[1] + t * ab[1]; + q[2] = a[2] + t * ab[2]; + return true; + } + // Else no intersection + return false; +} + +/*! \brief Returns the Closest Point in Ray 1 to Ray 2 + \param Origin1 The origin of Ray 1 + \param Direction1 The direction of Ray 1 + \param Origin1 The origin of Ray 2 + \param Direction1 The direction of Ray 3 + \param t the time "t" in Ray 1 that gives us the closest point + (closest_point=Origin1+(Direction1*t). + \return true if there is a closest point, false if the rays are paralell. +*/ +inline bool ClosestPointInRay(const dVector3 Origin1, + const dVector3 Direction1, + const dVector3 Origin2, + const dVector3 Direction2, + dReal& t) +{ + dVector3 w = {Origin1[0]-Origin2[0], + Origin1[1]-Origin2[1], + Origin1[2]-Origin2[2]}; + dReal a = dCalcVectorDot3(Direction1 , Direction1); + dReal b = dCalcVectorDot3(Direction1 , Direction2); + dReal c = dCalcVectorDot3(Direction2 , Direction2); + dReal d = dCalcVectorDot3(Direction1 , w); + dReal e = dCalcVectorDot3(Direction2 , w); + dReal denominator = (a*c)-(b*b); + if(denominator==0.0f) + { + return false; + } + t = ((a*e)-(b*d))/denominator; + return true; +} + +/*! \brief Returns the Closest Points from Segment 1 to Segment 2 + \param p1 start of segment 1 + \param q1 end of segment 1 + \param p2 start of segment 2 + \param q2 end of segment 2 + \param t the time "t" in Ray 1 that gives us the closest point + (closest_point=Origin1+(Direction1*t). + \return true if there is a closest point, false if the rays are paralell. + \note Adapted from Christer Ericson's Real Time Collision Detection Book. +*/ +inline void ClosestPointBetweenSegments(dVector3& p1, + dVector3& q1, + dVector3& p2, + dVector3& q2, + dVector3& c1, + dVector3& c2) +{ + // s & t were originaly part of the output args, but since + // we don't really need them, we'll just declare them in here + dReal s; + dReal t; + dVector3 d1 = {q1[0] - p1[0], + q1[1] - p1[1], + q1[2] - p1[2]}; + dVector3 d2 = {q2[0] - p2[0], + q2[1] - p2[1], + q2[2] - p2[2]}; + dVector3 r = {p1[0] - p2[0], + p1[1] - p2[1], + p1[2] - p2[2]}; + dReal a = dCalcVectorDot3(d1, d1); + dReal e = dCalcVectorDot3(d2, d2); + dReal f = dCalcVectorDot3(d2, r); + // Check if either or both segments degenerate into points + if (a <= dEpsilon && e <= dEpsilon) + { + // Both segments degenerate into points + s = t = 0.0f; + dVector3Copy(p1,c1); + dVector3Copy(p2,c2); + return; + } + if (a <= dEpsilon) + { + // First segment degenerates into a point + s = 0.0f; + t = f / e; // s = 0 => t = (b*s + f) / e = f / e + t = dxClamp(t, 0.0f, 1.0f); + } + else + { + dReal c = dCalcVectorDot3(d1, r); + if (e <= dEpsilon) + { + // Second segment degenerates into a point + t = 0.0f; + s = dxClamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a + } + else + { + // The general non degenerate case starts here + dReal b = dCalcVectorDot3(d1, d2); + dReal denom = a*e-b*b; // Always nonnegative + + // If segments not parallel, compute closest point on L1 to L2, and + // clamp to segment S1. Else pick arbitrary s (here 0) + if (denom != 0.0f) + { + s = dxClamp((b*f - c*e) / denom, 0.0f, 1.0f); + } + else s = 0.0f; +#if 0 + // Compute point on L2 closest to S1(s) using + // t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e + t = (b*s + f) / e; + + // If t in [0,1] done. Else clamp t, recompute s for the new value + // of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a + // and clamp s to [0, 1] + if (t < 0.0f) { + t = 0.0f; + s = dxClamp(-c / a, 0.0f, 1.0f); + } else if (t > 1.0f) { + t = 1.0f; + s = dxClamp((b - c) / a, 0.0f, 1.0f); + } +#else + dReal tnom = b*s + f; + if (tnom < 0.0f) + { + t = 0.0f; + s = dxClamp(-c / a, 0.0f, 1.0f); + } + else if (tnom > e) + { + t = 1.0f; + s = dxClamp((b - c) / a, 0.0f, 1.0f); + } + else + { + t = tnom / e; + } +#endif + } + } + + c1[0] = p1[0] + d1[0] * s; + c1[1] = p1[1] + d1[1] * s; + c1[2] = p1[2] + d1[2] * s; + c2[0] = p2[0] + d2[0] * t; + c2[1] = p2[1] + d2[1] * t; + c2[2] = p2[2] + d2[2] * t; +} + +#if 0 +dReal tnom = b*s + f; +if (tnom < 0.0f) { + t = 0.0f; + s = dxClamp(-c / a, 0.0f, 1.0f); +} else if (tnom > e) { + t = 1.0f; + s = dxClamp((b - c) / a, 0.0f, 1.0f); +} else { + t = tnom / e; +} +#endif + +/*! \brief Returns the Ray on which 2 planes intersect if they do. + \param p1 Plane 1 + \param p2 Plane 2 + \param p Contains the origin of the ray upon returning if planes intersect + \param d Contains the direction of the ray upon returning if planes intersect + \return true if the planes intersect, false if paralell. +*/ +inline bool IntersectPlanes(const dVector4 p1, const dVector4 p2, dVector3 p, dVector3 d) +{ + // Compute direction of intersection line + dCalcVectorCross3(d,p1,p2); + // If d is (near) zero, the planes are parallel (and separated) + // or coincident, so they're not considered intersecting + dReal denom = dCalcVectorDot3(d, d); + if (denom < dEpsilon) return false; + dVector3 n; + n[0]=p1[3]*p2[0] - p2[3]*p1[0]; + n[1]=p1[3]*p2[1] - p2[3]*p1[1]; + n[2]=p1[3]*p2[2] - p2[3]*p1[2]; + // Compute point on intersection line + dCalcVectorCross3(p,n,d); + p[0]/=denom; + p[1]/=denom; + p[2]/=denom; + return true; +} + + +#if 0 +/*! \brief Finds out if a point lies inside a convex + \param p Point to test + \param convex a pointer to convex to test against + \return true if the point lies inside the convex, false if not. +*/ +inline bool IsPointInConvex(dVector3 p, + dxConvex *convex) +{ + dVector3 lp,tmp; + // move point into convex space to avoid plane local to world calculations + tmp[0] = p[0] - convex->final_posr->pos[0]; + tmp[1] = p[1] - convex->final_posr->pos[1]; + tmp[2] = p[2] - convex->final_posr->pos[2]; + dMultiply1_331 (lp,convex->final_posr->R,tmp); + for(unsigned int i=0;i<convex->planecount;++i) + { + if(( + ((convex->planes+(i*4))[0]*lp[0])+ + ((convex->planes+(i*4))[1]*lp[1])+ + ((convex->planes+(i*4))[2]*lp[2])+ + -(convex->planes+(i*4))[3] + )>0) + { + return false; + } + } + return true; +} +#endif + +/*! \brief Finds out if a point lies inside a 2D polygon + \param p Point to test + \param polygon a pointer to the start of the convex polygon index buffer + \param out the closest point in the polygon if the point is not inside + \return true if the point lies inside of the polygon, false if not. +*/ +inline bool IsPointInPolygon(dVector3 p, + const unsigned int *polygon, + dReal *plane, + dxConvex *convex, + dVector3 out) +{ + // p is the point we want to check, + // polygon is a pointer to the polygon we + // are checking against, remember it goes + // number of vertices then that many indexes + // out returns the closest point on the border of the + // polygon if the point is not inside it. + dVector3 a; + dVector3 b; + dVector3 ab; + dVector3 ap; + dVector3 v; + + unsigned pointcount=polygon[0]; + dIASSERT(pointcount != 0); + polygon++; // skip past pointcount + + dMultiply0_331 (b,convex->final_posr->R, + &convex->points[(polygon[pointcount-1]*3)]); + b[0]=convex->final_posr->pos[0]+b[0]; + b[1]=convex->final_posr->pos[1]+b[1]; + b[2]=convex->final_posr->pos[2]+b[2]; + + for(unsigned i=0; i != pointcount; ++i) + { + a[0] = b[0]; + a[1] = b[1]; + a[2] = b[2]; + + dMultiply0_331 (b,convex->final_posr->R,&convex->points[(polygon[i]*3)]); + b[0]=convex->final_posr->pos[0]+b[0]; + b[1]=convex->final_posr->pos[1]+b[1]; + b[2]=convex->final_posr->pos[2]+b[2]; + + ab[0] = b[0] - a[0]; + ab[1] = b[1] - a[1]; + ab[2] = b[2] - a[2]; + ap[0] = p[0] - a[0]; + ap[1] = p[1] - a[1]; + ap[2] = p[2] - a[2]; + + dCalcVectorCross3(v, ab, plane); + + if (dCalcVectorDot3(ap, v) > REAL(0.0)) + { + dReal ab_m2 = dCalcVectorDot3(ab, ab); + dReal s = ab_m2 != REAL(0.0) ? dCalcVectorDot3(ab, ap) / ab_m2 : REAL(0.0); + + if (s <= REAL(0.0)) + { + out[0] = a[0]; + out[1] = a[1]; + out[2] = a[2]; + } + else if (s >= REAL(1.0)) + { + out[0] = b[0]; + out[1] = b[1]; + out[2] = b[2]; + } + else + { + out[0] = a[0] + ab[0] * s; + out[1] = a[1] + ab[1] * s; + out[2] = a[2] + ab[2] * s; + } + + return false; + } + } + + return true; +} + +int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxConvex *Convex = (dxConvex*) o1; + dxPlane *Plane = (dxPlane*) o2; + unsigned int contacts=0; + unsigned int maxc = flags & NUMC_MASK; + dVector3 v2; + +#define LTEQ_ZERO 0x10000000 +#define GTEQ_ZERO 0x20000000 +#define BOTH_SIGNS (LTEQ_ZERO | GTEQ_ZERO) + dIASSERT((BOTH_SIGNS & NUMC_MASK) == 0); // used in conditional operator later + + unsigned int totalsign = 0; + for(unsigned int i=0;i<Convex->pointcount;++i) + { + dMultiply0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]); + dVector3Add(Convex->final_posr->pos, v2, v2); + + unsigned int distance2sign = GTEQ_ZERO; + dReal distance2 = dVector3Dot(Plane->p, v2) - Plane->p[3]; // Ax + By + Cz - D + if((distance2 <= REAL(0.0))) + { + distance2sign = distance2 != REAL(0.0) ? LTEQ_ZERO : BOTH_SIGNS; + + if (contacts != maxc) + { + dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip); + dVector3Copy(Plane->p, target->normal); + dVector3Copy(v2, target->pos); + target->depth = -distance2; + target->g1 = Convex; + target->g2 = Plane; + target->side1 = -1; // TODO: set plane index? + target->side2 = -1; + contacts++; + } + } + + // Take new sign into account + totalsign |= distance2sign; + // Check if contacts are full and both signs have been already found + if (((contacts ^ maxc) | totalsign) == BOTH_SIGNS) // harder to comprehend but requires one register less + { + break; // Nothing can be changed any more + } + } + if (totalsign == BOTH_SIGNS) return contacts; + return 0; +#undef BOTH_SIGNS +#undef GTEQ_ZERO +#undef LTEQ_ZERO +} + +int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dConvexClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxSphere *Sphere = (dxSphere*) o1; + dxConvex *Convex = (dxConvex*) o2; + dReal dist,closestdist=dInfinity; + dVector4 plane; + // dVector3 contactpoint; + dVector3 offsetpos,out,temp; + const unsigned int *pPoly=Convex->polygons; + int closestplane=-1; + bool sphereinside=true; + /* + Do a good old sphere vs plane check first, + if a collision is found then check if the contact point + is within the polygon + */ + // offset the sphere final_posr->position into the convex space + offsetpos[0]=Sphere->final_posr->pos[0]-Convex->final_posr->pos[0]; + offsetpos[1]=Sphere->final_posr->pos[1]-Convex->final_posr->pos[1]; + offsetpos[2]=Sphere->final_posr->pos[2]-Convex->final_posr->pos[2]; + for(unsigned int i=0;i<Convex->planecount;++i) + { + // apply rotation to the plane + dMultiply0_331(plane,Convex->final_posr->R,&Convex->planes[(i*4)]); + plane[3]=(&Convex->planes[(i*4)])[3]; + // Get the distance from the sphere origin to the plane + dist = dVector3Dot(plane, offsetpos) - plane[3]; // Ax + By + Cz - D + if(dist>0) + { + // if we get here, we know the center of the sphere is + // outside of the convex hull. + if(dist<Sphere->radius) + { + // if we get here we know the sphere surface penetrates + // the plane + if(IsPointInPolygon(Sphere->final_posr->pos,pPoly,plane,Convex,out)) + { + // finally if we get here we know that the + // sphere is directly touching the inside of the polyhedron + contact->normal[0] = plane[0]; + contact->normal[1] = plane[1]; + contact->normal[2] = plane[2]; + contact->pos[0] = Sphere->final_posr->pos[0]+ + (-contact->normal[0]*Sphere->radius); + contact->pos[1] = Sphere->final_posr->pos[1]+ + (-contact->normal[1]*Sphere->radius); + contact->pos[2] = Sphere->final_posr->pos[2]+ + (-contact->normal[2]*Sphere->radius); + contact->depth = Sphere->radius-dist; + contact->g1 = Sphere; + contact->g2 = Convex; + contact->side1 = -1; + contact->side2 = -1; // TODO: set plane index? + return 1; + } + else + { + // the sphere may not be directly touching + // the polyhedron, but it may be touching + // a point or an edge, if the distance between + // the closest point on the poly (out) and the + // center of the sphere is less than the sphere + // radius we have a hit. + temp[0] = (Sphere->final_posr->pos[0]-out[0]); + temp[1] = (Sphere->final_posr->pos[1]-out[1]); + temp[2] = (Sphere->final_posr->pos[2]-out[2]); + dist=(temp[0]*temp[0])+(temp[1]*temp[1])+(temp[2]*temp[2]); + // avoid the sqrt unless really necesary + if(dist<(Sphere->radius*Sphere->radius)) + { + // We got an indirect hit + dist=dSqrt(dist); + contact->normal[0] = temp[0]/dist; + contact->normal[1] = temp[1]/dist; + contact->normal[2] = temp[2]/dist; + contact->pos[0] = Sphere->final_posr->pos[0]+ + (-contact->normal[0]*Sphere->radius); + contact->pos[1] = Sphere->final_posr->pos[1]+ + (-contact->normal[1]*Sphere->radius); + contact->pos[2] = Sphere->final_posr->pos[2]+ + (-contact->normal[2]*Sphere->radius); + contact->depth = Sphere->radius-dist; + contact->g1 = Sphere; + contact->g2 = Convex; + contact->side1 = -1; + contact->side2 = -1; // TODO: set plane index? + return 1; + } + } + } + sphereinside=false; + } + if(sphereinside) + { + if(closestdist>dFabs(dist)) + { + closestdist=dFabs(dist); + closestplane=i; + } + } + pPoly+=pPoly[0]+1; + } + if(sphereinside) + { + // if the center of the sphere is inside + // the Convex, we need to pop it out + dMultiply0_331(contact->normal, + Convex->final_posr->R, + &Convex->planes[(closestplane*4)]); + contact->pos[0] = Sphere->final_posr->pos[0]; + contact->pos[1] = Sphere->final_posr->pos[1]; + contact->pos[2] = Sphere->final_posr->pos[2]; + contact->depth = closestdist+Sphere->radius; + contact->g1 = Sphere; + contact->g2 = Convex; + contact->side1 = -1; + contact->side2 = -1; // TODO: set plane index? + return 1; + } + return 0; +} + +int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom * /*contact*/, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + //dxConvex *Convex = (dxConvex*) o1; + //dxBox *Box = (dxBox*) o2; + + return 0; +} + +int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom * /*contact*/, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + //dxConvex *Convex = (dxConvex*) o1; + //dxCapsule *Capsule = (dxCapsule*) o2; + + return 0; +} + +inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max) +{ + /* TODO: Use Support points here */ + dVector3 point; + dReal value; + //fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]); + dMultiply0_331(point,cvx.final_posr->R,cvx.points); + //fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]); + point[0]+=cvx.final_posr->pos[0]; + point[1]+=cvx.final_posr->pos[1]; + point[2]+=cvx.final_posr->pos[2]; + max = min = dCalcVectorDot3(point,axis)-axis[3];//(*) + for (unsigned int i = 1; i < cvx.pointcount; ++i) + { + dMultiply0_331(point,cvx.final_posr->R,cvx.points+(i*3)); + point[0]+=cvx.final_posr->pos[0]; + point[1]+=cvx.final_posr->pos[1]; + point[2]+=cvx.final_posr->pos[2]; + value=dCalcVectorDot3(point,axis)-axis[3];//(*) + if(value<min) + { + min=value; + } + else if(value>max) + { + max=value; + } + } + // *: usually using the distance part of the plane (axis) is + // not necesary, however, here we need it here in order to know + // which face to pick when there are 2 parallel sides. +} + +bool CheckEdgeIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,int& curc, + dContactGeom *contact, int skip) +{ + int maxc = flags & NUMC_MASK; + dIASSERT(maxc != 0); + dVector3 e1,e2,q; + dVector4 plane,depthplane; + dReal t; + for(unsigned int i = 0;i<cvx1.edgecount;++i) + { + // Rotate + dMultiply0_331(e1,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].first*3)); + // translate + e1[0]+=cvx1.final_posr->pos[0]; + e1[1]+=cvx1.final_posr->pos[1]; + e1[2]+=cvx1.final_posr->pos[2]; + // Rotate + dMultiply0_331(e2,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].second*3)); + // translate + e2[0]+=cvx1.final_posr->pos[0]; + e2[1]+=cvx1.final_posr->pos[1]; + e2[2]+=cvx1.final_posr->pos[2]; + const unsigned int* pPoly=cvx2.polygons; + for(sizeint j=0;j<cvx2.planecount;++j) + { + // Rotate + dMultiply0_331(plane,cvx2.final_posr->R,cvx2.planes+(j*4)); + dNormalize3(plane); + // Translate + plane[3]= + (cvx2.planes[(j*4)+3])+ + ((plane[0] * cvx2.final_posr->pos[0]) + + (plane[1] * cvx2.final_posr->pos[1]) + + (plane[2] * cvx2.final_posr->pos[2])); + dContactGeom *target = SAFECONTACT(flags, contact, curc, skip); + target->g1=&cvx1; // g1 is the one pushed + target->g2=&cvx2; + if(IntersectSegmentPlane(e1,e2,plane,t,target->pos)) + { + if(IsPointInPolygon(target->pos,pPoly,plane,&cvx2,q)) + { + target->depth = dInfinity; + for(sizeint k=0;k<cvx2.planecount;++k) + { + if(k==j) continue; // we're already at 0 depth on this plane + // Rotate + dMultiply0_331(depthplane,cvx2.final_posr->R,cvx2.planes+(k*4)); + dNormalize3(depthplane); + // Translate + depthplane[3]= + (cvx2.planes[(k*4)+3])+ + ((plane[0] * cvx2.final_posr->pos[0]) + + (plane[1] * cvx2.final_posr->pos[1]) + + (plane[2] * cvx2.final_posr->pos[2])); + dReal depth = (dVector3Dot(depthplane, target->pos) - depthplane[3]); // Ax + By + Cz - D + if((fabs(depth)<fabs(target->depth))&&((depth<-dEpsilon)||(depth>dEpsilon))) + { + target->depth=depth; + dVector3Copy(depthplane,target->normal); + } + } + ++curc; + if(curc==maxc) + return true; + } + } + pPoly+=pPoly[0]+1; + } + } + return false; +} + +/* +Helper struct +*/ +struct ConvexConvexSATOutput +{ + dReal min_depth; + int depth_type; + dVector3 dist; // distance from center to center, from cvx1 to cvx2 + dVector3 e1a,e1b,e2a,e2b; // e1a to e1b = edge in cvx1,e2a to e2b = edge in cvx2. +}; + +/*! \brief Does an axis separation test using cvx1 planes on cvx1 and cvx2, returns true for a collision false for no collision + \param cvx1 [IN] First Convex object, its planes are used to do the tests + \param cvx2 [IN] Second Convex object + \param min_depth [IN/OUT] Used to input as well as output the minimum depth so far, must be set to a huge value such as dInfinity for initialization. + \param g1 [OUT] Pointer to the convex which should be used in the returned contact as g1 + \param g2 [OUT] Pointer to the convex which should be used in the returned contact as g2 +*/ +inline bool CheckSATConvexFaces(dxConvex& cvx1, + dxConvex& cvx2, + ConvexConvexSATOutput& ccso) +{ + dReal min,max,min1,max1,min2,max2,depth; + dVector4 plane; + for(unsigned int i=0;i<cvx1.planecount;++i) + { + // -- Apply Transforms -- + // Rotate + dMultiply0_331(plane,cvx1.final_posr->R,cvx1.planes+(i*4)); + dNormalize3(plane); + // Translate + plane[3]= + (cvx1.planes[(i*4)+3])+ + ((plane[0] * cvx1.final_posr->pos[0]) + + (plane[1] * cvx1.final_posr->pos[1]) + + (plane[2] * cvx1.final_posr->pos[2])); + ComputeInterval(cvx1,plane,min1,max1); + ComputeInterval(cvx2,plane,min2,max2); + if(max2<min1 || max1<min2) return false; + min = dMAX(min1, min2); + max = dMIN(max1, max2); + depth = max-min; + /* + Take only into account the faces that penetrate cvx1 to determine + minimum depth + ((max2*min2)<=0) = different sign, or one is zero and thus + cvx2 barelly touches cvx1 + */ + if (((max2*min2)<=0) && (dFabs(depth)<dFabs(ccso.min_depth))) + { + // Flip plane because the contact normal must point INTO g1, + // plus the integrator seems to like positive depths better than negative ones + ccso.min_depth=-depth; + ccso.depth_type = 1; // 1 = face-something + } + } + return true; +} +/*! \brief Does an axis separation test using cvx1 and cvx2 edges, returns true for a collision false for no collision + \param cvx1 [IN] First Convex object + \param cvx2 [IN] Second Convex object + \param min_depth [IN/OUT] Used to input as well as output the minimum depth so far, must be set to a huge value such as dInfinity for initialization. + \param g1 [OUT] Pointer to the convex which should be used in the returned contact as g1 + \param g2 [OUT] Pointer to the convex which should be used in the returned contact as g2 +*/ +inline bool CheckSATConvexEdges(dxConvex& cvx1, + dxConvex& cvx2, + ConvexConvexSATOutput& ccso) +{ + // Test cross products of pairs of edges + dReal depth,min,max,min1,max1,min2,max2; + dVector4 plane; + dVector3 e1,e2,e1a,e1b,e2a,e2b; + dVector3 dist; + dVector3Copy(ccso.dist,dist); + unsigned int s1 = cvx1.SupportIndex(dist); + // invert direction + dVector3Inv(dist); + unsigned int s2 = cvx2.SupportIndex(dist); + for(unsigned int i = 0;i<cvx1.edgecount;++i) + { + // Skip edge if it doesn't contain the extremal vertex + if((cvx1.edges[i].first!=s1)&&(cvx1.edges[i].second!=s1)) continue; + // we only need to apply rotation here + dMultiply0_331(e1a,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].first*3)); + dMultiply0_331(e1b,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].second*3)); + e1[0]=e1b[0]-e1a[0]; + e1[1]=e1b[1]-e1a[1]; + e1[2]=e1b[2]-e1a[2]; + for(unsigned int j = 0;j<cvx2.edgecount;++j) + { + // Skip edge if it doesn't contain the extremal vertex + if((cvx2.edges[j].first!=s2)&&(cvx2.edges[j].second!=s2)) continue; + // we only need to apply rotation here + dMultiply0_331 (e2a,cvx2.final_posr->R,cvx2.points+(cvx2.edges[j].first*3)); + dMultiply0_331 (e2b,cvx2.final_posr->R,cvx2.points+(cvx2.edges[j].second*3)); + e2[0]=e2b[0]-e2a[0]; + e2[1]=e2b[1]-e2a[1]; + e2[2]=e2b[2]-e2a[2]; + dCalcVectorCross3(plane,e1,e2); + if(dCalcVectorDot3(plane,plane)<dEpsilon) /* edges are parallel */ continue; + dNormalize3(plane); + plane[3]=0; + ComputeInterval(cvx1,plane,min1,max1); + ComputeInterval(cvx2,plane,min2,max2); + if(max2 < min1 || max1 < min2) return false; + min = dMAX(min1, min2); + max = dMIN(max1, max2); + depth = max-min; + if (((dFabs(depth)+dEpsilon)<dFabs(ccso.min_depth))) + { + ccso.min_depth=depth; + ccso.depth_type = 2; // 2 means edge-edge + // use cached values, add position + dVector3Copy(e1a,ccso.e1a); + dVector3Copy(e1b,ccso.e1b); + ccso.e1a[0]+=cvx1.final_posr->pos[0]; + ccso.e1a[1]+=cvx1.final_posr->pos[1]; + ccso.e1a[2]+=cvx1.final_posr->pos[2]; + ccso.e1b[0]+=cvx1.final_posr->pos[0]; + ccso.e1b[1]+=cvx1.final_posr->pos[1]; + ccso.e1b[2]+=cvx1.final_posr->pos[2]; + dVector3Copy(e2a,ccso.e2a); + dVector3Copy(e2b,ccso.e2b); + ccso.e2a[0]+=cvx2.final_posr->pos[0]; + ccso.e2a[1]+=cvx2.final_posr->pos[1]; + ccso.e2a[2]+=cvx2.final_posr->pos[2]; + ccso.e2b[0]+=cvx2.final_posr->pos[0]; + ccso.e2b[1]+=cvx2.final_posr->pos[1]; + ccso.e2b[2]+=cvx2.final_posr->pos[2]; + } + } + } + return true; +} + +#if 0 +/*! \brief Returns the index of the plane/side of the incident convex (ccso.g2) + * which is closer to the reference convex (ccso.g1) side + * + * This function just looks for the incident face that is facing the reference face + * and is the closest to being parallel to it, which sometimes is. + */ +inline unsigned int GetIncidentSide(ConvexConvexSATOutput& ccso) +{ + dVector3 nis; // (N)ormal in (I)ncident convex (S)pace + dReal SavedDot; + dReal Dot; + unsigned int incident_side=0; + // Rotate the plane normal into incident convex space + // (things like this should be done all over this file, + // will look into that) + dMultiply1_331(nis,ccso.g2->final_posr->R,ccso.plane); + SavedDot = dCalcVectorDot3(nis,ccso.g2->planes); + for(unsigned int i=1;i<ccso.g2->planecount;++i) + { + Dot = dCalcVectorDot3(nis,ccso.g2->planes+(i*4)); + if(Dot>SavedDot) + { + SavedDot=Dot; + incident_side=i; + } + } + return incident_side; +} +#endif + +inline unsigned int GetSupportSide(dVector3& dir,dxConvex& cvx) +{ + dVector3 dics,tmp; // Direction in convex space + dReal SavedDot; + dReal Dot; + unsigned int side=0; + dVector3Copy(dir,tmp); + dNormalize3(tmp); + dMultiply1_331(dics,cvx.final_posr->R,tmp); + SavedDot = dCalcVectorDot3(dics,cvx.planes); + for(unsigned int i=1;i<cvx.planecount;++i) + { + Dot = dCalcVectorDot3(dics,cvx.planes+(i*4)); + if(Dot>SavedDot) + { + SavedDot=Dot; + side=i; + } + } + return side; +} + +/*! \brief Does an axis separation test between the 2 convex shapes +using faces and edges */ +int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags, + dContactGeom *contact, int skip) +{ + ConvexConvexSATOutput ccso; +#ifndef dNDEBUG + memset(&ccso, 0, sizeof(ccso)); // get rid of 'uninitialized values' warning +#endif + ccso.min_depth=dInfinity; // Min not min at all + ccso.depth_type=0; // no type + // precompute distance vector + dSubtractVectors3(ccso.dist, cvx2.final_posr->pos, cvx1.final_posr->pos); + int maxc = flags & NUMC_MASK; + dIASSERT(maxc != 0); + dVector3 i1,i2,r1,r2; // edges of incident and reference faces respectively + int contacts=0; + if(!CheckSATConvexFaces(cvx1,cvx2,ccso)) + { + return 0; + } + else + if(!CheckSATConvexFaces(cvx2,cvx1,ccso)) + { + return 0; + } + else if(!CheckSATConvexEdges(cvx1,cvx2,ccso)) + { + return 0; + } + // If we get here, there was a collision + if(ccso.depth_type==1) // face-face + { + // cvx1 MUST always be in contact->g1 and cvx2 in contact->g2 + // This was learned the hard way :( + unsigned int incident_side; + const unsigned int* pIncidentPoly; + const unsigned int* pIncidentPoints; + unsigned int reference_side; + const unsigned int* pReferencePoly; + const unsigned int* pReferencePoints; + dVector4 plane,rplane,iplane; + dVector3 tmp; + dVector3 dist,p; + dReal t,d,d1,d2; + bool outside,out; + dVector3Copy(ccso.dist,dist); + reference_side = GetSupportSide(dist,cvx1); + dNegateVector3(dist); + incident_side = GetSupportSide(dist,cvx2); + + pReferencePoly = cvx1.polygons; + pIncidentPoly = cvx2.polygons; + // Get Reference plane (We may not have to apply transforms Optimization Oportunity) + // Rotate + dMultiply0_331(rplane,cvx1.final_posr->R,cvx1.planes+(reference_side*4)); + dNormalize3(rplane); + // Translate + rplane[3]= + (cvx1.planes[(reference_side*4)+3])+ + ((rplane[0] * cvx1.final_posr->pos[0]) + + (rplane[1] * cvx1.final_posr->pos[1]) + + (rplane[2] * cvx1.final_posr->pos[2])); + // flip + rplane[0]=-rplane[0]; + rplane[1]=-rplane[1]; + rplane[2]=-rplane[2]; + rplane[3]=-rplane[3]; + for(unsigned int i=0;i<incident_side;++i) + { + pIncidentPoly+=pIncidentPoly[0]+1; + } + pIncidentPoints = pIncidentPoly+1; + // Get the first point of the incident face + dMultiply0_331(i2,cvx2.final_posr->R,&cvx2.points[(pIncidentPoints[0]*3)]); + dVector3Add(i2,cvx2.final_posr->pos,i2); + // Get the same point in the reference convex space + dVector3Copy(i2,r2); + dVector3Subtract(r2,cvx1.final_posr->pos,r2); + dVector3Copy(r2,tmp); + dMultiply1_331(r2,cvx1.final_posr->R,tmp); + for(unsigned int i=0;i<pIncidentPoly[0];++i) + { + // Move i2 to i1, r2 to r1 + dVector3Copy(i2,i1); + dVector3Copy(r2,r1); + dMultiply0_331(i2,cvx2.final_posr->R,&cvx2.points[(pIncidentPoints[(i+1)%pIncidentPoly[0]]*3)]); + dVector3Add(i2,cvx2.final_posr->pos,i2); + // Get the same point in the reference convex space + dVector3Copy(i2,r2); + dVector3Subtract(r2,cvx1.final_posr->pos,r2); + dVector3Copy(r2,tmp); + dMultiply1_331(r2,cvx1.final_posr->R,tmp); + outside=false; + for(unsigned int j=0;j<cvx1.planecount;++j) + { + plane[0]=cvx1.planes[(j*4)+0]; + plane[1]=cvx1.planes[(j*4)+1]; + plane[2]=cvx1.planes[(j*4)+2]; + plane[3]=cvx1.planes[(j*4)+3]; + // Get the distance from the points to the plane + d1 = r1[0]*plane[0]+ + r1[1]*plane[1]+ + r1[2]*plane[2]- + plane[3]; + d2 = r2[0]*plane[0]+ + r2[1]*plane[1]+ + r2[2]*plane[2]- + plane[3]; + if(d1*d2<0) + { + out = false; + + // Edge intersects plane + if (!IntersectSegmentPlane(r1,r2,plane,t,p)) + { + out = true; + } + + if (!out) + { + // Check the resulting point again to make sure it is inside the reference convex + for (unsigned int k = 0; k < cvx1.planecount; ++k) + { + d = p[0]*cvx1.planes[(k*4)+0]+ + p[1]*cvx1.planes[(k*4)+1]+ + p[2]*cvx1.planes[(k*4)+2]- + cvx1.planes[(k*4)+3]; + if(d>0) + { + out = true; + break; + } + } + } + + if(!out) + { +#if 0 + // Use t to move p into global space + p[0] = i1[0]+((i2[0]-i1[0])*t); + p[1] = i1[1]+((i2[1]-i1[1])*t); + p[2] = i1[2]+((i2[2]-i1[2])*t); +#else + // Apply reference convex transformations to p + // The commented out piece of code is likelly to + // produce less operations than this one, but + // this way we know we are getting the right data + dMultiply0_331(tmp,cvx1.final_posr->R,p); + dVector3Add(tmp,cvx1.final_posr->pos,p); +#endif + // get p's distance to reference plane + d = p[0]*rplane[0]+ + p[1]*rplane[1]+ + p[2]*rplane[2]- + rplane[3]; + if(d>0) + { + dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip); + dVector3Copy(p, target->pos); + dVector3Copy(rplane, target->normal); + target->g1 = &cvx1; + target->g2 = &cvx2; + target->depth = d; + ++contacts; + if (contacts==maxc) return contacts; + } + } + } + if(d1>0) + { + outside=true; + } + } + if(outside) continue; + d = i1[0]*rplane[0]+ + i1[1]*rplane[1]+ + i1[2]*rplane[2]- + rplane[3]; + if(d>0) + { + dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip); + dVector3Copy(i1, target->pos); + dVector3Copy(rplane, target->normal); + target->g1 = &cvx1; + target->g2 = &cvx2; + target->depth = d; + ++contacts; + if (contacts==maxc) return contacts; + } + } + // IF we get here, we got the easiest contacts to calculate, + // but there is still space in the contacts array for more. + // So, project the Reference's face points onto the Incident face + // plane and test them for inclusion in the reference plane as well. + // We already have computed intersections so, skip those. + + /* Get Incident plane, we need it for projection */ + /* Rotate */ + dMultiply0_331(iplane,cvx2.final_posr->R,cvx2.planes+(incident_side*4)); + dNormalize3(iplane); + /* Translate */ + iplane[3]= + (cvx2.planes[(incident_side*4)+3]) + + ((iplane[0] * cvx2.final_posr->pos[0]) + + (iplane[1] * cvx2.final_posr->pos[1]) + + (iplane[2] * cvx2.final_posr->pos[2])); + // get reference face + for(unsigned int i=0;i<reference_side;++i) + { + pReferencePoly+=pReferencePoly[0]+1; + } + pReferencePoints = pReferencePoly+1; + for(unsigned int i=0;i<pReferencePoly[0];++i) + { + dMultiply0_331(i1,cvx1.final_posr->R,&cvx1.points[(pReferencePoints[i]*3)]); + dVector3Add(cvx1.final_posr->pos,i1,i1); + // Project onto Incident face plane + t = -(i1[0]*iplane[0]+ + i1[1]*iplane[1]+ + i1[2]*iplane[2]- + iplane[3]); + i1[0]+=iplane[0]*t; + i1[1]+=iplane[1]*t; + i1[2]+=iplane[2]*t; + // Get the same point in the incident convex space + dVector3Copy(i1,r1); + dVector3Subtract(r1,cvx2.final_posr->pos,r1); + dVector3Copy(r1,tmp); + dMultiply1_331(r1,cvx2.final_posr->R,tmp); + // Check if it is outside the incident convex + out = false; + for(unsigned int j=0;j<cvx2.planecount;++j) + { + d = r1[0]*cvx2.planes[(j*4)+0]+ + r1[1]*cvx2.planes[(j*4)+1]+ + r1[2]*cvx2.planes[(j*4)+2]- + cvx2.planes[(j*4)+3]; + if(d>=0){out = true;break;}; + } + if(!out) + { + // check that the point is not a duplicate + outside = false; + for(int j=0;j<contacts;++j) + { + dContactGeom *cur_contact = SAFECONTACT(flags, contact, j, skip); + if((cur_contact->pos[0] == i1[0]) && + (cur_contact->pos[1] == i1[1]) && + (cur_contact->pos[2] == i1[2])) + { + outside=true; + } + } + if(!outside) + { + d = i1[0]*rplane[0]+ + i1[1]*rplane[1]+ + i1[2]*rplane[2]- + rplane[3]; + if(d>0) + { + dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip); + dVector3Copy(i1, target->pos); + dVector3Copy(rplane, target->normal); + target->g1 = &cvx1; + target->g2 = &cvx2; + target->depth = d; + ++contacts; + if (contacts==maxc) return contacts; + } + } + } + } + } + else if (ccso.depth_type == 2) // edge-edge + { + dVector3 c1, c2; + ClosestPointBetweenSegments(ccso.e1a, ccso.e1b, ccso.e2a, ccso.e2b, c1, c2); + + dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip); + dSubtractVectors3(target->normal, c2, c1); + dReal depth_square = dCalcVectorLengthSquare3(target->normal); + + if (dxSafeNormalize3(target->normal)) + { + target->depth = dSqrt(depth_square); + } + else + { + // If edges coincide return direction from one center to the other as the contact normal + dVector3Copy(ccso.dist, target->normal); + + if (!dxSafeNormalize3(target->normal)) + { + // If the both centers coincide as well return an arbitrary vector. The depth is going to be zero anyway. + dAssignVector3(target->normal, 1, 0, 0); + } + + target->depth = 0; // Since the edges coincide, return a contact of zero depth + } + + target->g1 = &cvx1; + target->g2 = &cvx2; + dVector3Copy(c1, target->pos); + contacts++; + } + return contacts; +} + +int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dConvexClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + dxConvex *Convex1 = (dxConvex*) o1; + dxConvex *Convex2 = (dxConvex*) o2; + return TestConvexIntersection(*Convex1,*Convex2,flags, + contact,skip); +} + +#if 0 +int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT( o1->type == dRayClass ); + dIASSERT( o2->type == dConvexClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + dxRay* ray = (dxRay*) o1; + dxConvex* convex = (dxConvex*) o2; + dVector3 origin,destination,contactpoint,out; + dReal depth; + dVector4 plane; + unsigned int *pPoly=convex->polygons; + // Calculate ray origin and destination + destination[0]=0; + destination[1]=0; + destination[2]= ray->length; + // -- Rotate -- + dMultiply0_331(destination,ray->final_posr->R,destination); + origin[0]=ray->final_posr->pos[0]; + origin[1]=ray->final_posr->pos[1]; + origin[2]=ray->final_posr->pos[2]; + destination[0]+=origin[0]; + destination[1]+=origin[1]; + destination[2]+=origin[2]; + for(int i=0;i<convex->planecount;++i) + { + // Rotate + dMultiply0_331(plane,convex->final_posr->R,convex->planes+(i*4)); + // Translate + plane[3]= + (convex->planes[(i*4)+3])+ + ((plane[0] * convex->final_posr->pos[0]) + + (plane[1] * convex->final_posr->pos[1]) + + (plane[2] * convex->final_posr->pos[2])); + if(IntersectSegmentPlane(origin, + destination, + plane, + depth, + contactpoint)) + { + if(IsPointInPolygon(contactpoint,pPoly,plane,convex,out)) + { + contact->pos[0]=contactpoint[0]; + contact->pos[1]=contactpoint[1]; + contact->pos[2]=contactpoint[2]; + contact->normal[0]=plane[0]; + contact->normal[1]=plane[1]; + contact->normal[2]=plane[2]; + contact->depth=depth; + contact->g1 = ray; + contact->g2 = convex; + contact->side1 = -1; + contact->side2 = -1; // TODO: set plane index? + return 1; + } + } + pPoly+=pPoly[0]+1; + } + return 0; +} +#else +// Ray - Convex collider by David Walters, June 2006 +int dCollideRayConvex(dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT(skip >= (int)sizeof(dContactGeom)); + dIASSERT(o1->type == dRayClass); + dIASSERT(o2->type == dConvexClass); + dIASSERT((flags & NUMC_MASK) >= 1); + + dxRay* ray = (dxRay*)o1; + dxConvex* convex = (dxConvex*)o2; + + contact->g1 = ray; + contact->g2 = convex; + contact->side1 = -1; + contact->side2 = -1; // TODO: set plane index? + + dReal alpha, beta, nsign; + int flag = 0; + + // + // Compute some useful info + // + + dVector3 ray_pos = { + ray->final_posr->pos[0] - convex->final_posr->pos[0], + ray->final_posr->pos[1] - convex->final_posr->pos[1], + ray->final_posr->pos[2] - convex->final_posr->pos[2] + }; + + dVector3 ray_dir = { + ray->final_posr->R[0 * 4 + 2], + ray->final_posr->R[1 * 4 + 2], + ray->final_posr->R[2 * 4 + 2] + }; + + dMultiply1_331(ray_pos, convex->final_posr->R, ray_pos); + dMultiply1_331(ray_dir, convex->final_posr->R, ray_dir); + + for (unsigned int i = 0; i < convex->planecount; ++i) + { + // Alias this plane. + const dReal* plane = convex->planes + (i * 4); + + // If alpha >= 0 then start point is outside of plane. + alpha = dCalcVectorDot3(plane, ray_pos) - plane[3]; + + // If any alpha is positive, then + // the ray start is _outside_ of the hull + if (alpha >= 0) + { + flag = 1; + break; + } + } + + // If the ray starts inside the convex hull, then everything is flipped. + nsign = (flag) ? REAL(1.0) : REAL(-1.0); + + + // + // Find closest contact point + // + + // Assume no contacts. + contact->depth = dInfinity; + + for (unsigned int i = 0; i < convex->planecount; ++i) + { + // Alias this plane. + const dReal* plane = convex->planes + (i * 4); + + // If alpha >= 0 then point is outside of plane. + alpha = nsign * (dCalcVectorDot3(plane, ray_pos) - plane[3]); + + // Compute [ plane-normal DOT ray-normal ], (/flip) + beta = dCalcVectorDot3(plane, ray_dir) * nsign; + + // Ray is pointing at the plane? ( beta < 0 ) + // Ray start to plane is within maximum ray length? + // Ray start to plane is closer than the current best distance? + if (beta < -dEpsilon && + alpha >= 0 && alpha <= ray->length && + alpha < contact->depth) + { + // Compute contact point on convex hull surface. + contact->pos[0] = ray_pos[0] + alpha * ray_dir[0]; + contact->pos[1] = ray_pos[1] + alpha * ray_dir[1]; + contact->pos[2] = ray_pos[2] + alpha * ray_dir[2]; + + flag = 0; + + // For all _other_ planes. + for (unsigned int j = 0; j < convex->planecount; ++j) + { + if (i == j) + continue; // Skip self. + + // Alias this plane. + const dReal* planej = convex->planes + (j * 4); + + // If beta >= 0 then start is outside of plane. + beta = dCalcVectorDot3(planej, contact->pos) - planej[3]; + + // If any beta is positive, then the contact point + // is not on the surface of the convex hull - it's just + // intersecting some part of its infinite extent. + if (beta > dEpsilon) + { + flag = 1; + break; + } + } + + // Contact point isn't outside hull's surface? then it's a good contact! + if (flag == 0) + { + // Store the contact normal, possibly flipped. + contact->normal[0] = nsign * plane[0]; + contact->normal[1] = nsign * plane[1]; + contact->normal[2] = nsign * plane[2]; + + // Store depth + contact->depth = alpha; + + if ((flags & CONTACTS_UNIMPORTANT) && contact->depth <= ray->length) + { + // Break on any contact if contacts are not important + break; + } + } + } + } + // Contact? + if (contact->depth <= ray->length) + { + // Adjust contact position and normal back to global space + dMultiply0_331(contact->pos, convex->final_posr->R, contact->pos); + dMultiply0_331(contact->normal, convex->final_posr->R, contact->normal); + contact->pos[0] += convex->final_posr->pos[0]; + contact->pos[1] += convex->final_posr->pos[1]; + contact->pos[2] += convex->final_posr->pos[2]; + return true; + } + return false; +} + +#endif +//<-- Convex Collision diff --git a/libs/ode-0.16.1/ode/src/coop_matrix_types.h b/libs/ode-0.16.1/ode/src/coop_matrix_types.h new file mode 100644 index 0000000..d94e04b --- /dev/null +++ b/libs/ode-0.16.1/ode/src/coop_matrix_types.h @@ -0,0 +1,158 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Cooperative matrix algorithm types +// Copyright (C) 2017-2019 Oleh Derevenko (odar@eleks.com - change all "a" to "e") + + +#ifndef _ODE_COOP_MATRIX_TYPES_H_ +#define _ODE_COOP_MATRIX_TYPES_H_ + + + +#include "threadingutils.h" +#include "common.h" +#include "error.h" + + +#ifndef dCOOPERATIVE_ENABLED + +#if dATOMICS_ENABLED && !dTHREADING_INTF_DISABLED + +#define dCOOPERATIVE_ENABLED 1 + + +#endif // #if dATOMICS_ENABLED && !dTHREADING_INTF_DISABLED + + +#endif // #ifndef dCOOPERATIVE_ENABLED + + +enum +{ + COOP_THREAD_DATA_ALIGNMENT_SIZE = 64, // Typical size of a cache line +}; + + +typedef uintptr cellindexint; + + +enum CellContextInstance +{ + CCI__MIN, + + CCI_FIRST = CCI__MIN, + CCI_SECOND, + + CCI__MAX, + CCI__LOG2_OF_MAX = 1, + + CCI__DEFAULT = CCI__MIN, +}; +dSASSERT(1 << CCI__LOG2_OF_MAX >= CCI__MAX); + +static inline +CellContextInstance buildNextContextInstance(CellContextInstance instance) +{ + dIASSERT(dIN_RANGE(instance, CCI__MIN, CCI__MAX)); + dSASSERT(CCI__MAX == 2); + + return (CellContextInstance)(CCI_FIRST + CCI_SECOND - instance); +} + + +enum +{ + CELLDESC_CCI_BITMASK = (1 << CCI__LOG2_OF_MAX) - 1, + CELLDESC_LOCK_BIT = 1 << CCI__LOG2_OF_MAX, + CELLDESC__HELPER_BITS = CELLDESC_CCI_BITMASK | CELLDESC_LOCK_BIT, + CELLDESC__COLINDEX_BASE = CELLDESC__HELPER_BITS + 1, +}; + +#define MAKE_CELLDESCRIPTOR(columnIndex, contextInstance, locked) ((cellindexint)((cellindexint)(columnIndex) * CELLDESC__COLINDEX_BASE + (contextInstance) + ((locked) ? CELLDESC_LOCK_BIT : 0))) +#define MARK_CELLDESCRIPTOR_LOCKED(descriptor) ((cellindexint)((descriptor) | CELLDESC_LOCK_BIT)) +#define GET_CELLDESCRIPTOR_COLUMNINDEX(descriptor) ((unsigned int)((cellindexint)(descriptor) / CELLDESC__COLINDEX_BASE)) +#define GET_CELLDESCRIPTOR_CONTEXTINSTANCE(descriptor) ((CellContextInstance)((descriptor) & CELLDESC_CCI_BITMASK)) +#define GET_CELLDESCRIPTOR_ISLOCKED(descriptor) (((descriptor) & CELLDESC_LOCK_BIT) != 0) + +#define INVALID_CELLDESCRIPTOR MAKE_CELLDESCRIPTOR(GET_CELLDESCRIPTOR_COLUMNINDEX(-1), CCI__MAX - 1, true) + + +enum BlockProcessingState +{ + BPS_COMPETING_FOR_A_BLOCK = -1, + BPS_NO_BLOCKS_PROCESSED, + BPS_SOME_BLOCKS_PROCESSED, +}; + + +class CooperativeAtomics +{ +public: + static atomicord32 AtomicDecrementUint32(volatile atomicord32 *paoDestination) + { +#if dCOOPERATIVE_ENABLED + return ::AtomicDecrement(paoDestination); +#else + dIASSERT(false); return 0; // The function is not supposed to be called in this case +#endif // #if dCOOPERATIVE_ENABLED + } + + static bool AtomicCompareExchangeUint32(volatile atomicord32 *paoDestination, atomicord32 aoComparand, atomicord32 aoExchange) + { +#if dCOOPERATIVE_ENABLED + return ::AtomicCompareExchange(paoDestination, aoComparand, aoExchange); +#else + dIASSERT(false); return false; // The function is not supposed to be called in this case +#endif // #if dCOOPERATIVE_ENABLED + } + + static bool AtomicCompareExchangeCellindexint(volatile cellindexint *destination, cellindexint comparand, cellindexint exchange) + { +#if dCOOPERATIVE_ENABLED + return ::AtomicCompareExchangePointer((volatile atomicptr *)destination, (atomicptr)comparand, (atomicptr)exchange); +#else + dIASSERT(false); return false; // The function is not supposed to be called in this case +#endif // #if dCOOPERATIVE_ENABLED + } + + static void AtomicStoreCellindexint(volatile cellindexint *destination, cellindexint value) + { +#if dCOOPERATIVE_ENABLED + ::AtomicStorePointer((volatile atomicptr *)destination, (atomicptr)value); +#else + dIASSERT(false); // The function is not supposed to be called in this case +#endif // #if dCOOPERATIVE_ENABLED + } + + static void AtomicReadReorderBarrier() + { +#if dCOOPERATIVE_ENABLED + ::AtomicReadReorderBarrier(); +#else + dIASSERT(false); // The function is not supposed to be called in this case +#endif // #if dCOOPERATIVE_ENABLED + } +}; + + +#endif // #ifndef _ODE_COOP_MATRIX_TYPES_H_ diff --git a/libs/ode-0.16.1/ode/src/cylinder.cpp b/libs/ode-0.16.1/ode/src/cylinder.cpp new file mode 100644 index 0000000..cf5cc64 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/cylinder.cpp @@ -0,0 +1,108 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + + +#define dMAX(A,B) ((A)>(B) ? (A) : (B)) + + +// flat cylinder public API + +dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) : +dxGeom (space,1) +{ + dAASSERT (_radius >= 0 && _length >= 0); + type = dCylinderClass; + radius = _radius; + lz = _length; + updateZeroSizedFlag(!_radius || !_length); +} + + +void dxCylinder::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal dOneMinusR2Square = (dReal)(REAL(1.0) - R[2]*R[2]); + dReal xrange = dFabs(R[2]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR2Square)); + dReal dOneMinusR6Square = (dReal)(REAL(1.0) - R[6]*R[6]); + dReal yrange = dFabs(R[6]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR6Square)); + dReal dOneMinusR10Square = (dReal)(REAL(1.0) - R[10]*R[10]); + dReal zrange = dFabs(R[10]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR10Square)); + + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length) +{ + return new dxCylinder (space,radius,length); +} + +void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length) +{ + dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder"); + dAASSERT (radius >= 0 && length >= 0); + dxCylinder *c = (dxCylinder*) cylinder; + c->radius = radius; + c->lz = length; + c->updateZeroSizedFlag(!radius || !length); + dGeomMoved (cylinder); +} + +void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length) +{ + dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder"); + dxCylinder *c = (dxCylinder*) cylinder; + *radius = c->radius; + *length = c->lz; +} + + diff --git a/libs/ode-0.16.1/ode/src/default_threading.cpp b/libs/ode-0.16.1/ode/src/default_threading.cpp new file mode 100644 index 0000000..7f255f6 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/default_threading.cpp @@ -0,0 +1,77 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading base wrapper class header file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * The default threading instance holder class implementation + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#include <ode/common.h> +#include <ode/threading_impl.h> +#include "config.h" +#include "default_threading.h" +#include "error.h" + + +/*static */dThreadingImplementationID DefaultThreadingHolder::m_defaultThreadingImpl = NULL; +/*static */const dThreadingFunctionsInfo *DefaultThreadingHolder::m_defaultThreadingFunctions = NULL; + + +/*static */ +bool DefaultThreadingHolder::initializeDefaultThreading() +{ + dIASSERT(m_defaultThreadingImpl == NULL); + + bool initResult = false; + + dThreadingImplementationID threadingImpl = dThreadingAllocateSelfThreadedImplementation(); + + if (threadingImpl != NULL) + { + m_defaultThreadingFunctions = dThreadingImplementationGetFunctions(threadingImpl); + m_defaultThreadingImpl = threadingImpl; + + initResult = true; + } + + return initResult; +} + +/*static */ +void DefaultThreadingHolder::finalizeDefaultThreading() +{ + dThreadingImplementationID threadingImpl = m_defaultThreadingImpl; + + if (threadingImpl != NULL) + { + dThreadingFreeImplementation(threadingImpl); + + m_defaultThreadingFunctions = NULL; + m_defaultThreadingImpl = NULL; + } +} + diff --git a/libs/ode-0.16.1/ode/src/default_threading.h b/libs/ode-0.16.1/ode/src/default_threading.h new file mode 100644 index 0000000..372a777 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/default_threading.h @@ -0,0 +1,55 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading base wrapper class header file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * A default threading instance holder class definition + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#ifndef _ODE__PRIVATE_DEFAULT_THREADING_H_ +#define _ODE__PRIVATE_DEFAULT_THREADING_H_ + + +#include <ode/threading.h> + + +class DefaultThreadingHolder +{ +public: + static bool initializeDefaultThreading(); + static void finalizeDefaultThreading(); + + static dThreadingImplementationID getDefaultThreadingImpl() { return m_defaultThreadingImpl; } + static const dThreadingFunctionsInfo *getDefaultThreadingFunctions() { return m_defaultThreadingFunctions; } + +private: + static dThreadingImplementationID m_defaultThreadingImpl; + static const dThreadingFunctionsInfo *m_defaultThreadingFunctions; +}; + + +#endif // #ifndef _ODE__PRIVATE_DEFAULT_THREADING_H_ diff --git a/libs/ode-0.16.1/ode/src/error.cpp b/libs/ode-0.16.1/ode/src/error.cpp new file mode 100644 index 0000000..0b1a979 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/error.cpp @@ -0,0 +1,179 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/error.h> +#include "config.h" + + +static dMessageFunction *error_function = 0; +static dMessageFunction *debug_function = 0; +static dMessageFunction *message_function = 0; + + +extern "C" void dSetErrorHandler (dMessageFunction *fn) +{ + error_function = fn; +} + + +extern "C" void dSetDebugHandler (dMessageFunction *fn) +{ + debug_function = fn; +} + + +extern "C" void dSetMessageHandler (dMessageFunction *fn) +{ + message_function = fn; +} + + +extern "C" dMessageFunction *dGetErrorHandler() +{ + return error_function; +} + + +extern "C" dMessageFunction *dGetDebugHandler() +{ + return debug_function; +} + + +extern "C" dMessageFunction *dGetMessageHandler() +{ + return message_function; +} + + +static void printMessage (int num, const char *msg1, const char *msg2, + va_list ap) +{ + fflush (stderr); + fflush (stdout); + if (num) fprintf (stderr,"\n%s %d: ",msg1,num); + else fprintf (stderr,"\n%s: ",msg1); + vfprintf (stderr,msg2,ap); + fprintf (stderr,"\n"); + fflush (stderr); +} + +//**************************************************************************** +// unix + +#ifndef WIN32 + +extern "C" void dError (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (error_function) error_function (num,msg,ap); + else printMessage (num,"ODE Error",msg,ap); + va_end (ap); + exit (1); +} + + +extern "C" void dDebug (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (debug_function) debug_function (num,msg,ap); + else printMessage (num,"ODE INTERNAL ERROR",msg,ap); + va_end (ap); + // *((char *)0) = 0; ... commit SEGVicide + abort(); +} + + +extern "C" void dMessage (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (message_function) message_function (num,msg,ap); + else printMessage (num,"ODE Message",msg,ap); + va_end (ap); +} + +#endif + +//**************************************************************************** +// windows + +#ifdef WIN32 + +// isn't cygwin annoying! +#ifdef CYGWIN +#define _snprintf snprintf +#define _vsnprintf vsnprintf +#endif + + +#include "windows.h" + + +extern "C" void dError (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (error_function) error_function (num,msg,ap); + else { + char s[1000],title[100]; + _snprintf (title,sizeof(title),"ODE Error %d",num); + _vsnprintf (s,sizeof(s),msg,ap); + s[sizeof(s)-1] = 0; + MessageBox(0,s,title,MB_OK | MB_ICONWARNING); + } + va_end (ap); + exit (1); +} + + +extern "C" void dDebug (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (debug_function) debug_function (num,msg,ap); + else { + char s[1000],title[100]; + _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num); + _vsnprintf (s,sizeof(s),msg,ap); + s[sizeof(s)-1] = 0; + MessageBox(0,s,title,MB_OK | MB_ICONSTOP); + } + va_end (ap); + abort(); +} + + +extern "C" void dMessage (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (message_function) message_function (num,msg,ap); + else printMessage (num,"ODE Message",msg,ap); + va_end (ap); +} + + +#endif diff --git a/libs/ode-0.16.1/ode/src/error.h b/libs/ode-0.16.1/ode/src/error.h new file mode 100644 index 0000000..4f561f8 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/error.h @@ -0,0 +1,101 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* Library private error handling functions and macros */ + +#ifndef _ODE__PRIVATE_ERROR_H_ +#define _ODE__PRIVATE_ERROR_H_ + +#include <ode/error.h> +#include <ode/common.h> + + + +/* debugging: + * IASSERT is an internal assertion, i.e. a consistency check. if it fails + * we want to know where. + * UASSERT is a user assertion, i.e. if it fails a nice error message + * should be printed for the user. + * AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)" + * is printed. + * DEBUGMSG just prints out a message + */ + +# if defined(__STDC__) && __STDC_VERSION__ >= 199901L +# define __FUNCTION__ __func__ +# endif +#ifndef dNODEBUG +# ifdef __GNUC__ +# define dIASSERT(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s() [%s:%u]",__FUNCTION__,__FILE__,__LINE__); } } +# define dUASSERT(a,msg) { if (!(a)) { dDebug (d_ERR_UASSERT, \ + msg " in %s()", __FUNCTION__); } } +# define dDEBUGMSG(msg) { dMessage (d_ERR_UASSERT, \ + msg " in %s() [%s:%u]", __FUNCTION__,__FILE__,__LINE__); } +# else // not __GNUC__ +# define dIASSERT(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s:%u",__FILE__,__LINE__); } } +# define dUASSERT(a,msg) { if (!(a)) { dDebug (d_ERR_UASSERT, \ + msg " (%s:%u)", __FILE__,__LINE__); } } +# define dDEBUGMSG(msg) { dMessage (d_ERR_UASSERT, \ + msg " (%s:%u)", __FILE__,__LINE__); } +# endif +# define dIVERIFY(a) dIASSERT(a) +# define dUVERIFY(a, msg) dUASSERT(a, msg) +#else +# define dIASSERT(a) ((void)0) +# define dUASSERT(a,msg) ((void)0) +# define dDEBUGMSG(msg) ((void)0) +# define dIVERIFY(a) ((void)(a)) +# define dUVERIFY(a, msg) ((void)(a)) +#endif + +#ifdef __GNUC__ +#define dUNUSED(Name) Name __attribute__((unused)) +#else // not __GNUC__ +#define dUNUSED(Name) Name +#endif + +#if __cplusplus >= 201103L +#define dSASSERT(e) static_assert(e, #e) +#define dSMSGASSERT(e, message) static_assert(e, message) +#else +#define d_SASSERT_INNER_TOKENPASTE(x, y) x ## y +#define d_SASSERT_TOKENPASTE(x, y) d_SASSERT_INNER_TOKENPASTE(x, y) +#define dSASSERT(e) typedef char dUNUSED(d_SASSERT_TOKENPASTE(d_StaticAssertionFailed_, __LINE__)[(e)?1:-1]) +#define dSMSGASSERT(e, message) dSASSERT(e) +#endif + +# ifdef __GNUC__ +# define dICHECK(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s() [%s:%u]",__FUNCTION__,__FILE__,__LINE__); *(int *)0 = 0; } } +# else // not __GNUC__ +# define dICHECK(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s:%u",__FILE__,__LINE__); *(int *)0 = 0; } } +# endif + +// Argument assert is a special case of user assert +#define dAASSERT(a) dUASSERT(a, "Bad argument(s)") +#define dAVERIFY(a) dUVERIFY(a, "Bad argument(s)") + + +#endif diff --git a/libs/ode-0.16.1/ode/src/export-dif.cpp b/libs/ode-0.16.1/ode/src/export-dif.cpp new file mode 100644 index 0000000..450021a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/export-dif.cpp @@ -0,0 +1,620 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Export a DIF (Dynamics Interchange Format) file. + */ + + +// @@@ TODO: +// * export all spaces, and geoms in spaces, not just ones attached to bodies +// (separate export function?) +// * say the space each geom is in, so reader can construct space heirarchy +// * limot --> separate out into limits and motors? +// * make sure ODE-specific parameters divided out + + +#include <ode/ode.h> +#include "config.h" +#include "objects.h" +#include "joints/joints.h" +#include "collision_kernel.h" + +//*************************************************************************** +// utility + +struct PrintingContext { + FILE *file; // file to write to + int precision; // digits of precision to print + int indent; // number of levels of indent + + void printIndent(); + void printReal (dReal x); + void print (const char *name, int x); + void print (const char *name, unsigned x); + void print (const char *name, dReal x); + void print (const char *name, const dReal *x, int n=3); + void print (const char *name, const char *x=0); + void printNonzero (const char *name, dReal x); + void printNonzero (const char *name, const dReal x[3]); +}; + + +void PrintingContext::printIndent() +{ + for (int i=0; i<indent; i++) fputc ('\t',file); +} + + +void PrintingContext::print (const char *name, int x) +{ + printIndent(); + fprintf (file,"%s = %d,\n",name,x); +} + +void PrintingContext::print (const char *name, unsigned x) +{ + printIndent(); + fprintf (file,"%s = %u,\n",name,x); +} + +void PrintingContext::printReal (dReal x) +{ + if (x==dInfinity) { + fprintf (file,"inf"); + } + else if (x==-dInfinity) { + fprintf (file,"-inf"); + } + else { + fprintf (file,"%.*g",precision,x); + } +} + + +void PrintingContext::print (const char *name, dReal x) +{ + printIndent(); + fprintf (file,"%s = ",name); + printReal (x); + fprintf (file,",\n"); +} + + +void PrintingContext::print (const char *name, const dReal *x, int n) +{ + printIndent(); + fprintf (file,"%s = {",name); + for (int i=0; i<n; i++) { + printReal (x[i]); + if (i < n-1) fputc (',',file); + } + fprintf (file,"},\n"); +} + + +void PrintingContext::print (const char *name, const char *x) +{ + printIndent(); + if (x) { + fprintf (file,"%s = \"%s\",\n",name,x); + } + else { + fprintf (file,"%s\n",name); + } +} + + +void PrintingContext::printNonzero (const char *name, dReal x) +{ + if (x != 0) print (name,x); +} + + +void PrintingContext::printNonzero (const char *name, const dReal x[3]) +{ + if (x[0] != 0 && x[1] != 0 && x[2] != 0) print (name,x); +} + +//*************************************************************************** +// joints + + +static void printLimot (PrintingContext &c, dxJointLimitMotor &limot, int num) +{ + if (num >= 0) { + c.printIndent(); + fprintf (c.file,"limit%d = {\n",num); + } + else { + c.print ("limit = {"); + } + c.indent++; + c.print ("low_stop",limot.lostop); + c.print ("high_stop",limot.histop); + c.printNonzero ("bounce",limot.bounce); + c.print ("ODE = {"); + c.indent++; + c.printNonzero ("stop_erp",limot.stop_erp); + c.printNonzero ("stop_cfm",limot.stop_cfm); + c.indent--; + c.print ("},"); + c.indent--; + c.print ("},"); + + if (num >= 0) { + c.printIndent(); + fprintf (c.file,"motor%d = {\n",num); + } + else { + c.print ("motor = {"); + } + c.indent++; + c.printNonzero ("vel",limot.vel); + c.printNonzero ("fmax",limot.fmax); + c.print ("ODE = {"); + c.indent++; + c.printNonzero ("fudge_factor",limot.fudge_factor); + c.printNonzero ("normal_cfm",limot.normal_cfm); + c.indent--; + c.print ("},"); + c.indent--; + c.print ("},"); +} + + +static const char *getJointName (dxJoint *j) +{ + switch (j->type()) { + case dJointTypeBall: return "ball"; + case dJointTypeHinge: return "hinge"; + case dJointTypeSlider: return "slider"; + case dJointTypeContact: return "contact"; + case dJointTypeUniversal: return "universal"; + case dJointTypeHinge2: return "ODE_hinge2"; + case dJointTypeFixed: return "fixed"; + case dJointTypeNull: return "null"; + case dJointTypeAMotor: return "ODE_angular_motor"; + case dJointTypeLMotor: return "ODE_linear_motor"; + case dJointTypePR: return "PR"; + case dJointTypePU: return "PU"; + case dJointTypePiston: return "piston"; + default: return "unknown"; + } +} + + +static void printBall (PrintingContext &c, dxJoint *j) +{ + dxJointBall *b = (dxJointBall*) j; + c.print ("anchor1",b->anchor1); + c.print ("anchor2",b->anchor2); +} + + +static void printHinge (PrintingContext &c, dxJoint *j) +{ + dxJointHinge *h = (dxJointHinge*) j; + c.print ("anchor1",h->anchor1); + c.print ("anchor2",h->anchor2); + c.print ("axis1",h->axis1); + c.print ("axis2",h->axis2); + c.print ("qrel",h->qrel,4); + printLimot (c,h->limot,-1); +} + + +static void printSlider (PrintingContext &c, dxJoint *j) +{ + dxJointSlider *s = (dxJointSlider*) j; + c.print ("axis1",s->axis1); + c.print ("qrel",s->qrel,4); + c.print ("offset",s->offset); + printLimot (c,s->limot,-1); +} + + +static void printContact (PrintingContext &c, dxJoint *j) +{ + dxJointContact *ct = (dxJointContact*) j; + int mode = ct->contact.surface.mode; + c.print ("pos",ct->contact.geom.pos); + c.print ("normal",ct->contact.geom.normal); + c.print ("depth",ct->contact.geom.depth); + //@@@ may want to write the geoms g1 and g2 that are involved, for debugging. + // to do this we must have written out all geoms in all spaces, not just + // geoms that are attached to bodies. + c.print ("mu",ct->contact.surface.mu); + if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2); + if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce); + if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel); + if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp); + if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm); + if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1); + if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2); + if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1); + if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2); + int fa = 0; // friction approximation code + if (mode & dContactApprox1_1) fa |= 1; + if (mode & dContactApprox1_2) fa |= 2; + if (fa) c.print ("friction_approximation",fa); + if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1); +} + + +static void printUniversal (PrintingContext &c, dxJoint *j) +{ + dxJointUniversal *u = (dxJointUniversal*) j; + c.print ("anchor1",u->anchor1); + c.print ("anchor2",u->anchor2); + c.print ("axis1",u->axis1); + c.print ("axis2",u->axis2); + c.print ("qrel1",u->qrel1,4); + c.print ("qrel2",u->qrel2,4); + printLimot (c,u->limot1,1); + printLimot (c,u->limot2,2); +} + + +static void printHinge2 (PrintingContext &c, dxJoint *j) +{ + dxJointHinge2 *h = (dxJointHinge2*) j; + c.print ("anchor1",h->anchor1); + c.print ("anchor2",h->anchor2); + c.print ("axis1",h->axis1); + c.print ("axis2",h->axis2); + c.print ("v1",h->v1); //@@@ much better to write out 'qrel' here, if it's available + c.print ("v2",h->v2); + c.print ("susp_erp",h->susp_erp); + c.print ("susp_cfm",h->susp_cfm); + printLimot (c,h->limot1,1); + printLimot (c,h->limot2,2); +} + +static void printPR (PrintingContext &c, dxJoint *j) +{ + dxJointPR *pr = (dxJointPR*) j; + c.print ("anchor2",pr->anchor2); + c.print ("axisR1",pr->axisR1); + c.print ("axisR2",pr->axisR2); + c.print ("axisP1",pr->axisP1); + c.print ("qrel",pr->qrel,4); + c.print ("offset",pr->offset); + printLimot (c,pr->limotP,1); + printLimot (c,pr->limotR,2); +} + +static void printPU (PrintingContext &c, dxJoint *j) +{ + dxJointPU *pu = (dxJointPU*) j; + c.print ("anchor1",pu->anchor1); + c.print ("anchor2",pu->anchor2); + c.print ("axis1",pu->axis1); + c.print ("axis2",pu->axis2); + c.print ("axisP",pu->axisP1); + c.print ("qrel1",pu->qrel1,4); + c.print ("qrel2",pu->qrel2,4); + printLimot (c,pu->limot1,1); + printLimot (c,pu->limot2,2); + printLimot (c,pu->limotP,3); +} + +static void printPiston (PrintingContext &c, dxJoint *j) +{ + dxJointPiston *rap = (dxJointPiston*) j; + c.print ("anchor1",rap->anchor1); + c.print ("anchor2",rap->anchor2); + c.print ("axis1",rap->axis1); + c.print ("axis2",rap->axis2); + c.print ("qrel",rap->qrel,4); + printLimot (c,rap->limotP,1); + printLimot (c, rap->limotR, 2); +} + +static void printFixed (PrintingContext &c, dxJoint *j) +{ + dxJointFixed *f = (dxJointFixed*) j; + c.print ("qrel",f->qrel); + c.print ("offset",f->offset); +} + +static void printLMotor (PrintingContext &c, dxJoint *j) +{ + dxJointLMotor *a = (dxJointLMotor*) j; + c.print("num", a->num); + c.printIndent(); + fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]); + c.print ("axis1",a->axis[0]); + c.print ("axis2",a->axis[1]); + c.print ("axis3",a->axis[2]); + for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1); +} + +struct dxAMotorJointPrinter +{ + static void print(PrintingContext &c, dxJointAMotor *a) + { + c.print ("num",a->m_num); + c.print ("mode",a->m_mode); + c.printIndent(); + fprintf (c.file,"rel = {%d,%d,%d},\n",a->m_rel[0],a->m_rel[1],a->m_rel[2]); + c.print ("axis1",a->m_axis[0]); + c.print ("axis2",a->m_axis[1]); + c.print ("axis3",a->m_axis[2]); + for (int i=0; i<3; i++) printLimot (c,a->m_limot[i],i+1); + c.print ("angle1",a->m_angle[0]); + c.print ("angle2",a->m_angle[1]); + c.print ("angle3",a->m_angle[2]); + } +}; + +static void printAMotor (PrintingContext &c, dxJoint *j) +{ + dxJointAMotor *a = (dxJointAMotor*) j; + dxAMotorJointPrinter::print(c, a); +} + +//*************************************************************************** +// geometry + +static void printGeom (PrintingContext &c, dxGeom *g); + +static void printSphere (PrintingContext &c, dxGeom *g) +{ + c.print ("type","sphere"); + c.print ("radius",dGeomSphereGetRadius (g)); +} + + +static void printBox (PrintingContext &c, dxGeom *g) +{ + dVector3 sides; + dGeomBoxGetLengths (g,sides); + c.print ("type","box"); + c.print ("sides",sides); +} + + +static void printCapsule (PrintingContext &c, dxGeom *g) +{ + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + c.print ("type","capsule"); + c.print ("radius",radius); + c.print ("length",length); +} + + +static void printCylinder (PrintingContext &c, dxGeom *g) +{ + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + c.print ("type","cylinder"); + c.print ("radius",radius); + c.print ("length",length); +} + + +static void printPlane (PrintingContext &c, dxGeom *g) +{ + dVector4 e; + dGeomPlaneGetParams (g,e); + c.print ("type","plane"); + c.print ("normal",e); + c.print ("d",e[3]); +} + + +static void printRay (PrintingContext &c, dxGeom *g) +{ + dReal length = dGeomRayGetLength (g); + c.print ("type","ray"); + c.print ("length",length); +} + + +static void printConvex (PrintingContext &c, dxGeom * /*g*/) +{ + c.print ("type","convex"); + ///@todo Print information about convex hull +} + + + +static void printTriMesh (PrintingContext &c, dxGeom * /*g*/) +{ + c.print ("type","trimesh"); + //@@@ i don't think that the trimesh accessor functions are really + // sufficient to read out all the triangle data, and anyway we + // should have a method of not duplicating trimesh data that is + // shared. +} + + +static void printHeightfieldClass (PrintingContext &c, dxGeom * /*g*/) +{ + c.print ("type","heightfield"); + ///@todo Print information about heightfield +} + + +static void printGeom (PrintingContext &c, dxGeom *g) +{ + unsigned long category = dGeomGetCategoryBits (g); + if (category != (unsigned long)(~0)) { + c.printIndent(); + fprintf (c.file,"category_bits = %lu\n",category); + } + unsigned long collide = dGeomGetCollideBits (g); + if (collide != (unsigned long)(~0)) { + c.printIndent(); + fprintf (c.file,"collide_bits = %lu\n",collide); + } + if (!dGeomIsEnabled (g)) { + c.print ("disabled",1); + } + switch (g->type) { + case dSphereClass: printSphere (c,g); break; + case dBoxClass: printBox (c,g); break; + case dCapsuleClass: printCapsule (c,g); break; + case dCylinderClass: printCylinder (c,g); break; + case dPlaneClass: printPlane (c,g); break; + case dRayClass: printRay (c,g); break; + case dConvexClass: printConvex (c,g); break; + case dTriMeshClass: printTriMesh (c,g); break; + case dHeightfieldClass: printHeightfieldClass (c,g); break; + } +} + +//*************************************************************************** +// world + +void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix) +{ + PrintingContext c; + c.file = file; +#if defined(dSINGLE) + c.precision = 7; +#else + c.precision = 15; +#endif + c.indent = 1; + + fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix); + c.print ("gravity",w->gravity); + c.print ("ODE = {"); + c.indent++; + c.print ("ERP",w->global_erp); + c.print ("CFM",w->global_cfm); + c.print ("auto_disable = {"); + c.indent++; + c.print ("linear_threshold",w->adis.linear_average_threshold); + c.print ("angular_threshold",w->adis.angular_average_threshold); + c.print ("average_samples",(int)w->adis.average_samples); + c.print ("idle_time",w->adis.idle_time); + c.print ("idle_steps",w->adis.idle_steps); + fprintf (file,"\t\t},\n\t},\n}\n"); + c.indent -= 3; + + // bodies + int num = 0; + fprintf (file,"%sbody = {}\n",prefix); + for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) { + b->tag = num; + fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix); + c.indent++; + c.print ("pos",b->posr.pos); + c.print ("q",b->q,4); + c.print ("lvel",b->lvel); + c.print ("avel",b->avel); + c.print ("mass",b->mass.mass); + fprintf (file,"\tI = {{"); + for (int i=0; i<3; i++) { + for (int j=0; j<3; j++) { + c.printReal (b->mass.I[i*4+j]); + if (j < 2) fputc (',',file); + } + if (i < 2) fprintf (file,"},{"); + } + fprintf (file,"}},\n"); + c.printNonzero ("com",b->mass.c); + c.print ("ODE = {"); + c.indent++; + if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1); + if (b->flags & dxBodyDisabled) c.print ("disabled",1); + if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1); + if (b->flags & dxBodyAutoDisable) { + c.print ("auto_disable = {"); + c.indent++; + c.print ("linear_threshold",b->adis.linear_average_threshold); + c.print ("angular_threshold",b->adis.angular_average_threshold); + c.print ("average_samples",(int)b->adis.average_samples); + c.print ("idle_time",b->adis.idle_time); + c.print ("idle_steps",b->adis.idle_steps); + c.print ("time_left",b->adis_timeleft); + c.print ("steps_left",b->adis_stepsleft); + c.indent--; + c.print ("},"); + } + c.printNonzero ("facc",b->facc); + c.printNonzero ("tacc",b->tacc); + if (b->flags & dxBodyFlagFiniteRotationAxis) { + c.print ("finite_rotation_axis",b->finite_rot_axis); + } + c.indent--; + c.print ("},"); + if (b->geom) { + c.print ("geometry = {"); + c.indent++; + for (dxGeom *g=b->geom; g; g=g->body_next) { + c.print ("{"); + c.indent++; + printGeom (c,g); + c.indent--; + c.print ("},"); + } + c.indent--; + c.print ("},"); + } + c.indent--; + c.print ("}"); + num++; + } + + // joints + num = 0; + fprintf (file,"%sjoint = {}\n",prefix); + for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) { + c.indent++; + const char *name = getJointName (j); + fprintf (file, + "%sjoint[%d] = dynamics.%s_joint {\n" + "\tworld = %sworld,\n" + "\tbody = {" + ,prefix,num,name,prefix); + + if ( j->node[0].body ) + fprintf (file,"%sbody[%d]",prefix,j->node[0].body->tag); + if ( j->node[1].body ) + fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag); + fprintf (file,"}\n"); + + switch (j->type()) { + case dJointTypeBall: printBall (c,j); break; + case dJointTypeHinge: printHinge (c,j); break; + case dJointTypeSlider: printSlider (c,j); break; + case dJointTypeContact: printContact (c,j); break; + case dJointTypeUniversal: printUniversal (c,j); break; + case dJointTypeHinge2: printHinge2 (c,j); break; + case dJointTypeFixed: printFixed (c,j); break; + case dJointTypeAMotor: printAMotor (c,j); break; + case dJointTypeLMotor: printLMotor (c,j); break; + case dJointTypePR: printPR (c,j); break; + case dJointTypePU: printPU (c,j); break; + case dJointTypePiston: printPiston (c,j); break; + default: c.print("unknown joint"); + } + c.indent--; + c.print ("}"); + num++; + } +} diff --git a/libs/ode-0.16.1/ode/src/fastdot.cpp b/libs/ode-0.16.1/ode/src/fastdot.cpp new file mode 100644 index 0000000..5594bc5 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastdot.cpp @@ -0,0 +1,46 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/* generated code, do not edit. */
+
+#include <ode/common.h>
+#include "config.h"
+#include "matrix.h"
+
+#include "fastdot_impl.h"
+
+
+/*extern */
+dReal dxDot (const dReal *a, const dReal *b, unsigned n)
+{
+ return calculateLargeVectorDot<1>(a, b, n);
+}
+
+
+#undef dDot
+
+/*extern */
+dReal dDot (const dReal *a, const dReal *b, int n)
+{
+ return dxDot (a, b, n);
+}
+
diff --git a/libs/ode-0.16.1/ode/src/fastdot_impl.h b/libs/ode-0.16.1/ode/src/fastdot_impl.h new file mode 100644 index 0000000..f32e717 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastdot_impl.h @@ -0,0 +1,51 @@ + + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_FASTDOT_IMPL_H_ +#define _ODE_FASTDOT_IMPL_H_ + + +template<unsigned b_stride> +dReal calculateLargeVectorDot (const dReal *a, const dReal *b, unsigned n) +{ + dReal sum = 0; + const dReal *a_end = a + (n & (int)(~3)); + for (; a != a_end; b += 4 * b_stride, a += 4) { + dReal p0 = a[0], p1 = a[1], p2 = a[2], p3 = a[3]; + dReal q0 = b[0 * b_stride], q1 = b[1 * b_stride], q2 = b[2 * b_stride], q3 = b[3 * b_stride]; + dReal m0 = p0 * q0; + dReal m1 = p1 * q1; + dReal m2 = p2 * q2; + dReal m3 = p3 * q3; + sum += m0 + m1 + m2 + m3; + } + a_end += (n & 3); + for (; a != a_end; b += b_stride, ++a) { + sum += (*a) * (*b); + } + return sum; +} + + +#endif diff --git a/libs/ode-0.16.1/ode/src/fastldltfactor.cpp b/libs/ode-0.16.1/ode/src/fastldltfactor.cpp new file mode 100644 index 0000000..9c1b921 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastldltfactor.cpp @@ -0,0 +1,462 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * LDLT factorization related code of ThreadedEquationSolverLDLT + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#include <ode/common.h> +#include <ode/matrix.h> +#include <ode/matrix_coop.h> +#include "config.h" +#include "threaded_solver_ldlt.h" +#include "threading_base.h" +#include "resource_control.h" +#include "error.h" + +#include "fastldltfactor_impl.h" + + +/*static */ +void ThreadedEquationSolverLDLT::estimateCooperativeFactoringLDLTResourceRequirements( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount) +{ + dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading(); + unsigned limitedThreadCount = restrictFactoringLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount); + + if (limitedThreadCount > 1) + { + doEstimateCooperativeFactoringLDLTResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount); + } +} + +/*static */ +void ThreadedEquationSolverLDLT::cooperativelyFactorLDLT( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip) +{ + dAASSERT(rowCount != 0); + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + unsigned limitedThreadCount = restrictFactoringLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount); + + if (limitedThreadCount <= 1) + { + factorMatrixAsLDLT<FLDLT_D_STRIDE>(A, d, rowCount, rowSkip); + } + else + { + doCooperativelyFactorLDLTValidated(resourceContainer, limitedThreadCount, A, d, rowCount, rowSkip); + } +} + + +/*static */ +unsigned ThreadedEquationSolverLDLT::restrictFactoringLDLTAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount) +{ + unsigned limitedThreadCount = 1; + +#if dCOOPERATIVE_ENABLED + const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; // Required by the implementation + unsigned solvingMaximalBlockCount = deriveSolvingL1StripeBlockCount(rowCount, solvingBlockStep); + dIASSERT(deriveSolvingL1StripeThreadCount(FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM - 1, 2) > 1); + + if (solvingMaximalBlockCount >= FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM) + { + limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, false); + } +#endif // #if dCOOPERATIVE_ENABLED + + return limitedThreadCount; +} + +/*static */ +void ThreadedEquationSolverLDLT::doEstimateCooperativeFactoringLDLTResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount) +{ + const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; // Required by the implementation + unsigned solvingTotalBlockCount = deriveSolvingL1StripeBlockCount(rowCount, solvingBlockStep); + dIASSERT(solvingTotalBlockCount >= 1); + + unsigned solvingLastBlockIndex = solvingTotalBlockCount - 1; + + const unsigned factorizingBlockARows = FFL1S_REGULAR_A_ROWS; + unsigned factorizingMaximalBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingLastBlockIndex, solvingBlockStep, factorizingBlockARows); + + unsigned blockSolvingMaximumThreads = deriveSolvingL1StripeThreadCount(solvingLastBlockIndex, allowedThreadCount); + unsigned blockFactorizingMaximumThreads = deriveScalingAndFactorizingL1StripeThreadCount(factorizingMaximalBlockCount, allowedThreadCount); + unsigned simultaneousCallCount = 1 // Final synchronization point + + 2 // intermediate synchronization points + + dMACRO_MAX(blockSolvingMaximumThreads, blockFactorizingMaximumThreads); + + FactorizationSolvingL1StripeMemoryEstimates solvingMemoryEstimates; + FactorizationScalingAndFactorizingL1StripeMemoryEstimates scalingAndFactorizingEstimates; + sizeint solvingMemoryRequired = estimateCooperativelySolvingL1Stripe_XMemoryRequirement(solvingTotalBlockCount, solvingMemoryEstimates); + sizeint factorizingMemoryRequired = estimateCooperativelyScalingAndFactorizingL1Stripe_XMemoryRequirement(blockFactorizingMaximumThreads, scalingAndFactorizingEstimates); + sizeint totalSizeRequired = solvingMemoryRequired + factorizingMemoryRequired; + const unsigned memoryAlignmentRequired = ALLOCATION_DEFAULT_ALIGNMENT; + + unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED; + summaryRequirementsDescriptor->mergeAnotherDescriptorIn(totalSizeRequired, memoryAlignmentRequired, simultaneousCallCount, featureRequirement); +} + +/*static */ +void ThreadedEquationSolverLDLT::doCooperativelyFactorLDLTValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(allowedThreadCount > 1); + + const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; // Required by the implementation + unsigned solvingTotalBlockCount = deriveSolvingL1StripeBlockCount(rowCount, solvingBlockStep); + dIASSERT(solvingTotalBlockCount >= 1); + + unsigned solvingLastBlockIndex = solvingTotalBlockCount - 1; + + const unsigned factorizingBlockARows = FFL1S_REGULAR_A_ROWS; + unsigned factorizingMaximalBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingLastBlockIndex, solvingBlockStep, factorizingBlockARows); + + unsigned blockFactorizingMaximumThreads = deriveScalingAndFactorizingL1StripeThreadCount(factorizingMaximalBlockCount, allowedThreadCount); + + dCallWaitID completionWait = resourceContainer->getStockCallWait(); + dAASSERT(completionWait != NULL); + + FactorizationSolvingL1StripeMemoryEstimates solvingMemoryEstimates; + FactorizationScalingAndFactorizingL1StripeMemoryEstimates scalingAndFactorizingEstimates; + sizeint solvingMemoryRequired = estimateCooperativelySolvingL1Stripe_XMemoryRequirement(solvingTotalBlockCount, solvingMemoryEstimates); + sizeint factorizingMemoryRequired = estimateCooperativelyScalingAndFactorizingL1Stripe_XMemoryRequirement(blockFactorizingMaximumThreads, scalingAndFactorizingEstimates); + sizeint totalSizeRequired = solvingMemoryRequired + factorizingMemoryRequired; + dIASSERT(totalSizeRequired <= resourceContainer->getMemoryBufferSize()); + + void *bufferAllocated = resourceContainer->getMemoryBufferPointer(); + dIASSERT(bufferAllocated != NULL); + dIASSERT(dALIGN_PTR(bufferAllocated, ALLOCATION_DEFAULT_ALIGNMENT) == bufferAllocated); + + atomicord32 solvingBlockCompletionProgress; + cellindexint *solvingBlockProgressDescriptors; + FactorizationSolveL1StripeCellContext *solvingCellContexts; + + FactorizationFactorizeL1StripeContext *factorizingFactorizationContext; + + void *bufferCurrentLocation = bufferAllocated; + bufferCurrentLocation = markCooperativelySolvingL1Stripe_XMemoryStructuresOut(bufferCurrentLocation, solvingMemoryEstimates, solvingBlockProgressDescriptors, solvingCellContexts); + bufferCurrentLocation = markCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructuresOut(bufferCurrentLocation, scalingAndFactorizingEstimates, factorizingFactorizationContext); + dIVERIFY(bufferCurrentLocation <= (uint8 *)bufferAllocated + totalSizeRequired); + + dCallReleaseeID calculationFinishReleasee; + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + threading->PostThreadedCall(NULL, &calculationFinishReleasee, 1, NULL, completionWait, &factotLDLT_completion_callback, NULL, 0, "FactorLDLT Completion"); + + FactorLDLTWorkerContext workerContext(threading, allowedThreadCount, A, d, solvingTotalBlockCount, rowCount, rowSkip, + solvingBlockCompletionProgress, solvingBlockProgressDescriptors, solvingCellContexts, + factorizingFactorizationContext, + calculationFinishReleasee); // The variable must exist in the outer scope + + dIASSERT(solvingTotalBlockCount >= FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM); + dSASSERT(FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM > 2); + + scaleAndFactorizeL1FirstRowStripe_2<FLDLT_D_STRIDE>(workerContext.m_ARow, workerContext.m_d, workerContext.m_rowSkip); + workerContext.incrementForNextBlock(); + + const unsigned blockIndex = 1; + dIASSERT(blockIndex == workerContext.m_solvingBlockIndex); + + initializeCooperativelySolvingL1Stripe_XMemoryStructures(blockIndex, solvingBlockCompletionProgress, solvingBlockProgressDescriptors, solvingCellContexts); + unsigned secondBlockSolvingThreadCount = deriveSolvingL1StripeThreadCount(blockIndex, allowedThreadCount); + + dCallReleaseeID secondBlockSolvingSyncReleasee; + threading->PostThreadedCall(NULL, &secondBlockSolvingSyncReleasee, secondBlockSolvingThreadCount, NULL, NULL, &factotLDLT_solvingCompleteSync_callback, &workerContext, 0, "FactorLDLT Solving Complete Sync"); + + if (secondBlockSolvingThreadCount > 1) + { + threading->PostThreadedCallsGroup(NULL, secondBlockSolvingThreadCount - 1, secondBlockSolvingSyncReleasee, &factotLDLT_solvingComplete_callback, &workerContext, "FactorLDLT Solving Complete"); + } + + factotLDLT_solvingComplete(workerContext, secondBlockSolvingThreadCount - 1); + threading->AlterThreadedCallDependenciesCount(secondBlockSolvingSyncReleasee, -1); + + threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "FactorLDLT End Wait"); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_solvingComplete_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee)) +{ + FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext; + + factotLDLT_solvingComplete(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex)); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::factotLDLT_solvingComplete(FactorLDLTWorkerContext &ref_context, unsigned ownThreadIndex) +{ + participateSolvingL1Stripe_X<FSL1S_BLOCK_SIZE, FSL1S_REGULAR_B_ROWS>(ref_context.m_A, ref_context.m_ARow, ref_context.m_solvingBlockIndex, ref_context.m_rowSkip, + ref_context.m_refSolvingBlockCompletionProgress, ref_context.m_solvingBlockProgressDescriptors, ref_context.m_solvingCellContexts, ownThreadIndex); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_solvingCompleteSync_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext; + + factotLDLT_solvingCompleteSync(*ptrContext); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::factotLDLT_solvingCompleteSync(FactorLDLTWorkerContext &ref_workerContext) +{ + unsigned solvingBlockIndex = ref_workerContext.m_solvingBlockIndex; + FactorizationFactorizeL1StripeContext *factorizingFactorizationContext = ref_workerContext.m_factorizingFactorizationContext; + + const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; + const unsigned factorizingBlockARows = FFL1S_REGULAR_A_ROWS; + unsigned factorizingBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingBlockIndex, solvingBlockStep, factorizingBlockARows); + unsigned blockFactorizingThreadCount = deriveScalingAndFactorizingL1StripeThreadCount(factorizingBlockCount, ref_workerContext.m_allowedThreadCount); + initializeCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructures(factorizingFactorizationContext, blockFactorizingThreadCount); + + dCallReleaseeID blockFactorizingSyncReleasee; + + dxThreadingBase *threading = ref_workerContext.m_threading; + if (solvingBlockIndex != ref_workerContext.m_totalBlockCount - 1) + { + threading->PostThreadedCall(NULL, &blockFactorizingSyncReleasee, blockFactorizingThreadCount, NULL, NULL, &factotLDLT_scalingAndFactorizingCompleteSync_callback, &ref_workerContext, 0, "FactorLDLT S'n'F Sync"); + } + else + { + blockFactorizingSyncReleasee = ref_workerContext.m_calculationFinishReleasee; + + if (blockFactorizingThreadCount > 1) + { + threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, blockFactorizingThreadCount - 1); + } + } + + if (blockFactorizingThreadCount > 1) + { + threading->PostThreadedCallsGroup(NULL, blockFactorizingThreadCount - 1, blockFactorizingSyncReleasee, &factotLDLT_scalingAndFactorizingComplete_callback, &ref_workerContext, "FactorLDLT S'n'F Complete"); + } + + factotLDLT_scalingAndFactorizingComplete(ref_workerContext, blockFactorizingThreadCount - 1); + threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, -1); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingComplete_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee)) +{ + FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext; + + factotLDLT_scalingAndFactorizingComplete(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex)); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingComplete(FactorLDLTWorkerContext &ref_workerContext, unsigned ownThreadIndex) +{ + unsigned factorizationRow = ref_workerContext.m_solvingBlockIndex * FSL1S_BLOCK_SIZE; + participateScalingAndFactorizingL1Stripe_X<FFL1S_REGULAR_A_ROWS, FLDLT_D_STRIDE>(ref_workerContext.m_ARow, ref_workerContext.m_d, factorizationRow, + ref_workerContext.m_rowSkip, ref_workerContext.m_factorizingFactorizationContext, ownThreadIndex); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingCompleteSync_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext; + + factotLDLT_scalingAndFactorizingCompleteSync(*ptrContext); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingCompleteSync(FactorLDLTWorkerContext &ref_workerContext) +{ + ref_workerContext.incrementForNextBlock(); + + unsigned blockIndex = ref_workerContext.m_solvingBlockIndex; + dIASSERT(blockIndex < ref_workerContext.m_totalBlockCount); + + atomicord32 &refSolvingBlockCompletionProgress = ref_workerContext.m_refSolvingBlockCompletionProgress; + cellindexint *solvingBlockProgressDescriptors = ref_workerContext.m_solvingBlockProgressDescriptors; + FactorizationSolveL1StripeCellContext *solvingCellContexts = ref_workerContext.m_solvingCellContexts; + + initializeCooperativelySolvingL1Stripe_XMemoryStructures(blockIndex, refSolvingBlockCompletionProgress, solvingBlockProgressDescriptors, solvingCellContexts); + unsigned blockSolvingThreadCount = deriveSolvingL1StripeThreadCount(blockIndex, ref_workerContext.m_allowedThreadCount); + + dCallReleaseeID blockSolvingSyncReleasee; + + dxThreadingBase *threading = ref_workerContext.m_threading; + if (blockIndex != ref_workerContext.m_totalBlockCount - 1 || ref_workerContext.m_rowCount % FSL1S_REGULAR_B_ROWS == 0) + { + threading->PostThreadedCall(NULL, &blockSolvingSyncReleasee, blockSolvingThreadCount, NULL, NULL, &factotLDLT_solvingCompleteSync_callback, &ref_workerContext, 0, "FactorLDLT Solving Complete Sync"); + + if (blockSolvingThreadCount > 1) + { + threading->PostThreadedCallsGroup(NULL, blockSolvingThreadCount - 1, blockSolvingSyncReleasee, &factotLDLT_solvingComplete_callback, &ref_workerContext, "FactorLDLT Solving Complete"); + } + + factotLDLT_solvingComplete(ref_workerContext, blockSolvingThreadCount - 1); + } + else + { + dSASSERT(FSL1S_REGULAR_B_ROWS == 2); + dSASSERT(FSL1S_FINAL_B_ROWS == 1); + + threading->PostThreadedCall(NULL, &blockSolvingSyncReleasee, blockSolvingThreadCount, NULL, NULL, &factotLDLT_solvingFinalSync_callback, &ref_workerContext, 0, "FactorLDLT Solving Final Sync"); + + if (blockSolvingThreadCount > 1) + { + threading->PostThreadedCallsGroup(NULL, blockSolvingThreadCount - 1, blockSolvingSyncReleasee, &factotLDLT_solvingFinal_callback, &ref_workerContext, "FactorLDLT Solving Final"); + } + + factotLDLT_solvingFinal(ref_workerContext, blockSolvingThreadCount - 1); + } + + threading->AlterThreadedCallDependenciesCount(blockSolvingSyncReleasee, -1); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_solvingFinal_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee)) +{ + FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext; + + factotLDLT_solvingFinal(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex)); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::factotLDLT_solvingFinal(FactorLDLTWorkerContext &ref_context, unsigned ownThreadIndex) +{ + participateSolvingL1Stripe_X<FSL1S_BLOCK_SIZE, FSL1S_FINAL_B_ROWS>(ref_context.m_A, ref_context.m_ARow, ref_context.m_solvingBlockIndex, ref_context.m_rowSkip, + ref_context.m_refSolvingBlockCompletionProgress, ref_context.m_solvingBlockProgressDescriptors, ref_context.m_solvingCellContexts, ownThreadIndex); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_solvingFinalSync_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext; + + factotLDLT_solvingFinalSync(*ptrContext); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::factotLDLT_solvingFinalSync(FactorLDLTWorkerContext &ref_workerContext) +{ + unsigned solvingBlockIndex = ref_workerContext.m_solvingBlockIndex; + FactorizationFactorizeL1StripeContext *factorizingFactorizationContext = ref_workerContext.m_factorizingFactorizationContext; + + const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; + const unsigned factorizingBlockARows = FFL1S_FINAL_A_ROWS; + unsigned factorizingBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingBlockIndex, solvingBlockStep, factorizingBlockARows); + unsigned blockFactorizingThreadCount = deriveScalingAndFactorizingL1StripeThreadCount(factorizingBlockCount, ref_workerContext.m_allowedThreadCount); + initializeCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructures(factorizingFactorizationContext, blockFactorizingThreadCount); + + dCallReleaseeID blockFactorizingSyncReleasee = ref_workerContext.m_calculationFinishReleasee; + dIASSERT(solvingBlockIndex == ref_workerContext.m_totalBlockCount - 1); + + dxThreadingBase *threading = ref_workerContext.m_threading; + + if (blockFactorizingThreadCount > 1) + { + threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, blockFactorizingThreadCount - 1); + threading->PostThreadedCallsGroup(NULL, blockFactorizingThreadCount - 1, blockFactorizingSyncReleasee, &factotLDLT_scalingAndFactorizingFinal_callback, &ref_workerContext, "FactorLDLT S'n'F Final"); + } + + factotLDLT_scalingAndFactorizingFinal(ref_workerContext, blockFactorizingThreadCount - 1); + threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, -1); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingFinal_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee)) +{ + FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext; + + factotLDLT_scalingAndFactorizingFinal(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex)); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingFinal(FactorLDLTWorkerContext &ref_workerContext, unsigned ownThreadIndex) +{ + unsigned factorizationRow = ref_workerContext.m_solvingBlockIndex * FSL1S_BLOCK_SIZE; + participateScalingAndFactorizingL1Stripe_X<FFL1S_FINAL_A_ROWS, FLDLT_D_STRIDE>(ref_workerContext.m_ARow, ref_workerContext.m_d, factorizationRow, + ref_workerContext.m_rowSkip, ref_workerContext.m_factorizingFactorizationContext, ownThreadIndex); +} + + +/*static */ +int ThreadedEquationSolverLDLT::factotLDLT_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + // Do nothing + return 1; +} + + +////////////////////////////////////////////////////////////////////////// +// Public interface functions + + +/*extern ODE_API */ +void dFactorLDLT(dReal *A, dReal *d, int n, int nskip1) +{ + factorMatrixAsLDLT<1>(A, d, n, nskip1); +} + + +/*extern ODE_API */ +void dEstimateCooperativelyFactorLDLTResourceRequirements(dResourceRequirementsID requirements, + unsigned maximalAllowedThreadCount, unsigned maximalRowCount) +{ + dAASSERT(requirements != NULL); + + dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements; + ThreadedEquationSolverLDLT::estimateCooperativeFactoringLDLTResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount); +} + +/*extern ODE_API */ +void dCooperativelyFactorLDLT(dResourceContainerID resources, unsigned allowedThreadCount, + dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip) +{ + dAASSERT(resources != NULL); + + dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources; + ThreadedEquationSolverLDLT::cooperativelyFactorLDLT(resourceContainer, allowedThreadCount, A, d, rowCount, rowSkip); +} diff --git a/libs/ode-0.16.1/ode/src/fastldltfactor_impl.h b/libs/ode-0.16.1/ode/src/fastldltfactor_impl.h new file mode 100644 index 0000000..8f633d3 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastldltfactor_impl.h @@ -0,0 +1,1530 @@ + + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Code style improvements and optimizations by Oleh Derevenko ????-2019 + * LDLT cooperative factorization code of ThreadedEquationSolverLDLT copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + +#ifndef _ODE_FASTLDLT_IMPL_H_ +#define _ODE_FASTLDLT_IMPL_H_ + + +#include "error.h" +#include "common.h" + + +static void solveL1Stripe_2 (const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip); +template<unsigned int d_stride> +void scaleAndFactorizeL1Stripe_2(dReal *ARow, dReal *d, unsigned rowIndex, unsigned rowSkip); +template<unsigned int d_stride> +inline void scaleAndFactorizeL1FirstRowStripe_2(dReal *ARow, dReal *d, unsigned rowSkip); + +static void solveStripeL1_1 (const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip); +template<unsigned int d_stride> +void scaleAndFactorizeL1Stripe_1(dReal *ARow, dReal *d, unsigned rowIndex); +template<unsigned int d_stride> +inline void scaleAndFactorizeL1FirstRowStripe_1(dReal *ARow, dReal *d); + + +template<unsigned int d_stride> +void factorMatrixAsLDLT(dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip) +{ + if (rowCount < 1) return; + + dReal *ARow = A; + unsigned blockStartRow = 0; + + const unsigned blockStep = 2; + const unsigned lastRowIndex = rowCount >= blockStep ? rowCount - blockStep + 1 : 0; + + /* compute blocks of 2 rows */ + bool subsequentPass = false; + for (; blockStartRow < lastRowIndex; subsequentPass = true, ARow += blockStep * rowSkip, blockStartRow += blockStep) + { + if (subsequentPass) + { + /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ + solveL1Stripe_2(A, ARow, blockStartRow, rowSkip); + scaleAndFactorizeL1Stripe_2<d_stride>(ARow, d, blockStartRow, rowSkip); + } + else + { + scaleAndFactorizeL1FirstRowStripe_2<d_stride>(ARow, d, rowSkip); + } + dSASSERT(blockStep == 2); + /* done factorizing 2 x 2 block */ + } + + /* compute the (less than 2) rows at the bottom */ + if (!subsequentPass || blockStartRow == lastRowIndex) + { + dSASSERT(blockStep == 2); // for the blockStartRow == lastRowIndex comparison above + + if (subsequentPass) + { + solveStripeL1_1(A, ARow, blockStartRow, rowSkip); + scaleAndFactorizeL1Stripe_1<d_stride>(ARow, d, blockStartRow); + } + else + { + scaleAndFactorizeL1FirstRowStripe_1<d_stride>(ARow, d); + } + dSASSERT(blockStep == 2); + /* done factorizing 1 x 1 block */ + } +} + +/* solve L*X=B, with B containing 2 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is rowSkip. + * B is an n*2 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also rowSkip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ +static +void solveL1Stripe_2(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(rowCount != 0); + dIASSERT(rowCount % 2 == 0); + + /* compute all 2 x 2 blocks of X */ + unsigned blockStartRow = 0; + for (bool exitLoop = false, subsequentPass = false; !exitLoop; subsequentPass = true, exitLoop = (blockStartRow += 2) == rowCount) + { + const dReal *ptrLElement; + dReal *ptrBElement; + + /* declare variables - Z matrix */ + dReal Z11, Z12, Z21, Z22; + + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + if (subsequentPass) + { + ptrLElement = L + blockStartRow * rowSkip; + ptrBElement = B; + + /* set Z matrix to 0 */ + Z11 = 0; Z12 = 0; Z21 = 0; Z22 = 0; + + /* the inner loop that computes outer products and adds them to Z */ + // The iteration starts with even number and decreases it by 2. So, it must end in zero + for (unsigned columnCounter = blockStartRow; ;) + { + /* declare p and q vectors, etc */ + dReal p1, q1, p2, q2; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[0]; + q1 = ptrBElement[0]; + Z11 += p1 * q1; + q2 = ptrBElement[rowSkip]; + Z12 += p1 * q2; + p2 = ptrLElement[rowSkip]; + Z21 += p2 * q1; + Z22 += p2 * q2; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[1]; + q1 = ptrBElement[1]; + Z11 += p1 * q1; + q2 = ptrBElement[1 + rowSkip]; + Z12 += p1 * q2; + p2 = ptrLElement[1 + rowSkip]; + Z21 += p2 * q1; + Z22 += p2 * q2; + + if (columnCounter > 6) + { + columnCounter -= 6; + + /* advance pointers */ + ptrLElement += 6; + ptrBElement += 6; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-4]; + q1 = ptrBElement[-4]; + Z11 += p1 * q1; + q2 = ptrBElement[-4 + rowSkip]; + Z12 += p1 * q2; + p2 = ptrLElement[-4 + rowSkip]; + Z21 += p2 * q1; + Z22 += p2 * q2; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-3]; + q1 = ptrBElement[-3]; + Z11 += p1 * q1; + q2 = ptrBElement[-3 + rowSkip]; + Z12 += p1 * q2; + p2 = ptrLElement[-3 + rowSkip]; + Z21 += p2 * q1; + Z22 += p2 * q2; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-2]; + q1 = ptrBElement[-2]; + Z11 += p1 * q1; + q2 = ptrBElement[-2 + rowSkip]; + Z12 += p1 * q2; + p2 = ptrLElement[-2 + rowSkip]; + Z21 += p2 * q1; + Z22 += p2 * q2; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-1]; + q1 = ptrBElement[-1]; + Z11 += p1 * q1; + q2 = ptrBElement[-1 + rowSkip]; + Z12 += p1 * q2; + p2 = ptrLElement[-1 + rowSkip]; + Z21 += p2 * q1; + Z22 += p2 * q2; + } + else + { + /* advance pointers */ + ptrLElement += 2; + ptrBElement += 2; + + if ((columnCounter -= 2) == 0) + { + break; + } + } + /* end of inner loop */ + } + } + else + { + ptrLElement = L/* + blockStartRow * rowSkip*/; dIASSERT(blockStartRow == 0); + ptrBElement = B; + + /* set Z matrix to 0 */ + Z11 = 0; Z12 = 0; Z21 = 0; Z22 = 0; + } + + /* finish computing the X(i) block */ + + dReal Y11 = ptrBElement[0] - Z11; + dReal Y12 = ptrBElement[rowSkip] - Z12; + + dReal p2 = ptrLElement[rowSkip]; + + ptrBElement[0] = Y11; + ptrBElement[rowSkip] = Y12; + + dReal Y21 = ptrBElement[1] - Z21 - p2 * Y11; + dReal Y22 = ptrBElement[1 + rowSkip] - Z22 - p2 * Y12; + + ptrBElement[1] = Y21; + ptrBElement[1 + rowSkip] = Y22; + /* end of outer loop */ + } +} + +template<unsigned int d_stride> +void scaleAndFactorizeL1Stripe_2(dReal *ARow, dReal *d, unsigned factorizationRow, unsigned rowSkip) +{ + dIASSERT(factorizationRow != 0); + dIASSERT(factorizationRow % 2 == 0); + + dReal *ptrAElement = ARow; + dReal *ptrDElement = d; + + /* scale the elements in a 2 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + dReal Z11 = 0, Z21 = 0, Z22 = 0; + + for (unsigned columnCounter = factorizationRow; ; ) + { + dReal p1, q1, p2, q2, dd; + + p1 = ptrAElement[0]; + p2 = ptrAElement[rowSkip]; + dd = ptrDElement[0 * d_stride]; + q1 = p1 * dd; + q2 = p2 * dd; + ptrAElement[0] = q1; + ptrAElement[rowSkip] = q2; + Z11 += p1 * q1; + Z21 += p2 * q1; + Z22 += p2 * q2; + + p1 = ptrAElement[1]; + p2 = ptrAElement[1 + rowSkip]; + dd = ptrDElement[1 * d_stride]; + q1 = p1 * dd; + q2 = p2 * dd; + ptrAElement[1] = q1; + ptrAElement[1 + rowSkip] = q2; + Z11 += p1 * q1; + Z21 += p2 * q1; + Z22 += p2 * q2; + + if (columnCounter > 6) + { + columnCounter -= 6; + + ptrAElement += 6; + ptrDElement += 6 * d_stride; + + p1 = ptrAElement[-4]; + p2 = ptrAElement[-4 + rowSkip]; + dd = ptrDElement[-4 * (int)d_stride]; + q1 = p1 * dd; + q2 = p2 * dd; + ptrAElement[-4] = q1; + ptrAElement[-4 + rowSkip] = q2; + Z11 += p1 * q1; + Z21 += p2 * q1; + Z22 += p2 * q2; + + p1 = ptrAElement[-3]; + p2 = ptrAElement[-3 + rowSkip]; + dd = ptrDElement[-3 * (int)d_stride]; + q1 = p1 * dd; + q2 = p2 * dd; + ptrAElement[-3] = q1; + ptrAElement[-3 + rowSkip] = q2; + Z11 += p1 * q1; + Z21 += p2 * q1; + Z22 += p2 * q2; + + p1 = ptrAElement[-2]; + p2 = ptrAElement[-2 + rowSkip]; + dd = ptrDElement[-2 * (int)d_stride]; + q1 = p1 * dd; + q2 = p2 * dd; + ptrAElement[-2] = q1; + ptrAElement[-2 + rowSkip] = q2; + Z11 += p1 * q1; + Z21 += p2 * q1; + Z22 += p2 * q2; + + p1 = ptrAElement[-1]; + p2 = ptrAElement[-1 + rowSkip]; + dd = ptrDElement[-1 * (int)d_stride]; + q1 = p1 * dd; + q2 = p2 * dd; + ptrAElement[-1] = q1; + ptrAElement[-1 + rowSkip] = q2; + Z11 += p1 * q1; + Z21 += p2 * q1; + Z22 += p2 * q2; + } + else + { + ptrAElement += 2; + ptrDElement += 2 * d_stride; + + if ((columnCounter -= 2) == 0) + { + break; + } + } + } + + /* solve for diagonal 2 x 2 block at A(i,i) */ + dReal Y11 = ptrAElement[0] - Z11; + dReal Y21 = ptrAElement[rowSkip] - Z21; + dReal Y22 = ptrAElement[1 + rowSkip] - Z22; + + /* factorize 2 x 2 block Y, ptrDElement */ + /* factorize row 1 */ + dReal dd = dRecip(Y11); + + ptrDElement[0 * d_stride] = dd; + dIASSERT(ptrDElement == d + (sizeint)factorizationRow * d_stride); + + /* factorize row 2 */ + dReal q2 = Y21 * dd; + ptrAElement[rowSkip] = q2; + + dReal sum = Y21 * q2; + ptrDElement[1 * d_stride] = dRecip(Y22 - sum); +} + +template<unsigned int d_stride> +void scaleAndFactorizeL1FirstRowStripe_2(dReal *ARow, dReal *d, unsigned rowSkip) +{ + dReal *ptrAElement = ARow; + dReal *ptrDElement = d; + + /* solve for diagonal 2 x 2 block at A(0,0) */ + dReal Y11 = ptrAElement[0]/* - Z11*/; + dReal Y21 = ptrAElement[rowSkip]/* - Z21*/; + dReal Y22 = ptrAElement[1 + rowSkip]/* - Z22*/; + + /* factorize 2 x 2 block Y, ptrDElement */ + /* factorize row 1 */ + dReal dd = dRecip(Y11); + + ptrDElement[0 * d_stride] = dd; + dIASSERT(ptrDElement == d/* + (sizeint)factorizationRow * d_stride*/); + + /* factorize row 2 */ + dReal q2 = Y21 * dd; + ptrAElement[rowSkip] = q2; + + dReal sum = Y21 * q2; + ptrDElement[1 * d_stride] = dRecip(Y22 - sum); +} + + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ +static +void solveStripeL1_1(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(rowCount != 0); + dIASSERT(rowCount % 2 == 0); + + /* compute all 2 x 1 blocks of X */ + unsigned blockStartRow = 0; + for (bool exitLoop = false, subsequentPass = false; !exitLoop; subsequentPass = true, exitLoop = (blockStartRow += 2) == rowCount) + { + const dReal *ptrLElement; + dReal *ptrBElement; + + /* declare variables - Z matrix */ + dReal Z11, Z21; + + if (subsequentPass) + { + ptrLElement = L + (sizeint)blockStartRow * rowSkip; + ptrBElement = B; + + /* set the Z matrix to 0 */ + Z11 = 0; Z21 = 0; + + /* compute all 2 x 1 block of X, from rows i..i+2-1 */ + + /* the inner loop that computes outer products and adds them to Z */ + // The iteration starts with even number and decreases it by 2. So, it must end in zero + for (unsigned columnCounter = blockStartRow; ; ) + { + /* declare p and q vectors, etc */ + dReal p1, q1, p2; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[0]; + q1 = ptrBElement[0]; + Z11 += p1 * q1; + p2 = ptrLElement[rowSkip]; + Z21 += p2 * q1; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[1]; + q1 = ptrBElement[1]; + Z11 += p1 * q1; + p2 = ptrLElement[1 + rowSkip]; + Z21 += p2 * q1; + + if (columnCounter > 6) + { + columnCounter -= 6; + + /* advance pointers */ + ptrLElement += 6; + ptrBElement += 6; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-4]; + q1 = ptrBElement[-4]; + Z11 += p1 * q1; + p2 = ptrLElement[-4 + rowSkip]; + Z21 += p2 * q1; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-3]; + q1 = ptrBElement[-3]; + Z11 += p1 * q1; + p2 = ptrLElement[-3 + rowSkip]; + Z21 += p2 * q1; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-2]; + q1 = ptrBElement[-2]; + Z11 += p1 * q1; + p2 = ptrLElement[-2 + rowSkip]; + Z21 += p2 * q1; + + /* compute outer product and add it to the Z matrix */ + p1 = ptrLElement[-1]; + q1 = ptrBElement[-1]; + Z11 += p1 * q1; + p2 = ptrLElement[-1 + rowSkip]; + Z21 += p2 * q1; + } + else + { + /* advance pointers */ + ptrLElement += 2; + ptrBElement += 2; + + if ((columnCounter -= 2) == 0) + { + break; + } + } + /* end of inner loop */ + } + } + else + { + ptrLElement = L/* + (sizeint)blockStartRow * rowSkip*/; dIASSERT(blockStartRow == 0); + ptrBElement = B; + + /* set the Z matrix to 0 */ + Z11 = 0; Z21 = 0; + } + + /* finish computing the X(i) block */ + dReal p2 = ptrLElement[rowSkip]; + + dReal Y11 = ptrBElement[0] - Z11; + dReal Y21 = ptrBElement[1] - Z21 - p2 * Y11; + + ptrBElement[0] = Y11; + ptrBElement[1] = Y21; + /* end of outer loop */ + } +} + +template<unsigned int d_stride> +void scaleAndFactorizeL1Stripe_1(dReal *ARow, dReal *d, unsigned factorizationRow) +{ + dReal *ptrAElement = ARow; + dReal *ptrDElement = d; + + /* scale the elements in a 1 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + dReal Z11 = 0, Z22 = 0; + + for (unsigned columnCounter = factorizationRow; ; ) + { + dReal p1, p2, q1, q2, dd1, dd2; + + p1 = ptrAElement[0]; + p2 = ptrAElement[1]; + dd1 = ptrDElement[0 * d_stride]; + dd2 = ptrDElement[1 * d_stride]; + q1 = p1 * dd1; + q2 = p2 * dd2; + ptrAElement[0] = q1; + ptrAElement[1] = q2; + Z11 += p1 * q1; + Z22 += p2 * q2; + + if (columnCounter > 6) + { + columnCounter -= 6; + + ptrAElement += 6; + ptrDElement += 6 * d_stride; + + p1 = ptrAElement[-4]; + p2 = ptrAElement[-3]; + dd1 = ptrDElement[-4 * (int)d_stride]; + dd2 = ptrDElement[-3 * (int)d_stride]; + q1 = p1 * dd1; + q2 = p2 * dd2; + ptrAElement[-4] = q1; + ptrAElement[-3] = q2; + Z11 += p1 * q1; + Z22 += p2 * q2; + + p1 = ptrAElement[-2]; + p2 = ptrAElement[-1]; + dd1 = ptrDElement[-2 * (int)d_stride]; + dd2 = ptrDElement[-1 * (int)d_stride]; + q1 = p1 * dd1; + q2 = p2 * dd2; + ptrAElement[-2] = q1; + ptrAElement[-1] = q2; + Z11 += p1 * q1; + Z22 += p2 * q2; + } + else + { + ptrAElement += 2; + ptrDElement += 2 * d_stride; + + if ((columnCounter -= 2) == 0) + { + break; + } + } + } + + dReal Y11 = ptrAElement[0] - (Z11 + Z22); + + /* solve for diagonal 1 x 1 block at A(i,i) */ + dIASSERT(ptrDElement == d + (sizeint)factorizationRow * d_stride); + /* factorize 1 x 1 block Y, ptrDElement */ + /* factorize row 1 */ + ptrDElement[0 * d_stride] = dRecip(Y11); +} + +template<unsigned int d_stride> +void scaleAndFactorizeL1FirstRowStripe_1(dReal *ARow, dReal *d) +{ + dReal *ptrAElement = ARow; + dReal *ptrDElement = d; + + dReal Y11 = ptrAElement[0]; + + /* solve for diagonal 1 x 1 block at A(0,0) */ + /* factorize 1 x 1 block Y, ptrDElement */ + /* factorize row 1 */ + ptrDElement[0 * d_stride] = dRecip(Y11); +} + + + + +template<unsigned int block_step, unsigned int b_rows> +/*static */ +void ThreadedEquationSolverLDLT::participateSolvingL1Stripe_X(const dReal *L, dReal *B, unsigned blockCount, unsigned rowSkip, + volatile atomicord32 &refBlockCompletionProgress/*=0*/, volatile cellindexint *blockProgressDescriptors/*=[blockCount]*/, + FactorizationSolveL1StripeCellContext *cellContexts/*=[CCI__MAX x blockCount] + [blockCount]*/, unsigned ownThreadIndex) +{ + const unsigned lookaheadRange = 64; + BlockProcessingState blockProcessingState = BPS_NO_BLOCKS_PROCESSED; + + unsigned completedBlocks = refBlockCompletionProgress; + unsigned currentBlock = completedBlocks; + dIASSERT(completedBlocks <= blockCount); + + for (bool exitLoop = completedBlocks == blockCount; !exitLoop; exitLoop = false) + { + bool goForLockedBlockPrimaryCalculation = false, goForLockedBlockDuplicateCalculation = false; + bool goAssigningTheResult = false, stayWithinTheBlock = false; + + dReal Z[block_step][b_rows]; + dReal Y[block_step][b_rows]; + + dReal *ptrBElement; + + CellContextInstance previousContextInstance; + unsigned completedColumnBlock; + + for (cellindexint testDescriptor = blockProgressDescriptors[currentBlock]; ; ) + { + if (testDescriptor == INVALID_CELLDESCRIPTOR) + { + // Invalid descriptor is the indication that the row has been fully calculated + // Test if this was the last row and break out if so. + if (currentBlock + 1 == blockCount) + { + exitLoop = true; + break; + } + + // Treat detected row advancement as a row processed + // blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; <-- performs better without it + break; + } + + CooperativeAtomics::AtomicReadReorderBarrier(); + // It is necessary to read up to date completedBblocks value after the descriptor retrieval + // as otherwise the logic below breaks + completedBlocks = refBlockCompletionProgress; + + if (!GET_CELLDESCRIPTOR_ISLOCKED(testDescriptor)) + { + completedColumnBlock = GET_CELLDESCRIPTOR_COLUMNINDEX(testDescriptor); + dIASSERT(completedColumnBlock < currentBlock || (completedColumnBlock == currentBlock && currentBlock == 0)); // Otherwise, why would the calculation have had stopped if the final column is reachable??? + dIASSERT(completedColumnBlock <= completedBlocks); // Since the descriptor is not locked + + if (completedColumnBlock == completedBlocks && currentBlock != completedBlocks) + { + dIASSERT(completedBlocks < currentBlock); + break; + } + + if (CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], testDescriptor, MARK_CELLDESCRIPTOR_LOCKED(testDescriptor))) + { + if (completedColumnBlock != 0) + { + CellContextInstance contextInstance = GET_CELLDESCRIPTOR_CONTEXTINSTANCE(testDescriptor); + previousContextInstance = contextInstance; + + const FactorizationSolveL1StripeCellContext &sourceContext = buildBlockContextRef(cellContexts, currentBlock, contextInstance); + sourceContext.loadPrecalculatedZs(Z); + } + else + { + previousContextInstance = CCI__MIN; + FactorizationSolveL1StripeCellContext::initializePrecalculatedZs(Z); + } + + goForLockedBlockPrimaryCalculation = true; + break; + } + + if (blockProcessingState != BPS_COMPETING_FOR_A_BLOCK) + { + break; + } + + testDescriptor = blockProgressDescriptors[currentBlock]; + } + else + { + if (blockProcessingState != BPS_COMPETING_FOR_A_BLOCK) + { + break; + } + + cellindexint verificativeDescriptor; + bool verificationFailure = false; + + completedColumnBlock = GET_CELLDESCRIPTOR_COLUMNINDEX(testDescriptor); + dIASSERT(completedColumnBlock != currentBlock || currentBlock == 0); // There is no reason for computations to stop at the very end other than being the initial value at the very first block + + if (completedColumnBlock != 0) + { + CellContextInstance contextInstance = GET_CELLDESCRIPTOR_CONTEXTINSTANCE(testDescriptor); + const FactorizationSolveL1StripeCellContext &sourceContext = buildBlockContextRef(cellContexts, currentBlock, contextInstance); + sourceContext.loadPrecalculatedZs(Z); + } + else + { + FactorizationSolveL1StripeCellContext::initializePrecalculatedZs(Z); + } + + if (completedColumnBlock != 0 && completedColumnBlock <= currentBlock) + { + // Make sure the descriptor is re-read after the precalculates + CooperativeAtomics::AtomicReadReorderBarrier(); + } + + if (completedColumnBlock <= currentBlock) + { + verificativeDescriptor = blockProgressDescriptors[currentBlock]; + verificationFailure = verificativeDescriptor != testDescriptor; + } + + if (!verificationFailure) + { + dIASSERT(completedColumnBlock <= currentBlock + 1); + + goForLockedBlockDuplicateCalculation = true; + break; + } + + testDescriptor = verificativeDescriptor; + } + } + + if (exitLoop) + { + break; + } + + if (goForLockedBlockPrimaryCalculation) + { + blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; + + // Declare and assign the variables at the top to not interfere with any branching -- the compiler is going to eliminate them anyway. + bool handleComputationTakenOver = false, rowEndReached = false; + + const dReal *ptrLElement; + unsigned finalColumnBlock; + + if (currentBlock != 0) + { + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + ptrLElement = L + (currentBlock * rowSkip + completedColumnBlock) * block_step; + ptrBElement = B + completedColumnBlock * block_step; + + /* the inner loop that computes outer products and adds them to Z */ + finalColumnBlock = dMACRO_MIN(currentBlock, completedBlocks); + dIASSERT(completedColumnBlock != finalColumnBlock/* || currentBlock == 0*/); + + // The iteration starts with even number and decreases it by 2. So, it must end in zero + for (unsigned columnCounter = finalColumnBlock - completedColumnBlock; ; ) + { + /* declare p and q vectors, etc */ + dReal p[block_step], q[b_rows]; + + /* compute outer product and add it to the Z matrix */ + p[0] = ptrLElement[0]; + q[0] = ptrBElement[0]; + Z[0][0] += p[0] * q[0]; + if (b_rows >= 2) + { + q[1] = ptrBElement[rowSkip]; + Z[0][1] += p[0] * q[1]; + } + p[1] = ptrLElement[rowSkip]; + Z[1][0] += p[1] * q[0]; + if (b_rows >= 2) + { + Z[1][1] += p[1] * q[1]; + } + + /* compute outer product and add it to the Z matrix */ + p[0] = ptrLElement[1]; + q[0] = ptrBElement[1]; + Z[0][0] += p[0] * q[0]; + if (b_rows >= 2) + { + q[1] = ptrBElement[1 + rowSkip]; + Z[0][1] += p[0] * q[1]; + } + p[1] = ptrLElement[1 + rowSkip]; + Z[1][0] += p[1] * q[0]; + if (b_rows >= 2) + { + Z[1][1] += p[1] * q[1]; + } + + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + + if (columnCounter > 2) + { + /* compute outer product and add it to the Z matrix */ + p[0] = ptrLElement[2]; + q[0] = ptrBElement[2]; + Z[0][0] += p[0] * q[0]; + if (b_rows >= 2) + { + q[1] = ptrBElement[2 + rowSkip]; + Z[0][1] += p[0] * q[1]; + } + p[1] = ptrLElement[2 + rowSkip]; + Z[1][0] += p[1] * q[0]; + if (b_rows >= 2) + { + Z[1][1] += p[1] * q[1]; + } + + /* compute outer product and add it to the Z matrix */ + p[0] = ptrLElement[3]; + q[0] = ptrBElement[3]; + Z[0][0] += p[0] * q[0]; + if (b_rows >= 2) + { + q[1] = ptrBElement[3 + rowSkip]; + Z[0][1] += p[0] * q[1]; + } + p[1] = ptrLElement[3 + rowSkip]; + Z[1][0] += p[1] * q[0]; + if (b_rows >= 2) + { + Z[1][1] += p[1] * q[1]; + } + + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + + /* advance pointers */ + ptrLElement += 2 * block_step; + ptrBElement += 2 * block_step; + columnCounter -= 2; + } + else + { + /* advance pointers */ + ptrLElement += block_step; + ptrBElement += block_step; + /* end of inner loop */ + + if (--columnCounter == 0) + { + if (finalColumnBlock == currentBlock) + { + rowEndReached = true; + break; + } + + // Take a look if any more rows have been completed... + completedBlocks = refBlockCompletionProgress; + dIASSERT(completedBlocks >= finalColumnBlock); + + if (completedBlocks == finalColumnBlock) + { + break; + } + + // ...continue if so. + unsigned columnCompletedSoFar = finalColumnBlock; + finalColumnBlock = dMACRO_MIN(currentBlock, completedBlocks); + columnCounter = finalColumnBlock - columnCompletedSoFar; + } + } + } + } + else + { + ptrLElement = L/* + (currentBlock * rowSkip + completedColumnBlock) * block_step*/; + ptrBElement = B/* + completedColumnBlock * block_step*/; + + rowEndReached = true; + } + + if (rowEndReached) + { + // Check whether there is still a need to proceed or if the computation has been taken over by another thread + cellindexint oldDescriptor = MAKE_CELLDESCRIPTOR(completedColumnBlock, previousContextInstance, true); + + if (blockProgressDescriptors[currentBlock] == oldDescriptor) + { + /* finish computing the X(i) block */ + Y[0][0] = ptrBElement[0] - Z[0][0]; + if (b_rows >= 2) + { + Y[0][1] = ptrBElement[rowSkip] - Z[0][1]; + } + + dReal p2 = ptrLElement[rowSkip]; + + Y[1][0] = ptrBElement[1] - Z[1][0] - p2 * Y[0][0]; + if (b_rows >= 2) + { + Y[1][1] = ptrBElement[1 + rowSkip] - Z[1][1] - p2 * Y[0][1]; + } + + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + + // Use atomic memory barrier to make sure memory reads of ptrBElement[] and blockProgressDescriptors[] are not swapped + CooperativeAtomics::AtomicReadReorderBarrier(); + + // The descriptor has not been altered yet - this means the ptrBElement[] values used above were not modified yet + // and the computation result is valid. + if (blockProgressDescriptors[currentBlock] == oldDescriptor) + { + // Assign the results to the result context (possibly in parallel with other threads + // that could and ought to be assigning exactly the same values) + FactorizationSolveL1StripeCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.storePrecalculatedZs(Y); + + // Assign the result assignment progress descriptor + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true); + CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], oldDescriptor, newDescriptor); // the result is to be ignored + + // Whether succeeded or not, the result is valid, so go on trying to assign it to the matrix + goAssigningTheResult = true; + } + else + { + // Otherwise, go on competing for copying the results + handleComputationTakenOver = true; + } + } + else + { + handleComputationTakenOver = true; + } + } + else + { + // If the final column has not been reached yet, store current values to the context. + // Select the other context instance as the previous one might be read by other threads. + CellContextInstance nextContextInstance = buildNextContextInstance(previousContextInstance); + FactorizationSolveL1StripeCellContext &destinationContext = buildBlockContextRef(cellContexts, currentBlock, nextContextInstance); + destinationContext.storePrecalculatedZs(Z); + + // Unlock the row until more columns can be used + cellindexint oldDescriptor = MAKE_CELLDESCRIPTOR(completedColumnBlock, previousContextInstance, true); + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(finalColumnBlock, nextContextInstance, false); + // The descriptor might have been updated by a competing thread + if (!CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], oldDescriptor, newDescriptor)) + { + // Adjust the ptrBElement to point to the result area... + ptrBElement = B + currentBlock * block_step; + // ...and go on handling the case + handleComputationTakenOver = true; + } + } + + if (handleComputationTakenOver) + { + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + // This can only happen if the row was (has become) the uppermost not fully completed one + // and the competing thread is at final stage of calculation (i.e., it has reached the currentBlock column). + if (existingDescriptor != INVALID_CELLDESCRIPTOR) + { + // If not fully completed this must be the final stage of the result assignment into the matrix + dIASSERT(existingDescriptor == MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true)); + + // Go on competing copying the result as anyway the block is the topmost not completed one + // and since there was competition for it, there is no other work that can be done right now. + const FactorizationSolveL1StripeCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.loadPrecalculatedZs(Y); + + goAssigningTheResult = true; + } + else + { + // everything is over -- just go handling next blocks + } + } + } + else if (goForLockedBlockDuplicateCalculation) + { + blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; + + bool skipToHandlingSubsequentRows = false, skiptoCopyingResult = false; + + /* declare variables */ + const dReal *ptrLElement; + + if (completedColumnBlock < currentBlock) + { + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + ptrLElement = L + (currentBlock * rowSkip + completedColumnBlock) * block_step; + ptrBElement = B + completedColumnBlock * block_step; + + /* the inner loop that computes outer products and adds them to Z */ + // The iteration starts with even number and decreases it by 2. So, it must end in zero + const unsigned finalColumnBlock = currentBlock; + dIASSERT(currentBlock == completedBlocks); // Why would we be competing for a row otherwise? + + unsigned lastCompletedColumn = completedColumnBlock; + unsigned columnCounter = finalColumnBlock - completedColumnBlock; + for (bool exitInnerLoop = false; !exitInnerLoop; exitInnerLoop = --columnCounter == 0) + { + /* declare p and q vectors, etc */ + dReal p[block_step], q[b_rows]; + + /* compute outer product and add it to the Z matrix */ + p[0] = ptrLElement[0]; + q[0] = ptrBElement[0]; + Z[0][0] += p[0] * q[0]; + if (b_rows >= 2) + { + q[1] = ptrBElement[rowSkip]; + Z[0][1] += p[0] * q[1]; + } + p[1] = ptrLElement[rowSkip]; + Z[1][0] += p[1] * q[0]; + if (b_rows >= 2) + { + Z[1][1] += p[1] * q[1]; + } + + /* compute outer product and add it to the Z matrix */ + p[0] = ptrLElement[1]; + q[0] = ptrBElement[1]; + Z[0][0] += p[0] * q[0]; + if (b_rows >= 2) + { + q[1] = ptrBElement[1 + rowSkip]; + Z[0][1] += p[0] * q[1]; + } + p[1] = ptrLElement[1 + rowSkip]; + Z[1][0] += p[1] * q[0]; + if (b_rows >= 2) + { + Z[1][1] += p[1] * q[1]; + } + + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + + // Check if the primary solver thread has not made any progress + cellindexint descriptorVerification = blockProgressDescriptors[currentBlock]; + unsigned newCompletedColumn = GET_CELLDESCRIPTOR_COLUMNINDEX(descriptorVerification); + + if (newCompletedColumn != lastCompletedColumn) + { + // Check, this is the first change the current thread detects. + // There is absolutely no reason in code for the computation to stop/resume twice + // while the current thread is competing. + dIASSERT(lastCompletedColumn == completedColumnBlock); + + if (descriptorVerification == INVALID_CELLDESCRIPTOR) + { + skipToHandlingSubsequentRows = true; + break; + } + + if (newCompletedColumn == currentBlock + 1) + { + skiptoCopyingResult = true; + break; + } + + // Check if the current thread is behind + if (newCompletedColumn > finalColumnBlock - columnCounter) + { + // If so, go starting over one more time + blockProcessingState = BPS_COMPETING_FOR_A_BLOCK; + stayWithinTheBlock = true; + skipToHandlingSubsequentRows = true; + break; + } + + // If current thread is ahead, just save new completed column for further comparisons and go on calculating + lastCompletedColumn = newCompletedColumn; + } + + /* advance pointers */ + ptrLElement += block_step; + ptrBElement += block_step; + /* end of inner loop */ + } + } + else if (completedColumnBlock > currentBlock) + { + dIASSERT(completedColumnBlock == currentBlock + 1); + + skiptoCopyingResult = true; + } + else + { + dIASSERT(currentBlock == 0); // Execution can get here within the very first block only + + /* assign the pointers appropriately and go on computing the results */ + ptrLElement = L/* + (currentBlock * rowSkip + completedColumnBlock) * block_step*/; + ptrBElement = B/* + completedColumnBlock * block_step*/; + } + + if (!skipToHandlingSubsequentRows) + { + if (!skiptoCopyingResult) + { + /* finish computing the X(i) block */ + Y[0][0] = ptrBElement[0] - Z[0][0]; + if (b_rows >= 2) + { + Y[0][1] = ptrBElement[rowSkip] - Z[0][1]; + } + + dReal p2 = ptrLElement[rowSkip]; + + Y[1][0] = ptrBElement[1] - Z[1][0] - p2 * Y[0][0]; + if (b_rows >= 2) + { + Y[1][1] = ptrBElement[1 + rowSkip] - Z[1][1] - p2 * Y[0][1]; + } + + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + + // Use atomic memory barrier to make sure memory reads of ptrBElement[] and blockProgressDescriptors[] are not swapped + CooperativeAtomics::AtomicReadReorderBarrier(); + + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + + if (existingDescriptor == INVALID_CELLDESCRIPTOR) + { + // Everything is over -- proceed to subsequent rows + skipToHandlingSubsequentRows = true; + } + else if (existingDescriptor == MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true)) + { + // The values computed above may not be valid. Copy the values already in the result context. + skiptoCopyingResult = true; + } + else + { + // The descriptor has not been altered yet - this means the ptrBElement[] values used above were not modified yet + // and the computation result is valid. + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true); // put the computation at the top so that the evaluation result from the expression above is reused + + // Assign the results to the result context (possibly in parallel with other threads + // that could and ought to be assigning exactly the same values) + FactorizationSolveL1StripeCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.storePrecalculatedZs(Y); + + // Assign the result assignment progress descriptor + CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], existingDescriptor, newDescriptor); // the result is to be ignored + + // Whether succeeded or not, the result is valid, so go on trying to assign it to the matrix + } + } + + if (!skipToHandlingSubsequentRows) + { + if (skiptoCopyingResult) + { + // Extract the result values stored in the result context + const FactorizationSolveL1StripeCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.loadPrecalculatedZs(Y); + + ptrBElement = B + currentBlock * block_step; + } + + goAssigningTheResult = true; + } + } + } + + if (goAssigningTheResult) + { + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + // Check if the assignment has not been completed yet + if (existingDescriptor != INVALID_CELLDESCRIPTOR) + { + // Assign the computation results to their places in the matrix + ptrBElement[0] = Y[0][0]; + ptrBElement[1] = Y[1][0]; + if (b_rows >= 2) + { + ptrBElement[rowSkip] = Y[0][1]; + ptrBElement[1 + rowSkip] = Y[1][1]; + } + + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + + ThrsafeIncrementIntUpToLimit(&refBlockCompletionProgress, currentBlock + 1); + dIASSERT(refBlockCompletionProgress >= currentBlock + 1); + + // And assign the completed status no matter what + CooperativeAtomics::AtomicStoreCellindexint(&blockProgressDescriptors[currentBlock], INVALID_CELLDESCRIPTOR); + } + else + { + // everything is over -- just go handling next blocks + } + } + + if (!stayWithinTheBlock) + { + completedBlocks = refBlockCompletionProgress; + + if (completedBlocks == blockCount) + { + break; + } + + currentBlock += 1; + + bool lookaheadBoundaryReached = false; + + if (currentBlock == blockCount || completedBlocks == 0) + { + lookaheadBoundaryReached = true; + } + else if (currentBlock >= completedBlocks + lookaheadRange) + { + lookaheadBoundaryReached = blockProcessingState > BPS_NO_BLOCKS_PROCESSED; + } + else if (currentBlock < completedBlocks) + { + // Treat detected row advancement as a row processed + // blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; <-- performs better without it + + currentBlock = completedBlocks; + } + + if (lookaheadBoundaryReached) + { + dIASSERT(blockProcessingState != BPS_COMPETING_FOR_A_BLOCK); // Why did not we compete??? + + // If no row has been processed in the previous pass, compete for the next row to avoid cycling uselessly + if (blockProcessingState <= BPS_NO_BLOCKS_PROCESSED) + { + // Abandon job if too few blocks remain + if (blockCount - completedBlocks <= ownThreadIndex) + { + break; + } + + blockProcessingState = BPS_COMPETING_FOR_A_BLOCK; + } + else + { + // If there was some progress, just continue to the next pass + blockProcessingState = BPS_NO_BLOCKS_PROCESSED; + } + + currentBlock = completedBlocks; + } + } + } +} + + +template<unsigned int a_rows, unsigned int d_stride> +/*static */ +void ThreadedEquationSolverLDLT::participateScalingAndFactorizingL1Stripe_X(dReal *ARow, dReal *d, unsigned factorizationRow, unsigned rowSkip, + FactorizationFactorizeL1StripeContext *factorizationContext, unsigned ownThreadIndex) +{ + dIASSERT(factorizationRow != 0); + dIASSERT(factorizationRow % 2 == 0); + + /* scale the elements in a 2 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + dReal sameZ[a_rows] = { REAL(0.0), }, mixedZ[dMACRO_MAX(a_rows - 1, 1)] = { REAL(0.0), }; + bool doneAnything = false; + + const unsigned blockSize = deriveScalingAndFactorizingL1StripeBlockSize(a_rows); + + const unsigned blockCount = deriveScalingAndFactorizingL1StripeBlockCountFromFactorizationRow(factorizationRow, blockSize); + dIASSERT(blockCount != 0); + + unsigned blockIndex; + while ((blockIndex = ThrsafeIncrementIntUpToLimit(&factorizationContext->m_nextColumnIndex, blockCount)) != blockCount) + { + doneAnything = true; + unsigned blockStartRow = blockIndex * blockSize; + + dReal *ptrAElement = ARow + blockStartRow; + dReal *ptrDElement = d + blockStartRow * d_stride; + for (unsigned columnCounter = blockIndex != blockCount - 1 ? blockSize : factorizationRow - blockStartRow; ; ) + { + dReal p1, q1, p2, q2, dd; + + p1 = ptrAElement[0]; + if (a_rows >= 2) + { + p2 = ptrAElement[rowSkip]; + } + dd = ptrDElement[0 * d_stride]; + q1 = p1 * dd; + if (a_rows >= 2) + { + q2 = p2 * dd; + } + ptrAElement[0] = q1; + if (a_rows >= 2) + { + ptrAElement[rowSkip] = q2; + } + sameZ[0] += p1 * q1; + if (a_rows >= 2) + { + sameZ[1] += p2 * q2; + mixedZ[0] += p2 * q1; + } + + p1 = ptrAElement[1]; + if (a_rows >= 2) + { + p2 = ptrAElement[1 + rowSkip]; + } + dd = ptrDElement[1 * d_stride]; + q1 = p1 * dd; + if (a_rows >= 2) + { + q2 = p2 * dd; + } + ptrAElement[1] = q1; + if (a_rows >= 2) + { + ptrAElement[1 + rowSkip] = q2; + } + sameZ[0] += p1 * q1; + if (a_rows >= 2) + { + sameZ[1] += p2 * q2; + mixedZ[0] += p2 * q1; + } + + if (columnCounter > 6) + { + columnCounter -= 6; + + ptrAElement += 6; + ptrDElement += 6 * d_stride; + + p1 = ptrAElement[-4]; + if (a_rows >= 2) + { + p2 = ptrAElement[-4 + rowSkip]; + } + dd = ptrDElement[-4 * (int)d_stride]; + q1 = p1 * dd; + if (a_rows >= 2) + { + q2 = p2 * dd; + } + ptrAElement[-4] = q1; + if (a_rows >= 2) + { + ptrAElement[-4 + rowSkip] = q2; + } + sameZ[0] += p1 * q1; + if (a_rows >= 2) + { + sameZ[1] += p2 * q2; + mixedZ[0] += p2 * q1; + } + + p1 = ptrAElement[-3]; + if (a_rows >= 2) + { + p2 = ptrAElement[-3 + rowSkip]; + } + dd = ptrDElement[-3 * (int)d_stride]; + q1 = p1 * dd; + if (a_rows >= 2) + { + q2 = p2 * dd; + } + ptrAElement[-3] = q1; + if (a_rows >= 2) + { + ptrAElement[-3 + rowSkip] = q2; + } + sameZ[0] += p1 * q1; + if (a_rows >= 2) + { + sameZ[1] += p2 * q2; + mixedZ[0] += p2 * q1; + } + + p1 = ptrAElement[-2]; + if (a_rows >= 2) + { + p2 = ptrAElement[-2 + rowSkip]; + } + dd = ptrDElement[-2 * (int)d_stride]; + q1 = p1 * dd; + if (a_rows >= 2) + { + q2 = p2 * dd; + } + ptrAElement[-2] = q1; + if (a_rows >= 2) + { + ptrAElement[-2 + rowSkip] = q2; + } + sameZ[0] += p1 * q1; + if (a_rows >= 2) + { + sameZ[1] += p2 * q2; + mixedZ[0] += p2 * q1; + } + + p1 = ptrAElement[-1]; + if (a_rows >= 2) + { + p2 = ptrAElement[-1 + rowSkip]; + } + dd = ptrDElement[-1 * (int)d_stride]; + q1 = p1 * dd; + if (a_rows >= 2) + { + q2 = p2 * dd; + } + ptrAElement[-1] = q1; + if (a_rows >= 2) + { + ptrAElement[-1 + rowSkip] = q2; + } + sameZ[0] += p1 * q1; + if (a_rows >= 2) + { + sameZ[1] += p2 * q2; + mixedZ[0] += p2 * q1; + } + } + else + { + ptrAElement += 2; + ptrDElement += 2 * d_stride; + + if ((columnCounter -= 2) == 0) + { + break; + } + } + } + } + + if (doneAnything) + { + unsigned partialSumThreadIndex; + for (bool exitLoop = false; !exitLoop; exitLoop = CooperativeAtomics::AtomicCompareExchangeUint32(&factorizationContext->m_sumThreadIndex, partialSumThreadIndex, ownThreadIndex + 1)) + { + partialSumThreadIndex = factorizationContext->m_sumThreadIndex; + + if (partialSumThreadIndex != 0) + { + const FactorizationFactorizeL1StripeThreadContext &partialSumContext = factorizationContext->m_threadContexts[partialSumThreadIndex - 1]; + factorizationContext->m_threadContexts[ownThreadIndex].assignDataSum<a_rows>(sameZ, mixedZ, partialSumContext); + } + else + { + factorizationContext->m_threadContexts[ownThreadIndex].assignDataAlone<a_rows>(sameZ, mixedZ); + } + } + } + + unsigned threadExitIndex = CooperativeAtomics::AtomicDecrementUint32(&factorizationContext->m_threadsRunning); + dIASSERT(threadExitIndex + 1U != 0); + + if (threadExitIndex == 0) + { + // Let the last thread retrieve the sum and perform final computations + unsigned sumThreadIndex = factorizationContext->m_sumThreadIndex; + dIASSERT(sumThreadIndex != 0); // The rowIndex was asserted to be not zero, so at least one thread must have done something + + const FactorizationFactorizeL1StripeThreadContext &sumContext = factorizationContext->m_threadContexts[sumThreadIndex - 1]; + sumContext.retrieveData<a_rows>(sameZ, mixedZ); + + dReal *ptrAElement = ARow + factorizationRow; + dReal *ptrDElement = d + factorizationRow * d_stride; + + /* solve for diagonal 2 x 2 block at A(i,i) */ + dReal Y11, Y21, Y22; + + Y11 = ptrAElement[0] - sameZ[0]; + if (a_rows >= 2) + { + Y21 = ptrAElement[rowSkip] - mixedZ[0]; + Y22 = ptrAElement[1 + rowSkip] - sameZ[1]; + } + + /* factorize 2 x 2 block Y, ptrDElement */ + /* factorize row 1 */ + dReal dd = dRecip(Y11); + + ptrDElement[0 * d_stride] = dd; + dIASSERT(ptrDElement == d + (sizeint)factorizationRow * d_stride); + + if (a_rows >= 2) + { + /* factorize row 2 */ + dReal q2 = Y21 * dd; + ptrAElement[rowSkip] = q2; + + dReal sum = Y21 * q2; + ptrDElement[1 * d_stride] = dRecip(Y22 - sum); + } + } +} + + +#endif // #ifndef _ODE_FASTLDLT_IMPL_H_ diff --git a/libs/ode-0.16.1/ode/src/fastldltsolve.cpp b/libs/ode-0.16.1/ode/src/fastldltsolve.cpp new file mode 100644 index 0000000..ca1ff4d --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastldltsolve.cpp @@ -0,0 +1,222 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * LDLT solving related code of ThreadedEquationSolverLDLT + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + +#include <ode/common.h> +#include <ode/matrix.h> +#include <ode/matrix_coop.h> +#include "config.h" +#include "threaded_solver_ldlt.h" +#include "threading_base.h" +#include "resource_control.h" + +#include "fastldltsolve_impl.h" + + +/*static */ +void ThreadedEquationSolverLDLT::estimateCooperativeSolvingLDLTResourceRequirements( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount) +{ + unsigned stageBlockCountSifficiencyMask; + dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading(); + unsigned limitedThreadCount = restrictSolvingLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount, stageBlockCountSifficiencyMask); + + if (limitedThreadCount > 1) + { + if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_STRAIGHT)) != 0) + { + doEstimateCooperativeSolvingL1StraightResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount); + } + + if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SCALING_VECTOR)) != 0) + { + doEstimateCooperativeScalingVectorResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount); + } + + if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_TRANSPOSED)) == 0) + { + doEstimateCooperativeSolvingL1TransposedResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount); + } + } +} + +/*static */ +void ThreadedEquationSolverLDLT::cooperativelySolveLDLT( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dAASSERT(rowCount != 0); + + unsigned stageBlockCountSifficiencyMask; + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + unsigned limitedThreadCount = restrictSolvingLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount, stageBlockCountSifficiencyMask); + + if (limitedThreadCount <= 1) + { + solveEquationSystemWithLDLT<SLDLT_D_STRIDE, SLDLT_B_STRIDE>(L, d, b, rowCount, rowSkip); + } + else + { + doCooperativelySolveLDLTValidated(resourceContainer, limitedThreadCount, stageBlockCountSifficiencyMask, L, d, b, rowCount, rowSkip); + } +} + +/*static */ +unsigned ThreadedEquationSolverLDLT::restrictSolvingLDLTAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount, unsigned &out_stageBlockCountSifficiencyMask) +{ + unsigned limitedThreadCount = 1; + unsigned stageBlockCountSifficiencyMask = 0; + +#if dCOOPERATIVE_ENABLED + { + const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation + unsigned solvingStraightBlockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep); + dIASSERT(deriveSolvingL1StraightThreadCount(SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1); + + if (solvingStraightBlockCount >= SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM) + { + stageBlockCountSifficiencyMask |= 1U << SLDLTS_SOLVING_STRAIGHT; + } + } + + { + const unsigned int blockStep = SV_BLOCK_SIZE; // Required by the implementation + unsigned scalingBlockCount = deriveScalingVectorBlockCount(rowCount, blockStep); + dIASSERT(deriveScalingVectorThreadCount(SV_COOPERATIVE_BLOCK_COUNT_MINIMUM - 1, 2) > 1); + + if (scalingBlockCount >= SV_COOPERATIVE_BLOCK_COUNT_MINIMUM) + { + stageBlockCountSifficiencyMask |= 1U << SLDLTS_SCALING_VECTOR; + } + } + + { + const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation + unsigned solvingTransposedBlockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep); + dIASSERT(deriveSolvingL1TransposedThreadCount(SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1); + + if (solvingTransposedBlockCount >= SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM) + { + stageBlockCountSifficiencyMask |= 1U << SLDLTS_SOLVING_TRANSPOSED; + } + } + + if (stageBlockCountSifficiencyMask != 0) + { + limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true); + } +#endif // #if dCOOPERATIVE_ENABLED + + out_stageBlockCountSifficiencyMask = stageBlockCountSifficiencyMask; + return limitedThreadCount; +} + + +/*static */ +void ThreadedEquationSolverLDLT::doCooperativelySolveLDLTValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, unsigned stageBlockCountSifficiencyMask, + const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(allowedThreadCount > 1); + + if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_STRAIGHT)) == 0) + { + solveL1Straight<SLDLT_B_STRIDE>(L, b, rowCount, rowSkip); + } + else + { + dSASSERT(SLDLT_B_STRIDE + 0 == SL1S_B_STRIDE); + + doCooperativelySolveL1StraightValidated(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip); + } + + if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SCALING_VECTOR)) == 0) + { + scaleLargeVector<SLDLT_B_STRIDE, SLDLT_D_STRIDE>(b, d, rowCount); + } + else + { + dSASSERT(SLDLT_B_STRIDE + 0 == SV_A_STRIDE); + dSASSERT(SLDLT_D_STRIDE + 0 == SV_D_STRIDE); + + doCooperativelyScaleVectorValidated(resourceContainer, allowedThreadCount, b, d, rowCount); + } + + if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_TRANSPOSED)) == 0) + { + solveL1Transposed<SLDLT_B_STRIDE>(L, b, rowCount, rowSkip); + } + else + { + dSASSERT(SLDLT_B_STRIDE + 0 == SL1T_B_STRIDE); + + doCooperativelySolveL1TransposedValidated(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip); + } +} + + +////////////////////////////////////////////////////////////////////////// +// Public interface functions + +/*extern ODE_API */ +void dSolveLDLT(const dReal *L, const dReal *d, dReal *b, int n, int nskip) +{ + dAASSERT(n != 0); + + if (n != 0) + { + dAASSERT(L != NULL); + dAASSERT(d != NULL); + dAASSERT(b != NULL); + + solveEquationSystemWithLDLT<1, 1>(L, d, b, n, nskip); + } +} + + +/*extern ODE_API */ +void dEstimateCooperativelySolveLDLTResourceRequirements(dResourceRequirementsID requirements, + unsigned maximalAllowedThreadCount, unsigned maximalRowCount) +{ + dAASSERT(requirements != NULL); + + dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements; + ThreadedEquationSolverLDLT::estimateCooperativeSolvingLDLTResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount); +} + +/*extern ODE_API */ +void dCooperativelySolveLDLT(dResourceContainerID resources, unsigned allowedThreadCount, + const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dAASSERT(resources != NULL); + + dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources; + ThreadedEquationSolverLDLT::cooperativelySolveLDLT(resourceContainer, allowedThreadCount, L, d, b, rowCount, rowSkip); +} + diff --git a/libs/ode-0.16.1/ode/src/fastldltsolve_impl.h b/libs/ode-0.16.1/ode/src/fastldltsolve_impl.h new file mode 100644 index 0000000..ad6f393 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastldltsolve_impl.h @@ -0,0 +1,49 @@ +
+
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_MATRIX_IMPL_H_
+#define _ODE_MATRIX_IMPL_H_
+
+
+#include "fastlsolve_impl.h"
+#include "fastltsolve_impl.h"
+#include "fastvecscale_impl.h"
+
+
+template<unsigned int d_stride, unsigned int b_stride>
+void solveEquationSystemWithLDLT(const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+ dAASSERT(L != NULL);
+ dAASSERT(d != NULL);
+ dAASSERT(b != NULL);
+ dAASSERT(rowCount > 0);
+ dAASSERT(rowSkip >= rowCount);
+
+ solveL1Straight<b_stride>(L, b, rowCount, rowSkip);
+ scaleLargeVector<b_stride, d_stride>(b, d, rowCount);
+ solveL1Transposed<b_stride>(L, b, rowCount, rowSkip);
+}
+
+
+#endif
diff --git a/libs/ode-0.16.1/ode/src/fastlsolve.cpp b/libs/ode-0.16.1/ode/src/fastlsolve.cpp new file mode 100644 index 0000000..6f7e6a4 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastlsolve.cpp @@ -0,0 +1,230 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * L1Straight Equation Solving Routines + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + +#include <ode/common.h> +#include <ode/matrix.h> +#include <ode/matrix_coop.h> +#include "config.h" +#include "threaded_solver_ldlt.h" +#include "threading_base.h" +#include "resource_control.h" +#include "error.h" + +#include "fastlsolve_impl.h" + + +/*static */ +void ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1StraightResourceRequirements( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount) +{ + dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading(); + unsigned limitedThreadCount = restrictSolvingL1StraightAllowedThreadCount(threading, allowedThreadCount, rowCount); + + if (limitedThreadCount > 1) + { + doEstimateCooperativeSolvingL1StraightResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount); + } +} + +/*static */ +void ThreadedEquationSolverLDLT::cooperativelySolveL1Straight( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dAASSERT(rowCount != 0); + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + unsigned limitedThreadCount = restrictSolvingL1StraightAllowedThreadCount(threading, allowedThreadCount, rowCount); + + if (limitedThreadCount <= 1) + { + solveL1Straight<SL1S_B_STRIDE>(L, b, rowCount, rowSkip); + } + else + { + doCooperativelySolveL1StraightValidated(resourceContainer, limitedThreadCount, L, b, rowCount, rowSkip); + } +} + + +/*static */ +unsigned ThreadedEquationSolverLDLT::restrictSolvingL1StraightAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount) +{ + unsigned limitedThreadCount = 1; + +#if dCOOPERATIVE_ENABLED + const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation + unsigned solvingBlockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep); + dIASSERT(deriveSolvingL1StraightThreadCount(SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1); + + if (solvingBlockCount >= SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM) + { + limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true); + } +#endif // #if dCOOPERATIVE_ENABLED + + return limitedThreadCount; +} + +/*static */ +void ThreadedEquationSolverLDLT::doEstimateCooperativeSolvingL1StraightResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount) +{ + const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation + unsigned blockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep); + dIASSERT(blockCount >= 1); + + unsigned threadCountToUse = deriveSolvingL1StraightThreadCount(blockCount, allowedThreadCount); + dIASSERT(threadCountToUse > 1); + + unsigned simultaneousCallCount = 1 + (threadCountToUse - 1); + + SolvingL1StraightMemoryEstimates solvingMemoryEstimates; + sizeint solvingMemoryRequired = estimateCooperativelySolvingL1StraightMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates); + const unsigned solvingAlignmentRequired = ALLOCATION_DEFAULT_ALIGNMENT; + + unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED; + summaryRequirementsDescriptor->mergeAnotherDescriptorIn(solvingMemoryRequired, solvingAlignmentRequired, simultaneousCallCount, featureRequirement); +} + +/*static */ +void ThreadedEquationSolverLDLT::doCooperativelySolveL1StraightValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(allowedThreadCount > 1); + + const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation + unsigned blockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep); + dIASSERT(blockCount >= 1); + + unsigned threadCountToUse = deriveSolvingL1StraightThreadCount(blockCount, allowedThreadCount); + dIASSERT(threadCountToUse > 1); + + dCallWaitID completionWait = resourceContainer->getStockCallWait(); + dAASSERT(completionWait != NULL); + + atomicord32 blockCompletionProgress; + cellindexint *blockProgressDescriptors; + SolveL1StraightCellContext *cellContexts; + + SolvingL1StraightMemoryEstimates solvingMemoryEstimates; + sizeint solvingMemoryRequired = estimateCooperativelySolvingL1StraightMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates); + dIASSERT(solvingMemoryRequired <= resourceContainer->getMemoryBufferSize()); + + void *bufferAllocated = resourceContainer->getMemoryBufferPointer(); + dIASSERT(bufferAllocated != NULL); + dIASSERT(dALIGN_PTR(bufferAllocated, ALLOCATION_DEFAULT_ALIGNMENT) == bufferAllocated); + + void *bufferCurrentLocation = bufferAllocated; + bufferCurrentLocation = markCooperativelySolvingL1StraightMemoryStructuresOut(bufferCurrentLocation, solvingMemoryEstimates, blockProgressDescriptors, cellContexts); + dIVERIFY(bufferCurrentLocation <= (uint8 *)bufferAllocated + solvingMemoryRequired); + + initializeCooperativelySolveL1StraightMemoryStructures<blockStep>(rowCount, blockCompletionProgress, blockProgressDescriptors, cellContexts); + + dCallReleaseeID calculationFinishReleasee; + SolveL1StraightWorkerContext workerContext; // The variable must exist in the outer scope + + workerContext.init(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts); + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + threading->PostThreadedCall(NULL, &calculationFinishReleasee, threadCountToUse - 1, NULL, completionWait, &solveL1Straight_completion_callback, NULL, 0, "SolveL1Straight Completion"); + threading->PostThreadedCallsGroup(NULL, threadCountToUse - 1, calculationFinishReleasee, &solveL1Straight_worker_callback, &workerContext, "SolveL1Straight Work"); + + participateSolvingL1Straight<blockStep, SL1S_B_STRIDE>(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts, threadCountToUse - 1); + + threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "SolveL1Straight End Wait"); +} + +/*static */ +int ThreadedEquationSolverLDLT::solveL1Straight_worker_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee)) +{ + SolveL1StraightWorkerContext *ptrContext = (SolveL1StraightWorkerContext *)callContext; + + solveL1Straight_worker(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex)); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::solveL1Straight_worker(SolveL1StraightWorkerContext &ref_context, unsigned ownThreadIndex) +{ + const unsigned blockStep = SL1S_BLOCK_SIZE; + + participateSolvingL1Straight<blockStep, SL1S_B_STRIDE>(ref_context.m_L, ref_context.m_b, ref_context.m_rowCount, ref_context.m_rowSkip, + *ref_context.m_ptrBlockCompletionProgress, ref_context.m_blockProgressDescriptors, ref_context.m_cellContexts, ownThreadIndex); +} + +/*static */ +int ThreadedEquationSolverLDLT::solveL1Straight_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + return 1; +} + + + +////////////////////////////////////////////////////////////////////////// +// Public interface functions + +/*extern ODE_API */ +void dSolveL1(const dReal *L, dReal *B, int n, int lskip1) +{ + dAASSERT(n != 0); + + if (n != 0) + { + dAASSERT(L != NULL); + dAASSERT(B != NULL); + + solveL1Straight<1>(L, B, n, lskip1); + } +} + + +/*extern ODE_API */ +void dEstimateCooperativelySolveL1StraightResourceRequirements(dResourceRequirementsID requirements, + unsigned maximalAllowedThreadCount, unsigned maximalRowCount) +{ + dAASSERT(requirements != NULL); + + dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements; + ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1StraightResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount); +} + +/*extern ODE_API */ +void dCooperativelySolveL1Straight(dResourceContainerID resources, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dAASSERT(resources != NULL); + + dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources; + ThreadedEquationSolverLDLT::cooperativelySolveL1Straight(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip); +} + diff --git a/libs/ode-0.16.1/ode/src/fastlsolve_impl.h b/libs/ode-0.16.1/ode/src/fastlsolve_impl.h new file mode 100644 index 0000000..f14ada7 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastlsolve_impl.h @@ -0,0 +1,1610 @@ + + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Code style improvements and optimizations by Oleh Derevenko ????-2019 + * L1Straight cooperative solving code of ThreadedEquationSolverLDLT copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + +#ifndef _ODE_FASTLSOLVE_IMPL_H_ +#define _ODE_FASTLSOLVE_IMPL_H_ + + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 4*4. + * if this is in the factorizer source file, n must be a multiple of 4. + */ + +template<unsigned int b_stride> +void solveL1Straight (const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(rowCount != 0); + + /* compute all 4 x 1 blocks of X */ + unsigned blockStartRow = 0; + bool subsequentPass = false; + bool goForLoopX4 = rowCount >= 4; + const unsigned loopX4LastRow = goForLoopX4 ? rowCount - 4 : 0; + for (; goForLoopX4; subsequentPass = true, goForLoopX4 = (blockStartRow += 4) <= loopX4LastRow) + { + /* declare variables - Z matrix, p and q vectors, etc */ + const dReal *ptrLElement; + dReal *ptrBElement; + + dReal Z11, Z21, Z31, Z41; + + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + if (subsequentPass) + { + ptrLElement = L + (1 + blockStartRow) * rowSkip; + ptrBElement = B; + /* set the Z matrix to 0 */ + Z11 = 0; Z21 = 0; Z31 = 0; Z41 = 0; + + /* the inner loop that computes outer products and adds them to Z */ + for (unsigned columnCounter = blockStartRow; ; ) + { + dReal q1, p1, p2, p3, p4; + + /* load p and q values */ + q1 = ptrBElement[0 * b_stride]; + p1 = (ptrLElement - rowSkip)[0]; + p2 = ptrLElement[0]; + ptrLElement += rowSkip; + p3 = ptrLElement[0]; + p4 = ptrLElement[0 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[1 * b_stride]; + p3 = ptrLElement[1]; + p4 = ptrLElement[1 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[1]; + p2 = ptrLElement[1]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[2 * b_stride]; + p1 = (ptrLElement - rowSkip)[2]; + p2 = ptrLElement[2]; + ptrLElement += rowSkip; + p3 = ptrLElement[2]; + p4 = ptrLElement[2 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[3 * b_stride]; + p3 = ptrLElement[3]; + p4 = ptrLElement[3 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[3]; + p2 = ptrLElement[3]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + if (columnCounter > 12) + { + columnCounter -= 12; + + /* advance pointers */ + ptrLElement += 12; + ptrBElement += 12 * b_stride; + + /* load p and q values */ + q1 = ptrBElement[-8 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-8]; + p2 = ptrLElement[-8]; + ptrLElement += rowSkip; + p3 = ptrLElement[-8]; + p4 = ptrLElement[-8 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-7 * (int)b_stride]; + p3 = ptrLElement[-7]; + p4 = ptrLElement[-7 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-7]; + p2 = ptrLElement[-7]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-6 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-6]; + p2 = ptrLElement[-6]; + ptrLElement += rowSkip; + p3 = ptrLElement[-6]; + p4 = ptrLElement[-6 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-5 * (int)b_stride]; + p3 = ptrLElement[-5]; + p4 = ptrLElement[-5 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-5]; + p2 = ptrLElement[-5]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-4 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-4]; + p2 = ptrLElement[-4]; + ptrLElement += rowSkip; + p3 = ptrLElement[-4]; + p4 = ptrLElement[-4 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-3 * (int)b_stride]; + p3 = ptrLElement[-3]; + p4 = ptrLElement[-3 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-3]; + p2 = ptrLElement[-3]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-2 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-2]; + p2 = ptrLElement[-2]; + ptrLElement += rowSkip; + p3 = ptrLElement[-2]; + p4 = ptrLElement[-2 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p3 = ptrLElement[-1]; + p4 = ptrLElement[-1 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-1]; + p2 = ptrLElement[-1]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + } + else + { + /* advance pointers */ + ptrLElement += 4; + ptrBElement += 4 * b_stride; + + if ((columnCounter -= 4) == 0) + { + break; + } + } + /* end of inner loop */ + } + } + else + { + ptrLElement = L + rowSkip/* + blockStartRow * rowSkip*/; dIASSERT(blockStartRow == 0); + ptrBElement = B; + /* set the Z matrix to 0 */ + Z11 = 0; Z21 = 0; Z31 = 0; Z41 = 0; + } + + /* finish computing the X(i) block */ + dReal Y11, Y21, Y31, Y41; + { + Y11 = ptrBElement[0 * b_stride] - Z11; + ptrBElement[0 * b_stride] = Y11; + } + { + dReal p2 = ptrLElement[0]; + Y21 = ptrBElement[1 * b_stride] - Z21 - p2 * Y11; + ptrBElement[1 * b_stride] = Y21; + } + ptrLElement += rowSkip; + { + dReal p3 = ptrLElement[0]; + dReal p3_1 = ptrLElement[1]; + Y31 = ptrBElement[2 * b_stride] - Z31 - p3 * Y11 - p3_1 * Y21; + ptrBElement[2 * b_stride] = Y31; + } + { + dReal p4 = ptrLElement[rowSkip]; + dReal p4_1 = ptrLElement[1 + rowSkip]; + dReal p4_2 = ptrLElement[2 + rowSkip]; + Y41 = ptrBElement[3 * b_stride] - Z41 - p4 * Y11 - p4_1 * Y21 - p4_2 * Y31; + ptrBElement[3 * b_stride] = Y41; + } + /* end of outer loop */ + } + + /* compute rows at end that are not a multiple of block size */ + for (; !subsequentPass || blockStartRow < rowCount; subsequentPass = true, ++blockStartRow) + { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + dReal *ptrBElement; + + dReal Z11, Z12; + + if (subsequentPass) + { + ptrBElement = B; + /* set the Z matrix to 0 */ + Z11 = 0; Z12 = 0; + + const dReal *ptrLElement = L + blockStartRow * rowSkip; + + /* the inner loop that computes outer products and adds them to Z */ + unsigned columnCounter = blockStartRow; + for (bool exitLoop = columnCounter < 4; !exitLoop; exitLoop = false) + { + dReal p1, p2, q1, q2; + + /* load p and q values */ + p1 = ptrLElement[0]; + p2 = ptrLElement[1]; + q1 = ptrBElement[0 * b_stride]; + q2 = ptrBElement[1 * b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z12 += p2 * q2; + + /* load p and q values */ + p1 = ptrLElement[2]; + p2 = ptrLElement[3]; + q1 = ptrBElement[2 * b_stride]; + q2 = ptrBElement[3 * b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z12 += p2 * q2; + + if (columnCounter >= (12 + 4)) + { + columnCounter -= 12; + + /* advance pointers */ + ptrLElement += 12; + ptrBElement += 12 * b_stride; + + /* load p and q values */ + p1 = ptrLElement[-8]; + p2 = ptrLElement[-7]; + q1 = ptrBElement[-8 * (int)b_stride]; + q2 = ptrBElement[-7 * (int)b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z12 += p2 * q2; + + /* load p and q values */ + p1 = ptrLElement[-6]; + p2 = ptrLElement[-5]; + q1 = ptrBElement[-6 * (int)b_stride]; + q2 = ptrBElement[-5 * (int)b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z12 += p2 * q2; + + /* load p and q values */ + p1 = ptrLElement[-4]; + p2 = ptrLElement[-3]; + q1 = ptrBElement[-4 * (int)b_stride]; + q2 = ptrBElement[-3 * (int)b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z12 += p2 * q2; + + /* load p and q values */ + p1 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + q1 = ptrBElement[-2 * (int)b_stride]; + q2 = ptrBElement[-1 * (int)b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z12 += p2 * q2; + } + else + { + /* advance pointers */ + ptrLElement += 4; + ptrBElement += 4 * b_stride; + + if ((columnCounter -= 4) < 4) + { + break; + } + } + /* end of inner loop */ + } + + /* compute left-over iterations */ + if ((columnCounter & 2) != 0) + { + dReal p1, p2, q1, q2; + + /* load p and q values */ + p1 = ptrLElement[0]; + p2 = ptrLElement[1]; + q1 = ptrBElement[0 * b_stride]; + q2 = ptrBElement[1 * b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z12 += p2 * q2; + + /* advance pointers */ + ptrLElement += 2; + ptrBElement += 2 * b_stride; + } + + if ((columnCounter & 1) != 0) + { + dReal p1, q1; + + /* load p and q values */ + p1 = ptrLElement[0]; + q1 = ptrBElement[0 * b_stride]; + + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + + /* advance pointers */ + // ptrLElement += 1; -- not needed any more + ptrBElement += 1 * b_stride; + } + + /* finish computing the X(i) block */ + dReal Y11 = ptrBElement[0 * b_stride] - (Z11 + Z12); + ptrBElement[0 * b_stride] = Y11; + } + } +} + + +template<unsigned int block_step> +/*static */ +sizeint ThreadedEquationSolverLDLT::estimateCooperativelySolvingL1StraightMemoryRequirement(unsigned rowCount, SolvingL1StraightMemoryEstimates &ref_solvingMemoryEstimates) +{ + unsigned blockCount = deriveSolvingL1StraightBlockCount(rowCount, block_step); + sizeint descriptorSizeRequired = dEFFICIENT_SIZE(sizeof(cellindexint) * blockCount); + sizeint contextSizeRequired = dEFFICIENT_SIZE(sizeof(SolveL1StraightCellContext) * (CCI__MAX + 1) * blockCount); + ref_solvingMemoryEstimates.assignData(descriptorSizeRequired, contextSizeRequired); + + sizeint totalSizeRequired = descriptorSizeRequired + contextSizeRequired; + return totalSizeRequired; +} + +template<unsigned int block_step> +/*static */ +void ThreadedEquationSolverLDLT::initializeCooperativelySolveL1StraightMemoryStructures(unsigned rowCount, + atomicord32 &out_blockCompletionProgress, cellindexint *blockProgressDescriptors, SolveL1StraightCellContext *dUNUSED(cellContexts)) +{ + unsigned blockCount = deriveSolvingL1StraightBlockCount(rowCount, block_step); + + out_blockCompletionProgress = 0; + memset(blockProgressDescriptors, 0, blockCount * sizeof(*blockProgressDescriptors)); +} + +template<unsigned int block_step, unsigned int b_stride> +void ThreadedEquationSolverLDLT::participateSolvingL1Straight(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip, + volatile atomicord32 &refBlockCompletionProgress/*=0*/, volatile cellindexint *blockProgressDescriptors/*=[blockCount]*/, + SolveL1StraightCellContext *cellContexts/*=[CCI__MAX x blockCount] + [blockCount]*/, unsigned ownThreadIndex) +{ + const unsigned lookaheadRange = 32; + const unsigned blockCount = deriveSolvingL1StraightBlockCount(rowCount, block_step), lastBlock = blockCount - 1; + /* compute rows at end that are not a multiple of block size */ + const unsigned loopX1RowCount = rowCount % block_step; + + BlockProcessingState blockProcessingState = BPS_NO_BLOCKS_PROCESSED; + + unsigned completedBlocks = refBlockCompletionProgress; + unsigned currentBlock = completedBlocks; + dIASSERT(completedBlocks <= blockCount); + + for (bool exitLoop = completedBlocks == blockCount; !exitLoop; exitLoop = false) + { + bool goForLockedBlockPrimaryCalculation = false, goForLockedBlockDuplicateCalculation = false; + bool goAssigningTheResult = false, stayWithinTheBlock = false; + + dReal Z[block_step]; + dReal Y[block_step]; + + dReal *ptrBElement; + + CellContextInstance previousContextInstance; + unsigned completedColumnBlock; + bool partialBlock; + + for (cellindexint testDescriptor = blockProgressDescriptors[currentBlock]; ; ) + { + if (testDescriptor == INVALID_CELLDESCRIPTOR) + { + // Invalid descriptor is the indication that the row has been fully calculated + // Test if this was the last row and break out if so. + if (currentBlock + 1 == blockCount) + { + exitLoop = true; + break; + } + + // Treat detected row advancement as a row processed + // blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; <-- performs better without it + break; + } + + CooperativeAtomics::AtomicReadReorderBarrier(); + // It is necessary to read up to date completedBblocks value after the descriptor retrieval + // as otherwise the logic below breaks + completedBlocks = refBlockCompletionProgress; + + if (!GET_CELLDESCRIPTOR_ISLOCKED(testDescriptor)) + { + completedColumnBlock = GET_CELLDESCRIPTOR_COLUMNINDEX(testDescriptor); + dIASSERT(completedColumnBlock < currentBlock || (completedColumnBlock == currentBlock && currentBlock == 0)); // Otherwise, why would the calculation have had stopped if the final column is reachable??? + dIASSERT(completedColumnBlock <= completedBlocks); // Since the descriptor is not locked + + if (completedColumnBlock == completedBlocks && currentBlock != completedBlocks) + { + dIASSERT(completedBlocks < currentBlock); + break; + } + + if (CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], testDescriptor, MARK_CELLDESCRIPTOR_LOCKED(testDescriptor))) + { + if (completedColumnBlock != 0) + { + CellContextInstance contextInstance = GET_CELLDESCRIPTOR_CONTEXTINSTANCE(testDescriptor); + previousContextInstance = contextInstance; + + const SolveL1StraightCellContext &sourceContext = buildBlockContextRef(cellContexts, currentBlock, contextInstance); + sourceContext.loadPrecalculatedZs(Z); + } + else + { + previousContextInstance = CCI__MIN; + SolveL1StraightCellContext::initializePrecalculatedZs(Z); + } + + goForLockedBlockPrimaryCalculation = true; + break; + } + + if (blockProcessingState != BPS_COMPETING_FOR_A_BLOCK) + { + break; + } + + testDescriptor = blockProgressDescriptors[currentBlock]; + } + else + { + if (blockProcessingState != BPS_COMPETING_FOR_A_BLOCK) + { + break; + } + + cellindexint verificativeDescriptor; + bool verificationFailure = false; + + completedColumnBlock = GET_CELLDESCRIPTOR_COLUMNINDEX(testDescriptor); + dIASSERT(completedColumnBlock != currentBlock || currentBlock == 0); // There is no reason for computations to stop at the very end other than being the initial value at the very first block + + if (completedColumnBlock != 0) + { + CellContextInstance contextInstance = GET_CELLDESCRIPTOR_CONTEXTINSTANCE(testDescriptor); + const SolveL1StraightCellContext &sourceContext = buildBlockContextRef(cellContexts, currentBlock, contextInstance); + sourceContext.loadPrecalculatedZs(Z); + } + else + { + SolveL1StraightCellContext::initializePrecalculatedZs(Z); + } + + if (completedColumnBlock != 0 && completedColumnBlock <= currentBlock) + { + // Make sure the descriptor is re-read after the precalculates + CooperativeAtomics::AtomicReadReorderBarrier(); + } + + if (completedColumnBlock <= currentBlock) + { + verificativeDescriptor = blockProgressDescriptors[currentBlock]; + verificationFailure = verificativeDescriptor != testDescriptor; + } + + if (!verificationFailure) + { + dIASSERT(completedColumnBlock <= currentBlock + 1); + + goForLockedBlockDuplicateCalculation = true; + break; + } + + testDescriptor = verificativeDescriptor; + } + } + + if (exitLoop) + { + break; + } + + if (goForLockedBlockPrimaryCalculation) + { + blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; + + // Declare and assign the variables at the top to not interfere with any branching -- the compiler is going to eliminate them anyway. + bool handleComputationTakenOver = false, rowEndReached = false; + + const dReal *ptrLElement; + unsigned finalColumnBlock; + + /* check if this is not the partial block of fewer rows */ + if (currentBlock != lastBlock || loopX1RowCount == 0) + { + partialBlock = false; + + if (currentBlock != 0) + { + ptrLElement = L + (sizeint)(1 + currentBlock * block_step) * rowSkip + completedColumnBlock * block_step; + ptrBElement = B + (sizeint)(completedColumnBlock * block_step) * b_stride; + + /* the inner loop that computes outer products and adds them to Z */ + finalColumnBlock = dMACRO_MIN(currentBlock, completedBlocks); + dIASSERT(completedColumnBlock != finalColumnBlock/* || currentBlock == 0*/); + + for (unsigned columnCounter = finalColumnBlock - completedColumnBlock; ; ) + { + dReal q1, p1, p2, p3, p4; + + /* load p and q values */ + q1 = ptrBElement[0 * b_stride]; + p1 = (ptrLElement - rowSkip)[0]; + p2 = ptrLElement[0]; + ptrLElement += rowSkip; + p3 = ptrLElement[0]; + p4 = ptrLElement[0 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[1 * b_stride]; + p3 = ptrLElement[1]; + p4 = ptrLElement[1 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[1]; + p2 = ptrLElement[1]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[2 * b_stride]; + p1 = (ptrLElement - rowSkip)[2]; + p2 = ptrLElement[2]; + ptrLElement += rowSkip; + p3 = ptrLElement[2]; + p4 = ptrLElement[2 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[3 * b_stride]; + p3 = ptrLElement[3]; + p4 = ptrLElement[3 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[3]; + p2 = ptrLElement[3]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + dSASSERT(block_step == 4); + + if (columnCounter > 3) + { + columnCounter -= 3; + + ptrLElement += 3 * block_step; + ptrBElement += 3 * block_step * b_stride; + + /* load p and q values */ + q1 = ptrBElement[-8 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-8]; + p2 = ptrLElement[-8]; + ptrLElement += rowSkip; + p3 = ptrLElement[-8]; + p4 = ptrLElement[-8 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-7 * (int)b_stride]; + p3 = ptrLElement[-7]; + p4 = ptrLElement[-7 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-7]; + p2 = ptrLElement[-7]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-6 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-6]; + p2 = ptrLElement[-6]; + ptrLElement += rowSkip; + p3 = ptrLElement[-6]; + p4 = ptrLElement[-6 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-5 * (int)b_stride]; + p3 = ptrLElement[-5]; + p4 = ptrLElement[-5 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-5]; + p2 = ptrLElement[-5]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-4 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-4]; + p2 = ptrLElement[-4]; + ptrLElement += rowSkip; + p3 = ptrLElement[-4]; + p4 = ptrLElement[-4 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-3 * (int)b_stride]; + p3 = ptrLElement[-3]; + p4 = ptrLElement[-3 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-3]; + p2 = ptrLElement[-3]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-2 * (int)b_stride]; + p1 = (ptrLElement - rowSkip)[-2]; + p2 = ptrLElement[-2]; + ptrLElement += rowSkip; + p3 = ptrLElement[-2]; + p4 = ptrLElement[-2 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p3 = ptrLElement[-1]; + p4 = ptrLElement[-1 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[-1]; + p2 = ptrLElement[-1]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + dSASSERT(block_step == 4); + } + else + { + ptrLElement += block_step; + ptrBElement += block_step * b_stride; + + if (--columnCounter == 0) + { + if (finalColumnBlock == currentBlock) + { + rowEndReached = true; + break; + } + + // Take a look if any more columns have been completed... + completedBlocks = refBlockCompletionProgress; + dIASSERT(completedBlocks >= finalColumnBlock); + + if (completedBlocks == finalColumnBlock) + { + break; + } + + // ...continue if so. + unsigned columnCompletedSoFar = finalColumnBlock; + finalColumnBlock = dMACRO_MIN(currentBlock, completedBlocks); + columnCounter = finalColumnBlock - columnCompletedSoFar; + } + } + /* end of inner loop */ + } + } + else + { + ptrLElement = L + (sizeint)(1/* + currentBlock * block_step*/) * rowSkip/* + completedColumnBlock * block_step*/; + ptrBElement = B/* + (sizeint)(completedColumnBlock * block_step) * b_stride*/; + dIASSERT(completedColumnBlock == 0); + + rowEndReached = true; + } + } + else + { + partialBlock = true; + + if (currentBlock != 0) + { + dReal tempZ[dMACRO_MAX(block_step - 1U, 1U)] = { REAL(0.0), }; + + ptrLElement = L + (sizeint)(/*1 + */currentBlock * block_step) * rowSkip + completedColumnBlock * block_step; + ptrBElement = B + (sizeint)(completedColumnBlock * block_step) * b_stride; + + /* the inner loop that computes outer products and adds them to Z */ + finalColumnBlock = dMACRO_MIN(currentBlock, completedBlocks); + dIASSERT(completedColumnBlock != finalColumnBlock/* || currentBlock == 0*/); + + for (unsigned partialRow = 0, columnCompletedSoFar = completedColumnBlock; ; ) + { + dReal Z1 = 0, Z2 = 0, Z3 = 0, Z4 = 0; + + for (unsigned columnCounter = finalColumnBlock - columnCompletedSoFar; ; ) + { + dReal q1, q2, q3, q4, p1, p2, p3, p4; + + /* load p and q values */ + q1 = ptrBElement[0 * b_stride]; + q2 = ptrBElement[1 * b_stride]; + q3 = ptrBElement[2 * b_stride]; + q4 = ptrBElement[3 * b_stride]; + p1 = ptrLElement[0]; + p2 = ptrLElement[1]; + p3 = ptrLElement[2]; + p4 = ptrLElement[3]; + + /* compute outer product and add it to the Z matrix */ + Z1 += p1 * q1; + Z2 += p2 * q2; + Z3 += p3 * q3; + Z4 += p4 * q4; + dSASSERT(block_step == 4); + + if (columnCounter > 3) + { + columnCounter -= 3; + + ptrLElement += 3 * block_step; + ptrBElement += 3 * block_step * b_stride; + + /* load p and q values */ + q1 = ptrBElement[-8 * (int)b_stride]; + q2 = ptrBElement[-7 * (int)b_stride]; + q3 = ptrBElement[-6 * (int)b_stride]; + q4 = ptrBElement[-5 * (int)b_stride]; + p1 = ptrLElement[-8]; + p2 = ptrLElement[-7]; + p3 = ptrLElement[-6]; + p4 = ptrLElement[-5]; + + /* compute outer product and add it to the Z matrix */ + Z1 += p1 * q1; + Z2 += p2 * q2; + Z3 += p3 * q3; + Z4 += p4 * q4; + + /* load p and q values */ + q1 = ptrBElement[-4 * (int)b_stride]; + q2 = ptrBElement[-3 * (int)b_stride]; + q3 = ptrBElement[-2 * (int)b_stride]; + q4 = ptrBElement[-1 * (int)b_stride]; + p1 = ptrLElement[-4]; + p2 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p4 = ptrLElement[-1]; + + /* compute outer product and add it to the Z matrix */ + Z1 += p1 * q1; + Z2 += p2 * q2; + Z3 += p3 * q3; + Z4 += p4 * q4; + dSASSERT(block_step == 4); + } + else + { + ptrLElement += block_step; + ptrBElement += block_step * b_stride; + + if (--columnCounter == 0) + { + break; + } + } + /* end of inner loop */ + } + + tempZ[partialRow] += Z1 + Z2 + Z3 + Z4; + + if (++partialRow == loopX1RowCount) + { + // Here switch is used to avoid accessing Z by parametrized index. + // So far all the accesses were performed by explicit constants + // what lets the compiler treat Z elements as individual variables + // rather than array elements. + Z[0] += tempZ[0]; + + if (loopX1RowCount >= 2) + { + Z[1] += tempZ[1]; + + if (loopX1RowCount > 2) + { + Z[2] += tempZ[2]; + } + } + dSASSERT(block_step == 4); + + if (finalColumnBlock == currentBlock) + { + if (loopX1RowCount > 2) + { + // Correct the LElement so that it points to the second row + // + // Note, that ff there is just one partial row, it does not matter that + // the LElement will remain pointing at the first row, + // since the former is not going to be used in that case. + ptrLElement -= /*(sizeint)*/rowSkip/* * (loopX1RowCount - 2)*/; dIASSERT(loopX1RowCount == 3); + } + dSASSERT(block_step == 4); + + rowEndReached = true; + break; + } + + // Take a look if any more columns have been completed... + completedBlocks = refBlockCompletionProgress; + dIASSERT(completedBlocks >= finalColumnBlock); + + if (completedBlocks == finalColumnBlock) + { + break; + } + + std::fill(tempZ, tempZ + loopX1RowCount, REAL(0.0)); + partialRow = 0; + + // Correct the LElement pointer to continue at the first partial row + ptrLElement -= (sizeint)rowSkip * (loopX1RowCount - 1); + + // ...continue if so. + columnCompletedSoFar = finalColumnBlock; + finalColumnBlock = dMACRO_MIN(currentBlock, completedBlocks); + } + else + { + ptrLElement += rowSkip - (finalColumnBlock - columnCompletedSoFar) * block_step; + ptrBElement -= (sizeint)((finalColumnBlock - columnCompletedSoFar) * block_step) * b_stride; + } + /* end of loop by individual rows */ + } + } + else + { + ptrLElement = L + (sizeint)(1/* + currentBlock * block_step*/) * rowSkip/* + completedColumnBlock * block_step*/; + ptrBElement = B/* + (sizeint)(completedColumnBlock * block_step) * b_stride*/; + dIASSERT(completedColumnBlock == 0); + + rowEndReached = true; + } + } + + if (rowEndReached) + { + // Check whether there is still a need to proceed or if the computation has been taken over by another thread + cellindexint oldDescriptor = MAKE_CELLDESCRIPTOR(completedColumnBlock, previousContextInstance, true); + + if (blockProgressDescriptors[currentBlock] == oldDescriptor) + { + /* finish computing the X(i) block */ + if (!partialBlock) + { + Y[0] = ptrBElement[0 * b_stride] - Z[0]; + + dReal p2 = ptrLElement[0]; + Y[1] = ptrBElement[1 * b_stride] - Z[1] - p2 * Y[0]; + + ptrLElement += rowSkip; + + dReal p3 = ptrLElement[0]; + dReal p3_1 = ptrLElement[1]; + Y[2] = ptrBElement[2 * b_stride] - Z[2] - p3 * Y[0] - p3_1 * Y[1]; + + dReal p4 = ptrLElement[rowSkip]; + dReal p4_1 = ptrLElement[1 + rowSkip]; + dReal p4_2 = ptrLElement[2 + rowSkip]; + Y[3] = ptrBElement[3 * b_stride] - Z[3] - p4 * Y[0] - p4_1 * Y[1] - p4_2 * Y[2]; + dSASSERT(block_step == 4); + } + else + { + Y[0] = ptrBElement[0 * b_stride] - Z[0]; + + if (loopX1RowCount >= 2) + { + dReal p2 = ptrLElement[0]; + Y[1] = ptrBElement[1 * b_stride] - Z[1] - p2 * Y[0]; + + if (loopX1RowCount > 2) + { + dReal p3 = ptrLElement[0 + rowSkip]; + dReal p3_1 = ptrLElement[1 + rowSkip]; + Y[2] = ptrBElement[2 * b_stride] - Z[2] - p3 * Y[0] - p3_1 * Y[1]; + } + } + dSASSERT(block_step == 4); + } + + // Use atomic memory barrier to make sure memory reads of ptrBElement[] and blockProgressDescriptors[] are not swapped + CooperativeAtomics::AtomicReadReorderBarrier(); + + // The descriptor has not been altered yet - this means the ptrBElement[] values used above were not modified yet + // and the computation result is valid. + if (blockProgressDescriptors[currentBlock] == oldDescriptor) + { + // Assign the results to the result context (possibly in parallel with other threads + // that could and ought to be assigning exactly the same values) + SolveL1StraightCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.storePrecalculatedZs(Y); + + // Assign the result assignment progress descriptor + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true); + CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], oldDescriptor, newDescriptor); // the result is to be ignored + + // Whether succeeded or not, the result is valid, so go on trying to assign it to the matrix + goAssigningTheResult = true; + } + else + { + // Otherwise, go on competing for copying the results + handleComputationTakenOver = true; + } + } + else + { + handleComputationTakenOver = true; + } + } + else + { + // If the final column has not been reached yet, store current values to the context. + // Select the other context instance as the previous one might be read by other threads. + CellContextInstance nextContextInstance = buildNextContextInstance(previousContextInstance); + SolveL1StraightCellContext &destinationContext = buildBlockContextRef(cellContexts, currentBlock, nextContextInstance); + destinationContext.storePrecalculatedZs(Z); + + // Unlock the row until more columns can be used + cellindexint oldDescriptor = MAKE_CELLDESCRIPTOR(completedColumnBlock, previousContextInstance, true); + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(finalColumnBlock, nextContextInstance, false); + // The descriptor might have been updated by a competing thread + if (!CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], oldDescriptor, newDescriptor)) + { + // Adjust the ptrBElement to point to the result area... + ptrBElement = B + (sizeint)(currentBlock * block_step) * b_stride; + // ...and go on handling the case + handleComputationTakenOver = true; + } + } + + if (handleComputationTakenOver) + { + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + // This can only happen if the row was (has become) the uppermost not fully completed one + // and the competing thread is at final stage of calculation (i.e., it has reached the currentBlock column). + if (existingDescriptor != INVALID_CELLDESCRIPTOR) + { + // If not fully completed this must be the final stage of the result assignment into the matrix + dIASSERT(existingDescriptor == MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true)); + + // Go on competing copying the result as anyway the block is the topmost not completed one + // and since there was competition for it, there is no other work that can be done right now. + const SolveL1StraightCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.loadPrecalculatedZs(Y); + + goAssigningTheResult = true; + } + else + { + // everything is over -- just go handling next blocks + } + } + } + else if (goForLockedBlockDuplicateCalculation) + { + blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; + + bool skipToHandlingSubsequentRows = false, skiptoCopyingResult = false; + + /* declare variables */ + const dReal *ptrLElement; + + if (completedColumnBlock < currentBlock) + { + /* check if this is not the partial block of fewer rows */ + if (currentBlock != lastBlock || loopX1RowCount == 0) + { + partialBlock = false; + + ptrLElement = L + (sizeint)(1 + currentBlock * block_step) * rowSkip + completedColumnBlock * block_step; + ptrBElement = B + (sizeint)(completedColumnBlock * block_step) * b_stride; + + /* the inner loop that computes outer products and adds them to Z */ + unsigned finalColumnBlock = currentBlock; + dIASSERT(currentBlock == completedBlocks); // Why would we be competing for a row otherwise? + + unsigned lastCompletedColumn = completedColumnBlock; + unsigned columnCounter = finalColumnBlock - completedColumnBlock; + for (bool exitInnerLoop = false; !exitInnerLoop; exitInnerLoop = --columnCounter == 0) + { + dReal q1, p1, p2, p3, p4; + + /* load p and q values */ + q1 = ptrBElement[0 * b_stride]; + p1 = (ptrLElement - rowSkip)[0]; + p2 = ptrLElement[0]; + ptrLElement += rowSkip; + p3 = ptrLElement[0]; + p4 = ptrLElement[0 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[1 * b_stride]; + p3 = ptrLElement[1]; + p4 = ptrLElement[1 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[1]; + p2 = ptrLElement[1]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[2 * b_stride]; + p1 = (ptrLElement - rowSkip)[2]; + p2 = ptrLElement[2]; + ptrLElement += rowSkip; + p3 = ptrLElement[2]; + p4 = ptrLElement[2 + rowSkip]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + + /* load p and q values */ + q1 = ptrBElement[3 * b_stride]; + p3 = ptrLElement[3]; + p4 = ptrLElement[3 + rowSkip]; + ptrLElement -= rowSkip; + p1 = (ptrLElement - rowSkip)[3]; + p2 = ptrLElement[3]; + + /* compute outer product and add it to the Z matrix */ + Z[0] += p1 * q1; + Z[1] += p2 * q1; + Z[2] += p3 * q1; + Z[3] += p4 * q1; + dSASSERT(block_step == 4); + + // Check if the primary solver thread has not made any progress + cellindexint descriptorVerification = blockProgressDescriptors[currentBlock]; + unsigned newCompletedColumn = GET_CELLDESCRIPTOR_COLUMNINDEX(descriptorVerification); + + if (newCompletedColumn != lastCompletedColumn) + { + // Check, this is the first change the current thread detects. + // There is absolutely no reason in code for the computation to stop/resume twice + // while the current thread is competing. + dIASSERT(lastCompletedColumn == completedColumnBlock); + + if (descriptorVerification == INVALID_CELLDESCRIPTOR) + { + skipToHandlingSubsequentRows = true; + break; + } + + if (newCompletedColumn == currentBlock + 1) + { + skiptoCopyingResult = true; + break; + } + + // Check if the current thread is behind + if (newCompletedColumn > finalColumnBlock - columnCounter) + { + // If so, go starting over one more time + blockProcessingState = BPS_COMPETING_FOR_A_BLOCK; + stayWithinTheBlock = true; + skipToHandlingSubsequentRows = true; + break; + } + + // If current thread is ahead, just save new completed column for further comparisons and go on calculating + lastCompletedColumn = newCompletedColumn; + } + + /* advance pointers */ + ptrLElement += block_step; + ptrBElement += block_step * b_stride; + /* end of inner loop */ + } + } + else + { + partialBlock = true; + + dReal tempZ[dMACRO_MAX(block_step - 1U, 1U)] = { REAL(0.0), }; + + ptrLElement = L + (sizeint)(/*1 + */currentBlock * block_step) * rowSkip + completedColumnBlock * block_step; + ptrBElement = B + (sizeint)(completedColumnBlock * block_step) * b_stride; + + /* the inner loop that computes outer products and adds them to Z */ + unsigned finalColumnBlock = currentBlock; + dIASSERT(currentBlock == completedBlocks); // Why would we be competing for a row otherwise? + + unsigned lastCompletedColumn = completedColumnBlock; + for (unsigned columnCounter = finalColumnBlock - completedColumnBlock; ; ) + { + dReal q1, q2, q3, q4; + + /* load q values */ + q1 = ptrBElement[0 * b_stride]; + q2 = ptrBElement[1 * b_stride]; + q3 = ptrBElement[2 * b_stride]; + q4 = ptrBElement[3 * b_stride]; + + for (unsigned partialRow = 0; ; ) + { + dReal p1, p2, p3, p4; + + /* load p values */ + p1 = ptrLElement[0]; + p2 = ptrLElement[1]; + p3 = ptrLElement[2]; + p4 = ptrLElement[3]; + + /* compute outer product and add it to the Z matrix */ + tempZ[partialRow] += p1 * q1 + p2 * q2 + p3 * q3 + p4 * q4; + dSASSERT(block_step == 4); + + if (++partialRow == loopX1RowCount) + { + break; + } + + ptrLElement += rowSkip; + } + + // Check if the primary solver thread has not made any progress + cellindexint descriptorVerification = blockProgressDescriptors[currentBlock]; + unsigned newCompletedColumn = GET_CELLDESCRIPTOR_COLUMNINDEX(descriptorVerification); + + if (newCompletedColumn != lastCompletedColumn) + { + // Check, this is the first change the current thread detects. + // There is absolutely no reason in code for the computation to stop/resume twice + // while the current thread is competing. + dIASSERT(lastCompletedColumn == completedColumnBlock); + + if (descriptorVerification == INVALID_CELLDESCRIPTOR) + { + skipToHandlingSubsequentRows = true; + break; + } + + if (newCompletedColumn == currentBlock + 1) + { + skiptoCopyingResult = true; + break; + } + + // Check if the current thread is behind + if (newCompletedColumn > finalColumnBlock - columnCounter) + { + // If so, go starting over one more time + blockProcessingState = BPS_COMPETING_FOR_A_BLOCK; + stayWithinTheBlock = true; + skipToHandlingSubsequentRows = true; + break; + } + + // If current thread is ahead, just save new completed column for further comparisons and go on calculating + lastCompletedColumn = newCompletedColumn; + } + + ptrLElement += block_step; + ptrBElement += block_step * b_stride; + + if (--columnCounter == 0) + { + // Here switch is used to avoid accessing Z by parametrized index. + // So far all the accesses were performed by explicit constants + // what lets the compiler treat Z elements as individual variables + // rather than array elements. + Z[0] += tempZ[0]; + + if (loopX1RowCount >= 2) + { + Z[1] += tempZ[1]; + + if (loopX1RowCount > 2) + { + Z[2] += tempZ[2]; + + // Correct the LElement so that it points to the second row + // + // Note, that if there is just one partial row, it does not matter that + // the LElement will remain pointing at the first row, + // since the former is not going to be used in that case. + ptrLElement -= /*(sizeint)*/rowSkip/* * (loopX1RowCount - 2)*/; dIASSERT(loopX1RowCount == 3); + } + } + dSASSERT(block_step == 4); + + break; + } + + /* advance pointers */ + ptrLElement -= (sizeint)rowSkip * (loopX1RowCount - 1); + /* end of inner loop */ + } + } + } + else if (completedColumnBlock > currentBlock) + { + dIASSERT(completedColumnBlock == currentBlock + 1); + + partialBlock = currentBlock == lastBlock && loopX1RowCount != 0; + + skiptoCopyingResult = true; + } + else + { + dIASSERT(currentBlock == 0); // Execution can get here within the very first block only + + partialBlock = rowCount < block_step; + + /* assign the pointers appropriately and go on computing the results */ + ptrLElement = L + (sizeint)(1/* + currentBlock * block_step*/) * rowSkip/* + completedColumnBlock * block_step*/; + ptrBElement = B/* + (sizeint)(completedColumnBlock * block_step) * b_stride*/; + } + + if (!skipToHandlingSubsequentRows) + { + if (!skiptoCopyingResult) + { + if (!partialBlock) + { + Y[0] = ptrBElement[0 * b_stride] - Z[0]; + + dReal p2 = ptrLElement[0]; + Y[1] = ptrBElement[1 * b_stride] - Z[1] - p2 * Y[0]; + + ptrLElement += rowSkip; + + dReal p3 = ptrLElement[0]; + dReal p3_1 = ptrLElement[1]; + Y[2] = ptrBElement[2 * b_stride] - Z[2] - p3 * Y[0] - p3_1 * Y[1]; + + dReal p4 = ptrLElement[rowSkip]; + dReal p4_1 = ptrLElement[1 + rowSkip]; + dReal p4_2 = ptrLElement[2 + rowSkip]; + Y[3] = ptrBElement[3 * b_stride] - Z[3] - p4 * Y[0] - p4_1 * Y[1] - p4_2 * Y[2]; + dSASSERT(block_step == 4); + } + else + { + Y[0] = ptrBElement[0 * b_stride] - Z[0]; + + if (loopX1RowCount >= 2) + { + dReal p2 = ptrLElement[0]; + Y[1] = ptrBElement[1 * b_stride] - Z[1] - p2 * Y[0]; + + if (loopX1RowCount > 2) + { + dReal p3 = ptrLElement[0 + rowSkip]; + dReal p3_1 = ptrLElement[1 + rowSkip]; + Y[2] = ptrBElement[2 * b_stride] - Z[2] - p3 * Y[0] - p3_1 * Y[1]; + } + } + dSASSERT(block_step == 4); + } + + CooperativeAtomics::AtomicReadReorderBarrier(); + + // Use atomic load to make sure memory reads of ptrBElement[] and blockProgressDescriptors[] are not swapped + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + + if (existingDescriptor == INVALID_CELLDESCRIPTOR) + { + // Everything is over -- proceed to subsequent rows + skipToHandlingSubsequentRows = true; + } + else if (existingDescriptor == MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true)) + { + // The values computed above may not be valid. Copy the values already in the result context. + skiptoCopyingResult = true; + } + else + { + // The descriptor has not been altered yet - this means the ptrBElement[] values used above were not modified yet + // and the computation result is valid. + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true); // put the computation at the top so that the evaluation result from the expression above is reused + + // Assign the results to the result context (possibly in parallel with other threads + // that could and ought to be assigning exactly the same values) + SolveL1StraightCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.storePrecalculatedZs(Y); + + // Assign the result assignment progress descriptor + CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], existingDescriptor, newDescriptor); // the result is to be ignored + + // Whether succeeded or not, the result is valid, so go on trying to assign it to the matrix + } + } + + if (!skipToHandlingSubsequentRows) + { + if (skiptoCopyingResult) + { + // Extract the result values stored in the result context + const SolveL1StraightCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.loadPrecalculatedZs(Y); + + ptrBElement = B + (sizeint)(currentBlock * block_step) * b_stride; + } + + goAssigningTheResult = true; + } + } + } + + if (goAssigningTheResult) + { + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + // Check if the assignment has not been completed yet + if (existingDescriptor != INVALID_CELLDESCRIPTOR) + { + // Assign the computation results to the B vector + if (!partialBlock) + { + ptrBElement[0 * b_stride] = Y[0]; + ptrBElement[1 * b_stride] = Y[1]; + ptrBElement[2 * b_stride] = Y[2]; + ptrBElement[3 * b_stride] = Y[3]; + dSASSERT(block_step == 4); + } + else + { + ptrBElement[0 * b_stride] = Y[0]; + + if (loopX1RowCount >= 2) + { + ptrBElement[1 * b_stride] = Y[1]; + + if (loopX1RowCount > 2) + { + ptrBElement[2 * b_stride] = Y[2]; + } + } + dSASSERT(block_step == 4); + } + + ThrsafeIncrementIntUpToLimit(&refBlockCompletionProgress, currentBlock + 1); + dIASSERT(refBlockCompletionProgress >= currentBlock + 1); + + // And assign the completed status no matter what + CooperativeAtomics::AtomicStoreCellindexint(&blockProgressDescriptors[currentBlock], INVALID_CELLDESCRIPTOR); + } + else + { + // everything is over -- just go handling next blocks + } + } + + if (!stayWithinTheBlock) + { + completedBlocks = refBlockCompletionProgress; + + if (completedBlocks == blockCount) + { + break; + } + + currentBlock += 1; + + bool lookaheadBoundaryReached = false; + + if (currentBlock == blockCount || completedBlocks == 0) + { + lookaheadBoundaryReached = true; + } + else if (currentBlock >= completedBlocks + lookaheadRange) + { + lookaheadBoundaryReached = blockProcessingState > BPS_NO_BLOCKS_PROCESSED; + } + else if (currentBlock < completedBlocks) + { + // Treat detected row advancement as a row processed + // blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; <-- performs better without it + + currentBlock = completedBlocks; + } + + if (lookaheadBoundaryReached) + { + dIASSERT(blockProcessingState != BPS_COMPETING_FOR_A_BLOCK); // Why did not we compete??? + + // If no row has been processed in the previous pass, compete for the next row to avoid cycling uselessly + if (blockProcessingState <= BPS_NO_BLOCKS_PROCESSED) + { + // Abandon job if too few blocks remain + if (blockCount - completedBlocks <= ownThreadIndex) + { + break; + } + + blockProcessingState = BPS_COMPETING_FOR_A_BLOCK; + } + else + { + // If there was some progress, just continue to the next pass + blockProcessingState = BPS_NO_BLOCKS_PROCESSED; + } + + currentBlock = completedBlocks; + } + } + } +} + + +#endif diff --git a/libs/ode-0.16.1/ode/src/fastltsolve.cpp b/libs/ode-0.16.1/ode/src/fastltsolve.cpp new file mode 100644 index 0000000..e9c7ec5 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastltsolve.cpp @@ -0,0 +1,229 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * L1Transposed Equation Solving Routines + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + +#include <ode/common.h> +#include <ode/matrix.h> +#include <ode/matrix_coop.h> +#include "config.h" +#include "threaded_solver_ldlt.h" +#include "threading_base.h" +#include "resource_control.h" +#include "error.h" + +#include "fastltsolve_impl.h" + + +/*static */ +void ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1TransposedResourceRequirements( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount) +{ + dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading(); + unsigned limitedThreadCount = restrictSolvingL1TransposedAllowedThreadCount(threading, allowedThreadCount, rowCount); + + if (limitedThreadCount > 1) + { + doEstimateCooperativeSolvingL1TransposedResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount); + } +} + +/*static */ +void ThreadedEquationSolverLDLT::cooperativelySolveL1Transposed( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(rowCount != 0); + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + unsigned limitedThreadCount = restrictSolvingL1TransposedAllowedThreadCount(threading, allowedThreadCount, rowCount); + + if (limitedThreadCount <= 1) + { + solveL1Transposed<SL1T_B_STRIDE>(L, b, rowCount, rowSkip); + } + else + { + doCooperativelySolveL1TransposedValidated(resourceContainer, limitedThreadCount, L, b, rowCount, rowSkip); + } +} + + +/*static */ +unsigned ThreadedEquationSolverLDLT::restrictSolvingL1TransposedAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount) +{ + unsigned limitedThreadCount = 1; + +#if dCOOPERATIVE_ENABLED + const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation + unsigned solvingBlockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep); + dIASSERT(deriveSolvingL1TransposedThreadCount(SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1); + + if (solvingBlockCount >= SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM) + { + limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true); + } +#endif // #if dCOOPERATIVE_ENABLED + + return limitedThreadCount; +} + +/*static */ +void ThreadedEquationSolverLDLT::doEstimateCooperativeSolvingL1TransposedResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount) +{ + const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation + unsigned blockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep); + dIASSERT(blockCount >= 1); + + unsigned threadCountToUse = deriveSolvingL1TransposedThreadCount(blockCount, allowedThreadCount); + dIASSERT(threadCountToUse > 1); + + unsigned simultaneousCallCount = 1 + (threadCountToUse - 1); + + SolvingL1TransposedMemoryEstimates solvingMemoryEstimates; + sizeint solvingMemoryRequired = estimateCooperativelySolvingL1TransposedMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates); + const unsigned solvingAlignmentRequired = ALLOCATION_DEFAULT_ALIGNMENT; + + unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED; + summaryRequirementsDescriptor->mergeAnotherDescriptorIn(solvingMemoryRequired, solvingAlignmentRequired, simultaneousCallCount, featureRequirement); +} + +/*static */ +void ThreadedEquationSolverLDLT::doCooperativelySolveL1TransposedValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(allowedThreadCount > 1); + + const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation + unsigned blockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep); + dIASSERT(blockCount >= 1); + + unsigned threadCountToUse = deriveSolvingL1TransposedThreadCount(blockCount, allowedThreadCount); + dIASSERT(threadCountToUse > 1); + + dCallWaitID completionWait = resourceContainer->getStockCallWait(); + dAASSERT(completionWait != NULL); + + atomicord32 blockCompletionProgress; + cellindexint *blockProgressDescriptors; + SolveL1TransposedCellContext *cellContexts; + + SolvingL1TransposedMemoryEstimates solvingMemoryEstimates; + sizeint solvingMemoryRequired = estimateCooperativelySolvingL1TransposedMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates); + dIASSERT(solvingMemoryRequired <= resourceContainer->getMemoryBufferSize()); + + void *bufferAllocated = resourceContainer->getMemoryBufferPointer(); + dIASSERT(bufferAllocated != NULL); + dIASSERT(dALIGN_PTR(bufferAllocated, ALLOCATION_DEFAULT_ALIGNMENT) == bufferAllocated); + + void *bufferCurrentLocation = bufferAllocated; + bufferCurrentLocation = markCooperativelySolvingL1TransposedMemoryStructuresOut(bufferCurrentLocation, solvingMemoryEstimates, blockProgressDescriptors, cellContexts); + dIVERIFY(bufferCurrentLocation <= (uint8 *)bufferAllocated + solvingMemoryRequired); + + initializeCooperativelySolveL1TransposedMemoryStructures<blockStep>(rowCount, blockCompletionProgress, blockProgressDescriptors, cellContexts); + + dCallReleaseeID calculationFinishReleasee; + SolveL1TransposedWorkerContext workerContext; // The variable must exist in the outer scope + + workerContext.init(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts); + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + threading->PostThreadedCall(NULL, &calculationFinishReleasee, threadCountToUse - 1, NULL, completionWait, &solveL1Transposed_completion_callback, NULL, 0, "SolveL1Transposed Completion"); + threading->PostThreadedCallsGroup(NULL, threadCountToUse - 1, calculationFinishReleasee, &solveL1Transposed_worker_callback, &workerContext, "SolveL1Transposed Work"); + + participateSolvingL1Transposed<blockStep, SL1T_B_STRIDE>(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts, threadCountToUse - 1); + + threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "SolveL1Transposed End Wait"); +} + +/*static */ +int ThreadedEquationSolverLDLT::solveL1Transposed_worker_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee)) +{ + SolveL1TransposedWorkerContext *ptrContext = (SolveL1TransposedWorkerContext *)callContext; + + solveL1Transposed_worker(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex)); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::solveL1Transposed_worker(SolveL1TransposedWorkerContext &ref_context, unsigned ownThreadIndex) +{ + const unsigned blockStep = SL1T_BLOCK_SIZE; + participateSolvingL1Transposed<blockStep, SL1T_B_STRIDE>(ref_context.m_L, ref_context.m_b, ref_context.m_rowCount, ref_context.m_rowSkip, + *ref_context.m_ptrBlockCompletionProgress, ref_context.m_blockProgressDescriptors, ref_context.m_cellContexts, ownThreadIndex); +} + +/*static */ +int ThreadedEquationSolverLDLT::solveL1Transposed_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + return 1; +} + + + +////////////////////////////////////////////////////////////////////////// +// Public interface functions + +/*extern ODE_API */ +void dSolveL1T(const dReal *L, dReal *B, int rowCount, int rowSkip) +{ + dAASSERT(rowCount != 0); + + if (rowCount != 0) + { + dAASSERT(L != NULL); + dAASSERT(B != NULL); + + solveL1Transposed<1>(L, B, rowCount, rowSkip); + } +} + + +/*extern ODE_API */ +void dEstimateCooperativelySolveL1TransposedResourceRequirements(dResourceRequirementsID requirements, + unsigned maximalAllowedThreadCount, unsigned maximalRowCount) +{ + dAASSERT(requirements != NULL); + + dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements; + ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1TransposedResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount); +} + +/*extern ODE_API */ +void dCooperativelySolveL1Transposed(dResourceContainerID resources, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip) +{ + dAASSERT(resources != NULL); + + dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources; + ThreadedEquationSolverLDLT::cooperativelySolveL1Transposed(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip); +} + diff --git a/libs/ode-0.16.1/ode/src/fastltsolve_impl.h b/libs/ode-0.16.1/ode/src/fastltsolve_impl.h new file mode 100644 index 0000000..ca30d9c --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastltsolve_impl.h @@ -0,0 +1,1440 @@ + + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Code style improvements and optimizations by Oleh Derevenko ????-2019 + * L1Transposed cooperative solving code of ThreadedEquationSolverLDLT copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#ifndef _ODE_FASTLTSOLVE_IMPL_H_ +#define _ODE_FASTLTSOLVE_IMPL_H_ + + +/* solve L^T * x=b, with b containing 1 right hand side. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is rowSkip. + * b is an n*1 matrix that contains the right hand side. + * b is overwritten with x. + * this processes blocks of 4. + */ + +template<unsigned int b_stride> +void solveL1Transposed(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip) +{ + dIASSERT(rowCount != 0); + + /* special handling for L and B because we're solving L1 *transpose* */ + const dReal *lastLElement = L + (sizeint)(rowCount - 1) * (rowSkip + 1); + dReal *lastBElement = B + (sizeint)(rowCount - 1) * b_stride; + + /* compute rows at end that are not a multiple of block size */ + const unsigned loopX1RowCount = rowCount % 4; + + unsigned blockStartRow = loopX1RowCount; + bool subsequentPass = false; + + /* compute rightmost bottom X(i) block */ + if (loopX1RowCount != 0) + { + subsequentPass = true; + + const dReal *ptrLElement = lastLElement; + dReal *ptrBElement = lastBElement; + + dReal Y11 = ptrBElement[0 * b_stride]/* - Z11*/; + // ptrBElement[0 * b_stride] = Y11; -- unchanged + + if (loopX1RowCount >= 2) + { + dReal p2 = ptrLElement[-1]; + dReal Y21 = ptrBElement[-1 * (int)b_stride]/* - Z21 */- p2 * Y11; + ptrBElement[-1 * (int)b_stride] = Y21; + + if (loopX1RowCount > 2) + { + dReal p3 = ptrLElement[-2]; + dReal p3_1 = (ptrLElement - rowSkip)[-2]; + dReal Y31 = ptrBElement[-2 * (int)b_stride]/* - Z31 */- p3 * Y11 - p3_1 * Y21; + ptrBElement[-2 * (int)b_stride] = Y31; + } + } + } + + /* compute all 4 x 1 blocks of X */ + for (; !subsequentPass || blockStartRow < rowCount; subsequentPass = true, blockStartRow += 4) + { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + + /* declare variables - Z matrix, p and q vectors, etc */ + const dReal *ptrLElement; + dReal *ptrBElement; + + dReal Z41, Z31, Z21, Z11; + + if (subsequentPass) + { + ptrLElement = lastLElement - blockStartRow; + ptrBElement = lastBElement; + + /* set the Z matrix to 0 */ + Z41 = 0; Z31 = 0; Z21 = 0; Z11 = 0; + + unsigned rowCounter = blockStartRow; + + if (rowCounter % 2 != 0) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + ptrBElement -= 1 * b_stride; + rowCounter -= 1; + } + + if (rowCounter % 4 != 0) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + ptrBElement -= 2 * b_stride; + rowCounter -= 2; + } + + /* the inner loop that computes outer products and adds them to Z */ + for (bool exitLoop = rowCounter == 0; !exitLoop; exitLoop = false) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-2 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-3 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + if (rowCounter > 12) + { + rowCounter -= 12; + + ptrBElement -= 12 * b_stride; + + /* load p and q values */ + q1 = ptrBElement[8 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[7 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[6 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[5 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[4 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[3 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[2 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[1 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z41 += p4 * q1; + Z31 += p3 * q1; + Z21 += p2 * q1; + Z11 += p1 * q1; + } + else + { + ptrBElement -= 4 * b_stride; + + if ((rowCounter -= 4) == 0) + { + break; + } + } + /* end of inner loop */ + } + } + else + { + ptrLElement = lastLElement/* - blockStartRow*/; dIASSERT(blockStartRow == 0); + ptrBElement = lastBElement; + + /* set the Z matrix to 0 */ + Z41 = 0; Z31 = 0; Z21 = 0; Z11 = 0; + } + + /* finish computing the X(i) block */ + dReal Y11, Y21, Y31, Y41; + { + Y11 = ptrBElement[0 * b_stride] - Z11; + ptrBElement[0 * b_stride] = Y11; + } + { + dReal p2 = ptrLElement[-1]; + Y21 = ptrBElement[-1 * (int)b_stride] - Z21 - p2 * Y11; + ptrBElement[-1 * (int)b_stride] = Y21; + } + { + dReal p3 = ptrLElement[-2]; + dReal p3_1 = (ptrLElement - rowSkip)[-2]; + Y31 = ptrBElement[-2 * (int)b_stride] - Z31 - p3 * Y11 - p3_1 * Y21; + ptrBElement[-2 * (int)b_stride] = Y31; + } + { + dReal p4 = ptrLElement[-3]; + dReal p4_1 = (ptrLElement - rowSkip)[-3]; + dReal p4_2 = (ptrLElement - rowSkip * 2)[-3]; + Y41 = ptrBElement[-3 * (int)b_stride] - Z41 - p4 * Y11 - p4_1 * Y21 - p4_2 * Y31; + ptrBElement[-3 * (int)b_stride] = Y41; + } + /* end of outer loop */ + } +} + + + +template<unsigned int block_step> +/*static */ +sizeint ThreadedEquationSolverLDLT::estimateCooperativelySolvingL1TransposedMemoryRequirement(unsigned rowCount, SolvingL1TransposedMemoryEstimates &ref_solvingMemoryEstimates) +{ + unsigned blockCount = deriveSolvingL1TransposedBlockCount(rowCount, block_step); + sizeint descriptorSizeRequired = dEFFICIENT_SIZE(sizeof(cellindexint) * blockCount); + sizeint contextSizeRequired = dEFFICIENT_SIZE(sizeof(SolveL1TransposedCellContext) * (CCI__MAX + 1) * blockCount); + ref_solvingMemoryEstimates.assignData(descriptorSizeRequired, contextSizeRequired); + + sizeint totalSizeRequired = descriptorSizeRequired + contextSizeRequired; + return totalSizeRequired; +} + +template<unsigned int block_step> +/*static */ +void ThreadedEquationSolverLDLT::initializeCooperativelySolveL1TransposedMemoryStructures(unsigned rowCount, + atomicord32 &out_blockCompletionProgress, cellindexint *blockProgressDescriptors, SolveL1TransposedCellContext *dUNUSED(cellContexts)) +{ + unsigned blockCount = deriveSolvingL1TransposedBlockCount(rowCount, block_step); + + out_blockCompletionProgress = 0; + memset(blockProgressDescriptors, 0, blockCount * sizeof(*blockProgressDescriptors)); +} + +template<unsigned int block_step, unsigned int b_stride> +/*static */ +void ThreadedEquationSolverLDLT::participateSolvingL1Transposed(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip, + volatile atomicord32 &refBlockCompletionProgress/*=0*/, volatile cellindexint *blockProgressDescriptors/*=[blockCount]*/, + SolveL1TransposedCellContext *cellContexts/*=[CCI__MAX x blockCount] + [blockCount]*/, unsigned ownThreadIndex) +{ + const unsigned lookaheadRange = 32; + const unsigned blockCount = deriveSolvingL1TransposedBlockCount(rowCount, block_step); + /* compute rows at end that are not a multiple of block size */ + const unsigned loopX1RowCount = rowCount % block_step; + + /* special handling for L and B because we're solving L1 *transpose* */ + const dReal *lastLElement = L + (rowCount - 1) * ((sizeint)rowSkip + 1); + dReal *lastBElement = B + (rowCount - 1) * (sizeint)b_stride; + + /* elements adjusted as if the last block was full block_step elements */ + unsigned x1AdjustmentElements = (block_step - loopX1RowCount) % block_step; + const dReal *columnAdjustedLastLElement = lastLElement + x1AdjustmentElements; + const dReal *fullyAdjustedLastLElement = columnAdjustedLastLElement + (sizeint)rowSkip * x1AdjustmentElements; + dReal *adjustedLastBElement = lastBElement + b_stride * x1AdjustmentElements; + + BlockProcessingState blockProcessingState = BPS_NO_BLOCKS_PROCESSED; + + unsigned completedBlocks = refBlockCompletionProgress; + unsigned currentBlock = completedBlocks; + dIASSERT(completedBlocks <= blockCount); + + for (bool exitLoop = completedBlocks == blockCount; !exitLoop; exitLoop = false) + { + bool goForLockedBlockPrimaryCalculation = false, goForLockedBlockDuplicateCalculation = false; + bool goAssigningTheResult = false, stayWithinTheBlock = false; + + dReal Z[block_step]; + dReal Y[block_step]; + + dReal *ptrBElement; + + CellContextInstance previousContextInstance; + unsigned completedRowBlock; + bool partialBlock; + + for (cellindexint testDescriptor = blockProgressDescriptors[currentBlock]; ; ) + { + if (testDescriptor == INVALID_CELLDESCRIPTOR) + { + // Invalid descriptor is the indication that the row has been fully calculated + // Test if this was the last row and break out if so. + if (currentBlock + 1 == blockCount) + { + exitLoop = true; + break; + } + + // Treat detected row advancement as a row processed + // blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; <-- performs better without it + break; + } + + CooperativeAtomics::AtomicReadReorderBarrier(); + // It is necessary to read up to date completedBblocks value after the descriptor retrieval + // as otherwise the logic below breaks + completedBlocks = refBlockCompletionProgress; + + if (!GET_CELLDESCRIPTOR_ISLOCKED(testDescriptor)) + { + completedRowBlock = GET_CELLDESCRIPTOR_COLUMNINDEX(testDescriptor); + dIASSERT(completedRowBlock < currentBlock || (completedRowBlock == currentBlock && currentBlock == 0)); // Otherwise, why would the calculation have had stopped if the final column is reachable??? + dIASSERT(completedRowBlock <= completedBlocks); // Since the descriptor is not locked + + if (completedRowBlock == completedBlocks && currentBlock != completedBlocks) + { + dIASSERT(completedBlocks < currentBlock); + break; + } + + if (CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], testDescriptor, MARK_CELLDESCRIPTOR_LOCKED(testDescriptor))) + { + if (completedRowBlock != 0) + { + CellContextInstance contextInstance = GET_CELLDESCRIPTOR_CONTEXTINSTANCE(testDescriptor); + previousContextInstance = contextInstance; + + const SolveL1TransposedCellContext &sourceContext = buildBlockContextRef(cellContexts, currentBlock, contextInstance); + sourceContext.loadPrecalculatedZs(Z); + } + else + { + previousContextInstance = CCI__MIN; + SolveL1TransposedCellContext::initializePrecalculatedZs(Z); + } + + goForLockedBlockPrimaryCalculation = true; + break; + } + + if (blockProcessingState != BPS_COMPETING_FOR_A_BLOCK) + { + break; + } + + testDescriptor = blockProgressDescriptors[currentBlock]; + } + else + { + if (blockProcessingState != BPS_COMPETING_FOR_A_BLOCK) + { + break; + } + + cellindexint verificativeDescriptor; + bool verificationFailure = false; + + completedRowBlock = GET_CELLDESCRIPTOR_COLUMNINDEX(testDescriptor); + dIASSERT(completedRowBlock != currentBlock || currentBlock == 0); // There is no reason for computations to stop at the very end other than being the initial value at the very first block + + if (completedRowBlock != 0) + { + CellContextInstance contextInstance = GET_CELLDESCRIPTOR_CONTEXTINSTANCE(testDescriptor); + const SolveL1TransposedCellContext &sourceContext = buildBlockContextRef(cellContexts, currentBlock, contextInstance); + sourceContext.loadPrecalculatedZs(Z); + } + else + { + SolveL1TransposedCellContext::initializePrecalculatedZs(Z); + } + + if (completedRowBlock != 0 && completedRowBlock <= currentBlock) + { + // Make sure the descriptor is re-read after the precalculates + CooperativeAtomics::AtomicReadReorderBarrier(); + } + + if (completedRowBlock <= currentBlock) + { + verificativeDescriptor = blockProgressDescriptors[currentBlock]; + verificationFailure = verificativeDescriptor != testDescriptor; + } + + if (!verificationFailure) + { + dIASSERT(completedRowBlock <= currentBlock + 1); + + goForLockedBlockDuplicateCalculation = true; + break; + } + + testDescriptor = verificativeDescriptor; + } + } + + if (exitLoop) + { + break; + } + + if (goForLockedBlockPrimaryCalculation) + { + blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; + + // Declare and assign the variables at the top to not interfere with any branching -- the compiler is going to eliminate them anyway. + bool handleComputationTakenOver = false, columnEndReached = false; + + const dReal *ptrLElement; + unsigned finalRowBlock; + + /* check if this is not the partial block of fewer rows */ + if (currentBlock != 0 || loopX1RowCount == 0) + { + partialBlock = false; + + ptrLElement = completedRowBlock != 0 + ? fullyAdjustedLastLElement - currentBlock * block_step - (sizeint)(completedRowBlock * block_step) * rowSkip + : columnAdjustedLastLElement - currentBlock * block_step; + ptrBElement = completedRowBlock != 0 + ? adjustedLastBElement - (sizeint)(completedRowBlock * block_step) * b_stride + : lastBElement; + + finalRowBlock = dMACRO_MIN(currentBlock, completedBlocks); + dIASSERT(finalRowBlock != completedRowBlock || finalRowBlock == 0); + + unsigned rowCounter = finalRowBlock - completedRowBlock; + bool exitLoop = rowCounter == 0; + + if (exitLoop) + { + columnEndReached = true; + } + else if (completedRowBlock == 0 && currentBlock != 0 && loopX1RowCount != 0) + { + if ((loopX1RowCount & 1) != 0) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + ptrBElement -= 1 * b_stride; + } + + if ((loopX1RowCount & 2) != 0) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + ptrBElement -= 2 * b_stride; + } + dSASSERT(block_step == 4); + + if (--rowCounter == 0) + { + do + { + if (finalRowBlock == currentBlock) + { + columnEndReached = true; + exitLoop = true; + break; + } + + // Take a look if any more columns have been completed... + completedBlocks = refBlockCompletionProgress; + dIASSERT(completedBlocks >= finalRowBlock); + + if (completedBlocks == finalRowBlock) + { + exitLoop = true; + break; + } + + // ...continue if so. + unsigned rowCompletedSoFar = finalRowBlock; + finalRowBlock = dMACRO_MIN(currentBlock, completedBlocks); + rowCounter = finalRowBlock - rowCompletedSoFar; + } + while (false); + } + } + + for (; !exitLoop; exitLoop = false) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-2 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-3 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + dSASSERT(block_step == 4); + + if (rowCounter > 3) + { + rowCounter -= 3; + + ptrBElement -= 3 * block_step * b_stride; + + /* load p and q values */ + q1 = ptrBElement[8 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[7 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[6 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[5 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[4 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[3 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[2 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[1 * b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + dSASSERT(block_step == 4); + } + else + { + ptrBElement -= block_step * b_stride; + + if (--rowCounter == 0) + { + if (finalRowBlock == currentBlock) + { + columnEndReached = true; + break; + } + + // Take a look if any more columns have been completed... + completedBlocks = refBlockCompletionProgress; + dIASSERT(completedBlocks >= finalRowBlock); + + if (completedBlocks == finalRowBlock) + { + break; + } + + // ...continue if so. + unsigned rowCompletedSoFar = finalRowBlock; + finalRowBlock = dMACRO_MIN(currentBlock, completedBlocks); + rowCounter = finalRowBlock - rowCompletedSoFar; + } + } + /* end of inner loop */ + } + } + else /* compute rightmost bottom X(i) block */ + { + partialBlock = true; + + ptrLElement = lastLElement; + ptrBElement = lastBElement; + dIASSERT(completedRowBlock == 0); + + columnEndReached = true; + } + + if (columnEndReached) + { + // Check whether there is still a need to proceed or if the computation has been taken over by another thread + cellindexint oldDescriptor = MAKE_CELLDESCRIPTOR(completedRowBlock, previousContextInstance, true); + + if (blockProgressDescriptors[currentBlock] == oldDescriptor) + { + if (partialBlock) + { + Y[0] = ptrBElement[0 * b_stride]/* - Z[0]*/; + + if (loopX1RowCount >= 2) + { + dReal p2 = ptrLElement[-1]; + Y[1] = ptrBElement[-1 * (int)b_stride]/* - Z[1] */- p2 * Y[0]; + + if (loopX1RowCount > 2) + { + dReal p3 = ptrLElement[-2]; + dReal p3_1 = (ptrLElement - rowSkip)[-2]; + Y[2] = ptrBElement[-2 * (int)b_stride]/* - Z[2] */- p3 * Y[0] - p3_1 * Y[1]; + } + } + + dSASSERT(block_step == 4); + } + else + { + Y[0] = ptrBElement[0 * b_stride] - Z[0]; + + dReal p2 = ptrLElement[-1]; + Y[1] = ptrBElement[-1 * (int)b_stride] - Z[1] - p2 * Y[0]; + + dReal p3 = ptrLElement[-2]; + dReal p3_1 = (ptrLElement - rowSkip)[-2]; + Y[2] = ptrBElement[-2 * (int)b_stride] - Z[2] - p3 * Y[0] - p3_1 * Y[1]; + + dReal p4 = ptrLElement[-3]; + dReal p4_1 = (ptrLElement - rowSkip)[-3]; + dReal p4_2 = (ptrLElement - rowSkip * 2)[-3]; + Y[3] = ptrBElement[-3 * (int)b_stride] - Z[3] - p4 * Y[0] - p4_1 * Y[1] - p4_2 * Y[2]; + + dSASSERT(block_step == 4); + } + + // Use atomic memory barrier to make sure memory reads of ptrBElement[] and blockProgressDescriptors[] are not swapped + CooperativeAtomics::AtomicReadReorderBarrier(); + + // The descriptor has not been altered yet - this means the ptrBElement[] values used above were not modified yet + // and the computation result is valid. + if (blockProgressDescriptors[currentBlock] == oldDescriptor) + { + // Assign the results to the result context (possibly in parallel with other threads + // that could and ought to be assigning exactly the same values) + SolveL1TransposedCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.storePrecalculatedZs(Y); + + // Assign the result assignment progress descriptor + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true); + CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], oldDescriptor, newDescriptor); // the result is to be ignored + + // Whether succeeded or not, the result is valid, so go on trying to assign it to the matrix + goAssigningTheResult = true; + } + else + { + // Otherwise, go on competing for copying the results + handleComputationTakenOver = true; + } + } + else + { + handleComputationTakenOver = true; + } + } + else + { + // If the final column has not been reached yet, store current values to the context. + // Select the other context instance as the previous one might be read by other threads. + CellContextInstance nextContextInstance = buildNextContextInstance(previousContextInstance); + SolveL1TransposedCellContext &destinationContext = buildBlockContextRef(cellContexts, currentBlock, nextContextInstance); + destinationContext.storePrecalculatedZs(Z); + + // Unlock the row until more columns can be used + cellindexint oldDescriptor = MAKE_CELLDESCRIPTOR(completedRowBlock, previousContextInstance, true); + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(finalRowBlock, nextContextInstance, false); + // The descriptor might have been updated by a competing thread + if (!CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], oldDescriptor, newDescriptor)) + { + // Adjust the ptrBElement to point to the result area... + ptrBElement = adjustedLastBElement - (sizeint)(currentBlock * block_step) * b_stride; + dIASSERT(currentBlock != 0 || adjustedLastBElement == lastBElement); + // ...and go on handling the case + handleComputationTakenOver = true; + } + } + + if (handleComputationTakenOver) + { + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + // This can only happen if the row was (has become) the uppermost not fully completed one + // and the competing thread is at final stage of calculation (i.e., it has reached the currentBlock column). + if (existingDescriptor != INVALID_CELLDESCRIPTOR) + { + // If not fully completed this must be the final stage of the result assignment into the matrix + dIASSERT(existingDescriptor == MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true)); + + // Go on competing copying the result as anyway the block is the topmost not completed one + // and since there was competition for it, there is no other work that can be done right now. + const SolveL1TransposedCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.loadPrecalculatedZs(Y); + + goAssigningTheResult = true; + } + else + { + // everything is over -- just go handling next blocks + } + } + } + else if (goForLockedBlockDuplicateCalculation) + { + blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; + + bool skipToHandlingSubsequentRows = false, skiptoCopyingResult = false; + + /* declare variables */ + const dReal *ptrLElement; + + if (completedRowBlock < currentBlock) + { + partialBlock = false; + + ptrLElement = completedRowBlock != 0 + ? fullyAdjustedLastLElement - currentBlock * block_step - (sizeint)(completedRowBlock * block_step) * rowSkip + : columnAdjustedLastLElement - currentBlock * block_step; + ptrBElement = completedRowBlock != 0 + ? adjustedLastBElement - (sizeint)(completedRowBlock * block_step) * b_stride + : lastBElement; + + unsigned finalRowBlock = currentBlock/*std::min(currentBlock, completedBlocks)*/; + dIASSERT(currentBlock == completedBlocks); // Why would we be competing for a row otherwise? + + bool exitInnerLoop = false; + unsigned lastCompletedRow = completedRowBlock; + unsigned rowCounter = finalRowBlock - completedRowBlock; + + if (completedRowBlock == 0/* && currentBlock != 0 */&& loopX1RowCount != 0) + { + if ((loopX1RowCount & 1) != 0) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + ptrBElement -= 1 * b_stride; + } + + if ((loopX1RowCount & 2) != 0) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + ptrBElement -= 2 * b_stride; + } + dSASSERT(block_step == 4); + + if (--rowCounter == 0) + { + exitInnerLoop = true; + } + } + + for (; !exitInnerLoop; exitInnerLoop = --rowCounter == 0) + { + dReal q1, p4, p3, p2, p1; + + /* load p and q values */ + q1 = ptrBElement[0 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-1 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-2 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + + /* load p and q values */ + q1 = ptrBElement[-3 * (int)b_stride]; + p4 = ptrLElement[-3]; + p3 = ptrLElement[-2]; + p2 = ptrLElement[-1]; + p1 = ptrLElement[0]; + ptrLElement -= rowSkip; + + /* compute outer product and add it to the Z matrix */ + Z[3] += p4 * q1; + Z[2] += p3 * q1; + Z[1] += p2 * q1; + Z[0] += p1 * q1; + dSASSERT(block_step == 4); + + // Check if the primary solver thread has not made any progress + cellindexint descriptorVerification = blockProgressDescriptors[currentBlock]; + unsigned newCompletedRow = GET_CELLDESCRIPTOR_COLUMNINDEX(descriptorVerification); + + if (newCompletedRow != lastCompletedRow) + { + // Check, this is the first change the current thread detects. + // There is absolutely no reason in code for the computation to stop/resume twice + // while the current thread is competing. + dIASSERT(lastCompletedRow == completedRowBlock); + + if (descriptorVerification == INVALID_CELLDESCRIPTOR) + { + skipToHandlingSubsequentRows = true; + break; + } + + if (newCompletedRow == currentBlock + 1) + { + skiptoCopyingResult = true; + break; + } + + // Check if the current thread is behind + if (newCompletedRow > finalRowBlock - rowCounter) + { + // If so, go starting over one more time + blockProcessingState = BPS_COMPETING_FOR_A_BLOCK; + stayWithinTheBlock = true; + skipToHandlingSubsequentRows = true; + break; + } + + // If current thread is ahead, just save new completed column for further comparisons and go on calculating + lastCompletedRow = newCompletedRow; + } + + /* advance pointers */ + ptrBElement -= block_step * b_stride; + /* end of inner loop */ + } + } + else if (completedRowBlock > currentBlock) + { + dIASSERT(completedRowBlock == currentBlock + 1); + + partialBlock = currentBlock == 0 && loopX1RowCount != 0; + + skiptoCopyingResult = true; + } + else + { + dIASSERT(currentBlock == 0); // Execution can get here within the very first block only + + partialBlock = /*currentBlock == 0 && */loopX1RowCount != 0; + + /* just assign the pointers appropriately and go on computing the results */ + ptrLElement = lastLElement; + ptrBElement = lastBElement; + } + + if (!skipToHandlingSubsequentRows) + { + if (!skiptoCopyingResult) + { + if (partialBlock) + { + Y[0] = ptrBElement[0 * b_stride]/* - Z[0]*/; + + if (loopX1RowCount >= 2) + { + dReal p2 = ptrLElement[-1]; + Y[1] = ptrBElement[-1 * (int)b_stride]/* - Z[1] */- p2 * Y[0]; + + if (loopX1RowCount > 2) + { + dReal p3 = ptrLElement[-2]; + dReal p3_1 = (ptrLElement - rowSkip)[-2]; + Y[2] = ptrBElement[-2 * (int)b_stride]/* - Z[2] */- p3 * Y[0] - p3_1 * Y[1]; + } + } + + dSASSERT(block_step == 4); + } + else + { + Y[0] = ptrBElement[0 * b_stride] - Z[0]; + + dReal p2 = ptrLElement[-1]; + Y[1] = ptrBElement[-1 * (int)b_stride] - Z[1] - p2 * Y[0]; + + dReal p3 = ptrLElement[-2]; + dReal p3_1 = (ptrLElement - rowSkip)[-2]; + Y[2] = ptrBElement[-2 * (int)b_stride] - Z[2] - p3 * Y[0] - p3_1 * Y[1]; + + dReal p4 = ptrLElement[-3]; + dReal p4_1 = (ptrLElement - rowSkip)[-3]; + dReal p4_2 = (ptrLElement - rowSkip * 2)[-3]; + Y[3] = ptrBElement[-3 * (int)b_stride] - Z[3] - p4 * Y[0] - p4_1 * Y[1] - p4_2 * Y[2]; + + dSASSERT(block_step == 4); + } + + // Use atomic memory barrier to make sure memory reads of ptrBElement[] and blockProgressDescriptors[] are not swapped + CooperativeAtomics::AtomicReadReorderBarrier(); + + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + + if (existingDescriptor == INVALID_CELLDESCRIPTOR) + { + // Everything is over -- proceed to subsequent rows + skipToHandlingSubsequentRows = true; + } + else if (existingDescriptor == MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true)) + { + // The values computed above may not be valid. Copy the values already in the result context. + skiptoCopyingResult = true; + } + else + { + // The descriptor has not been altered yet - this means the ptrBElement[] values used above were not modified yet + // and the computation result is valid. + cellindexint newDescriptor = MAKE_CELLDESCRIPTOR(currentBlock + 1, CCI__MIN, true); // put the computation at the top so that the evaluation result from the expression above is reused + + // Assign the results to the result context (possibly in parallel with other threads + // that could and ought to be assigning exactly the same values) + SolveL1TransposedCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.storePrecalculatedZs(Y); + + // Assign the result assignment progress descriptor + CooperativeAtomics::AtomicCompareExchangeCellindexint(&blockProgressDescriptors[currentBlock], existingDescriptor, newDescriptor); // the result is to be ignored + + // Whether succeeded or not, the result is valid, so go on trying to assign it to the matrix + } + } + + if (!skipToHandlingSubsequentRows) + { + if (skiptoCopyingResult) + { + // Extract the result values stored in the result context + const SolveL1TransposedCellContext &resultContext = buildResultContextRef(cellContexts, currentBlock, blockCount); + resultContext.loadPrecalculatedZs(Y); + + ptrBElement = currentBlock != 0 ? adjustedLastBElement - (sizeint)(currentBlock * block_step) * b_stride : lastBElement; + } + + goAssigningTheResult = true; + } + } + } + + if (goAssigningTheResult) + { + cellindexint existingDescriptor = blockProgressDescriptors[currentBlock]; + // Check if the assignment has not been completed yet + if (existingDescriptor != INVALID_CELLDESCRIPTOR) + { + // Assign the computation results to B vector + if (partialBlock) + { + // ptrBElement[0 * b_stride] = Y[0]; -- unchanged + + if (loopX1RowCount >= 2) + { + ptrBElement[-1 * (int)b_stride] = Y[1]; + + if (loopX1RowCount > 2) + { + ptrBElement[-2 * (int)b_stride] = Y[2]; + } + } + dSASSERT(block_step == 4); + } + else + { + ptrBElement[0 * b_stride] = Y[0]; + ptrBElement[-1 * (int)b_stride] = Y[1]; + ptrBElement[-2 * (int)b_stride] = Y[2]; + ptrBElement[-3 * (int)b_stride] = Y[3]; + dSASSERT(block_step == 4); + } + + ThrsafeIncrementIntUpToLimit(&refBlockCompletionProgress, currentBlock + 1); + dIASSERT(refBlockCompletionProgress >= currentBlock + 1); + + // And assign the completed status no matter what + CooperativeAtomics::AtomicStoreCellindexint(&blockProgressDescriptors[currentBlock], INVALID_CELLDESCRIPTOR); + } + else + { + // everything is over -- just go handling next blocks + } + } + + if (!stayWithinTheBlock) + { + completedBlocks = refBlockCompletionProgress; + + if (completedBlocks == blockCount) + { + break; + } + + currentBlock += 1; + + bool lookaheadBoundaryReached = false; + + if (currentBlock == blockCount || completedBlocks == 0) + { + lookaheadBoundaryReached = true; + } + else if (currentBlock >= completedBlocks + lookaheadRange) + { + lookaheadBoundaryReached = blockProcessingState > BPS_NO_BLOCKS_PROCESSED; + } + else if (currentBlock < completedBlocks) + { + // Treat detected row advancement as a row processed + // blockProcessingState = BPS_SOME_BLOCKS_PROCESSED; <-- performs better without it + + currentBlock = completedBlocks; + } + + if (lookaheadBoundaryReached) + { + dIASSERT(blockProcessingState != BPS_COMPETING_FOR_A_BLOCK); // Why did not we compete??? + + // If no row has been processed in the previous pass, compete for the next row to avoid cycling uselessly + if (blockProcessingState <= BPS_NO_BLOCKS_PROCESSED) + { + // Abandon job if too few blocks remain + if (blockCount - completedBlocks <= ownThreadIndex) + { + break; + } + + blockProcessingState = BPS_COMPETING_FOR_A_BLOCK; + } + else + { + // If there was some progress, just continue to the next pass + blockProcessingState = BPS_NO_BLOCKS_PROCESSED; + } + + currentBlock = completedBlocks; + } + } + } +} + + +#endif diff --git a/libs/ode-0.16.1/ode/src/fastvecscale.cpp b/libs/ode-0.16.1/ode/src/fastvecscale.cpp new file mode 100644 index 0000000..9927d89 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastvecscale.cpp @@ -0,0 +1,204 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Vector scaling related code of ThreadedEquationSolverLDLT + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#include <ode/common.h> +#include <ode/matrix.h> +#include <ode/matrix_coop.h> +#include "config.h" +#include "threaded_solver_ldlt.h" +#include "threading_base.h" +#include "resource_control.h" +#include "error.h" + +#include "fastvecscale_impl.h" + + +/*static */ +void ThreadedEquationSolverLDLT::estimateCooperativeScalingVectorResourceRequirements( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned elementCount) +{ + dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading(); + unsigned limitedThreadCount = restrictScalingVectorAllowedThreadCount(threading, allowedThreadCount, elementCount); + + if (limitedThreadCount > 1) + { + doEstimateCooperativeScalingVectorResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, elementCount); + } +} + +/*static */ +void ThreadedEquationSolverLDLT::cooperativelyScaleVector(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *vectorData, const dReal *scaleData, unsigned elementCount) +{ + dAASSERT(elementCount != 0); + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + unsigned limitedThreadCount = restrictScalingVectorAllowedThreadCount(threading, allowedThreadCount, elementCount); + + if (limitedThreadCount <= 1) + { + scaleLargeVector<SV_A_STRIDE, SV_D_STRIDE>(vectorData, scaleData, elementCount); + } + else + { + doCooperativelyScaleVectorValidated(resourceContainer, limitedThreadCount, vectorData, scaleData, elementCount); + } +} + +/*static */ +unsigned ThreadedEquationSolverLDLT::restrictScalingVectorAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned elementCount) +{ + unsigned limitedThreadCount = 1; + +#if dCOOPERATIVE_ENABLED + const unsigned int blockStep = SV_BLOCK_SIZE; // Required by the implementation + unsigned scalingBlockCount = deriveScalingVectorBlockCount(elementCount, blockStep); + dIASSERT(deriveScalingVectorThreadCount(SV_COOPERATIVE_BLOCK_COUNT_MINIMUM - 1, 2) > 1); + + if (scalingBlockCount >= SV_COOPERATIVE_BLOCK_COUNT_MINIMUM) + { + limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true); + } +#endif // #if dCOOPERATIVE_ENABLED + + return limitedThreadCount; +} + +/*static */ +void ThreadedEquationSolverLDLT::doEstimateCooperativeScalingVectorResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned elementCount) +{ + unsigned simultaneousCallCount = 1 + (allowedThreadCount - 1); + + sizeint scalingMemoryRequired = 0; + const unsigned scalingAlignmentRequired = 0; + + unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED; + summaryRequirementsDescriptor->mergeAnotherDescriptorIn(scalingMemoryRequired, scalingAlignmentRequired, simultaneousCallCount, featureRequirement); +} + +/*static */ +void ThreadedEquationSolverLDLT::doCooperativelyScaleVectorValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *vectorData, const dReal *scaleData, unsigned elementCount) +{ + dIASSERT(allowedThreadCount > 1); + + const unsigned int blockStep = SV_BLOCK_SIZE; // Required by the implementation + unsigned scalingBlockCount = deriveScalingVectorBlockCount(elementCount, blockStep); + dIASSERT(scalingBlockCount > 0U); + + unsigned threadCountToUse = deriveScalingVectorThreadCount(scalingBlockCount - 1, allowedThreadCount); + dIASSERT(threadCountToUse > 1); + + dCallWaitID completionWait = resourceContainer->getStockCallWait(); + dAASSERT(completionWait != NULL); + + atomicord32 blockCompletionProgress; + + initializeCooperativelyScaleVectorMemoryStructures(blockCompletionProgress); + + dCallReleaseeID calculationFinishReleasee; + ScaleVectorWorkerContext workerContext; // The variable must exist in the outer scope + + workerContext.init(vectorData, scaleData, elementCount, blockCompletionProgress); + + dxThreadingBase *threading = resourceContainer->getThreadingInstance(); + threading->PostThreadedCall(NULL, &calculationFinishReleasee, threadCountToUse - 1, NULL, completionWait, &scaleVector_completion_callback, NULL, 0, "ScaleVector Completion"); + threading->PostThreadedCallsGroup(NULL, threadCountToUse - 1, calculationFinishReleasee, &scaleVector_worker_callback, &workerContext, "ScaleVector Work"); + + participateScalingVector<blockStep, SV_A_STRIDE, SV_D_STRIDE>(vectorData, scaleData, elementCount, blockCompletionProgress); + + threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "ScaleVector End Wait"); +} + + +/*static */ +int ThreadedEquationSolverLDLT::scaleVector_worker_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + ScaleVectorWorkerContext *ptrContext = (ScaleVectorWorkerContext *)callContext; + + scaleVector_worker(*ptrContext); + + return 1; +} + +/*static */ +void ThreadedEquationSolverLDLT::scaleVector_worker(ScaleVectorWorkerContext &ref_context) +{ + const unsigned blockStep = SV_BLOCK_SIZE; + + participateScalingVector<blockStep, SV_A_STRIDE, SV_D_STRIDE>(ref_context.m_vectorData, ref_context.m_scaleData, ref_context.m_elementCount, *ref_context.m_ptrBlockCompletionProgress); +} + +/*static */ +int ThreadedEquationSolverLDLT::scaleVector_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee)) +{ + return 1; +} + + +////////////////////////////////////////////////////////////////////////// +// Public interface functions + +/*extern ODE_API */ +void dScaleVector(dReal *a, const dReal *d, int n) +{ + scaleLargeVector<1, 1>(a, d, n); +} + +/*extern ODE_API_DEPRECATED ODE_API */ +void dVectorScale(dReal *a, const dReal *d, int n) +{ + scaleLargeVector<1, 1>(a, d, n); +} + + +/*extern ODE_API */ +void dEstimateCooperativelyScaleVectorResourceRequirements(dResourceRequirementsID requirements, + unsigned maximalAllowedThreadCount, unsigned maximalElementCount) +{ + dAASSERT(requirements != NULL); + + dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements; + ThreadedEquationSolverLDLT::estimateCooperativeScalingVectorResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalElementCount); +} + +/*extern ODE_API */ +void dCooperativelyScaleVector(dResourceContainerID resources, unsigned allowedThreadCount, + dReal *dataVector, const dReal *scaleVector, unsigned elementCount) +{ + dAASSERT(resources != NULL); + + dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources; + ThreadedEquationSolverLDLT::cooperativelyScaleVector(resourceContainer, allowedThreadCount, dataVector, scaleVector, elementCount); +} + diff --git a/libs/ode-0.16.1/ode/src/fastvecscale_impl.h b/libs/ode-0.16.1/ode/src/fastvecscale_impl.h new file mode 100644 index 0000000..c483fdd --- /dev/null +++ b/libs/ode-0.16.1/ode/src/fastvecscale_impl.h @@ -0,0 +1,171 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/*
+ * Vector scaling function implementation
+ * Improvements and cooperative implementation copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+#ifndef _ODE_FASTVECSCALE_IMPL_H_
+#define _ODE_FASTVECSCALE_IMPL_H_
+
+
+
+template<unsigned int a_stride, unsigned int d_stride>
+void scaleLargeVector(dReal *aStart, const dReal *dStart, unsigned elementCount)
+{
+ dAASSERT (aStart && dStart && elementCount >= 0);
+
+ const unsigned step = 4;
+
+ dReal *ptrA = aStart;
+ const dReal *ptrD = dStart;
+ const dReal *const dStepsEnd = dStart + (sizeint)(elementCount & ~(step - 1)) * d_stride;
+ for (; ptrD != dStepsEnd; ptrA += step * a_stride, ptrD += step * d_stride)
+ {
+ dReal a0 = ptrA[0], a1 = ptrA[1 * a_stride], a2 = ptrA[2 * a_stride], a3 = ptrA[3 * a_stride];
+ dReal d0 = ptrD[0], d1 = ptrD[1 * d_stride], d2 = ptrD[2 * d_stride], d3 = ptrD[3 * d_stride];
+ a0 *= d0;
+ a1 *= d1;
+ a2 *= d2;
+ a3 *= d3;
+ ptrA[0] = a0; ptrA[1 * a_stride] = a1; ptrA[2 * a_stride] = a2; ptrA[3 * a_stride] = a3;
+ dSASSERT(step == 4);
+ }
+
+ switch (elementCount & (step - 1))
+ {
+ case 3:
+ {
+ dReal a2 = ptrA[2 * a_stride];
+ dReal d2 = ptrD[2 * d_stride];
+ ptrA[2 * a_stride] = a2 * d2;
+ // break; -- proceed to case 2
+ }
+
+ case 2:
+ {
+ dReal a1 = ptrA[1 * a_stride];
+ dReal d1 = ptrD[1 * d_stride];
+ ptrA[1 * a_stride] = a1 * d1;
+ // break; -- proceed to case 1
+ }
+
+ case 1:
+ {
+ dReal a0 = ptrA[0];
+ dReal d0 = ptrD[0];
+ ptrA[0] = a0 * d0;
+ break;
+ }
+ }
+ dSASSERT(step == 4);
+}
+
+
+template<unsigned int block_step, unsigned int a_stride, unsigned int d_stride>
+/*static */
+void ThreadedEquationSolverLDLT::participateScalingVector(dReal *ptrAStart, const dReal *ptrDStart, const unsigned elementCount,
+ volatile atomicord32 &refBlockCompletionProgress/*=0*/)
+{
+ dAASSERT (ptrAStart != NULL);
+ dAASSERT(ptrDStart != NULL);
+ dAASSERT(elementCount >= 0);
+
+ const unsigned wrapSize = 4;
+ dSASSERT(block_step % wrapSize == 0);
+
+ const unsigned completeBlockCount = elementCount / block_step;
+ const unsigned trailingBlockElements = elementCount % block_step;
+
+ unsigned blockIndex;
+ while ((blockIndex = ThrsafeIncrementIntUpToLimit(&refBlockCompletionProgress, completeBlockCount)) != completeBlockCount)
+ {
+ dReal *ptrAElement = ptrAStart + (sizeint)(blockIndex * block_step) * a_stride;
+ const dReal *ptrDElement = ptrDStart + (sizeint)(blockIndex * block_step) * d_stride;
+ const dReal *const ptrDBlockEnd = ptrDElement + block_step * d_stride;
+ dSASSERT((sizeint)block_step * a_stride < UINT_MAX);
+ dSASSERT((sizeint)block_step * d_stride < UINT_MAX);
+
+ for (; ptrDElement != ptrDBlockEnd; ptrAElement += wrapSize * a_stride, ptrDElement += wrapSize * d_stride)
+ {
+ dReal a0 = ptrAElement[0], a1 = ptrAElement[1 * a_stride], a2 = ptrAElement[2 * a_stride], a3 = ptrAElement[3 * a_stride];
+ dReal d0 = ptrDElement[0], d1 = ptrDElement[1 * d_stride], d2 = ptrDElement[2 * d_stride], d3 = ptrDElement[3 * d_stride];
+ a0 *= d0;
+ a1 *= d1;
+ a2 *= d2;
+ a3 *= d3;
+ ptrAElement[0] = a0; ptrAElement[1 * a_stride] = a1; ptrAElement[2 * a_stride] = a2; ptrAElement[3 * a_stride] = a3;
+ dSASSERT(wrapSize == 4);
+ }
+ }
+
+ if (trailingBlockElements != 0 && (blockIndex = ThrsafeIncrementIntUpToLimit(&refBlockCompletionProgress, completeBlockCount + 1)) != completeBlockCount + 1)
+ {
+ dReal *ptrAElement = ptrAStart + (sizeint)(completeBlockCount * block_step) * a_stride;
+ const dReal *ptrDElement = ptrDStart + (sizeint)(completeBlockCount * block_step) * d_stride;
+ const dReal *const ptrDBlockEnd = ptrDElement + (trailingBlockElements & ~(wrapSize - 1)) * d_stride;
+
+ for (; ptrDElement != ptrDBlockEnd; ptrAElement += wrapSize * a_stride, ptrDElement += wrapSize * d_stride)
+ {
+ dReal a0 = ptrAElement[0], a1 = ptrAElement[1 * a_stride], a2 = ptrAElement[2 * a_stride], a3 = ptrAElement[3 * a_stride];
+ dReal d0 = ptrDElement[0], d1 = ptrDElement[1 * d_stride], d2 = ptrDElement[2 * d_stride], d3 = ptrDElement[3 * d_stride];
+ a0 *= d0;
+ a1 *= d1;
+ a2 *= d2;
+ a3 *= d3;
+ ptrAElement[0] = a0; ptrAElement[1 * a_stride] = a1; ptrAElement[2 * a_stride] = a2; ptrAElement[3 * a_stride] = a3;
+ dSASSERT(wrapSize == 4);
+ }
+
+ switch (trailingBlockElements & (wrapSize - 1))
+ {
+ case 3:
+ {
+ dReal a2 = ptrAElement[2 * a_stride];
+ dReal d2 = ptrDElement[2 * d_stride];
+ ptrAElement[2 * a_stride] = a2 * d2;
+ // break; -- proceed to case 2
+ }
+
+ case 2:
+ {
+ dReal a1 = ptrAElement[1 * a_stride];
+ dReal d1 = ptrDElement[1 * d_stride];
+ ptrAElement[1 * a_stride] = a1 * d1;
+ // break; -- proceed to case 1
+ }
+
+ case 1:
+ {
+ dReal a0 = ptrAElement[0];
+ dReal d0 = ptrDElement[0];
+ ptrAElement[0] = a0 * d0;
+ break;
+ }
+ }
+ dSASSERT(wrapSize == 4);
+ }
+}
+
+
+#endif
diff --git a/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.cpp b/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.cpp new file mode 100644 index 0000000..0e107b0 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.cpp @@ -0,0 +1,95 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/collision.h> +#include "config.h" + + +#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "error.h" + + +/*static */ +dReal dxGImpactContactsExportHelper::FindContactsMarginalDepth(dReal *pdepths, unsigned contactcount, unsigned maxcontacts, dReal mindepth, dReal maxdepth) +{ + dReal result; + + while (true) + { + dReal firstdepth = REAL(0.5) * (mindepth + maxdepth); + dReal lowdepth = maxdepth, highdepth = mindepth; + + unsigned marginindex = 0; + unsigned highindex = marginindex; + dIASSERT(contactcount != 0); + + for (unsigned i = 0; i < contactcount; i++) + { + dReal depth = pdepths[i]; + + if (depth < firstdepth) + { + dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth; + if (highdepth < depth) { highdepth = depth; } + } + else if (depth > firstdepth) + { + pdepths[highindex++] = depth; + if (depth < lowdepth) { lowdepth = depth; } + } + } + + unsigned countabove = highindex - marginindex; + if (maxcontacts < countabove) + { + contactcount = countabove; + pdepths += marginindex; + mindepth = lowdepth; + } + else if (maxcontacts == countabove) + { + result = dNextAfter(firstdepth, dInfinity); + break; + } + else + { + unsigned countbelow = marginindex; + if (maxcontacts <= contactcount - countbelow) + { + result = firstdepth; + break; + } + + maxcontacts -= contactcount - countbelow; + contactcount = countbelow; + maxdepth = highdepth; + } + } + + return result; +} + + +#endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + diff --git a/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h b/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h new file mode 100644 index 0000000..149ac91 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h @@ -0,0 +1,177 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#ifndef _ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_ +#define _ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_ + + +#include "collision_kernel.h" +#include "collision_util.h" +#include "util.h" + + +#ifndef ALLOCA +#define ALLOCA(x) dALLOCA16(x) +#endif + + +struct dxGImpactContactsExportHelper +{ +public: + template<class dxGImpactContactAccessor> + static unsigned ExportMaxDepthGImpactContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride) + { + unsigned result; + + unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK); + if (contactcount > maxcontacts) + { + ExportExcesssiveContacts(srccontacts, contactcount, Flags, Contacts, Stride); + result = maxcontacts; + } + else + { + ExportFitContacts(srccontacts, contactcount, Flags, Contacts, Stride); + result = contactcount; + } + + return result; + } + +private: + template<class dxGImpactContactAccessor> + static void ExportExcesssiveContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride); + template<class dxGImpactContactAccessor> + static void ExportFitContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride); + template<class dxGImpactContactAccessor> + static dReal FindContactsMarginalDepth(dxGImpactContactAccessor &srccontacts, unsigned contactcount, unsigned maxcontacts); + static dReal FindContactsMarginalDepth(dReal *pdepths, unsigned contactcount, unsigned maxcontacts, dReal mindepth, dReal maxdepth); +}; + + +template<class dxGImpactContactAccessor> +/*static */ +void dxGImpactContactsExportHelper::ExportExcesssiveContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride) +{ + unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK); + dReal marginaldepth = FindContactsMarginalDepth(srccontacts, contactcount, maxcontacts); + + unsigned contactshead = 0, contacttail = maxcontacts; + for (unsigned i = 0; i < contactcount; i++) + { + dReal depth = srccontacts.RetrieveDepthByIndex(i); + + if (depth > marginaldepth) + { + dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contactshead, Stride); + srccontacts.ExportContactGeomByIndex(pcontact, i); + + if (++contactshead == maxcontacts) + { + break; + } + } + else if (depth == marginaldepth && contactshead < contacttail) + { + --contacttail; + + dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contacttail, Stride); + srccontacts.ExportContactGeomByIndex(pcontact, i); + } + } +} + +template<class dxGImpactContactAccessor> +/*static */ +void dxGImpactContactsExportHelper::ExportFitContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride) +{ + for (unsigned i = 0; i < contactcount; i++) + { + dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, i, Stride); + + srccontacts.ExportContactGeomByIndex(pcontact, i); + } +} + +template<class dxGImpactContactAccessor> +/*static */ +dReal dxGImpactContactsExportHelper::FindContactsMarginalDepth(dxGImpactContactAccessor &srccontacts, unsigned contactcount, unsigned maxcontacts) +{ + dReal result; + + dReal *pdepths = (dReal *)ALLOCA(contactcount * sizeof(dReal)); + unsigned marginindex = 0; + unsigned highindex = marginindex; + + dReal firstdepth = srccontacts.RetrieveDepthByIndex(0); + dReal mindepth = firstdepth, maxdepth = firstdepth; + dIASSERT(contactcount > 1); + + for (unsigned i = 1; i < contactcount; i++) + { + dReal depth = srccontacts.RetrieveDepthByIndex(i); + + if (depth < firstdepth) + { + dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth; + if (depth < mindepth) { mindepth = depth; } + } + else if (depth > firstdepth) + { + pdepths[highindex++] = depth; + if (maxdepth < depth) { maxdepth = depth; } + } + } + + unsigned countabove = highindex - marginindex; + if (maxcontacts < countabove) + { + result = FindContactsMarginalDepth(pdepths + marginindex, countabove, maxcontacts, firstdepth, maxdepth); + } + else if (maxcontacts == countabove) + { + result = dNextAfter(firstdepth, dInfinity); + } + else + { + unsigned countbelow = marginindex; + if (maxcontacts <= contactcount - countbelow) + { + result = firstdepth; + } + else + { + result = FindContactsMarginalDepth(pdepths, countbelow, maxcontacts - (contactcount - countbelow), mindepth, firstdepth); + } + } + + return result; +} + + +#endif //_ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_ diff --git a/libs/ode-0.16.1/ode/src/gimpact_gim_contact_accessor.h b/libs/ode-0.16.1/ode/src/gimpact_gim_contact_accessor.h new file mode 100644 index 0000000..2b252b4 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/gimpact_gim_contact_accessor.h @@ -0,0 +1,62 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#ifndef _ODE_GIMPACT_GIM_CONTACT_ACCESSOR_H_ +#define _ODE_GIMPACT_GIM_CONTACT_ACCESSOR_H_ + + +struct dxGIMCContactAccessor +{ + dxGIMCContactAccessor(GIM_CONTACT *ptrimeshcontacts, dGeomID g1, dGeomID g2) : m_ptrimeshcontacts(ptrimeshcontacts), m_g1(g1), m_g2(g2), m_gotside2ovr(false), m_side2ovr() {} + dxGIMCContactAccessor(GIM_CONTACT *ptrimeshcontacts, dGeomID g1, dGeomID g2, int side2ovr) : m_ptrimeshcontacts(ptrimeshcontacts), m_g1(g1), m_g2(g2), m_gotside2ovr(true), m_side2ovr(side2ovr) {} + + dReal RetrieveDepthByIndex(unsigned index) const { return m_ptrimeshcontacts[index].m_depth; } + + void ExportContactGeomByIndex(dContactGeom *pcontact, unsigned index) const + { + const GIM_CONTACT *ptrimeshcontact = m_ptrimeshcontacts + index; + pcontact->pos[0] = ptrimeshcontact->m_point[0]; + pcontact->pos[1] = ptrimeshcontact->m_point[1]; + pcontact->pos[2] = ptrimeshcontact->m_point[2]; + pcontact->pos[3] = REAL(1.0); + + pcontact->normal[0] = ptrimeshcontact->m_normal[0]; + pcontact->normal[1] = ptrimeshcontact->m_normal[1]; + pcontact->normal[2] = ptrimeshcontact->m_normal[2]; + pcontact->normal[3] = 0; + + pcontact->depth = ptrimeshcontact->m_depth; + pcontact->g1 = m_g1; + pcontact->g2 = m_g2; + pcontact->side1 = ptrimeshcontact->m_feature1; + pcontact->side2 = !m_gotside2ovr ? ptrimeshcontact->m_feature2 : m_side2ovr; + } + + const GIM_CONTACT *m_ptrimeshcontacts; + dGeomID m_g1, m_g2; + bool m_gotside2ovr; + int m_side2ovr; +}; + + +#endif //_ODE_GIMPACT_GIM_CONTACT_ACCESSOR_H_ diff --git a/libs/ode-0.16.1/ode/src/gimpact_plane_contact_accessor.h b/libs/ode-0.16.1/ode/src/gimpact_plane_contact_accessor.h new file mode 100644 index 0000000..035dcfd --- /dev/null +++ b/libs/ode-0.16.1/ode/src/gimpact_plane_contact_accessor.h @@ -0,0 +1,62 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#ifndef _ODE_GIMPACT_PLANE_CONTACT_ACCESSOR_H_ +#define _ODE_GIMPACT_PLANE_CONTACT_ACCESSOR_H_ + + +struct dxPlaneContactAccessor +{ + dxPlaneContactAccessor(const vec4f *planecontact_results, const dReal *plane, dGeomID g1, dGeomID g2) : m_planecontact_results(planecontact_results), m_plane(plane), m_g1(g1), m_g2(g2) {} + + dReal RetrieveDepthByIndex(unsigned index) const { return m_planecontact_results[index][3]; } + + void ExportContactGeomByIndex(dContactGeom *pcontact, unsigned index) const + { + const vec4f *planecontact = m_planecontact_results + index; + + pcontact->pos[0] = (*planecontact)[0]; + pcontact->pos[1] = (*planecontact)[1]; + pcontact->pos[2] = (*planecontact)[2]; + pcontact->pos[3] = REAL(1.0); + + const dReal *plane = m_plane; + pcontact->normal[0] = plane[0]; + pcontact->normal[1] = plane[1]; + pcontact->normal[2] = plane[2]; + pcontact->normal[3] = 0; + + pcontact->depth = (*planecontact)[3]; + pcontact->g1 = m_g1; // trimesh geom + pcontact->g2 = m_g2; // plane geom + pcontact->side1 = -1; // note: don't have the triangle index, but OPCODE *does* do this properly + pcontact->side2 = -1; + } + + const vec4f *m_planecontact_results; + const dReal *m_plane; + dGeomID m_g1, m_g2; +}; + + +#endif //_ODE_GIMPACT_PLANE_CONTACT_ACCESSOR_H_ diff --git a/libs/ode-0.16.1/ode/src/heightfield.cpp b/libs/ode-0.16.1/ode/src/heightfield.cpp new file mode 100644 index 0000000..71699db --- /dev/null +++ b/libs/ode-0.16.1/ode/src/heightfield.cpp @@ -0,0 +1,1876 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// dHeightfield Collider +// Paul Cheyrou-Lagreze aka Tuan Kuranes 2006 Speed enhancements http://www.pop-3d.com +// Martijn Buijs 2006 http://home.planet.nl/~buijs512/ +// Based on Terrain & Cone contrib by: +// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com +// Some code inspired by Magic Software + + +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" +#include "heightfield.h" + + + +#if dTRIMESH_ENABLED +#include "collision_trimesh_colliders.h" +#endif // dTRIMESH_ENABLED + +#define dMIN(A,B) ((A)>(B) ? (B) : (A)) +#define dMAX(A,B) ((A)>(B) ? (A) : (B)) + + +// Three-way MIN and MAX +#define dMIN3(A,B,C) ( (A)<(B) ? dMIN((A),(C)) : dMIN((B),(C)) ) +#define dMAX3(A,B,C) ( (A)>(B) ? dMAX((A),(C)) : dMAX((B),(C)) ) + +#define dOPESIGN(a, op1, op2,b) \ + (a)[0] op1 op2 ((b)[0]); \ + (a)[1] op1 op2 ((b)[1]); \ + (a)[2] op1 op2 ((b)[2]); + +#define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) { \ + \ + dVector3Copy (MyPoint, (myRay).final_posr->pos); \ + (myRay).final_posr->R[2] = (MyVector)[0]; \ + (myRay).final_posr->R[6] = (MyVector)[1]; \ + (myRay).final_posr->R[10] = (MyVector)[2]; \ + dGeomMoved (&myRay); \ + } + +#define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \ + \ + (MyPlane)->p[0] = (MyPlaneDef)[0]; \ + (MyPlane)->p[1] = (MyPlaneDef)[1]; \ + (MyPlane)->p[2] = (MyPlaneDef)[2]; \ + (MyPlane)->p[3] = (MyPlaneDef)[3]; \ + dGeomMoved (MyPlane); \ + } +//////// Local Build Option //////////////////////////////////////////////////// + +// Uncomment this #define to use the (0,0) corner of the geom as the origin, +// rather than the center. This was the way the original heightfield worked, +// but as it does not match the way all other geometries work, so for constancy it +// was changed to work like this. + +// #define DHEIGHTFIELD_CORNER_ORIGIN + + +// Uncomment this #define to add heightfield triangles edge colliding +// Code is not guaranteed and I didn't find the need to add that as +// colliding planes triangles and edge triangles seems enough. +// #define _HEIGHTFIELDEDGECOLLIDING + + +//////// dxHeightfieldData ///////////////////////////////////////////////////////////// + +// dxHeightfieldData constructor +dxHeightfieldData::dxHeightfieldData(): + m_fWidth( 0 ), + m_fDepth( 0 ), + m_fSampleWidth( 0 ), + m_fSampleDepth( 0 ), + m_fSampleZXAspect( 0 ), + m_fInvSampleWidth( 0 ), + m_fInvSampleDepth( 0 ), + + m_fHalfWidth( 0 ), + m_fHalfDepth( 0 ), + + m_fMinHeight( 0 ), + m_fMaxHeight( 0 ), + m_fThickness( 0 ), + m_fScale( 0 ), + m_fOffset( 0 ), + + m_nWidthSamples( 0 ), + m_nDepthSamples( 0 ), + m_bCopyHeightData( 0 ), + m_bWrapMode( 0 ), + m_nGetHeightMode( 0 ), + + m_pHeightData( NULL ), + m_pUserData( NULL ), + + m_pGetHeightCallback( NULL ) +{ + memset( m_contacts, 0, sizeof( m_contacts ) ); +} + +// build Heightfield data +void dxHeightfieldData::SetData( int nWidthSamples, int nDepthSamples, + dReal fWidth, dReal fDepth, + dReal fScale, dReal fOffset, dReal fThickness, + int bWrapMode ) +{ + dIASSERT( fWidth > REAL( 0.0 ) ); + dIASSERT( fDepth > REAL( 0.0 ) ); + dIASSERT( nWidthSamples > 0 ); + dIASSERT( nDepthSamples > 0 ); + + // x,z bounds + m_fWidth = fWidth; + m_fDepth = fDepth; + + // cache half x,z bounds + m_fHalfWidth = fWidth / REAL( 2.0 ); + m_fHalfDepth = fDepth / REAL( 2.0 ); + + // scale and offset + m_fScale = fScale; + m_fOffset = fOffset; + + // infinite min height bounds + m_fThickness = fThickness; + + // number of vertices per side + m_nWidthSamples = nWidthSamples; + m_nDepthSamples = nDepthSamples; + + m_fSampleWidth = m_fWidth / ( m_nWidthSamples - REAL( 1.0 ) ); + m_fSampleDepth = m_fDepth / ( m_nDepthSamples - REAL( 1.0 ) ); + + m_fSampleZXAspect = m_fSampleDepth / m_fSampleWidth; + + m_fInvSampleWidth = REAL( 1.0 ) / m_fSampleWidth; + m_fInvSampleDepth = REAL( 1.0 ) / m_fSampleDepth; + + // finite or repeated terrain? + m_bWrapMode = bWrapMode; +} + + +// recomputes heights bounds +void dxHeightfieldData::ComputeHeightBounds() +{ + int i; + dReal h; + unsigned char *data_byte; + short *data_short; + float *data_float; + double *data_double; + + switch ( m_nGetHeightMode ) + { + + // callback + case 0: + // change nothing, keep using default or user specified bounds + return; + + // byte + case 1: + data_byte = (unsigned char*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++) + { + h = data_byte[i]; + if (h < m_fMinHeight) m_fMinHeight = h; + if (h > m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + // short + case 2: + data_short = (short*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++) + { + h = data_short[i]; + if (h < m_fMinHeight) m_fMinHeight = h; + if (h > m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + // float + case 3: + data_float = (float*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++) + { + h = data_float[i]; + if (h < m_fMinHeight) m_fMinHeight = h; + if (h > m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + // double + case 4: + data_double = (double*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++) + { + h = static_cast< dReal >( data_double[i] ); + if (h < m_fMinHeight) m_fMinHeight = h; + if (h > m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + } + + // scale and offset + m_fMinHeight *= m_fScale; + m_fMaxHeight *= m_fScale; + m_fMinHeight += m_fOffset; + m_fMaxHeight += m_fOffset; + + // add thickness + m_fMinHeight -= m_fThickness; +} + + +// returns whether point is over terrain Cell triangle? +bool dxHeightfieldData::IsOnHeightfield2 ( const HeightFieldVertex * const CellCorner, + const dReal * const pos, const bool isABC) const +{ + // WARNING!!! + // This function must be written in the way to make sure that every point on + // XZ plane falls in one and only one triangle. Keep that in mind if you + // intend to change the code. + // Also remember about computational errors and possible mismatches in + // values if they are calculated differently in different places in the code. + // Currently both the implementation has been optimized and effects of + // computational errors have been eliminated. + + dReal MaxX, MinX; + dReal MaxZ, MinZ; + + if (isABC) + { + // point A + MinX = CellCorner->vertex[0]; + if (pos[0] < MinX) + return false; + + MaxX = (CellCorner->coords[0] + 1) * m_fSampleWidth; + if (pos[0] >= MaxX) + return false; + + MinZ = CellCorner->vertex[2]; + if (pos[2] < MinZ) + return false; + + MaxZ = (CellCorner->coords[1] + 1) * m_fSampleDepth; + if (pos[2] >= MaxZ) + return false; + + return (MaxZ - pos[2]) > (pos[0] - MinX) * m_fSampleZXAspect; + } + else + { + // point D + MaxX = CellCorner->vertex[0]; + if (pos[0] >= MaxX) + return false; + + MinX = (CellCorner->coords[0] - 1) * m_fSampleWidth; + if (pos[0] < MinX) + return false; + + MaxZ = CellCorner->vertex[2]; + if (pos[2] >= MaxZ) + return false; + + MinZ = (CellCorner->coords[1] - 1) * m_fSampleDepth; + if (pos[2] < MinZ) + return false; + + return (MaxZ - pos[2]) <= (pos[0] - MinX) * m_fSampleZXAspect; + } +} + + +// returns height at given sample coordinates +dReal dxHeightfieldData::GetHeight( int x, int z ) +{ + dReal h=0; + unsigned char *data_byte; + short *data_short; + float *data_float; + double *data_double; + + if ( m_bWrapMode == 0 ) + { + // Finite + if ( x < 0 ) x = 0; + if ( z < 0 ) z = 0; + if ( x > m_nWidthSamples - 1 ) x = m_nWidthSamples - 1; + if ( z > m_nDepthSamples - 1 ) z = m_nDepthSamples - 1; + } + else + { + // Infinite + x %= m_nWidthSamples - 1; + z %= m_nDepthSamples - 1; + if ( x < 0 ) x += m_nWidthSamples - 1; + if ( z < 0 ) z += m_nDepthSamples - 1; + } + + switch ( m_nGetHeightMode ) + { + + // callback (dReal) + case 0: + h = (*m_pGetHeightCallback)(m_pUserData, x, z); + break; + + // byte + case 1: + data_byte = (unsigned char*)m_pHeightData; + h = data_byte[x+(z * m_nWidthSamples)]; + break; + + // short + case 2: + data_short = (short*)m_pHeightData; + h = data_short[x+(z * m_nWidthSamples)]; + break; + + // float + case 3: + data_float = (float*)m_pHeightData; + h = data_float[x+(z * m_nWidthSamples)]; + break; + + // double + case 4: + data_double = (double*)m_pHeightData; + h = (dReal)( data_double[x+(z * m_nWidthSamples)] ); + break; + } + + return (h * m_fScale) + m_fOffset; +} + + +// returns height at given coordinates +dReal dxHeightfieldData::GetHeight( dReal x, dReal z ) +{ + dReal dnX = dFloor( x * m_fInvSampleWidth ); + dReal dnZ = dFloor( z * m_fInvSampleDepth ); + + dReal dx = ( x - ( dnX * m_fSampleWidth ) ) * m_fInvSampleWidth; + dReal dz = ( z - ( dnZ * m_fSampleDepth ) ) * m_fInvSampleDepth; + + int nX = int( dnX ); + int nZ = int( dnZ ); + + //dIASSERT( ( dx + dEpsilon >= 0.0f ) && ( dx - dEpsilon <= 1.0f ) ); + //dIASSERT( ( dz + dEpsilon >= 0.0f ) && ( dz - dEpsilon <= 1.0f ) ); + + dReal y, y0; + + if ( dx + dz <= REAL( 1.0 ) ) // Use <= comparison to prefer simpler branch + { + y0 = GetHeight( nX, nZ ); + + y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * dx + + ( GetHeight( nX, nZ + 1 ) - y0 ) * dz; + } + else + { + y0 = GetHeight( nX + 1, nZ + 1 ); + + y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * ( REAL(1.0) - dz ) + + ( GetHeight( nX, nZ + 1 ) - y0 ) * ( REAL(1.0) - dx ); + } + + return y; +} + + +// dxHeightfieldData destructor +dxHeightfieldData::~dxHeightfieldData() +{ + unsigned char *data_byte; + short *data_short; + float *data_float; + double *data_double; + + if ( m_bCopyHeightData ) + { + switch ( m_nGetHeightMode ) + { + + // callback + case 0: + // do nothing + break; + + // byte + case 1: + dIASSERT( m_pHeightData ); + data_byte = (unsigned char*)m_pHeightData; + delete [] data_byte; + break; + + // short + case 2: + dIASSERT( m_pHeightData ); + data_short = (short*)m_pHeightData; + delete [] data_short; + break; + + // float + case 3: + dIASSERT( m_pHeightData ); + data_float = (float*)m_pHeightData; + delete [] data_float; + break; + + // double + case 4: + dIASSERT( m_pHeightData ); + data_double = (double*)m_pHeightData; + delete [] data_double; + break; + + } + } +} + + +//////// dxHeightfield ///////////////////////////////////////////////////////////////// + + +// dxHeightfield constructor +dxHeightfield::dxHeightfield( dSpaceID space, + dHeightfieldDataID data, + int bPlaceable ) : + dxGeom( space, bPlaceable ), + tempPlaneBuffer(0), + tempPlaneInstances(0), + tempPlaneBufferSize(0), + tempTriangleBuffer(0), + tempTriangleBufferSize(0), + tempHeightBuffer(0), + tempHeightInstances(0), + tempHeightBufferSizeX(0), + tempHeightBufferSizeZ(0) +{ + type = dHeightfieldClass; + this->m_p_data = data; +} + + +// compute axis aligned bounding box +void dxHeightfield::computeAABB() +{ + const dxHeightfieldData *d = m_p_data; + + if ( d->m_bWrapMode == 0 ) + { + // Finite + if ( gflags & GEOM_PLACEABLE ) + { + dReal dx[6], dy[6], dz[6]; + + // Y-axis + if (d->m_fMinHeight != -dInfinity) + { + dy[0] = ( final_posr->R[ 1] * d->m_fMinHeight ); + dy[1] = ( final_posr->R[ 5] * d->m_fMinHeight ); + dy[2] = ( final_posr->R[ 9] * d->m_fMinHeight ); + } + else + { + // Multiplication is performed to obtain infinity of correct sign + dy[0] = ( final_posr->R[ 1] ? final_posr->R[ 1] * -dInfinity : REAL(0.0) ); + dy[1] = ( final_posr->R[ 5] ? final_posr->R[ 5] * -dInfinity : REAL(0.0) ); + dy[2] = ( final_posr->R[ 9] ? final_posr->R[ 9] * -dInfinity : REAL(0.0) ); + } + + if (d->m_fMaxHeight != dInfinity) + { + dy[3] = ( final_posr->R[ 1] * d->m_fMaxHeight ); + dy[4] = ( final_posr->R[ 5] * d->m_fMaxHeight ); + dy[5] = ( final_posr->R[ 9] * d->m_fMaxHeight ); + } + else + { + dy[3] = ( final_posr->R[ 1] ? final_posr->R[ 1] * dInfinity : REAL(0.0) ); + dy[4] = ( final_posr->R[ 5] ? final_posr->R[ 5] * dInfinity : REAL(0.0) ); + dy[5] = ( final_posr->R[ 9] ? final_posr->R[ 9] * dInfinity : REAL(0.0) ); + } + +#ifdef DHEIGHTFIELD_CORNER_ORIGIN + + // X-axis + dx[0] = 0; dx[3] = ( final_posr->R[ 0] * d->m_fWidth ); + dx[1] = 0; dx[4] = ( final_posr->R[ 4] * d->m_fWidth ); + dx[2] = 0; dx[5] = ( final_posr->R[ 8] * d->m_fWidth ); + + // Z-axis + dz[0] = 0; dz[3] = ( final_posr->R[ 2] * d->m_fDepth ); + dz[1] = 0; dz[4] = ( final_posr->R[ 6] * d->m_fDepth ); + dz[2] = 0; dz[5] = ( final_posr->R[10] * d->m_fDepth ); + +#else // DHEIGHTFIELD_CORNER_ORIGIN + + // X-axis + dx[0] = ( final_posr->R[ 0] * -d->m_fHalfWidth ); + dx[1] = ( final_posr->R[ 4] * -d->m_fHalfWidth ); + dx[2] = ( final_posr->R[ 8] * -d->m_fHalfWidth ); + dx[3] = ( final_posr->R[ 0] * d->m_fHalfWidth ); + dx[4] = ( final_posr->R[ 4] * d->m_fHalfWidth ); + dx[5] = ( final_posr->R[ 8] * d->m_fHalfWidth ); + + // Z-axis + dz[0] = ( final_posr->R[ 2] * -d->m_fHalfDepth ); + dz[1] = ( final_posr->R[ 6] * -d->m_fHalfDepth ); + dz[2] = ( final_posr->R[10] * -d->m_fHalfDepth ); + dz[3] = ( final_posr->R[ 2] * d->m_fHalfDepth ); + dz[4] = ( final_posr->R[ 6] * d->m_fHalfDepth ); + dz[5] = ( final_posr->R[10] * d->m_fHalfDepth ); + +#endif // DHEIGHTFIELD_CORNER_ORIGIN + + // X extents + aabb[0] = final_posr->pos[0] + + dMIN3( dMIN( dx[0], dx[3] ), dMIN( dy[0], dy[3] ), dMIN( dz[0], dz[3] ) ); + aabb[1] = final_posr->pos[0] + + dMAX3( dMAX( dx[0], dx[3] ), dMAX( dy[0], dy[3] ), dMAX( dz[0], dz[3] ) ); + + // Y extents + aabb[2] = final_posr->pos[1] + + dMIN3( dMIN( dx[1], dx[4] ), dMIN( dy[1], dy[4] ), dMIN( dz[1], dz[4] ) ); + aabb[3] = final_posr->pos[1] + + dMAX3( dMAX( dx[1], dx[4] ), dMAX( dy[1], dy[4] ), dMAX( dz[1], dz[4] ) ); + + // Z extents + aabb[4] = final_posr->pos[2] + + dMIN3( dMIN( dx[2], dx[5] ), dMIN( dy[2], dy[5] ), dMIN( dz[2], dz[5] ) ); + aabb[5] = final_posr->pos[2] + + dMAX3( dMAX( dx[2], dx[5] ), dMAX( dy[2], dy[5] ), dMAX( dz[2], dz[5] ) ); + } + else + { + +#ifdef DHEIGHTFIELD_CORNER_ORIGIN + + aabb[0] = 0; aabb[1] = d->m_fWidth; + aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight; + aabb[4] = 0; aabb[5] = d->m_fDepth; + +#else // DHEIGHTFIELD_CORNER_ORIGIN + + aabb[0] = -d->m_fHalfWidth; aabb[1] = +d->m_fHalfWidth; + aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight; + aabb[4] = -d->m_fHalfDepth; aabb[5] = +d->m_fHalfDepth; + +#endif // DHEIGHTFIELD_CORNER_ORIGIN + + } + } + else + { + // Infinite + if ( gflags & GEOM_PLACEABLE ) + { + aabb[0] = -dInfinity; aabb[1] = +dInfinity; + aabb[2] = -dInfinity; aabb[3] = +dInfinity; + aabb[4] = -dInfinity; aabb[5] = +dInfinity; + } + else + { + aabb[0] = -dInfinity; aabb[1] = +dInfinity; + aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight; + aabb[4] = -dInfinity; aabb[5] = +dInfinity; + } + } + +} + + +// dxHeightfield destructor +dxHeightfield::~dxHeightfield() +{ + resetTriangleBuffer(); + resetPlaneBuffer(); + resetHeightBuffer(); +} + +void dxHeightfield::allocateTriangleBuffer(sizeint numTri) +{ + sizeint alignedNumTri = AlignBufferSize(numTri, TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT); + tempTriangleBufferSize = alignedNumTri; + tempTriangleBuffer = new HeightFieldTriangle[alignedNumTri]; +} + +void dxHeightfield::resetTriangleBuffer() +{ + delete[] tempTriangleBuffer; +} + +void dxHeightfield::allocatePlaneBuffer(sizeint numTri) +{ + sizeint alignedNumTri = AlignBufferSize(numTri, TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT); + tempPlaneBufferSize = alignedNumTri; + tempPlaneBuffer = new HeightFieldPlane *[alignedNumTri]; + tempPlaneInstances = new HeightFieldPlane[alignedNumTri]; + + HeightFieldPlane *ptrPlaneMatrix = tempPlaneInstances; + for (sizeint indexTri = 0; indexTri != alignedNumTri; indexTri++) + { + tempPlaneBuffer[indexTri] = ptrPlaneMatrix; + ptrPlaneMatrix += 1; + } +} + +void dxHeightfield::resetPlaneBuffer() +{ + delete[] tempPlaneInstances; + delete[] tempPlaneBuffer; +} + +void dxHeightfield::allocateHeightBuffer(sizeint numX, sizeint numZ) +{ + sizeint alignedNumX = AlignBufferSize(numX, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X); + sizeint alignedNumZ = AlignBufferSize(numZ, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z); + tempHeightBufferSizeX = alignedNumX; + tempHeightBufferSizeZ = alignedNumZ; + tempHeightBuffer = new HeightFieldVertex *[alignedNumX]; + sizeint numCells = alignedNumX * alignedNumZ; + tempHeightInstances = new HeightFieldVertex [numCells]; + + HeightFieldVertex *ptrHeightMatrix = tempHeightInstances; + for (sizeint indexX = 0; indexX != alignedNumX; indexX++) + { + tempHeightBuffer[indexX] = ptrHeightMatrix; + ptrHeightMatrix += alignedNumZ; + } +} + +void dxHeightfield::resetHeightBuffer() +{ + delete[] tempHeightInstances; + delete[] tempHeightBuffer; +} +//////// Heightfield data interface //////////////////////////////////////////////////// + + +dHeightfieldDataID dGeomHeightfieldDataCreate() +{ + return new dxHeightfieldData(); +} + + +void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d, + void* pUserData, dHeightfieldGetHeight* pCallback, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "argument not Heightfield data" ); + dIASSERT( pCallback ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // callback + d->m_nGetHeightMode = 0; + d->m_pUserData = pUserData; + d->m_pGetHeightCallback = pCallback; + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + + // default bounds + d->m_fMinHeight = -dInfinity; + d->m_fMaxHeight = dInfinity; +} + + +void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d, + const unsigned char *pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 1; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new unsigned char[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( unsigned char ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + + +void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d, + const short* pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 2; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new short[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( short ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + + +void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d, + const float *pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 3; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new float[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( float ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + +void dGeomHeightfieldDataBuildDouble( dHeightfieldDataID d, + const double *pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 4; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new double[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( double ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + + + + +void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight ) +{ + dUASSERT(d, "Argument not Heightfield data"); + d->m_fMinHeight = ( minHeight * d->m_fScale ) + d->m_fOffset - d->m_fThickness; + d->m_fMaxHeight = ( maxHeight * d->m_fScale ) + d->m_fOffset; +} + + +void dGeomHeightfieldDataDestroy( dHeightfieldDataID d ) +{ + dUASSERT(d, "argument not Heightfield data"); + delete d; +} + + +//////// Heightfield geom interface //////////////////////////////////////////////////// + + +dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable ) +{ + return new dxHeightfield( space, data, bPlaceable ); +} + + +void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d ) +{ + dxHeightfield* geom = (dxHeightfield*) g; + geom->m_p_data = d; +} + + +dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g ) +{ + dxHeightfield* geom = (dxHeightfield*) g; + return geom->m_p_data; +} + +//////// dxHeightfield ///////////////////////////////////////////////////////////////// + + +// Typedef for generic 'get point depth' function +typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z ); + + +#define DMESS(A) \ + dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \ + x,z,(A), \ + pContact->depth, \ + dGeomSphereGetRadius(o2), \ + pContact->pos[0], \ + pContact->pos[1], \ + pContact->pos[2], \ + pContact->normal[0], \ + pContact->normal[1], \ + pContact->normal[2]); + +static inline bool DescendingTriangleSort(const HeightFieldTriangle * const A, const HeightFieldTriangle * const B) +{ + return ((A->maxAAAB - B->maxAAAB) > dEpsilon); +} +static inline bool DescendingPlaneSort(const HeightFieldPlane * const A, const HeightFieldPlane * const B) +{ + return ((A->maxAAAB - B->maxAAAB) > dEpsilon); +} + +void dxHeightfield::sortPlanes(const sizeint numPlanes) +{ + bool has_swapped = true; + do + { + has_swapped = false;//reset flag + for (sizeint i = 0; i < numPlanes - 1; i++) + { + //if they are in the wrong order + if (DescendingPlaneSort(tempPlaneBuffer[i], tempPlaneBuffer[i + 1])) + { + //exchange them + HeightFieldPlane * tempPlane = tempPlaneBuffer[i]; + tempPlaneBuffer[i] = tempPlaneBuffer[i + 1]; + tempPlaneBuffer[i + 1] = tempPlane; + + //we have swapped at least once, list may not be sorted yet + has_swapped = true; + } + } + } //if no swaps were made during this pass, the list has been sorted + while (has_swapped); +} + +static inline dReal DistancePointToLine(const dVector3 &_point, + const dVector3 &_pt0, + const dVector3 &_Edge, + const dReal _Edgelength) +{ + dVector3 v; + dVector3Subtract(_point, _pt0, v); + dVector3 s; + dVector3Copy (_Edge, s); + const dReal dot = dVector3Dot(v, _Edge) / _Edgelength; + dVector3Scale(s, dot); + dVector3Subtract(v, s, v); + return dVector3Length(v); +} + + + + +int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ, + dxGeom* o2, const int numMaxContactsPossible, + int flags, dContactGeom* contact, + int skip ) +{ + dContactGeom *pContact = 0; + int x, z; + // check if not above or inside terrain first + // while filling a heightmap partial temporary buffer + const unsigned int numX = (maxX - minX) + 1; + const unsigned int numZ = (maxZ - minZ) + 1; + const dReal minO2Height = o2->aabb[2]; + const dReal maxO2Height = o2->aabb[3]; + unsigned int x_local, z_local; + dReal maxY = - dInfinity; + dReal minY = dInfinity; + // localize and const for faster access + const dReal cfSampleWidth = m_p_data->m_fSampleWidth; + const dReal cfSampleDepth = m_p_data->m_fSampleDepth; + { + if (tempHeightBufferSizeX < numX || tempHeightBufferSizeZ < numZ) + { + resetHeightBuffer(); + allocateHeightBuffer(numX, numZ); + } + + dReal Xpos, Ypos; + + for ( x = minX, x_local = 0; x_local < numX; x++, x_local++) + { + Xpos = x * cfSampleWidth; // Always calculate pos via multiplication to avoid computational error accumulation during multiple additions + + const dReal c_Xpos = Xpos; + HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local]; + for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++) + { + Ypos = z * cfSampleDepth; // Always calculate pos via multiplication to avoid computational error accumulation during multiple additions + + const dReal h = m_p_data->GetHeight(x, z); + HeightFieldRow[z_local].vertex[0] = c_Xpos; + HeightFieldRow[z_local].vertex[1] = h; + HeightFieldRow[z_local].vertex[2] = Ypos; + HeightFieldRow[z_local].coords[0] = x; + HeightFieldRow[z_local].coords[1] = z; + + maxY = dMAX(maxY, h); + minY = dMIN(minY, h); + } + } + if (minO2Height - maxY > -dEpsilon ) + { + //totally above heightfield + return 0; + } + if (minY - maxO2Height > -dEpsilon ) + { + // totally under heightfield + pContact = CONTACT(contact, 0); + + pContact->pos[0] = o2->final_posr->pos[0]; + pContact->pos[1] = minY; + pContact->pos[2] = o2->final_posr->pos[2]; + + pContact->normal[0] = 0; + pContact->normal[1] = - 1; + pContact->normal[2] = 0; + + pContact->depth = minY - maxO2Height; + + pContact->side1 = -1; + pContact->side2 = -1; + + return 1; + } + } + // get All Planes that could collide against. + dColliderFn *geomRayNCollider=0; + dColliderFn *geomNPlaneCollider=0; + dGetDepthFn *geomNDepthGetter=0; + + // int max_collisionContact = numMaxContactsPossible; -- not used + switch (o2->type) + { + case dRayClass: + geomRayNCollider = NULL; + geomNPlaneCollider = dCollideRayPlane; + geomNDepthGetter = NULL; + //max_collisionContact = 1; + break; + + case dSphereClass: + geomRayNCollider = dCollideRaySphere; + geomNPlaneCollider = dCollideSpherePlane; + geomNDepthGetter = dGeomSpherePointDepth; + //max_collisionContact = 3; + break; + + case dBoxClass: + geomRayNCollider = dCollideRayBox; + geomNPlaneCollider = dCollideBoxPlane; + geomNDepthGetter = dGeomBoxPointDepth; + //max_collisionContact = 8; + break; + + case dCapsuleClass: + geomRayNCollider = dCollideRayCapsule; + geomNPlaneCollider = dCollideCapsulePlane; + geomNDepthGetter = dGeomCapsulePointDepth; + // max_collisionContact = 3; + break; + + case dCylinderClass: + geomRayNCollider = dCollideRayCylinder; + geomNPlaneCollider = dCollideCylinderPlane; + geomNDepthGetter = NULL;// TODO: dGeomCCylinderPointDepth + //max_collisionContact = 3; + break; + + case dConvexClass: + geomRayNCollider = dCollideRayConvex; + geomNPlaneCollider = dCollideConvexPlane; + geomNDepthGetter = NULL;// TODO: dGeomConvexPointDepth; + //max_collisionContact = 3; + break; + +#if dTRIMESH_ENABLED + + case dTriMeshClass: + geomRayNCollider = dCollideRayTrimesh; + geomNPlaneCollider = dCollideTrimeshPlane; + geomNDepthGetter = NULL;// TODO: dGeomTrimeshPointDepth; + //max_collisionContact = 3; + break; + +#endif // dTRIMESH_ENABLED + + default: + dIASSERT(0); // Shouldn't ever get here. + break; + + } + + dxPlane myplane(0,0,0,0,0); + dxPlane* sliding_plane = &myplane; + dReal triplane[4]; + int i; + + // check some trivial case. + // Vector Up plane + if (maxY - minY < dEpsilon) + { + // it's a single plane. + triplane[0] = 0; + triplane[1] = 1; + triplane[2] = 0; + triplane[3] = minY; + dGeomPlaneSetNoNormalize (sliding_plane, triplane); + // find collision and compute contact points + const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip); + dIASSERT(numTerrainContacts <= numMaxContactsPossible); + for (i = 0; i < numTerrainContacts; i++) + { + pContact = CONTACT(contact, i*skip); + dOPESIGN(pContact->normal, =, -, triplane); + } + return numTerrainContacts; + } + + /* -- This block is invalid as per Martijn Buijs <buijs512@planet.nl> + + The problem seems to be based on the erroneously assumption that if two of + the four vertices of a 'grid' are at the same height, the entire grid can be + represented as a single plane. It works for an axis aligned slope, but fails + on all 4 grids of a 3x3 spike feature. Since the plane normal is constructed + from only 3 vertices (only one of the two triangles) this often results in + discontinuities at the grid edges (causing small jumps when the contact + point moves from one grid to another). + + // unique plane + { + // check for very simple plane heightfield + dReal minXHeightDelta = dInfinity, maxXHeightDelta = - dInfinity; + dReal minZHeightDelta = dInfinity, maxZHeightDelta = - dInfinity; + + + dReal lastXHeight = tempHeightBuffer[0][0].vertex[1]; + for ( x_local = 1; x_local < numX; x_local++) + { + HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local]; + + const dReal deltaX = HeightFieldRow[0].vertex[1] - lastXHeight; + + maxXHeightDelta = dMAX (maxXHeightDelta, deltaX); + minXHeightDelta = dMIN (minXHeightDelta, deltaX); + + dReal lastZHeight = HeightFieldRow[0].vertex[1]; + for ( z_local = 1; z_local < numZ; z_local++) + { + const dReal deltaZ = (HeightFieldRow[z_local].vertex[1] - lastZHeight); + + maxZHeightDelta = dMAX (maxZHeightDelta, deltaZ); + minZHeightDelta = dMIN (minZHeightDelta, deltaZ); + + } + } + + if (maxZHeightDelta - minZHeightDelta < dEpsilon && + maxXHeightDelta - minXHeightDelta < dEpsilon ) + { + // it's a single plane. + const dVector3 &A = tempHeightBuffer[0][0].vertex; + const dVector3 &B = tempHeightBuffer[1][0].vertex; + const dVector3 &C = tempHeightBuffer[0][1].vertex; + + // define 2 edges and a point that will define collision plane + { + dVector3 Edge1, Edge2; + dVector3Subtract(C, A, Edge1); + dVector3Subtract(B, A, Edge2); + dVector3Cross(Edge1, Edge2, triplane); + } + + // Define Plane + // Normalize plane normal + const dReal dinvlength = REAL(1.0) / dVector3Length(triplane); + triplane[0] *= dinvlength; + triplane[1] *= dinvlength; + triplane[2] *= dinvlength; + // get distance to origin from plane + triplane[3] = dVector3Dot(triplane, A); + + dGeomPlaneSetNoNormalize (sliding_plane, triplane); + // find collision and compute contact points + const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip); + dIASSERT(numTerrainContacts <= numMaxContactsPossible); + for (i = 0; i < numTerrainContacts; i++) + { + pContact = CONTACT(contact, i*skip); + dOPESIGN(pContact->normal, =, -, triplane); + } + return numTerrainContacts; + } + } + */ + + int numTerrainContacts = 0; + dContactGeom *PlaneContact = m_p_data->m_contacts; + + const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2; + if (tempTriangleBufferSize < numTriMax) + { + resetTriangleBuffer(); + allocateTriangleBuffer(numTriMax); + } + + // Sorting triangle/plane resulting from heightfield zone + // Perhaps that would be necessary in case of too much limited + // maximum contact point... + // or in complex mesh case (trimesh and convex) + // need some test or insights on this before enabling this. + const bool isContactNumPointsLimited = + true; + // (numMaxContacts < 8) + // || o2->type == dConvexClass + // || o2->type == dTriMeshClass + // || (numMaxContacts < (int)numTriMax) + + + + // if small heightfield triangle related to O2 colliding + // or no Triangle colliding at all. + bool needFurtherPasses = (o2->type == dTriMeshClass); + //compute Ratio between Triangle size and O2 aabb size + // no FurtherPasses are needed in ray class + if (o2->type != dRayClass && needFurtherPasses == false) + { + const dReal xratio = (o2->aabb[1] - o2->aabb[0]) * m_p_data->m_fInvSampleWidth; + if (xratio > REAL(1.5)) + needFurtherPasses = true; + else + { + const dReal zratio = (o2->aabb[5] - o2->aabb[4]) * m_p_data->m_fInvSampleDepth; + if (zratio > REAL(1.5)) + needFurtherPasses = true; + } + + } + + unsigned int numTri = 0; + HeightFieldVertex *A, *B, *C, *D; + /* (y is up) + A--------B-...x + | /| + | / | + | / | + | / | + | / | + | / | + | / | + |/ | + C--------D + . + . + . + z + */ + // keep only triangle that does intersect geom + + const unsigned int maxX_local = maxX - minX; + const unsigned int maxZ_local = maxZ - minZ; + + for ( x_local = 0; x_local < maxX_local; x_local++) + { + HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local]; + HeightFieldVertex *HeightFieldNextRow = tempHeightBuffer[x_local + 1]; + + // First A + C = &HeightFieldRow [0]; + // First B + D = &HeightFieldNextRow[0]; + + for ( z_local = 0; z_local < maxZ_local; z_local++) + { + A = C; + B = D; + + C = &HeightFieldRow [z_local + 1]; + D = &HeightFieldNextRow[z_local + 1]; + + const dReal AHeight = A->vertex[1]; + const dReal BHeight = B->vertex[1]; + const dReal CHeight = C->vertex[1]; + const dReal DHeight = D->vertex[1]; + + const bool isACollide = AHeight > minO2Height; + const bool isBCollide = BHeight > minO2Height; + const bool isCCollide = CHeight > minO2Height; + const bool isDCollide = DHeight > minO2Height; + + A->state = !(isACollide); + B->state = !(isBCollide); + C->state = !(isCCollide); + D->state = !(isDCollide); + + if (isACollide || isBCollide || isCCollide) + { + HeightFieldTriangle * const CurrTriUp = &tempTriangleBuffer[numTri++]; + + CurrTriUp->state = false; + + // changing point order here implies to change it in isOnHeightField + CurrTriUp->vertices[0] = A; + CurrTriUp->vertices[1] = B; + CurrTriUp->vertices[2] = C; + + if (isContactNumPointsLimited) + CurrTriUp->setMinMax(); + CurrTriUp->isUp = true; + } + + if (isBCollide || isCCollide || isDCollide) + { + HeightFieldTriangle * const CurrTriDown = &tempTriangleBuffer[numTri++]; + + CurrTriDown->state = false; + // changing point order here implies to change it in isOnHeightField + + CurrTriDown->vertices[0] = D; + CurrTriDown->vertices[1] = B; + CurrTriDown->vertices[2] = C; + + + if (isContactNumPointsLimited) + CurrTriDown->setMinMax(); + CurrTriDown->isUp = false; + } + + + if (needFurtherPasses && + (isBCollide || isCCollide) + && + (AHeight > CHeight && + AHeight > BHeight && + DHeight > CHeight && + DHeight > BHeight)) + { + // That means Edge BC is concave, therefore + // BC Edge and B and C vertices cannot collide + + B->state = true; + C->state = true; + } + // should find a way to check other edges (AB, BD, CD) too for concavity + } + } + + // at least on triangle should intersect geom + dIASSERT (numTri != 0); + // pass1: VS triangle as Planes + // Group Triangle by same plane definition + // as Terrain often has many triangles using same plane definition + // then collide against that list of triangles. + { + + dVector3 Edge1, Edge2; + //compute all triangles normals. + for (unsigned int k = 0; k < numTri; k++) + { + HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k]; + + // define 2 edges and a point that will define collision plane + dVector3Subtract(itTriangle->vertices[2]->vertex, itTriangle->vertices[0]->vertex, Edge1); + dVector3Subtract(itTriangle->vertices[1]->vertex, itTriangle->vertices[0]->vertex, Edge2); + + // find a perpendicular vector to the triangle + if (itTriangle->isUp) + dVector3Cross(Edge1, Edge2, triplane); + else + dVector3Cross(Edge2, Edge1, triplane); + + // Define Plane + // Normalize plane normal + const dReal dinvlength = REAL(1.0) / dVector3Length(triplane); + triplane[0] *= dinvlength; + triplane[1] *= dinvlength; + triplane[2] *= dinvlength; + // get distance to origin from plane + triplane[3] = dVector3Dot(triplane, itTriangle->vertices[0]->vertex); + + // saves normal for collision check (planes, triangles, vertices and edges.) + dVector3Copy(triplane, itTriangle->planeDef); + // saves distance for collision check (planes, triangles, vertices and edges.) + itTriangle->planeDef[3] = triplane[3]; + } + + // group by Triangles by Planes sharing shame plane definition + if (tempPlaneBufferSize < numTri) + { + resetPlaneBuffer(); + allocatePlaneBuffer(numTri); + } + + unsigned int numPlanes = 0; + for (unsigned int k = 0; k < numTri; k++) + { + HeightFieldTriangle * const tri_base = &tempTriangleBuffer[k]; + + if (tri_base->state == true) + continue;// already tested or added to plane list. + + HeightFieldPlane * const currPlane = tempPlaneBuffer[numPlanes]; + currPlane->resetTriangleListSize(numTri - k); + currPlane->addTriangle(tri_base); + // saves normal for collision check (planes, triangles, vertices and edges.) + dVector3Copy(tri_base->planeDef, currPlane->planeDef); + // saves distance for collision check (planes, triangles, vertices and edges.) + currPlane->planeDef[3]= tri_base->planeDef[3]; + + const dReal normx = tri_base->planeDef[0]; + const dReal normy = tri_base->planeDef[1]; + const dReal normz = tri_base->planeDef[2]; + const dReal dist = tri_base->planeDef[3]; + + for (unsigned int m = k + 1; m < numTri; m++) + { + + HeightFieldTriangle * const tri_test = &tempTriangleBuffer[m]; + if (tri_test->state == true) + continue;// already tested or added to plane list. + + // normals and distance are the same. + if ( + dFabs(normy - tri_test->planeDef[1]) < dEpsilon && + dFabs(dist - tri_test->planeDef[3]) < dEpsilon && + dFabs(normx - tri_test->planeDef[0]) < dEpsilon && + dFabs(normz - tri_test->planeDef[2]) < dEpsilon + ) + { + currPlane->addTriangle (tri_test); + tri_test->state = true; + } + } + + tri_base->state = true; + if (isContactNumPointsLimited) + currPlane->setMinMax(); + + numPlanes++; + } + + // sort planes + if (isContactNumPointsLimited) + sortPlanes(numPlanes); + +#if !defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) + /* + Note by Oleh_Derevenko: + It seems to be incorrect to limit contact count by some particular value + since some of them (and even all of them) may be culled in following condition. + However I do not see an easy way to fix this. + If not that culling the flags modification should be changed here and + additionally repeated after some contacts have been generated (in "if (didCollide)"). + The maximum of contacts in flags would then be set to minimum of contacts + remaining and HEIGHTFIELDMAXCONTACTPERCELL. + */ + int planeTestFlags = (flags & ~NUMC_MASK) | HEIGHTFIELDMAXCONTACTPERCELL; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); +#else // if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) + int numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + int planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); +#endif + + for (unsigned int k = 0; k < numPlanes; k++) + { + HeightFieldPlane * const itPlane = tempPlaneBuffer[k]; + + //set Geom + dGeomPlaneSetNoNormalize (sliding_plane, itPlane->planeDef); + //dGeomPlaneSetParams (sliding_plane, triangle_Plane[0], triangle_Plane[1], triangle_Plane[2], triangle_Plane[3]); + // find collision and compute contact points + bool didCollide = false; + const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, planeTestFlags, PlaneContact, sizeof(dContactGeom)); + const sizeint planeTriListSize = itPlane->trianglelistCurrentSize; + for (i = 0; i < numPlaneContacts; i++) + { + dContactGeom *planeCurrContact = PlaneContact + i; + // Check if contact point found in plane is inside Triangle. + const dVector3 &pCPos = planeCurrContact->pos; + for (sizeint b = 0; planeTriListSize > b; b++) + { + if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0], + pCPos, + itPlane->trianglelist[b]->isUp)) + { + pContact = CONTACT(contact, numTerrainContacts*skip); + dVector3Copy(pCPos, pContact->pos); + dOPESIGN(pContact->normal, =, -, itPlane->planeDef); + pContact->depth = planeCurrContact->depth; + pContact->side1 = planeCurrContact->side1; + pContact->side2 = planeCurrContact->side2; + numTerrainContacts++; + if ( numTerrainContacts == numMaxContactsPossible ) + return numTerrainContacts; + + didCollide = true; + break; + } + } + } + if (didCollide) + { +#if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) + /* Note by Oleh_Derevenko: + This code is not used - see another note above + */ + numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); +#endif + for (sizeint b = 0; planeTriListSize > b; b++) + { + // flag Triangles Vertices as collided + // to prevent any collision test of those + for (i = 0; i < 3; i++) + itPlane->trianglelist[b]->vertices[i]->state = true; + } + } + else + { + // flag triangle as not collided so that Vertices or Edge + // of that triangles will be checked. + for (sizeint b = 0; planeTriListSize > b; b++) + { + itPlane->trianglelist[b]->state = false; + } + } + } + } + + + + // pass2: VS triangle vertices + if (needFurtherPasses) + { + dxRay tempRay(0, 1); + dReal depth; + bool vertexCollided; + + // Only one contact is necessary for ray test + int rayTestFlags = (flags & ~NUMC_MASK) | 1; + dIASSERT((1 & ~NUMC_MASK) == 0); + // + // Find Contact Penetration Depth of each vertices + // + for (unsigned int k = 0; k < numTri; k++) + { + const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k]; + if (itTriangle->state == true) + continue;// plane triangle did already collide. + + for (sizeint i = 0; i < 3; i++) + { + HeightFieldVertex *vertex = itTriangle->vertices[i]; + if (vertex->state == true) + continue;// vertice did already collide. + + vertexCollided = false; + const dVector3 &triVertex = vertex->vertex; + if ( geomNDepthGetter ) + { + depth = geomNDepthGetter( o2, + triVertex[0], triVertex[1], triVertex[2] ); + if (depth > dEpsilon) + vertexCollided = true; + } + else + { + // We don't have a GetDepth function, so do a ray cast instead. + // NOTE: This isn't ideal, and a GetDepth function should be + // written for all geom classes. + tempRay.length = (minO2Height - triVertex[1]) * REAL(1000.0); + + //dGeomRaySet( &tempRay, pContact->pos[0], pContact->pos[1], pContact->pos[2], + // - itTriangle->Normal[0], - itTriangle->Normal[1], - itTriangle->Normal[2] ); + dGeomRaySetNoNormalize(tempRay, triVertex, itTriangle->planeDef); + + if ( geomRayNCollider( &tempRay, o2, rayTestFlags, PlaneContact, sizeof( dContactGeom ) ) ) + { + depth = PlaneContact[0].depth; + vertexCollided = true; + } + } + if (vertexCollided) + { + pContact = CONTACT(contact, numTerrainContacts*skip); + //create contact using vertices + dVector3Copy (triVertex, pContact->pos); + //create contact using Plane Normal + dOPESIGN(pContact->normal, =, -, itTriangle->planeDef); + + pContact->depth = depth; + pContact->side1 = -1; + pContact->side2 = -1; + + numTerrainContacts++; + if ( numTerrainContacts == numMaxContactsPossible ) + return numTerrainContacts; + + vertex->state = true; + } + } + } + } + +#ifdef _HEIGHTFIELDEDGECOLLIDING + // pass3: VS triangle Edges + if (needFurtherPasses) + { + dVector3 Edge; + dxRay edgeRay(0, 1); + + int numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + int triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); + + for (unsigned int k = 0; k < numTri; k++) + { + const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k]; + + if (itTriangle->state == true) + continue;// plane did already collide. + + for (sizeint m = 0; m < 3; m++) + { + const sizeint next = (m + 1) % 3; + HeightFieldVertex *vertex0 = itTriangle->vertices[m]; + HeightFieldVertex *vertex1 = itTriangle->vertices[next]; + + // not concave or under the AABB + // nor triangle already collided against vertices + if (vertex0->state == true && vertex1->state == true) + continue;// plane did already collide. + + dVector3Subtract(vertex1->vertex, vertex0->vertex, Edge); + edgeRay.length = dVector3Length (Edge); + dGeomRaySetNoNormalize(edgeRay, vertex1->vertex, Edge); + int prevTerrainContacts = numTerrainContacts; + pContact = CONTACT(contact, prevTerrainContacts*skip); + const int numCollision = geomRayNCollider(&edgeRay,o2,triTestFlags,pContact,skip); + dIASSERT(numCollision <= numMaxContactsPerTri); + + if (numCollision) + { + numTerrainContacts += numCollision; + + do + { + pContact = CONTACT(contact, prevTerrainContacts*skip); + + //create contact using Plane Normal + dOPESIGN(pContact->normal, =, -, itTriangle->planeDef); + + pContact->depth = DistancePointToLine(pContact->pos, vertex1->vertex, Edge, edgeRay.length); + } + while (++prevTerrainContacts != numTerrainContacts); + + if ( numTerrainContacts == numMaxContactsPossible ) + return numTerrainContacts; + + numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); + } + } + + itTriangle->vertices[0]->state = true; + itTriangle->vertices[1]->state = true; + itTriangle->vertices[2]->state = true; + } + } +#endif // _HEIGHTFIELDEDGECOLLIDING + return numTerrainContacts; +} + +int dCollideHeightfield( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contact, int skip ) +{ + dIASSERT( skip >= (int)sizeof(dContactGeom) ); + dIASSERT( o1->type == dHeightfieldClass ); + dIASSERT((flags & NUMC_MASK) >= 1); + + int i; + + // if ((flags & NUMC_MASK) == 0) -- An assertion check is made on entry + // { flags = (flags & ~NUMC_MASK) | 1; dIASSERT((1 & ~NUMC_MASK) == 0); } + + int numMaxTerrainContacts = (flags & NUMC_MASK); + + dxHeightfield *terrain = (dxHeightfield*) o1; + + dVector3 posbak; + dMatrix3 Rbak; + dReal aabbbak[6]; + int gflagsbak; + dVector3 pos0,pos1; + dMatrix3 R1; + + int numTerrainContacts = 0; + int numTerrainOrigContacts = 0; + + //@@ Should find a way to set reComputeAABB to false in default case + // aka DHEIGHTFIELD_CORNER_ORIGIN not defined and terrain not PLACEABLE + // so that we can free some memory and speed up things a bit + // while saving some precision loss +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + const bool reComputeAABB = true; +#else + const bool reComputeAABB = ( terrain->gflags & GEOM_PLACEABLE ) ? true : false; +#endif //DHEIGHTFIELD_CORNER_ORIGIN + + // + // Transform O2 into Heightfield Space + // + if (reComputeAABB) + { + // Backup original o2 position, rotation and AABB. + dVector3Copy( o2->final_posr->pos, posbak ); + dMatrix3Copy( o2->final_posr->R, Rbak ); + memcpy( aabbbak, o2->aabb, sizeof( dReal ) * 6 ); + gflagsbak = o2->gflags; + } + + if ( terrain->gflags & GEOM_PLACEABLE ) + { + // Transform o2 into heightfield space. + dSubtractVectors3( pos0, o2->final_posr->pos, terrain->final_posr->pos ); + dMultiply1_331( pos1, terrain->final_posr->R, pos0 ); + dMultiply1_333( R1, terrain->final_posr->R, o2->final_posr->R ); + + // Update o2 with transformed position and rotation. + dVector3Copy( pos1, o2->final_posr->pos ); + dMatrix3Copy( R1, o2->final_posr->R ); + } + +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + o2->final_posr->pos[ 0 ] += terrain->m_p_data->m_fHalfWidth; + o2->final_posr->pos[ 2 ] += terrain->m_p_data->m_fHalfDepth; +#endif // DHEIGHTFIELD_CORNER_ORIGIN + + // Rebuild AABB for O2 + if (reComputeAABB) + o2->computeAABB(); + + // + // Collide + // + + //check if inside boundaries + // using O2 aabb + // aabb[6] is (minx, maxx, miny, maxy, minz, maxz) + const bool wrapped = terrain->m_p_data->m_bWrapMode != 0; + + if ( !wrapped ) + { + if ( o2->aabb[0] > terrain->m_p_data->m_fWidth //MinX + || o2->aabb[4] > terrain->m_p_data->m_fDepth)//MinZ + goto dCollideHeightfieldExit; + + if ( o2->aabb[1] < 0 //MaxX + || o2->aabb[5] < 0)//MaxZ + goto dCollideHeightfieldExit; + } + + { // To narrow scope of following variables + const dReal fInvSampleWidth = terrain->m_p_data->m_fInvSampleWidth; + int nMinX = (int)dFloor(dNextAfter(o2->aabb[0] * fInvSampleWidth, -dInfinity)); + int nMaxX = (int)dCeil(dNextAfter(o2->aabb[1] * fInvSampleWidth, dInfinity)); + const dReal fInvSampleDepth = terrain->m_p_data->m_fInvSampleDepth; + int nMinZ = (int)dFloor(dNextAfter(o2->aabb[4] * fInvSampleDepth, -dInfinity)); + int nMaxZ = (int)dCeil(dNextAfter(o2->aabb[5] * fInvSampleDepth, dInfinity)); + + if ( !wrapped ) + { + nMinX = dMAX( nMinX, 0 ); + nMaxX = dMIN( nMaxX, terrain->m_p_data->m_nWidthSamples - 1 ); + nMinZ = dMAX( nMinZ, 0 ); + nMaxZ = dMIN( nMaxZ, terrain->m_p_data->m_nDepthSamples - 1 ); + + dIASSERT ((nMinX < nMaxX) && (nMinZ < nMaxZ)); + } + + numTerrainOrigContacts = numTerrainContacts; + numTerrainContacts += terrain->dCollideHeightfieldZone( + nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts, + flags,CONTACT(contact,numTerrainContacts*skip),skip ); + dIASSERT( numTerrainContacts <= numMaxTerrainContacts ); + } + + dContactGeom *pContact; + for ( i = numTerrainOrigContacts; i != numTerrainContacts; ++i ) + { + pContact = CONTACT(contact,i*skip); + pContact->g1 = o1; + pContact->g2 = o2; + // pContact->side1 = -1; -- Oleh_Derevenko: sides must not be erased here as they are set by respective colliders during ray/plane tests + // pContact->side2 = -1; + } + + + //------------------------------------------------------------------------------ + +dCollideHeightfieldExit: + + if (reComputeAABB) + { + // Restore o2 position, rotation and AABB + dVector3Copy( posbak, o2->final_posr->pos ); + dMatrix3Copy( Rbak, o2->final_posr->R ); + memcpy( o2->aabb, aabbbak, sizeof(dReal)*6 ); + o2->gflags = gflagsbak; + + // + // Transform Contacts to World Space + // + if ( terrain->gflags & GEOM_PLACEABLE ) + { + for ( i = 0; i < numTerrainContacts; ++i ) + { + pContact = CONTACT(contact,i*skip); + dCopyVector3( pos0, pContact->pos ); + +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + pos0[ 0 ] -= terrain->m_p_data->m_fHalfWidth; + pos0[ 2 ] -= terrain->m_p_data->m_fHalfDepth; +#endif // !DHEIGHTFIELD_CORNER_ORIGIN + + dMultiply0_331( pContact->pos, terrain->final_posr->R, pos0 ); + + dAddVectors3( pContact->pos, pContact->pos, terrain->final_posr->pos ); + dCopyVector3( pos0, pContact->normal ); + + dMultiply0_331( pContact->normal, terrain->final_posr->R, pos0 ); + } + } +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + else + { + for ( i = 0; i < numTerrainContacts; ++i ) + { + pContact = CONTACT(contact,i*skip); + pContact->pos[ 0 ] -= terrain->m_p_data->m_fHalfWidth; + pContact->pos[ 2 ] -= terrain->m_p_data->m_fHalfDepth; + } + } +#endif // !DHEIGHTFIELD_CORNER_ORIGIN + } + // Return contact count. + return numTerrainContacts; +} + + + diff --git a/libs/ode-0.16.1/ode/src/heightfield.h b/libs/ode-0.16.1/ode/src/heightfield.h new file mode 100644 index 0000000..9b27f34 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/heightfield.h @@ -0,0 +1,245 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// dHeightfield Collider +// Martijn Buijs 2006 http://home.planet.nl/~buijs512/ +// Based on Terrain & Cone contrib by: +// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com + +#ifndef _DHEIGHTFIELD_H_ +#define _DHEIGHTFIELD_H_ +//------------------------------------------------------------------------------ + +#include <ode/common.h> +#include "collision_kernel.h" + + +#define HEIGHTFIELDMAXCONTACTPERCELL 10 + + +class HeightFieldVertex; +class HeightFieldEdge; +class HeightFieldTriangle; + +// +// dxHeightfieldData +// +// Heightfield Data structure +// +struct dxHeightfieldData +{ + dReal m_fWidth; // World space heightfield dimension on X axis + dReal m_fDepth; // World space heightfield dimension on Z axis + dReal m_fSampleWidth; // Vertex spacing on X axis edge (== m_vWidth / (m_nWidthSamples-1)) + dReal m_fSampleDepth; // Vertex spacing on Z axis edge (== m_vDepth / (m_nDepthSamples-1)) + dReal m_fSampleZXAspect; // Relation of Z axis spacing to X axis spacing (== m_fSampleDepth / m_fSampleWidth) + dReal m_fInvSampleWidth; // Cache of inverse Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1)) + dReal m_fInvSampleDepth; // Cache of inverse Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1)) + + dReal m_fHalfWidth; // Cache of half of m_fWidth + dReal m_fHalfDepth; // Cache of half of m_fDepth + + dReal m_fMinHeight; // Min sample height value (scaled and offset) + dReal m_fMaxHeight; // Max sample height value (scaled and offset) + dReal m_fThickness; // Surface thickness (added to bottom AABB) + dReal m_fScale; // Sample value multiplier + dReal m_fOffset; // Vertical sample offset + + int m_nWidthSamples; // Vertex count on X axis edge (number of samples) + int m_nDepthSamples; // Vertex count on Z axis edge (number of samples) + int m_bCopyHeightData; // Do we own the sample data? + int m_bWrapMode; // Heightfield wrapping mode (0=finite, 1=infinite) + int m_nGetHeightMode; // GetHeight mode ( 0=callback, 1=byte, 2=short, 3=float ) + + const void* m_pHeightData; // Sample data array + void* m_pUserData; // Callback user data + + dContactGeom m_contacts[HEIGHTFIELDMAXCONTACTPERCELL]; + + dHeightfieldGetHeight* m_pGetHeightCallback; // Callback pointer. + + dxHeightfieldData(); + ~dxHeightfieldData(); + + void SetData( int nWidthSamples, int nDepthSamples, + dReal fWidth, dReal fDepth, + dReal fScale, dReal fOffset, + dReal fThickness, int bWrapMode ); + + void ComputeHeightBounds(); + + bool IsOnHeightfield2 ( const HeightFieldVertex * const CellCorner, + const dReal * const pos, const bool isABC) const; + + dReal GetHeight(int x, int z); + dReal GetHeight(dReal x, dReal z); + +}; + +typedef int HeightFieldVertexCoords[2]; + +class HeightFieldVertex +{ +public: + HeightFieldVertex(){}; + + dVector3 vertex; + HeightFieldVertexCoords coords; + bool state; +}; + +class HeightFieldEdge +{ +public: + HeightFieldEdge(){}; + + HeightFieldVertex *vertices[2]; +}; + +class HeightFieldTriangle +{ +public: + HeightFieldTriangle(){}; + + inline void setMinMax() + { + maxAAAB = vertices[0]->vertex[1] > vertices[1]->vertex[1] ? vertices[0]->vertex[1] : vertices[1]->vertex[1]; + maxAAAB = vertices[2]->vertex[1] > maxAAAB ? vertices[2]->vertex[1] : maxAAAB; + }; + + HeightFieldVertex *vertices[3]; + dReal planeDef[4]; + dReal maxAAAB; + + bool isUp; + bool state; +}; + +class HeightFieldPlane +{ +public: + HeightFieldPlane(): + trianglelist(0), + trianglelistReservedSize(0), + trianglelistCurrentSize(0) + { + } + + ~HeightFieldPlane() + { + delete [] trianglelist; + } + + inline void setMinMax() + { + const sizeint asize = trianglelistCurrentSize; + if (asize > 0) + { + maxAAAB = trianglelist[0]->maxAAAB; + for (sizeint k = 1; asize > k; k++) + { + if (trianglelist[k]->maxAAAB > maxAAAB) + maxAAAB = trianglelist[k]->maxAAAB; + } + } + }; + + void resetTriangleListSize(const sizeint newSize) + { + if (trianglelistReservedSize < newSize) + { + delete [] trianglelist; + trianglelistReservedSize = newSize; + trianglelist = new HeightFieldTriangle *[newSize]; + } + trianglelistCurrentSize = 0; + } + + void addTriangle(HeightFieldTriangle *tri) + { + dIASSERT(trianglelistCurrentSize < trianglelistReservedSize); + + trianglelist[trianglelistCurrentSize++] = tri; + } + + HeightFieldTriangle **trianglelist; + sizeint trianglelistReservedSize; + sizeint trianglelistCurrentSize; + + dReal maxAAAB; + dReal planeDef[4]; +}; + +// +// dxHeightfield +// +// Heightfield geom structure +// +struct dxHeightfield : public dxGeom +{ + dxHeightfieldData* m_p_data; + + dxHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable ); + ~dxHeightfield(); + + void computeAABB(); + + int dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ, + dxGeom *o2, const int numMaxContacts, + int flags, dContactGeom *contact, int skip ); + + enum + { + TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 4, + TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X = 4, + TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z = 4, + TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 1 // Triangles are easy to reallocate and hard to predict + }; + + static inline sizeint AlignBufferSize(sizeint value, sizeint alignment) { dIASSERT((alignment & (alignment - 1)) == 0); return (value + (alignment - 1)) & ~(alignment - 1); } + + void allocateTriangleBuffer(sizeint numTri); + void resetTriangleBuffer(); + void allocatePlaneBuffer(sizeint numTri); + void resetPlaneBuffer(); + void allocateHeightBuffer(sizeint numX, sizeint numZ); + void resetHeightBuffer(); + + void sortPlanes(const sizeint numPlanes); + + HeightFieldPlane **tempPlaneBuffer; + HeightFieldPlane *tempPlaneInstances; + sizeint tempPlaneBufferSize; + + HeightFieldTriangle *tempTriangleBuffer; + sizeint tempTriangleBufferSize; + + HeightFieldVertex **tempHeightBuffer; + HeightFieldVertex *tempHeightInstances; + sizeint tempHeightBufferSizeX; + sizeint tempHeightBufferSizeZ; + +}; + + +//------------------------------------------------------------------------------ +#endif //_DHEIGHTFIELD_H_ diff --git a/libs/ode-0.16.1/ode/src/joints/Makefile.am b/libs/ode-0.16.1/ode/src/joints/Makefile.am new file mode 100644 index 0000000..194ef60 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/Makefile.am @@ -0,0 +1,37 @@ +AM_CPPFLAGS = -I$(top_srcdir)/include \ + -I$(top_builddir)/include \ + -I$(top_srcdir)/ode/src \ + -D__ODE__ + + +if ENABLE_OU + +AM_CPPFLAGS += -I$(top_srcdir)/ou/include + + +endif + + +noinst_LTLIBRARIES = libjoints.la + +libjoints_la_SOURCES = joints.h \ + joint.h joint.cpp \ + joint_internal.h \ + ball.h ball.cpp \ + dball.h dball.cpp \ + dhinge.h dhinge.cpp \ + transmission.h transmission.cpp \ + hinge.h hinge.cpp \ + slider.h slider.cpp \ + contact.h contact.cpp \ + universal.h universal.cpp \ + hinge2.h hinge2.cpp \ + fixed.h fixed.cpp \ + null.h null.cpp \ + amotor.h amotor.cpp \ + lmotor.h lmotor.cpp \ + plane2d.h plane2d.cpp \ + pu.h pu.cpp \ + pr.h pr.cpp \ + piston.h piston.cpp + diff --git a/libs/ode-0.16.1/ode/src/joints/Makefile.in b/libs/ode-0.16.1/ode/src/joints/Makefile.in new file mode 100644 index 0000000..9e43f9f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/Makefile.in @@ -0,0 +1,668 @@ +# Makefile.in generated by automake 1.15 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994-2014 Free Software Foundation, Inc. + +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ + +VPATH = @srcdir@ +am__is_gnu_make = { \ + if test -z '$(MAKELEVEL)'; then \ + false; \ + elif test -n '$(MAKE_HOST)'; then \ + true; \ + elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \ + true; \ + else \ + false; \ + fi; \ +} +am__make_running_with_option = \ + case $${target_option-} in \ + ?) ;; \ + *) echo "am__make_running_with_option: internal error: invalid" \ + "target option '$${target_option-}' specified" >&2; \ + exit 1;; \ + esac; \ + has_opt=no; \ + sane_makeflags=$$MAKEFLAGS; \ + if $(am__is_gnu_make); then \ + sane_makeflags=$$MFLAGS; \ + else \ + case $$MAKEFLAGS in \ + *\\[\ \ ]*) \ + bs=\\; \ + sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \ + | sed "s/$$bs$$bs[$$bs $$bs ]*//g"`;; \ + esac; \ + fi; \ + skip_next=no; \ + strip_trailopt () \ + { \ + flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \ + }; \ + for flg in $$sane_makeflags; do \ + test $$skip_next = yes && { skip_next=no; continue; }; \ + case $$flg in \ + *=*|--*) continue;; \ + -*I) strip_trailopt 'I'; skip_next=yes;; \ + -*I?*) strip_trailopt 'I';; \ + -*O) strip_trailopt 'O'; skip_next=yes;; \ + -*O?*) strip_trailopt 'O';; \ + -*l) strip_trailopt 'l'; skip_next=yes;; \ + -*l?*) strip_trailopt 'l';; \ + -[dEDm]) skip_next=yes;; \ + -[JT]) skip_next=yes;; \ + esac; \ + case $$flg in \ + *$$target_option*) has_opt=yes; break;; \ + esac; \ + done; \ + test $$has_opt = yes +am__make_dryrun = (target_option=n; $(am__make_running_with_option)) +am__make_keepgoing = (target_option=k; $(am__make_running_with_option)) +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ +@ENABLE_OU_TRUE@am__append_1 = -I$(top_srcdir)/ou/include +subdir = ode/src/joints +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \ + $(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \ + $(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \ + $(top_srcdir)/m4/pkg.m4 $(top_srcdir)/configure.ac +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +DIST_COMMON = $(srcdir)/Makefile.am $(am__DIST_COMMON) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/ode/src/config.h +CONFIG_CLEAN_FILES = +CONFIG_CLEAN_VPATH_FILES = +LTLIBRARIES = $(noinst_LTLIBRARIES) +libjoints_la_LIBADD = +am_libjoints_la_OBJECTS = joint.lo ball.lo dball.lo dhinge.lo \ + transmission.lo hinge.lo slider.lo contact.lo universal.lo \ + hinge2.lo fixed.lo null.lo amotor.lo lmotor.lo plane2d.lo \ + pu.lo pr.lo piston.lo +libjoints_la_OBJECTS = $(am_libjoints_la_OBJECTS) +AM_V_lt = $(am__v_lt_@AM_V@) +am__v_lt_ = $(am__v_lt_@AM_DEFAULT_V@) +am__v_lt_0 = --silent +am__v_lt_1 = +AM_V_P = $(am__v_P_@AM_V@) +am__v_P_ = $(am__v_P_@AM_DEFAULT_V@) +am__v_P_0 = false +am__v_P_1 = : +AM_V_GEN = $(am__v_GEN_@AM_V@) +am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@) +am__v_GEN_0 = @echo " GEN " $@; +am__v_GEN_1 = +AM_V_at = $(am__v_at_@AM_V@) +am__v_at_ = $(am__v_at_@AM_DEFAULT_V@) +am__v_at_0 = @ +am__v_at_1 = +DEFAULT_INCLUDES = -I.@am__isrc@ -I$(top_builddir)/ode/src +depcomp = $(SHELL) $(top_srcdir)/depcomp +am__depfiles_maybe = depfiles +am__mv = mv -f +CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \ + $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) +LTCXXCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CXX) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CXXFLAGS) $(CXXFLAGS) +AM_V_CXX = $(am__v_CXX_@AM_V@) +am__v_CXX_ = $(am__v_CXX_@AM_DEFAULT_V@) +am__v_CXX_0 = @echo " CXX " $@; +am__v_CXX_1 = +CXXLD = $(CXX) +CXXLINK = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CXXLD) $(AM_CXXFLAGS) \ + $(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CXXLD = $(am__v_CXXLD_@AM_V@) +am__v_CXXLD_ = $(am__v_CXXLD_@AM_DEFAULT_V@) +am__v_CXXLD_0 = @echo " CXXLD " $@; +am__v_CXXLD_1 = +COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \ + $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) +LTCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CC) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CFLAGS) $(CFLAGS) +AM_V_CC = $(am__v_CC_@AM_V@) +am__v_CC_ = $(am__v_CC_@AM_DEFAULT_V@) +am__v_CC_0 = @echo " CC " $@; +am__v_CC_1 = +CCLD = $(CC) +LINK = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CCLD) $(AM_CFLAGS) $(CFLAGS) \ + $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CCLD = $(am__v_CCLD_@AM_V@) +am__v_CCLD_ = $(am__v_CCLD_@AM_DEFAULT_V@) +am__v_CCLD_0 = @echo " CCLD " $@; +am__v_CCLD_1 = +SOURCES = $(libjoints_la_SOURCES) +DIST_SOURCES = $(libjoints_la_SOURCES) +am__can_run_installinfo = \ + case $$AM_UPDATE_INFO_DIR in \ + n|no|NO) false;; \ + *) (install-info --version) >/dev/null 2>&1;; \ + esac +am__tagged_files = $(HEADERS) $(SOURCES) $(TAGS_FILES) $(LISP) +# Read a list of newline-separated strings from the standard input, +# and print each of them once, without duplicates. Input order is +# *not* preserved. +am__uniquify_input = $(AWK) '\ + BEGIN { nonempty = 0; } \ + { items[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in items) print i; }; } \ +' +# Make sure the list of sources is unique. This is necessary because, +# e.g., the same source file might be shared among _SOURCES variables +# for different programs/libraries. +am__define_uniq_tagged_files = \ + list='$(am__tagged_files)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | $(am__uniquify_input)` +ETAGS = etags +CTAGS = ctags +am__DIST_COMMON = $(srcdir)/Makefile.in $(top_srcdir)/depcomp +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +ACLOCAL = @ACLOCAL@ +ALLOCA = @ALLOCA@ +AMTAR = @AMTAR@ +AM_DEFAULT_VERBOSITY = @AM_DEFAULT_VERBOSITY@ +AR = @AR@ +AS = @AS@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CCD_CFLAGS = @CCD_CFLAGS@ +CCD_LIBS = @CCD_LIBS@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXCPP = @CXXCPP@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DLLTOOL = @DLLTOOL@ +DOXYGEN = @DOXYGEN@ +DSYMUTIL = @DSYMUTIL@ +DUMPBIN = @DUMPBIN@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +EXTRA_LIBTOOL_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ +FGREP = @FGREP@ +GL_LIBS = @GL_LIBS@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LD = @LD@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LIBSTDCXX = @LIBSTDCXX@ +LIBTOOL = @LIBTOOL@ +LIPO = @LIPO@ +LN_S = @LN_S@ +LTLIBOBJS = @LTLIBOBJS@ +LT_SYS_LIBRARY_PATH = @LT_SYS_LIBRARY_PATH@ +MAKEINFO = @MAKEINFO@ +MANIFEST_TOOL = @MANIFEST_TOOL@ +MKDIR_P = @MKDIR_P@ +NM = @NM@ +NMEDIT = @NMEDIT@ +OBJDUMP = @OBJDUMP@ +OBJEXT = @OBJEXT@ +ODE_PRECISION = @ODE_PRECISION@ +ODE_VERSION = @ODE_VERSION@ +ODE_VERSION_INFO = @ODE_VERSION_INFO@ +OTOOL = @OTOOL@ +OTOOL64 = @OTOOL64@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_URL = @PACKAGE_URL@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +PKG_CONFIG = @PKG_CONFIG@ +PKG_CONFIG_LIBDIR = @PKG_CONFIG_LIBDIR@ +PKG_CONFIG_PATH = @PKG_CONFIG_PATH@ +RANLIB = @RANLIB@ +SED = @SED@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +VERSION = @VERSION@ +WINDRES = @WINDRES@ +X11_CFLAGS = @X11_CFLAGS@ +X11_LIBS = @X11_LIBS@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_AR = @ac_ct_AR@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +ac_ct_DUMPBIN = @ac_ct_DUMPBIN@ +ac_ct_WINDRES = @ac_ct_WINDRES@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +build = @build@ +build_alias = @build_alias@ +build_cpu = @build_cpu@ +build_os = @build_os@ +build_vendor = @build_vendor@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host = @host@ +host_alias = @host_alias@ +host_cpu = @host_cpu@ +host_os = @host_os@ +host_vendor = @host_vendor@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +runstatedir = @runstatedir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +subdirs = @subdirs@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +top_build_prefix = @top_build_prefix@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include \ + -I$(top_srcdir)/ode/src -D__ODE__ $(am__append_1) +noinst_LTLIBRARIES = libjoints.la +libjoints_la_SOURCES = joints.h \ + joint.h joint.cpp \ + joint_internal.h \ + ball.h ball.cpp \ + dball.h dball.cpp \ + dhinge.h dhinge.cpp \ + transmission.h transmission.cpp \ + hinge.h hinge.cpp \ + slider.h slider.cpp \ + contact.h contact.cpp \ + universal.h universal.cpp \ + hinge2.h hinge2.cpp \ + fixed.h fixed.cpp \ + null.h null.cpp \ + amotor.h amotor.cpp \ + lmotor.h lmotor.cpp \ + plane2d.h plane2d.cpp \ + pu.h pu.cpp \ + pr.h pr.cpp \ + piston.h piston.cpp + +all: all-am + +.SUFFIXES: +.SUFFIXES: .cpp .lo .o .obj +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \ + && { if test -f $@; then exit 0; else break; fi; }; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/src/joints/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/src/joints/Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(am__aclocal_m4_deps): + +clean-noinstLTLIBRARIES: + -test -z "$(noinst_LTLIBRARIES)" || rm -f $(noinst_LTLIBRARIES) + @list='$(noinst_LTLIBRARIES)'; \ + locs=`for p in $$list; do echo $$p; done | \ + sed 's|^[^/]*$$|.|; s|/[^/]*$$||; s|$$|/so_locations|' | \ + sort -u`; \ + test -z "$$locs" || { \ + echo rm -f $${locs}; \ + rm -f $${locs}; \ + } + +libjoints.la: $(libjoints_la_OBJECTS) $(libjoints_la_DEPENDENCIES) $(EXTRA_libjoints_la_DEPENDENCIES) + $(AM_V_CXXLD)$(CXXLINK) $(libjoints_la_OBJECTS) $(libjoints_la_LIBADD) $(LIBS) + +mostlyclean-compile: + -rm -f *.$(OBJEXT) + +distclean-compile: + -rm -f *.tab.c + +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/amotor.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/ball.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/contact.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/dball.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/dhinge.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fixed.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/hinge.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/hinge2.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/joint.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/lmotor.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/null.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/piston.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/plane2d.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/pr.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/pu.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/slider.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/transmission.Plo@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/universal.Plo@am__quote@ + +.cpp.o: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ $< + +.cpp.obj: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.cpp.lo: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(LTCXXCOMPILE) -c -o $@ $< + +mostlyclean-libtool: + -rm -f *.lo + +clean-libtool: + -rm -rf .libs _libs + +ID: $(am__tagged_files) + $(am__define_uniq_tagged_files); mkid -fID $$unique +tags: tags-am +TAGS: tags + +tags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + set x; \ + here=`pwd`; \ + $(am__define_uniq_tagged_files); \ + shift; \ + if test -z "$(ETAGS_ARGS)$$*$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + if test $$# -gt 0; then \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + "$$@" $$unique; \ + else \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$unique; \ + fi; \ + fi +ctags: ctags-am + +CTAGS: ctags +ctags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + $(am__define_uniq_tagged_files); \ + test -z "$(CTAGS_ARGS)$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && $(am__cd) $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) "$$here" +cscopelist: cscopelist-am + +cscopelist-am: $(am__tagged_files) + list='$(am__tagged_files)'; \ + case "$(srcdir)" in \ + [\\/]* | ?:[\\/]*) sdir="$(srcdir)" ;; \ + *) sdir=$(subdir)/$(srcdir) ;; \ + esac; \ + for i in $$list; do \ + if test -f "$$i"; then \ + echo "$(subdir)/$$i"; \ + else \ + echo "$$sdir/$$i"; \ + fi; \ + done >> $(top_builddir)/cscope.files + +distclean-tags: + -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d "$(distdir)/$$file"; then \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \ + else \ + test -f "$(distdir)/$$file" \ + || cp -p $$d/$$file "$(distdir)/$$file" \ + || exit 1; \ + fi; \ + done +check-am: all-am +check: check-am +all-am: Makefile $(LTLIBRARIES) +installdirs: +install: install-am +install-exec: install-exec-am +install-data: install-data-am +uninstall: uninstall-am + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-am +install-strip: + if test -z '$(STRIP)'; then \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + install; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \ + fi +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-am + +clean-am: clean-generic clean-libtool clean-noinstLTLIBRARIES \ + mostlyclean-am + +distclean: distclean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +distclean-am: clean-am distclean-compile distclean-generic \ + distclean-tags + +dvi: dvi-am + +dvi-am: + +html: html-am + +html-am: + +info: info-am + +info-am: + +install-data-am: + +install-dvi: install-dvi-am + +install-dvi-am: + +install-exec-am: + +install-html: install-html-am + +install-html-am: + +install-info: install-info-am + +install-info-am: + +install-man: + +install-pdf: install-pdf-am + +install-pdf-am: + +install-ps: install-ps-am + +install-ps-am: + +installcheck-am: + +maintainer-clean: maintainer-clean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-am + +mostlyclean-am: mostlyclean-compile mostlyclean-generic \ + mostlyclean-libtool + +pdf: pdf-am + +pdf-am: + +ps: ps-am + +ps-am: + +uninstall-am: + +.MAKE: install-am install-strip + +.PHONY: CTAGS GTAGS TAGS all all-am check check-am clean clean-generic \ + clean-libtool clean-noinstLTLIBRARIES cscopelist-am ctags \ + ctags-am distclean distclean-compile distclean-generic \ + distclean-libtool distclean-tags distdir dvi dvi-am html \ + html-am info info-am install install-am install-data \ + install-data-am install-dvi install-dvi-am install-exec \ + install-exec-am install-html install-html-am install-info \ + install-info-am install-man install-pdf install-pdf-am \ + install-ps install-ps-am install-strip installcheck \ + installcheck-am installdirs maintainer-clean \ + maintainer-clean-generic mostlyclean mostlyclean-compile \ + mostlyclean-generic mostlyclean-libtool pdf pdf-am ps ps-am \ + tags tags-am uninstall uninstall-am + +.PRECIOUS: Makefile + + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/libs/ode-0.16.1/ode/src/joints/amotor.cpp b/libs/ode-0.16.1/ode/src/joints/amotor.cpp new file mode 100644 index 0000000..aa30c76 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/amotor.cpp @@ -0,0 +1,810 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "common.h" +#include "amotor.h" +#include "joint_internal.h" +#include "odeou.h" + + +/*extern */ +void dJointSetAMotorNumAxes(dJointID j, int num) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + dAASSERT(dIN_RANGE(num, dSA__MIN, dSA__MAX + 1)); + checktype(joint, AMotor); + + num = dCLAMP(num, dSA__MIN, dSA__MAX); + + joint->setNumAxes(num); +} + +/*extern */ +void dJointSetAMotorAxis(dJointID j, int anum, int rel/*=dJointBodyRelativity*/, + dReal x, dReal y, dReal z) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + dAASSERT(dIN_RANGE(rel, dJBR__MIN, dJBR__MAX)); + checktype(joint, AMotor); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + joint->setAxisValue(anum, (dJointBodyRelativity)rel, x, y, z); +} + +/*extern */ +void dJointSetAMotorAngle(dJointID j, int anum, dReal angle) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + checktype(joint, AMotor); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + joint->setAngleValue(anum, angle); +} + +/*extern */ +void dJointSetAMotorParam(dJointID j, int parameter, dReal value) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + checktype(joint, AMotor); + + int anum = parameter >> 8; + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + int limotParam = parameter & 0xff; + joint->setLimotParameter(anum, limotParam, value); +} + +/*extern */ +void dJointSetAMotorMode(dJointID j, int mode) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + checktype(joint, AMotor); + + joint->setOperationMode(mode); +} + +/*extern */ +int dJointGetAMotorNumAxes(dJointID j) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + checktype(joint, AMotor); + + return joint->getNumAxes(); +} + +/*extern */ +void dJointGetAMotorAxis(dJointID j, int anum, dVector3 result) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + checktype(joint, AMotor); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + joint->getAxisValue(result, anum); +} + +/*extern */ +int dJointGetAMotorAxisRel(dJointID j, int anum) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + checktype(joint, AMotor); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + int result = joint->getAxisBodyRelativity(anum); + return result; +} + +/*extern */ +dReal dJointGetAMotorAngle(dJointID j, int anum) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + checktype(joint, AMotor); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + dReal result = joint->getAngleValue(anum); + return result; +} + +/*extern */ +dReal dJointGetAMotorAngleRate(dJointID j, int anum) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + checktype(joint, AMotor); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + dReal result = joint->calculateAngleRate(anum); + return result; +} + +/*extern */ +dReal dJointGetAMotorParam(dJointID j, int parameter) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + checktype(joint, AMotor); + + int anum = parameter >> 8; + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + + anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1); + + int limotParam = parameter & 0xff; + dReal result = joint->getLimotParameter(anum, limotParam); + return result; +} + +/*extern */ +int dJointGetAMotorMode(dJointID j) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + checktype(joint, AMotor); + + int result = joint->getOperationMode(); + return result; +} + +/*extern */ +void dJointAddAMotorTorques(dJointID j, dReal torque1, dReal torque2, dReal torque3) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint != NULL); + checktype(joint, AMotor); + + joint->addTorques(torque1, torque2, torque3); +} + + +//**************************************************************************** + +BEGIN_NAMESPACE_OU(); +template<> +const dJointBodyRelativity CEnumUnsortedElementArray<dSpaceAxis, dSA__MAX, dJointBodyRelativity, 0x160703D5>::m_aetElementArray[] = +{ + dJBR_BODY1, // dSA_X, + dJBR_GLOBAL, // dSA_Y, + dJBR_BODY2, // dSA_Z, +}; +END_NAMESPACE_OU(); +static const CEnumUnsortedElementArray<dSpaceAxis, dSA__MAX, dJointBodyRelativity, 0x160703D5> g_abrEulerAxisAllowedBodyRelativities; + +static inline +dSpaceAxis EncodeJointConnectedBodyEulerAxis(dJointConnectedBody cbBodyIndex) +{ + dSASSERT(dJCB__MAX == 2); + + return cbBodyIndex == dJCB_FIRST_BODY ? dSA_X : dSA_Z; +} + +static inline +dSpaceAxis EncodeOtherEulerAxis(dSpaceAxis saOneAxis) +{ + dIASSERT(saOneAxis == EncodeJointConnectedBodyEulerAxis(dJCB_FIRST_BODY) || saOneAxis == EncodeJointConnectedBodyEulerAxis(dJCB_SECOND_BODY)); + dSASSERT(dJCB__MAX == 2); + + return (dSpaceAxis)(dSA_X + dSA_Z - saOneAxis); +} + + +//**************************************************************************** +// angular motor + +dxJointAMotor::dxJointAMotor(dxWorld *w) : + dxJointAMotor_Parent(w), + m_mode(dAMotorUser), + m_num(0) +{ + std::fill(m_rel, m_rel + dARRAY_SIZE(m_rel), dJBR__DEFAULT); + { for (int i = 0; i != dARRAY_SIZE(m_axis); ++i) { dZeroVector3(m_axis[i]); } } + { for (int i = 0; i != dARRAY_SIZE(m_references); ++i) { dZeroVector3(m_references[i]); } } + std::fill(m_angle, m_angle + dARRAY_SIZE(m_angle), REAL(0.0)); + { for (int i = 0; i != dARRAY_SIZE(m_limot); ++i) { m_limot[i].init(w); } } +} + + +/*virtual */ +dxJointAMotor::~dxJointAMotor() +{ + // The virtual destructor +} + + +/*virtual */ +void dxJointAMotor::getSureMaxInfo(SureMaxInfo* info) +{ + info->max_m = m_num; +} + +/*virtual */ +void dxJointAMotor::getInfo1(dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; + + // compute the axes and angles, if in Euler mode + if (m_mode == dAMotorEuler) + { + dVector3 ax[dSA__MAX]; + computeGlobalAxes(ax); + computeEulerAngles(ax); + } + + // see if we're powered or at a joint limit for each axis + const unsigned num = m_num; + for (unsigned i = 0; i != num; ++i) + { + if (m_limot[i].testRotationalLimit(m_angle[i]) + || m_limot[i].fmax > 0) + { + info->m++; + } + } +} + +/*virtual */ +void dxJointAMotor::getInfo2(dReal worldFPS, dReal /*worldERP*/, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex) +{ + // compute the axes (if not global) + dVector3 ax[dSA__MAX]; + computeGlobalAxes(ax); + + // in Euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the Euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + dVector3 *axptr[dSA__MAX]; + for (int j = dSA__MIN; j != dSA__MAX; ++j) { axptr[j] = &ax[j]; } + + dVector3 ax0_cross_ax1; + dVector3 ax1_cross_ax2; + + if (m_mode == dAMotorEuler) + { + dCalcVectorCross3(ax0_cross_ax1, ax[dSA_X], ax[dSA_Y]); + axptr[dSA_Z] = &ax0_cross_ax1; + dCalcVectorCross3(ax1_cross_ax2, ax[dSA_Y], ax[dSA_Z]); + axptr[dSA_X] = &ax1_cross_ax2; + } + + sizeint rowTotalSkip = 0, pairTotalSkip = 0; + + const unsigned num = m_num; + for (unsigned i = 0; i != num; ++i) + { + if (m_limot[i].addLimot(this, worldFPS, J1 + rowTotalSkip, J2 + rowTotalSkip, pairRhsCfm + pairTotalSkip, pairLoHi + pairTotalSkip, *(axptr[i]), 1)) + { + rowTotalSkip += rowskip; + pairTotalSkip += pairskip; + } + } +} + +/*virtual */ +dJointType dxJointAMotor::type() const +{ + return dJointTypeAMotor; +} + +/*virtual */ +sizeint dxJointAMotor::size() const +{ + return sizeof(*this); +} + + +void dxJointAMotor::setOperationMode(int mode) +{ + m_mode = mode; + + if (mode == dAMotorEuler) + { + m_num = dSA__MAX; + setEulerReferenceVectors(); + } +} + + +void dxJointAMotor::setNumAxes(unsigned num) +{ + if (m_mode == dAMotorEuler) + { + m_num = dSA__MAX; + } + else + { + m_num = num; + } +} + + +dJointBodyRelativity dxJointAMotor::getAxisBodyRelativity(unsigned anum) const +{ + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + + dJointBodyRelativity rel = m_rel[anum]; + if (dJBREncodeBodyRelativityStatus(rel) && GetIsJointReverse()) + { + rel = dJBRSwapBodyRelativity(rel); // turns 1 into 2, 2 into 1 + } + + return rel; +} + + +void dxJointAMotor::setAxisValue(unsigned anum, dJointBodyRelativity rel, + dReal x, dReal y, dReal z) +{ + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + dAASSERT(m_mode != dAMotorEuler || !dJBREncodeBodyRelativityStatus(rel) || rel == g_abrEulerAxisAllowedBodyRelativities.Encode((dSpaceAxis)anum)); + + // x,y,z is always in global coordinates regardless of rel, so we may have + // to convert it to be relative to a body + dVector3 r; + dAssignVector3(r, x, y, z); + + // adjust rel to match the internal body order + if (dJBREncodeBodyRelativityStatus(rel) && GetIsJointReverse()) + { + rel = dJBRSwapBodyRelativity(rel); // turns 1 into 2, 2, into 1 + } + + m_rel[anum] = rel; + + bool assigned = false; + + if (dJBREncodeBodyRelativityStatus(rel)) + { + if (rel == dJBR_BODY1) + { + dMultiply1_331(m_axis[anum], this->node[0].body->posr.R, r); + assigned = true; + } + // rel == 2 + else if (this->node[1].body != NULL) + { + dIASSERT(rel == dJBR_BODY2); + + dMultiply1_331(m_axis[anum], this->node[1].body->posr.R, r); + assigned = true; + } + } + + if (!assigned) + { + dCopyVector3(m_axis[anum], r); + } + + dNormalize3(m_axis[anum]); + + if (m_mode == dAMotorEuler) + { + setEulerReferenceVectors(); + } +} + +void dxJointAMotor::getAxisValue(dVector3 result, unsigned anum) const +{ + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + + switch (m_mode) + { + case dAMotorUser: + { + doGetUserAxis(result, anum); + break; + } + + case dAMotorEuler: + { + doGetEulerAxis(result, anum); + break; + } + + default: + { + dIASSERT(false); + break; + } + } +} + + +void dxJointAMotor::doGetUserAxis(dVector3 result, unsigned anum) const +{ + bool retrieved = false; + + if (dJBREncodeBodyRelativityStatus(m_rel[anum])) + { + if (m_rel[anum] == dJBR_BODY1) + { + dMultiply0_331(result, this->node[0].body->posr.R, m_axis[anum]); + retrieved = true; + } + else if (this->node[1].body != NULL) + { + dMultiply0_331(result, this->node[1].body->posr.R, m_axis[anum]); + retrieved = true; + } + } + + if (!retrieved) + { + dCopyVector3(result, m_axis[anum]); + } +} + +void dxJointAMotor::doGetEulerAxis(dVector3 result, unsigned anum) const +{ + // If we're in Euler mode, joint->axis[1] doesn't + // have anything sensible in it. So don't just return + // that, find the actual effective axis. + // Likewise, the actual axis of rotation for the + // the other axes is different from what's stored. + dVector3 axes[dSA__MAX]; + computeGlobalAxes(axes); + + if (anum == dSA_Y) + { + dCopyVector3(result, axes[dSA_Y]); + } + else if (anum < dSA_Y) // Comparing against the same constant lets compiler reuse EFLAGS register for another conditional jump + { + dSASSERT(dSA_X < dSA_Y); // Otherwise the condition above is incorrect + dIASSERT(anum == dSA_X); + + // This won't be unit length in general, + // but it's what's used in getInfo2 + // This may be why things freak out as + // the body-relative axes get close to each other. + dCalcVectorCross3(result, axes[dSA_Y], axes[dSA_Z]); + } + else + { + dSASSERT(dSA_Z > dSA_Y); // Otherwise the condition above is incorrect + dIASSERT(anum == dSA_Z); + + // Same problem as above. + dCalcVectorCross3(result, axes[dSA_X], axes[dSA_Y]); + } +} + + +void dxJointAMotor::setAngleValue(unsigned anum, dReal angle) +{ + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + dAASSERT(m_mode == dAMotorUser); // This only works for the dAMotorUser + + if (m_mode == dAMotorUser) + { + m_angle[anum] = angle; + } +} + + +dReal dxJointAMotor::calculateAngleRate(unsigned anum) const +{ + dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); + dAASSERT(this->node[0].body != NULL); // Don't call for angle rate before the joint is set up + + dVector3 axis; + getAxisValue(axis, anum); + + // NOTE! + // For reverse joints, the rate is negated at the function exit to create swapped bodies effect + dReal rate = dDOT(axis, this->node[0].body->avel); + + if (this->node[1].body != NULL) + { + rate -= dDOT(axis, this->node[1].body->avel); + } + + // Negating the rate for reverse joints creates an effect of body swapping + dReal result = !GetIsJointReverse() ? rate : -rate; + return result; +} + + +void dxJointAMotor::addTorques(dReal torque1, dReal torque2, dReal torque3) +{ + unsigned num = getNumAxes(); + dAASSERT(dIN_RANGE(num, dSA__MIN, dSA__MAX + 1)); + + dVector3 sum; + dVector3 torqueVector; + dVector3 axes[dSA__MAX]; + + + if (num != dSA__MIN) + { + computeGlobalAxes(axes); + + if (!GetIsJointReverse()) + { + dAssignVector3(torqueVector, torque1, torque2, torque3); + } + else + { + // Negating torques creates an effect of swapped bodies later + dAssignVector3(torqueVector, -torque1, -torque2, -torque3); + } + } + + switch (num) + { + case dSA_Z + 1: + { + dAddThreeScaledVectors3(sum, axes[dSA_Z], axes[dSA_Y], axes[dSA_X], torqueVector[dSA_Z], torqueVector[dSA_Y], torqueVector[dSA_X]); + break; + } + + case dSA_Y + 1: + { + dAddScaledVectors3(sum, axes[dSA_Y], axes[dSA_X], torqueVector[dSA_Y], torqueVector[dSA_X]); + break; + } + + case dSA_X + 1: + { + dCopyScaledVector3(sum, axes[dSA_X], torqueVector[dSA_X]); + break; + } + + default: + { + dSASSERT(dSA_Z > dSA_Y); // Otherwise the addends order needs to be switched + dSASSERT(dSA_Y > dSA_X); + + // Do nothing + break; + } + } + + if (num != dSA__MIN) + { + dAASSERT(this->node[0].body != NULL); // Don't add torques unless you set the joint up first! + + // NOTE! + // For reverse joints, the torqueVector negated at function entry produces the effect of swapped bodies + dBodyAddTorque(this->node[0].body, sum[dV3E_X], sum[dV3E_Y], sum[dV3E_Z]); + + if (this->node[1].body != NULL) + { + dBodyAddTorque(this->node[1].body, -sum[dV3E_X], -sum[dV3E_Y], -sum[dV3E_Z]); + } + } +} + + +// compute the 3 axes in global coordinates +void dxJointAMotor::computeGlobalAxes(dVector3 ax[dSA__MAX]) const +{ + switch (m_mode) + { + case dAMotorUser: + { + doComputeGlobalUserAxes(ax); + break; + } + + case dAMotorEuler: + { + doComputeGlobalEulerAxes(ax); + break; + } + + default: + { + dIASSERT(false); + break; + } + } +} + +void dxJointAMotor::doComputeGlobalUserAxes(dVector3 ax[dSA__MAX]) const +{ + unsigned num = m_num; + for (unsigned i = 0; i != num; ++i) + { + bool assigned = false; + + if (m_rel[i] == dJBR_BODY1) + { + // relative to b1 + dMultiply0_331(ax[i], this->node[0].body->posr.R, m_axis[i]); + assigned = true; + } + else if (m_rel[i] == dJBR_BODY2) + { + // relative to b2 + if (this->node[1].body != NULL) + { + dMultiply0_331(ax[i], this->node[1].body->posr.R, m_axis[i]); + assigned = true; + } + } + + if (!assigned) + { + // global - just copy it + dCopyVector3(ax[i], m_axis[i]); + } + } +} + +void dxJointAMotor::doComputeGlobalEulerAxes(dVector3 ax[dSA__MAX]) const +{ + // special handling for Euler mode + + dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis(); + dMultiply0_331(ax[firstBodyAxis], this->node[0].body->posr.R, m_axis[firstBodyAxis]); + + dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis); + + if (this->node[1].body != NULL) + { + dMultiply0_331(ax[secondBodyAxis], this->node[1].body->posr.R, m_axis[secondBodyAxis]); + } + else + { + dCopyVector3(ax[secondBodyAxis], m_axis[secondBodyAxis]); + } + + dCalcVectorCross3(ax[dSA_Y], ax[dSA_Z], ax[dSA_X]); + dNormalize3(ax[dSA_Y]); +} + + +void dxJointAMotor::computeEulerAngles(dVector3 ax[dSA__MAX]) +{ + // assumptions: + // global axes already calculated --> ax + // axis[0] is relative to body 1 --> global ax[0] + // axis[2] is relative to body 2 --> global ax[2] + // ax[1] = ax[2] x ax[0] + // original ax[0] and ax[2] are perpendicular + // reference1 is perpendicular to ax[0] (in body 1 frame) + // reference2 is perpendicular to ax[2] (in body 2 frame) + // all ax[] and reference vectors are unit length + + // calculate references in global frame + dVector3 refs[dJCB__MAX]; + dMultiply0_331(refs[dJCB_FIRST_BODY], this->node[0].body->posr.R, m_references[dJCB_FIRST_BODY]); + + if (this->node[1].body != NULL) + { + dMultiply0_331(refs[dJCB_SECOND_BODY], this->node[1].body->posr.R, m_references[dJCB_SECOND_BODY]); + } + else + { + dCopyVector3(refs[dJCB_SECOND_BODY], m_references[dJCB_SECOND_BODY]); + } + + + // get q perpendicular to both ax[0] and ref1, get first euler angle + dVector3 q; + dJointConnectedBody firstAxisBody = BuildFirstEulerAxisBody(); + + dCalcVectorCross3(q, ax[dSA_X], refs[firstAxisBody]); + m_angle[dSA_X] = -dAtan2(dCalcVectorDot3(ax[dSA_Z], q), dCalcVectorDot3(ax[dSA_Z], refs[firstAxisBody])); + + // get q perpendicular to both ax[0] and ax[1], get second euler angle + dCalcVectorCross3(q, ax[dSA_X], ax[dSA_Y]); + m_angle[dSA_Y] = -dAtan2(dCalcVectorDot3(ax[dSA_Z], ax[dSA_X]), dCalcVectorDot3(ax[dSA_Z], q)); + + dJointConnectedBody secondAxisBody = EncodeJointOtherConnectedBody(firstAxisBody); + + // get q perpendicular to both ax[1] and ax[2], get third euler angle + dCalcVectorCross3(q, ax[dSA_Y], ax[dSA_Z]); + m_angle[dSA_Z] = -dAtan2(dCalcVectorDot3(refs[secondAxisBody], ax[dSA_Y]), dCalcVectorDot3(refs[secondAxisBody], q)); +} + + +// set the reference vectors as follows: +// * reference1 = current axis[2] relative to body 1 +// * reference2 = current axis[0] relative to body 2 +// this assumes that: +// * axis[0] is relative to body 1 +// * axis[2] is relative to body 2 + +void dxJointAMotor::setEulerReferenceVectors() +{ + if (/*this->node[0].body != NULL && */this->node[1].body != NULL) + { + dIASSERT(this->node[0].body != NULL); + + dVector3 r; // axis[2] and axis[0] in global coordinates + + dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis(); + dMultiply0_331(r, this->node[0].body->posr.R, m_axis[firstBodyAxis]); + dMultiply1_331(m_references[dJCB_SECOND_BODY], this->node[1].body->posr.R, r); + + dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis); + dMultiply0_331(r, this->node[1].body->posr.R, m_axis[secondBodyAxis]); + dMultiply1_331(m_references[dJCB_FIRST_BODY], this->node[0].body->posr.R, r); + } + else + { + // We want to handle angular motors attached to passive geoms + // Replace missing node.R with identity + if (this->node[0].body != NULL) + { + dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis(); + dMultiply0_331(m_references[dJCB_SECOND_BODY], this->node[0].body->posr.R, m_axis[firstBodyAxis]); + + dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis); + dMultiply1_331(m_references[dJCB_FIRST_BODY], this->node[0].body->posr.R, m_axis[secondBodyAxis]); + } + } +} + +/*inline */ +dSpaceAxis dxJointAMotor::BuildFirstBodyEulerAxis() const +{ + return EncodeJointConnectedBodyEulerAxis(BuildFirstEulerAxisBody()); +} + +/*inline */ +dJointConnectedBody dxJointAMotor::BuildFirstEulerAxisBody() const +{ + return !GetIsJointReverse() ? dJCB_FIRST_BODY : dJCB_SECOND_BODY; +} + diff --git a/libs/ode-0.16.1/ode/src/joints/amotor.h b/libs/ode-0.16.1/ode/src/joints/amotor.h new file mode 100644 index 0000000..2fd421c --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/amotor.h @@ -0,0 +1,105 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_AMOTOR_H_
+#define _ODE_JOINT_AMOTOR_H_
+
+#include "joint.h"
+
+
+// angular motor
+
+typedef dxJoint dxJointAMotor_Parent;
+class dxJointAMotor:
+ public dxJointAMotor_Parent
+{
+public:
+ dxJointAMotor(dxWorld *w);
+ virtual ~dxJointAMotor();
+
+public:
+ virtual void getSureMaxInfo(SureMaxInfo* info);
+ virtual void getInfo1(Info1* info);
+ virtual void getInfo2(dReal worldFPS, dReal worldERP,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex);
+ virtual dJointType type() const;
+ virtual sizeint size() const;
+
+public:
+ void setOperationMode(int mode);
+ int getOperationMode() const { return m_mode; }
+
+ void setNumAxes(unsigned num);
+ int getNumAxes() const { return m_num; }
+
+ dJointBodyRelativity getAxisBodyRelativity(unsigned anum) const;
+
+ void setAxisValue(unsigned anum, dJointBodyRelativity rel, dReal x, dReal y, dReal z);
+ void getAxisValue(dVector3 result, unsigned anum) const;
+
+private:
+ void doGetUserAxis(dVector3 result, unsigned anum) const;
+ void doGetEulerAxis(dVector3 result, unsigned anum) const;
+
+public:
+ void setAngleValue(unsigned anum, dReal angle);
+ dReal getAngleValue(unsigned anum) const { dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); return m_angle[anum]; }
+
+ dReal calculateAngleRate(unsigned anum) const;
+
+ void setLimotParameter(unsigned anum, int limotParam, dReal value) { dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); m_limot[anum].set(limotParam, value); }
+ dReal getLimotParameter(unsigned anum, int limotParam) const { dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); return m_limot[anum].get(limotParam); }
+
+public:
+ void addTorques(dReal torque1, dReal torque2, dReal torque3);
+
+private:
+ void computeGlobalAxes(dVector3 ax[dSA__MAX]) const;
+ void doComputeGlobalUserAxes(dVector3 ax[dSA__MAX]) const;
+ void doComputeGlobalEulerAxes(dVector3 ax[dSA__MAX]) const;
+
+ void computeEulerAngles(dVector3 ax[dSA__MAX]);
+ void setEulerReferenceVectors();
+
+private:
+ inline dSpaceAxis BuildFirstBodyEulerAxis() const;
+ inline dJointConnectedBody BuildFirstEulerAxisBody() const;
+
+private:
+ friend struct dxAMotorJointPrinter;
+
+private:
+ int m_mode; // a dAMotorXXX constant
+ unsigned m_num; // number of axes (0..3)
+ dJointBodyRelativity m_rel[dSA__MAX]; // what the axes are relative to (global,b1,b2)
+ dVector3 m_axis[dSA__MAX]; // three axes
+ // these vectors are used for calculating Euler angles
+ dVector3 m_references[dJCB__MAX]; // original axis[2], relative to body 1; original axis[0], relative to body 2
+ dReal m_angle[dSA__MAX]; // user-supplied angles for axes
+ dxJointLimitMotor m_limot[dJBR__MAX]; // limit+motor info for axes
+};
+
+
+#endif
+
diff --git a/libs/ode-0.16.1/ode/src/joints/ball.cpp b/libs/ode-0.16.1/ode/src/joints/ball.cpp new file mode 100644 index 0000000..c295b85 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/ball.cpp @@ -0,0 +1,186 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "ball.h"
+#include "joint_internal.h"
+
+//****************************************************************************
+// ball and socket
+
+dxJointBall::dxJointBall( dxWorld *w ) :
+ dxJoint( w )
+{
+ dSetZero( anchor1, 4 );
+ dSetZero( anchor2, 4 );
+ erp = world->global_erp;
+ cfm = world->global_cfm;
+}
+
+
+void
+dxJointBall::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = 3;
+}
+
+
+void
+dxJointBall::getInfo1( dxJoint::Info1 *info )
+{
+ info->m = 3;
+ info->nub = 3;
+}
+
+
+void
+dxJointBall::getInfo2( dReal worldFPS, dReal /*worldERP*/,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+{
+ pairRhsCfm[GI2_CFM] = cfm;
+ pairRhsCfm[pairskip + GI2_CFM] = cfm;
+ pairRhsCfm[2 * pairskip + GI2_CFM] = cfm;
+ setBall( this, worldFPS, this->erp, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 );
+}
+
+
+
+
+
+void dJointSetBallAnchor( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointBall* joint = ( dxJointBall* )j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, Ball );
+ setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
+}
+
+
+void dJointSetBallAnchor2( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointBall* joint = ( dxJointBall* )j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, Ball );
+ joint->anchor2[0] = x;
+ joint->anchor2[1] = y;
+ joint->anchor2[2] = z;
+ joint->anchor2[3] = 0;
+}
+
+void dJointGetBallAnchor( dJointID j, dVector3 result )
+{
+ dxJointBall* joint = ( dxJointBall* )j;
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+ checktype( joint, Ball );
+ if ( joint->flags & dJOINT_REVERSE )
+ getAnchor2( joint, result, joint->anchor2 );
+ else
+ getAnchor( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetBallAnchor2( dJointID j, dVector3 result )
+{
+ dxJointBall* joint = ( dxJointBall* )j;
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+ checktype( joint, Ball );
+ if ( joint->flags & dJOINT_REVERSE )
+ getAnchor( joint, result, joint->anchor1 );
+ else
+ getAnchor2( joint, result, joint->anchor2 );
+}
+
+
+void dxJointBall::set( int num, dReal value )
+{
+ switch ( num )
+ {
+ case dParamCFM:
+ cfm = value;
+ break;
+ case dParamERP:
+ erp = value;
+ break;
+ }
+}
+
+
+dReal dxJointBall::get( int num )
+{
+ switch ( num )
+ {
+ case dParamCFM:
+ return cfm;
+ case dParamERP:
+ return erp;
+ default:
+ return 0;
+ }
+}
+
+
+void dJointSetBallParam( dJointID j, int parameter, dReal value )
+{
+ dxJointBall* joint = ( dxJointBall* )j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, Ball );
+ joint->set( parameter, value );
+}
+
+
+dReal dJointGetBallParam( dJointID j, int parameter )
+{
+ dxJointBall* joint = ( dxJointBall* )j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, Ball );
+ return joint->get( parameter );
+}
+
+
+dJointType
+dxJointBall::type() const
+{
+ return dJointTypeBall;
+}
+
+sizeint
+dxJointBall::size() const
+{
+ return sizeof( *this );
+}
+
+void
+dxJointBall::setRelativeValues()
+{
+ dVector3 anchor;
+ dJointGetBallAnchor(this, anchor);
+ setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
+}
+
+
+
diff --git a/libs/ode-0.16.1/ode/src/joints/ball.h b/libs/ode-0.16.1/ode/src/joints/ball.h new file mode 100644 index 0000000..d8d22a5 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/ball.h @@ -0,0 +1,54 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_BALL_H_
+#define _ODE_JOINT_BALL_H_
+
+#include "joint.h"
+
+// ball and socket
+
+struct dxJointBall : public dxJoint
+{
+ dVector3 anchor1; // anchor w.r.t first body
+ dVector3 anchor2; // anchor w.r.t second body
+ dReal erp; // error reduction
+ dReal cfm; // constraint force mix in
+ void set( int num, dReal value );
+ dReal get( int num );
+
+ dxJointBall( dxWorld *w );
+ virtual void getSureMaxInfo( SureMaxInfo* info );
+ virtual void getInfo1( Info1* info );
+ virtual void getInfo2( dReal worldFPS, dReal worldERP,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex );
+ virtual dJointType type() const;
+ virtual sizeint size() const;
+
+ virtual void setRelativeValues();
+};
+
+
+#endif
+
diff --git a/libs/ode-0.16.1/ode/src/joints/contact.cpp b/libs/ode-0.16.1/ode/src/joints/contact.cpp new file mode 100644 index 0000000..5ab3482 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/contact.cpp @@ -0,0 +1,361 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "contact.h" +#include "joint_internal.h" + + + + //**************************************************************************** + // contact + +dxJointContact::dxJointContact(dxWorld *w) : + dxJoint(w) +{ +} + + +void +dxJointContact::getSureMaxInfo(SureMaxInfo* info) +{ + // ...as the actual m is very likely to hit the maximum + info->max_m = (contact.surface.mode&dContactRolling) ? 6 : 3; +} + + +void +dxJointContact::getInfo1(dxJoint::Info1 *info) +{ + // make sure mu's >= 0, then calculate number of constraint rows and number + // of unbounded rows. + int m = 1, nub = 0; + + // Anisotropic sliding and rolling and spinning friction + if (contact.surface.mode & dContactAxisDep) { + if (contact.surface.mu < 0) { + contact.surface.mu = 0; + } + else if (contact.surface.mu > 0) { + if (contact.surface.mu == dInfinity) { nub++; } + m++; + } + + if (contact.surface.mu2 < 0) { + contact.surface.mu2 = 0; + } + else if (contact.surface.mu2 > 0) { + if (contact.surface.mu2 == dInfinity) { nub++; } + m++; + } + + if ((contact.surface.mode & dContactRolling) != 0) { + if (contact.surface.rho < 0) { + contact.surface.rho = 0; + } + else { + if (contact.surface.rho == dInfinity) { nub++; } + m++; + } + + if (contact.surface.rho2 < 0) { + contact.surface.rho2 = 0; + } + else { + if (contact.surface.rho2 == dInfinity) { nub++; } + m++; + } + + if (contact.surface.rhoN < 0) { + contact.surface.rhoN = 0; + } + else { + if (contact.surface.rhoN == dInfinity) { nub++; } + m++; + } + } + } + else { + if (contact.surface.mu < 0) { + contact.surface.mu = 0; + } + else if (contact.surface.mu > 0) { + if (contact.surface.mu == dInfinity) { nub += 2; } + m += 2; + } + + if ((contact.surface.mode & dContactRolling) != 0) { + if (contact.surface.rho < 0) { + contact.surface.rho = 0; + } + else { + if (contact.surface.rho == dInfinity) { nub += 3; } + m += 3; + } + } + } + + the_m = m; + info->m = m; + info->nub = nub; +} + + +void +dxJointContact::getInfo2(dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex) +{ + enum + { + ROW_NORMAL, + + ROW__OPTIONAL_MIN, + }; + + const int surface_mode = contact.surface.mode; + + // set right hand side and cfm value for normal + dReal erp = (surface_mode & dContactSoftERP) != 0 ? contact.surface.soft_erp : worldERP; + dReal k = worldFPS * erp; + + dReal depth = contact.geom.depth - world->contactp.min_depth; + if (depth < 0) depth = 0; + + dReal motionN = (surface_mode & dContactMotionN) != 0 ? contact.surface.motionN : REAL(0.0); + const dReal pushout = k * depth + motionN; + + bool apply_bounce = (surface_mode & dContactBounce) != 0 && contact.surface.bounce_vel >= 0; + dReal outgoing = 0; + + // note: this cap should not limit bounce velocity + const dReal maxvel = world->contactp.max_vel; + dReal c = pushout > maxvel ? maxvel : pushout; + + // c1,c2 = contact points with respect to body PORs + dVector3 c1, c2 = { 0, }; + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + if ((flags & dJOINT_REVERSE) != 0) { + dCopyNegatedVector3(normal, contact.geom.normal); + } + else { + dCopyVector3(normal, contact.geom.normal); + } + + dxBody *b1 = node[1].body; + if (b1) { + dSubtractVectors3(c2, contact.geom.pos, b1->posr.pos); + // set Jacobian for b1 normal + dCopyNegatedVector3(J2 + ROW_NORMAL * rowskip + GI2__JL_MIN, normal); + dCalcVectorCross3(J2 + ROW_NORMAL * rowskip + GI2__JA_MIN, normal, c2); //== dCalcVectorCross3( J2 + GI2__JA_MIN, c2, normal ); dNegateVector3( J2 + GI2__JA_MIN ); + if (apply_bounce) { + outgoing /*+*/= dCalcVectorDot3(J2 + ROW_NORMAL * rowskip + GI2__JA_MIN, node[1].body->avel) + - dCalcVectorDot3(normal, node[1].body->lvel); + } + } + + dxBody *b0 = node[0].body; + dSubtractVectors3(c1, contact.geom.pos, b0->posr.pos); + // set Jacobian for b0 normal + dCopyVector3(J1 + ROW_NORMAL * rowskip + GI2__JL_MIN, normal); + dCalcVectorCross3(J1 + ROW_NORMAL * rowskip + GI2__JA_MIN, c1, normal); + if (apply_bounce) { + // calculate outgoing velocity (-ve for incoming contact) + outgoing += dCalcVectorDot3(J1 + ROW_NORMAL * rowskip + GI2__JA_MIN, node[0].body->avel) + + dCalcVectorDot3(normal, node[0].body->lvel); + } + + // deal with bounce + if (apply_bounce) { + dReal negated_outgoing = motionN - outgoing; + // only apply bounce if the outgoing velocity is greater than the + // threshold, and if the resulting c[rowNormal] exceeds what we already have. + dIASSERT(contact.surface.bounce_vel >= 0); + if (/*contact.surface.bounce_vel >= 0 &&*/ + negated_outgoing > contact.surface.bounce_vel) { + const dReal newc = contact.surface.bounce * negated_outgoing + motionN; + if (newc > c) { c = newc; } + } + } + + pairRhsCfm[ROW_NORMAL * pairskip + GI2_RHS] = c; + + if ((surface_mode & dContactSoftCFM) != 0) { + pairRhsCfm[ROW_NORMAL * pairskip + GI2_CFM] = contact.surface.soft_cfm; + } + + // set LCP limits for normal + pairLoHi[ROW_NORMAL * pairskip + GI2_LO] = 0; + pairLoHi[ROW_NORMAL * pairskip + GI2_HI] = dInfinity; + + + if (the_m > 1) { // if no friction, there is nothing else to do + // now do jacobian for tangential forces + dVector3 t1, t2; // two vectors tangential to normal + + if ((surface_mode & dContactFDir1) != 0) { // use fdir1 ? + dCopyVector3(t1, contact.fdir1); + dCalcVectorCross3(t2, normal, t1); + } + else { + dPlaneSpace(normal, t1, t2); + } + + int row = ROW__OPTIONAL_MIN; + int currRowSkip = row * rowskip, currPairSkip = row * pairskip; + + // first friction direction + const dReal mu = contact.surface.mu; + + if (mu > 0) { + dCopyVector3(J1 + currRowSkip + GI2__JL_MIN, t1); + dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, c1, t1); + + if (node[1].body) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, t1); + dCalcVectorCross3(J2 + currRowSkip + GI2__JA_MIN, t1, c2); //== dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, c2, t1 ); dNegateVector3( J2 + rowskip + GI2__JA_MIN ); + } + + // set right hand side + if ((surface_mode & dContactMotion1) != 0) { + pairRhsCfm[currPairSkip + GI2_RHS] = contact.surface.motion1; + } + // set slip (constraint force mixing) + if ((surface_mode & dContactSlip1) != 0) { + pairRhsCfm[currPairSkip + GI2_CFM] = contact.surface.slip1; + } + + // set LCP bounds and friction index. this depends on the approximation + // mode + pairLoHi[currPairSkip + GI2_LO] = -mu; + pairLoHi[currPairSkip + GI2_HI] = mu; + + if ((surface_mode & dContactApprox1_1) != 0) { + findex[row] = 0; + } + + ++row; + currRowSkip += rowskip; currPairSkip += pairskip; + } + + // second friction direction + const dReal mu2 = (surface_mode & dContactMu2) != 0 ? contact.surface.mu2 : mu; + + if (mu2 > 0) { + dCopyVector3(J1 + currRowSkip + GI2__JL_MIN, t2); + dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, c1, t2); + + if (node[1].body) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, t2); + dCalcVectorCross3(J2 + currRowSkip + GI2__JA_MIN, t2, c2); //== dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, c2, t2 ); dNegateVector3( J2 + currRowSkip + GI2__JA_MIN ); + } + + // set right hand side + if ((surface_mode & dContactMotion2) != 0) { + pairRhsCfm[currPairSkip + GI2_RHS] = contact.surface.motion2; + } + // set slip (constraint force mixing) + if ((surface_mode & dContactSlip2) != 0) { + pairRhsCfm[currPairSkip + GI2_CFM] = contact.surface.slip2; + } + + // set LCP bounds and friction index. this depends on the approximation + // mode + pairLoHi[currPairSkip + GI2_LO] = -mu2; + pairLoHi[currPairSkip + GI2_HI] = mu2; + + if ((surface_mode & dContactApprox1_2) != 0) { + findex[row] = 0; + } + + ++row; + currRowSkip += rowskip; currPairSkip += pairskip; + } + + // Handle rolling/spinning friction + if ((surface_mode & dContactRolling) != 0) { + + const dReal *const ax[3] = { + t1, // Rolling around t1 creates movement parallel to t2 + t2, + normal // Spinning axis + }; + + const int approx_bits[3] = { dContactApprox1_1, dContactApprox1_2, dContactApprox1_N }; + + // Get the coefficients + dReal rho[3]; + rho[0] = contact.surface.rho; + if ((surface_mode & dContactAxisDep) != 0) { + rho[1] = contact.surface.rho2; + rho[2] = contact.surface.rhoN; + } + else { + rho[1] = rho[0]; + rho[2] = rho[0]; + } + + for (int i = 0; i != 3; ++i) { + if (rho[i] > 0) { + // Set the angular axis + dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, ax[i]); + + if (b1) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, ax[i]); + } + + // Set the lcp limits + pairLoHi[currPairSkip + GI2_LO] = -rho[i]; + pairLoHi[currPairSkip + GI2_HI] = rho[i]; + + // Should we use proportional force? + if ((surface_mode & approx_bits[i]) != 0) { + // Make limits proportional to normal force + findex[row] = 0; + } + + ++row; + currRowSkip += rowskip; currPairSkip += pairskip; + } + } + } + } +} + +dJointType +dxJointContact::type() const +{ + return dJointTypeContact; +} + + +sizeint +dxJointContact::size() const +{ + return sizeof(*this); +} + diff --git a/libs/ode-0.16.1/ode/src/joints/contact.h b/libs/ode-0.16.1/ode/src/joints/contact.h new file mode 100644 index 0000000..604a4fb --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/contact.h @@ -0,0 +1,48 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_CONTACT_H_ +#define _ODE_JOINT_CONTACT_H_ + +#include "joint.h" + +// contact + +struct dxJointContact : public dxJoint +{ + int the_m; // number of rows computed by getInfo1 + dContact contact; + + dxJointContact( dxWorld* w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex); + virtual dJointType type() const; + virtual sizeint size() const; +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/dball.cpp b/libs/ode-0.16.1/ode/src/joints/dball.cpp new file mode 100644 index 0000000..3754646 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/dball.cpp @@ -0,0 +1,314 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "dball.h" +#include "joint_internal.h" + +/* + * Double Ball joint: tries to maintain a fixed distance between two anchor + * points. + */ + +dxJointDBall::dxJointDBall(dxWorld *w) : + dxJoint(w) +{ + dSetZero(anchor1, 3); + dSetZero(anchor2, 3); + targetDistance = 0; + erp = world->global_erp; + cfm = world->global_cfm; +} + +void +dxJointDBall::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 1; +} +void +dxJointDBall::getInfo1( dxJoint::Info1 *info ) +{ + info->m = 1; + info->nub = 1; +} + +void +dxJointDBall::getInfo2( dReal worldFPS, dReal /*worldERP*/, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + dVector3 globalA1, globalA2; + dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1); + + if (node[1].body) { + dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2); + } else { + dCopyVector3(globalA2, anchor2); + } + + dVector3 q; + dSubtractVectors3(q, globalA1, globalA2); + +#ifdef dSINGLE + const dReal MIN_LENGTH = REAL(1e-7); +#else + const dReal MIN_LENGTH = REAL(1e-12); +#endif + + if (dCalcVectorLength3(q) < MIN_LENGTH) { + // too small, let's choose an arbitrary direction + // heuristic: difference in velocities at anchors + dVector3 v1, v2; + dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1); + + if (node[1].body) { + dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2); + } else { + dZeroVector3(v2); + } + + dSubtractVectors3(q, v1, v2); + + if (dCalcVectorLength3(q) < MIN_LENGTH) { + // this direction is as good as any + dAssignVector3(q, 1, 0, 0); + } + } + dNormalize3(q); + + dCopyVector3(J1 + GI2__JL_MIN, q); + + dVector3 relA1; + dBodyVectorToWorld(node[0].body, + anchor1[0], anchor1[1], anchor1[2], + relA1); + + dMatrix3 a1m; + dZeroMatrix3(a1m); + dSetCrossMatrixMinus(a1m, relA1, 4); + + dMultiply1_331(J1 + GI2__JA_MIN, a1m, q); + + if (node[1].body) { + dCopyNegatedVector3(J2 + GI2__JL_MIN, q); + + dVector3 relA2; + dBodyVectorToWorld(node[1].body, + anchor2[0], anchor2[1], anchor2[2], + relA2); + dMatrix3 a2m; + dZeroMatrix3(a2m); + dSetCrossMatrixPlus(a2m, relA2, 4); + dMultiply1_331(J2 + GI2__JA_MIN, a2m, q); + } + + const dReal k = worldFPS * this->erp; + pairRhsCfm[GI2_RHS] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2)); + pairRhsCfm[GI2_CFM] = this->cfm; +} + + +void +dxJointDBall::updateTargetDistance() +{ + dVector3 p1, p2; + + if (node[0].body) + dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], p1); + else + dCopyVector3(p1, anchor1); + if (node[1].body) + dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], p2); + else + dCopyVector3(p2, anchor2); + + targetDistance = dCalcPointsDistance3(p1, p2); +} + + +void dJointSetDBallAnchor1( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + + if ( joint->flags & dJOINT_REVERSE ) { + if (joint->node[1].body) + dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2); + else { + joint->anchor2[0] = x; + joint->anchor2[1] = y; + joint->anchor2[2] = z; + } + } else { + if (joint->node[0].body) + dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1); + else { + joint->anchor1[0] = x; + joint->anchor1[1] = y; + joint->anchor1[2] = z; + } + } + + joint->updateTargetDistance(); +} + + +void dJointSetDBallAnchor2( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + + + if ( joint->flags & dJOINT_REVERSE ) { + if (joint->node[0].body) + dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1); + else { + joint->anchor1[0] = x; + joint->anchor1[1] = y; + joint->anchor1[2] = z; + } + } else { + if (joint->node[1].body) + dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2); + else { + joint->anchor2[0] = x; + joint->anchor2[1] = y; + joint->anchor2[2] = z; + } + } + + joint->updateTargetDistance(); +} + +dReal dJointGetDBallDistance(dJointID j) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->targetDistance; +} + +void dJointSetDBallDistance(dJointID j, dReal dist) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + dUASSERT( dist>=0, "target distance must be non-negative" ); + + joint->targetDistance = dist; +} + + +void dJointGetDBallAnchor1( dJointID j, dVector3 result ) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + + if ( joint->flags & dJOINT_REVERSE ) { + if (joint->node[1].body) + dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result); + else + dCopyVector3(result, joint->anchor2); + } else { + if (joint->node[0].body) + dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result); + else + dCopyVector3(result, joint->anchor1); + } +} + + +void dJointGetDBallAnchor2( dJointID j, dVector3 result ) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + + if ( joint->flags & dJOINT_REVERSE ) { + if (joint->node[0].body) + dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result); + else + dCopyVector3(result, joint->anchor1); + } else { + if (joint->node[1].body) + dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result); + else + dCopyVector3(result, joint->anchor2); + } +} + + +void dJointSetDBallParam( dJointID j, int parameter, dReal value ) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + + switch ( parameter ) { + case dParamCFM: + joint->cfm = value; + break; + case dParamERP: + joint->erp = value; + break; + } +} + + +dReal dJointGetDBallParam( dJointID j, int parameter ) +{ + dxJointDBall* joint = static_cast<dxJointDBall*>(j); + dUASSERT( joint, "bad joint argument" ); + + switch ( parameter ) { + case dParamCFM: + return joint->cfm; + case dParamERP: + return joint->erp; + default: + return 0; + } +} + + +dJointType +dxJointDBall::type() const +{ + return dJointTypeDBall; +} + +sizeint +dxJointDBall::size() const +{ + return sizeof( *this ); +} + +void +dxJointDBall::setRelativeValues() +{ + updateTargetDistance(); +} + + + diff --git a/libs/ode-0.16.1/ode/src/joints/dball.h b/libs/ode-0.16.1/ode/src/joints/dball.h new file mode 100644 index 0000000..e52fc6c --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/dball.h @@ -0,0 +1,58 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_DBALL_H_ +#define _ODE_JOINT_DBALL_H_ + +#include "joint.h" + +// ball and socket + +struct dxJointDBall : public dxJoint +{ + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dReal erp; // error reduction + dReal cfm; // constraint force mix in + dReal targetDistance; + + void set( int num, dReal value ); + dReal get( int num ); + + void updateTargetDistance(); + + dxJointDBall( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + virtual void setRelativeValues(); +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/dhinge.cpp b/libs/ode-0.16.1/ode/src/joints/dhinge.cpp new file mode 100644 index 0000000..e300bf5 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/dhinge.cpp @@ -0,0 +1,220 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "dhinge.h" +#include "joint_internal.h" + +/* + * Double Hinge joint + */ + +dxJointDHinge::dxJointDHinge(dxWorld* w) : + dxJointDBall(w) +{ + dSetZero(axis1, 3); + dSetZero(axis2, 3); +} + + +void +dxJointDHinge::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 4; +} + + +void +dxJointDHinge::getInfo1( dxJoint::Info1* info ) +{ + info->m = 4; + info->nub = 4; +} + + +void +dxJointDHinge::getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + dxJointDBall::getInfo2( worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, pairLoHi, findex ); // sets row0 + + dVector3 globalAxis1; + dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1); + + dxBody *body1 = node[1].body; + + // angular constraints, perpendicular to axis + dVector3 p, q; + dPlaneSpace(globalAxis1, p, q); + + dCopyVector3(J1 + rowskip + GI2__JA_MIN, p); + if ( body1 ) { + dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, p); + } + + dCopyVector3(J1 + 2 * rowskip + GI2__JA_MIN, q); + if ( body1 ) { + dCopyNegatedVector3(J2 + 2 * rowskip + GI2__JA_MIN, q); + } + + dVector3 globalAxis2; + if ( body1 ) { + dBodyVectorToWorld(body1, axis2[0], axis2[1], axis2[2], globalAxis2); + } else { + dCopyVector3(globalAxis2, axis2); + } + + // similar to the hinge joint + dVector3 u; + dCalcVectorCross3(u, globalAxis1, globalAxis2); + + const dReal k = worldFPS * this->erp; + pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( u, p ); + pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( u, q ); + + + + + /* + * Constraint along the axis: translation along it should couple angular movement. + * This is just the ball-and-socket derivation, projected onto the hinge axis, + * producing a single constraint at the end. + * + * The choice of "ball" position can be arbitrary; we could place it at the center + * of one of the bodies, canceling out its rotational jacobian; or we could make + * everything symmetrical by just placing at the midpoint between the centers. + * + * I like symmetry, so I'll use the second approach here. I'll call the midpoint h. + * + * Of course, if the second body is NULL, the first body is pretty much locked + * along this axis, and the linear constraint is enough. + */ + + int rowskip_mul_3 = 3 * rowskip; + dCopyVector3(J1 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1); + + if ( body1 ) { + dVector3 h; + dAddScaledVectors3(h, node[0].body->posr.pos, body1->posr.pos, -0.5, 0.5); + + dCalcVectorCross3(J1 + rowskip_mul_3 + GI2__JA_MIN, h, globalAxis1); + + dCopyNegatedVector3(J2 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1); + dCopyVector3(J2 + rowskip_mul_3 + GI2__JA_MIN, J1 + rowskip_mul_3 + GI2__JA_MIN); + } + + // error correction: both anchors should lie on the same plane perpendicular to the axis + dVector3 globalA1, globalA2; + dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1); + + if ( body1 ) { + dBodyGetRelPointPos(body1, anchor2[0], anchor2[1], anchor2[2], globalA2); + } else { + dCopyVector3(globalA2, anchor2); + } + + dVector3 d; + dSubtractVectors3(d, globalA1, globalA2); // displacement error + pairRhsCfm[3 * pairskip + GI2_RHS] = -k * dCalcVectorDot3(globalAxis1, d); +} + +void dJointSetDHingeAxis( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointDHinge* joint = static_cast<dxJointDHinge*>(j); + dUASSERT( joint, "bad joint argument" ); + + dBodyVectorFromWorld(joint->node[0].body, x, y, z, joint->axis1); + if (joint->node[1].body) + dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2); + else { + joint->axis2[0] = x; + joint->axis2[1] = y; + joint->axis2[2] = z; + } + dNormalize3(joint->axis1); + dNormalize3(joint->axis2); +} + +void dJointGetDHingeAxis( dJointID j, dVector3 result ) +{ + dxJointDHinge* joint = static_cast<dxJointDHinge*>(j); + dUASSERT( joint, "bad joint argument" ); + + dBodyVectorToWorld(joint->node[0].body, joint->axis1[0], joint->axis1[1], joint->axis1[2], result); +} + + +void dJointSetDHingeAnchor1( dJointID j, dReal x, dReal y, dReal z ) +{ + dJointSetDBallAnchor1(j, x, y, z); +} + + +void dJointSetDHingeAnchor2( dJointID j, dReal x, dReal y, dReal z ) +{ + dJointSetDBallAnchor2(j, x, y, z); +} + +dReal dJointGetDHingeDistance(dJointID j) +{ + return dJointGetDBallDistance(j); +} + + +void dJointGetDHingeAnchor1( dJointID j, dVector3 result ) +{ + dJointGetDBallAnchor1(j, result); +} + + +void dJointGetDHingeAnchor2( dJointID j, dVector3 result ) +{ + dJointGetDBallAnchor2(j, result); +} + + +void dJointSetDHingeParam( dJointID j, int parameter, dReal value ) +{ + dJointSetDBallParam(j, parameter, value); +} + + +dReal dJointGetDHingeParam( dJointID j, int parameter ) +{ + return dJointGetDBallParam(j, parameter); +} + +dJointType +dxJointDHinge::type() const +{ + return dJointTypeDHinge; +} + +sizeint +dxJointDHinge::size() const +{ + return sizeof( *this ); +} diff --git a/libs/ode-0.16.1/ode/src/joints/dhinge.h b/libs/ode-0.16.1/ode/src/joints/dhinge.h new file mode 100644 index 0000000..efc5688 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/dhinge.h @@ -0,0 +1,46 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_DHINGE_ +#define _ODE_JOINT_DHINGE_ + +#include "dball.h" + +struct dxJointDHinge : public dxJointDBall +{ + dVector3 axis1, axis2; + + dxJointDHinge(dxWorld *w); + + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + +}; + + +#endif diff --git a/libs/ode-0.16.1/ode/src/joints/fixed.cpp b/libs/ode-0.16.1/ode/src/joints/fixed.cpp new file mode 100644 index 0000000..527bf48 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/fixed.cpp @@ -0,0 +1,216 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "fixed.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// fixed joint
+
+dxJointFixed::dxJointFixed ( dxWorld *w ) :
+ dxJoint ( w )
+{
+ dSetZero ( offset, 4 );
+ dSetZero ( qrel, 4 );
+ erp = world->global_erp;
+ cfm = world->global_cfm;
+}
+
+
+void
+dxJointFixed::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = 6;
+}
+
+
+void
+dxJointFixed::getInfo1 ( dxJoint::Info1 *info )
+{
+ info->m = 6;
+ info->nub = 6;
+}
+
+
+void
+dxJointFixed::getInfo2 ( dReal worldFPS, dReal worldERP,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+{
+ // Three rows for orientation
+ setFixedOrientation ( this, worldFPS, worldERP,
+ rowskip, J1 + dSA__MAX * rowskip, J2 + dSA__MAX * rowskip,
+ pairskip, pairRhsCfm + dSA__MAX * pairskip, qrel );
+
+ // Three rows for position.
+ // set Jacobian
+ J1[GI2_JLX] = 1;
+ J1[rowskip + GI2_JLY] = 1;
+ J1[2 * rowskip + GI2_JLZ] = 1;
+
+ dReal k = worldFPS * this->erp;
+ dxBody *b0 = node[0].body, *b1 = node[1].body;
+
+ dVector3 ofs;
+ dMultiply0_331 ( ofs, b0->posr.R, offset );
+
+ if ( b1 ) {
+ dSetCrossMatrixPlus( J1 + GI2__JA_MIN, ofs, rowskip );
+
+ J2[GI2_JLX] = -1;
+ J2[rowskip + GI2_JLY] = -1;
+ J2[2 * rowskip + GI2_JLZ] = -1;
+ }
+
+ // set right hand side for the first three rows (linear)
+ if ( b1 ) {
+ for ( int j = 0, currPairSkip = 0; j < 3; currPairSkip += pairskip, ++j ) {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * ( b1->posr.pos[j] - b0->posr.pos[j] + ofs[j] );
+ }
+ } else {
+ for ( int j = 0, currPairSkip = 0; j < 3; currPairSkip += pairskip, ++j ) {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * ( offset[j] - b0->posr.pos[j] );
+ }
+ }
+
+ dReal cfm = this->cfm;
+ pairRhsCfm[GI2_CFM] = cfm;
+ pairRhsCfm[pairskip + GI2_CFM] = cfm;
+ pairRhsCfm[2 * pairskip + GI2_CFM] = cfm;
+}
+
+
+void dJointSetFixed ( dJointID j )
+{
+ dxJointFixed* joint = ( dxJointFixed* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Fixed );
+ int i;
+
+ // This code is taken from dJointSetSliderAxis(), we should really put the
+ // common code in its own function.
+ // compute the offset between the bodies
+ if ( joint->node[0].body )
+ {
+ if ( joint->node[1].body )
+ {
+ dReal ofs[4];
+ for ( i = 0; i < 4; i++ )
+ ofs[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i];
+ dMultiply1_331 ( joint->offset, joint->node[0].body->posr.R, ofs );
+ }
+ else
+ {
+ joint->offset[0] = joint->node[0].body->posr.pos[0];
+ joint->offset[1] = joint->node[0].body->posr.pos[1];
+ joint->offset[2] = joint->node[0].body->posr.pos[2];
+ }
+ }
+
+ joint->computeInitialRelativeRotation();
+}
+
+void dxJointFixed::set ( int num, dReal value )
+{
+ switch ( num )
+ {
+ case dParamCFM:
+ cfm = value;
+ break;
+ case dParamERP:
+ erp = value;
+ break;
+ }
+}
+
+
+dReal dxJointFixed::get ( int num )
+{
+ switch ( num )
+ {
+ case dParamCFM:
+ return cfm;
+ case dParamERP:
+ return erp;
+ default:
+ return 0;
+ }
+}
+
+
+void dJointSetFixedParam ( dJointID j, int parameter, dReal value )
+{
+ dxJointFixed* joint = ( dxJointFixed* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Fixed );
+ joint->set ( parameter, value );
+}
+
+
+dReal dJointGetFixedParam ( dJointID j, int parameter )
+{
+ dxJointFixed* joint = ( dxJointFixed* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Fixed );
+ return joint->get ( parameter );
+}
+
+
+dJointType
+dxJointFixed::type() const
+{
+ return dJointTypeFixed;
+}
+
+
+sizeint
+dxJointFixed::size() const
+{
+ return sizeof ( *this );
+}
+
+void
+dxJointFixed::computeInitialRelativeRotation()
+{
+ if (node[0].body )
+ {
+ if (node[1].body )
+ {
+ dQMultiply1 (qrel, node[0].body->q, node[1].body->q );
+ }
+ else
+ {
+ // set qrel to the transpose of the first body q
+ qrel[0] = node[0].body->q[0];
+ qrel[1] = -node[0].body->q[1];
+ qrel[2] = -node[0].body->q[2];
+ qrel[3] = -node[0].body->q[3];
+ }
+ }
+}
+
diff --git a/libs/ode-0.16.1/ode/src/joints/fixed.h b/libs/ode-0.16.1/ode/src/joints/fixed.h new file mode 100644 index 0000000..c0f6932 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/fixed.h @@ -0,0 +1,54 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_FIXED_H_ +#define _ODE_JOINT_FIXED_H_ + +#include "joint.h" + + +// fixed + +struct dxJointFixed : public dxJoint +{ + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // relative offset between the bodies + dReal erp; // error reduction parameter + dReal cfm; // constraint force mix-in + void set ( int num, dReal value ); + dReal get ( int num ); + + dxJointFixed ( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1 ( Info1* info ); + virtual void getInfo2 ( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + void computeInitialRelativeRotation(); +}; + + +#endif diff --git a/libs/ode-0.16.1/ode/src/joints/hinge.cpp b/libs/ode-0.16.1/ode/src/joints/hinge.cpp new file mode 100644 index 0000000..70dcd78 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/hinge.cpp @@ -0,0 +1,394 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "hinge.h" +#include "joint_internal.h" + + +//**************************************************************************** +// hinge + +dxJointHinge::dxJointHinge( dxWorld *w ) : + dxJoint( w ) +{ + dSetZero( anchor1, 4 ); + dSetZero( anchor2, 4 ); + dSetZero( axis1, 4 ); + axis1[0] = 1; + dSetZero( axis2, 4 ); + axis2[0] = 1; + dSetZero( qrel, 4 ); + limot.init( world ); +} + + +void +dxJointHinge::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + +void +dxJointHinge::getInfo1( dxJoint::Info1 *info ) +{ + info->nub = 5; + + // see if joint is powered + if ( limot.fmax > 0 ) + info->m = 6; // powered hinge needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + if (( limot.lostop >= -M_PI || limot.histop <= M_PI ) && + limot.lostop <= limot.histop ) + { + dReal angle = getHingeAngle( node[0].body, + node[1].body, + axis1, qrel ); + if ( limot.testRotationalLimit( angle ) ) + info->m = 6; + } +} + + +void dxJointHinge::getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + // set the three ball-and-socket rows + setBall( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 ); + + // set the two hinge rows. the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body + dVector3 p, q; // plane space vectors for ax1 + dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); + dPlaneSpace( ax1, p, q ); + + dxBody *body1 = node[1].body; + + int currRowSkip = 3 * rowskip; + dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, p); + if ( body1 ) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, p); + } + + currRowSkip += rowskip; + dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, q); + if ( body1 ) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, q); + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + + dVector3 b; + if ( body1 ) { + dVector3 ax2; + dMultiply0_331( ax2, body1->posr.R, axis2 ); + dCalcVectorCross3( b, ax1, ax2 ); + } else { + dCalcVectorCross3( b, ax1, axis2 ); + } + + dReal k = worldFPS * worldERP; + int currPairSkip = 3 * pairskip; + pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( b, p ); + currPairSkip += pairskip; + pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( b, q ); + + // if the hinge is powered, or has joint limits, add in the stuff + currRowSkip += rowskip; + currPairSkip += pairskip; + limot.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 ); +} + + + +void dJointSetHingeAnchor( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge ); + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + joint->computeInitialRelativeRotation(); +} + + +void dJointSetHingeAnchorDelta( dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge ); + + if ( joint->node[0].body ) + { + dReal q[4]; + q[0] = x - joint->node[0].body->posr.pos[0]; + q[1] = y - joint->node[0].body->posr.pos[1]; + q[2] = z - joint->node[0].body->posr.pos[2]; + q[3] = 0; + dMultiply1_331( joint->anchor1, joint->node[0].body->posr.R, q ); + + if ( joint->node[1].body ) + { + q[0] = x - joint->node[1].body->posr.pos[0]; + q[1] = y - joint->node[1].body->posr.pos[1]; + q[2] = z - joint->node[1].body->posr.pos[2]; + q[3] = 0; + dMultiply1_331( joint->anchor2, joint->node[1].body->posr.R, q ); + } + else + { + // Move the relative displacement between the passive body and the + // anchor in the same direction as the passive body has just moved + joint->anchor2[0] = x + dx; + joint->anchor2[1] = y + dy; + joint->anchor2[2] = z + dz; + } + } + joint->anchor1[3] = 0; + joint->anchor2[3] = 0; + + joint->computeInitialRelativeRotation(); +} + + + +void dJointSetHingeAxis( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge ); + setAxes( joint, x, y, z, joint->axis1, joint->axis2 ); + joint->computeInitialRelativeRotation(); +} + + +void dJointSetHingeAxisOffset( dJointID j, dReal x, dReal y, dReal z, dReal dangle ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge ); + setAxes( joint, x, y, z, joint->axis1, joint->axis2 ); + joint->computeInitialRelativeRotation(); + + if ( joint->flags & dJOINT_REVERSE ) dangle = -dangle; + + dQuaternion qAngle, qOffset; + dQFromAxisAndAngle(qAngle, x, y, z, dangle); + dQMultiply3(qOffset, qAngle, joint->qrel); + joint->qrel[0] = qOffset[0]; + joint->qrel[1] = qOffset[1]; + joint->qrel[2] = qOffset[2]; + joint->qrel[3] = qOffset[3]; +} + + + +void dJointGetHingeAnchor( dJointID j, dVector3 result ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Hinge ); + if ( joint->flags & dJOINT_REVERSE ) + getAnchor2( joint, result, joint->anchor2 ); + else + getAnchor( joint, result, joint->anchor1 ); +} + + +void dJointGetHingeAnchor2( dJointID j, dVector3 result ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Hinge ); + if ( joint->flags & dJOINT_REVERSE ) + getAnchor( joint, result, joint->anchor1 ); + else + getAnchor2( joint, result, joint->anchor2 ); +} + + +void dJointGetHingeAxis( dJointID j, dVector3 result ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Hinge ); + getAxis( joint, result, joint->axis1 ); +} + + +void dJointSetHingeParam( dJointID j, int parameter, dReal value ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge ); + joint->limot.set( parameter, value ); +} + + +dReal dJointGetHingeParam( dJointID j, int parameter ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge ); + return joint->limot.get( parameter ); +} + + +dReal dJointGetHingeAngle( dJointID j ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dAASSERT( joint ); + checktype( joint, Hinge ); + if ( joint->node[0].body ) + { + dReal ang = getHingeAngle( joint->node[0].body, + joint->node[1].body, + joint->axis1, + joint->qrel ); + if ( joint->flags & dJOINT_REVERSE ) + return -ang; + else + return ang; + } + else return 0; +} + + +dReal dJointGetHingeAngleRate( dJointID j ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dAASSERT( joint ); + checktype( joint, Hinge ); + if ( joint->node[0].body ) + { + dVector3 axis; + dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 ); + dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + if ( joint->flags & dJOINT_REVERSE ) rate = - rate; + return rate; + } + else return 0; +} + + +void dJointAddHingeTorque( dJointID j, dReal torque ) +{ + dxJointHinge* joint = ( dxJointHinge* )j; + dVector3 axis; + dAASSERT( joint ); + checktype( joint, Hinge ); + + if ( joint->flags & dJOINT_REVERSE ) + torque = -torque; + + getAxis( joint, axis, joint->axis1 ); + axis[0] *= torque; + axis[1] *= torque; + axis[2] *= torque; + + if ( joint->node[0].body != 0 ) + dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] ); + if ( joint->node[1].body != 0 ) + dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] ); +} + + +dJointType +dxJointHinge::type() const +{ + return dJointTypeHinge; +} + + + +sizeint +dxJointHinge::size() const +{ + return sizeof( *this ); +} + + +void +dxJointHinge::setRelativeValues() +{ + dVector3 vec; + dJointGetHingeAnchor(this, vec); + setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 ); + + dJointGetHingeAxis(this, vec); + setAxes( this, vec[0], vec[1], vec[2], axis1, axis2 ); + computeInitialRelativeRotation(); +} + + +/// Compute initial relative rotation body1 -> body2, or env -> body1 +void +dxJointHinge::computeInitialRelativeRotation() +{ + if ( node[0].body ) + { + if ( node[1].body ) + { + dQMultiply1( qrel, node[0].body->q, node[1].body->q ); + } + else + { + // set qrel to the transpose of the first body q + qrel[0] = node[0].body->q[0]; + qrel[1] = -node[0].body->q[1]; + qrel[2] = -node[0].body->q[2]; + qrel[3] = -node[0].body->q[3]; + } + } +} + diff --git a/libs/ode-0.16.1/ode/src/joints/hinge.h b/libs/ode-0.16.1/ode/src/joints/hinge.h new file mode 100644 index 0000000..0fb4dba --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/hinge.h @@ -0,0 +1,57 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_HINGE_H_ +#define _ODE_JOINT_HINGE_H_ + +#include "joint.h" + + +// hinge + +struct dxJointHinge : public dxJoint +{ + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dxJointLimitMotor limot; // limit and motor information + + dxJointHinge( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + virtual void setRelativeValues(); + + void computeInitialRelativeRotation(); +}; + + + +#endif diff --git a/libs/ode-0.16.1/ode/src/joints/hinge2.cpp b/libs/ode-0.16.1/ode/src/joints/hinge2.cpp new file mode 100644 index 0000000..89d5e30 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/hinge2.cpp @@ -0,0 +1,546 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "hinge2.h" +#include "joint_internal.h" + + + + +//**************************************************************************** +// hinge 2. note that this joint must be attached to two bodies for it to work + +dReal +dxJointHinge2::measureAngle1() const +{ + // bring axis 2 into first body's reference frame + dVector3 p, q; + if (node[1].body) + dMultiply0_331( p, node[1].body->posr.R, axis2 ); + else + dCopyVector3(p, axis2); + + if (node[0].body) + dMultiply1_331( q, node[0].body->posr.R, p ); + else + dCopyVector3(q, p); + + dReal x = dCalcVectorDot3( v1, q ); + dReal y = dCalcVectorDot3( v2, q ); + return -dAtan2( y, x ); +} + +dReal +dxJointHinge2::measureAngle2() const +{ + // bring axis 1 into second body's reference frame + dVector3 p, q; + if (node[0].body) + dMultiply0_331( p, node[0].body->posr.R, axis1 ); + else + dCopyVector3(p, axis1); + + if (node[1].body) + dMultiply1_331( q, node[1].body->posr.R, p ); + else + dCopyVector3(q, p); + + dReal x = dCalcVectorDot3( w1, q ); + dReal y = dCalcVectorDot3( w2, q ); + return -dAtan2( y, x ); +} + + +dxJointHinge2::dxJointHinge2( dxWorld *w ) : + dxJoint( w ) +{ + dSetZero( anchor1, 4 ); + dSetZero( anchor2, 4 ); + dSetZero( axis1, 4 ); + axis1[0] = 1; + dSetZero( axis2, 4 ); + axis2[1] = 1; + c0 = 0; + s0 = 0; + + dSetZero( v1, 4 ); + v1[0] = 1; + dSetZero( v2, 4 ); + v2[1] = 1; + + limot1.init( world ); + limot2.init( world ); + + susp_erp = world->global_erp; + susp_cfm = world->global_cfm; + + flags |= dJOINT_TWOBODIES; +} + + +void +dxJointHinge2::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + +void +dxJointHinge2::getInfo1( dxJoint::Info1 *info ) +{ + info->m = 4; + info->nub = 4; + + // see if we're powered or at a joint limit for axis 1 + limot1.limit = 0; + if (( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) && + limot1.lostop <= limot1.histop ) + { + dReal angle = measureAngle1(); + limot1.testRotationalLimit( angle ); + } + if ( limot1.limit || limot1.fmax > 0 ) info->m++; + + // see if we're powering axis 2 (we currently never limit this axis) + limot2.limit = 0; + if ( limot2.fmax > 0 ) info->m++; +} + + +//////////////////////////////////////////////////////////////////////////////// +/// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are +/// relative to body 1 and 2 initially) and then computes the constrained +/// rotational axis as the cross product of ax1 and ax2. +/// the sin and cos of the angle between axis 1 and 2 is computed, this comes +/// from dot and cross product rules. +/// +/// @param ax1 Will contain the joint axis1 in world frame +/// @param ax2 Will contain the joint axis2 in world frame +/// @param axis Will contain the cross product of ax1 x ax2 +/// @param sin_angle +/// @param cos_angle +//////////////////////////////////////////////////////////////////////////////// +void +dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross, + dReal &sin_angle, dReal &cos_angle) const +{ + dMultiply0_331 (ax1, node[0].body->posr.R, axis1); + dMultiply0_331 (ax2, node[1].body->posr.R, axis2); + dCalcVectorCross3(axCross,ax1,ax2); + sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]); + cos_angle = dCalcVectorDot3 (ax1,ax2); +} + + +void +dxJointHinge2::getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + // get information we need to set the hinge row + dReal s, c; + dVector3 q; + + dVector3 ax1, ax2; + getAxisInfo( ax1, ax2, q, s, c ); + dNormalize3( q ); // @@@ quicker: divide q by s ? + + // set the three ball-and-socket rows (aligned to the suspension axis ax1) + setBall2( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2, ax1, susp_erp ); + // set parameter for the suspension + pairRhsCfm[GI2_CFM] = susp_cfm; + + // set the hinge row + int currRowSkip = 3 * rowskip; + dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, q); + if ( node[1].body ) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, q); + } + + // compute the right hand side for the constrained rotational DOF. + // axis 1 and axis 2 are separated by an angle `theta'. the desired + // separation angle is theta0. sin(theta0) and cos(theta0) are recorded + // in the joint structure. the correcting angular velocity is: + // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize + // = (erp*fps) * (theta0-theta) + // (theta0-theta) can be computed using the following small-angle-difference + // approximation: + // theta0-theta ~= tan(theta0-theta) + // = sin(theta0-theta)/cos(theta0-theta) + // = (c*s0 - s*c0) / (c*c0 + s*s0) + // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 + // where c = cos(theta), s = sin(theta) + // c0 = cos(theta0), s0 = sin(theta0) + + dReal k = worldFPS * worldERP; + + int currPairSkip = 3 * pairskip; + pairRhsCfm[currPairSkip + GI2_RHS] = k * ( c0 * s - this->s0 * c ); + + currRowSkip += rowskip; currPairSkip += pairskip; + // if the axis1 hinge is powered, or has joint limits, add in more stuff + if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) { + currRowSkip += rowskip; currPairSkip += pairskip; + } + + // if the axis2 hinge is powered, add in more stuff + limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 ); +} + + +// compute vectors v1 and v2 (embedded in body1), used to measure angle +// between body 1 and body 2 + +void +dxJointHinge2::makeV1andV2() +{ + if ( node[0].body ) + { + // get axis 1 and 2 in global coords + dVector3 ax1, ax2, v; + dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); + dMultiply0_331( ax2, node[1].body->posr.R, axis2 ); + + // modify axis 2 so it's perpendicular to axis 1 + dReal k = dCalcVectorDot3( ax1, ax2 ); + dAddVectorScaledVector3(ax2, ax2, ax1, -k); + + if (dxSafeNormalize3( ax2 )) { + // make v1 = modified axis2, v2 = axis1 x (modified axis2) + dCalcVectorCross3( v, ax1, ax2 ); + dMultiply1_331( v1, node[0].body->posr.R, ax2 ); + dMultiply1_331( v2, node[0].body->posr.R, v ); + } + else { + dUASSERT(false, "Hinge2 axes must be chosen to be linearly independent"); + } + } +} + +// same as above, but for the second axis + +void +dxJointHinge2::makeW1andW2() +{ + if ( node[1].body ) + { + // get axis 1 and 2 in global coords + dVector3 ax1, ax2, w; + dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); + dMultiply0_331( ax2, node[1].body->posr.R, axis2 ); + + // modify axis 1 so it's perpendicular to axis 2 + dReal k = dCalcVectorDot3( ax2, ax1 ); + dAddVectorScaledVector3(ax1, ax1, ax2, -k); + + if (dxSafeNormalize3( ax1 )) { + // make w1 = modified axis1, w2 = axis2 x (modified axis1) + dCalcVectorCross3( w, ax2, ax1 ); + dMultiply1_331( w1, node[1].body->posr.R, ax1 ); + dMultiply1_331( w2, node[1].body->posr.R, w ); + } + else { + dUASSERT(false, "Hinge2 axes must be chosen to be linearly independent"); + } + } +} + + +/*ODE_API */ +void dJointSetHinge2Anchor( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + + joint->makeV1andV2(); + joint->makeW1andW2(); +} + + +/*ODE_API */ +void dJointSetHinge2Axes (dJointID j, const dReal *axis1/*=[dSA__MAX],=NULL*/, const dReal *axis2/*=[dSA__MAX],=NULL*/) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + + dAASSERT(axis1 != NULL || axis2 != NULL); + dAASSERT(joint->node[0].body != NULL || axis1 == NULL); + dAASSERT(joint->node[1].body != NULL || axis2 == NULL); + + if ( axis1 != NULL ) + { + setAxes(joint, axis1[dSA_X], axis1[dSA_Y], axis1[dSA_Z], joint->axis1, NULL); + } + + if ( axis2 != NULL ) + { + setAxes(joint, axis2[dSA_X], axis2[dSA_Y], axis2[dSA_Z], NULL, joint->axis2); + } + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax1, ax2, ax; + joint->getAxisInfo( ax1, ax2, ax, joint->s0, joint->c0 ); + + joint->makeV1andV2(); + joint->makeW1andW2(); +} + + +/*ODE_API_DEPRECATED ODE_API */ +void dJointSetHinge2Axis1( dJointID j, dReal x, dReal y, dReal z ) +{ + dVector3 axis1; + axis1[dSA_X] = x; axis1[dSA_Y] = y; axis1[dSA_Z] = z; + dJointSetHinge2Axes(j, axis1, NULL); +} + +/*ODE_API_DEPRECATED ODE_API */ +void dJointSetHinge2Axis2( dJointID j, dReal x, dReal y, dReal z ) +{ + dVector3 axis2; + axis2[dSA_X] = x; axis2[dSA_Y] = y; axis2[dSA_Z] = z; + dJointSetHinge2Axes(j, NULL, axis2); +} + + +void dJointSetHinge2Param( dJointID j, int parameter, dReal value ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + if (( parameter & 0xff00 ) == 0x100 ) + { + joint->limot2.set( parameter & 0xff, value ); + } + else + { + if ( parameter == dParamSuspensionERP ) joint->susp_erp = value; + else if ( parameter == dParamSuspensionCFM ) joint->susp_cfm = value; + else joint->limot1.set( parameter, value ); + } +} + + +void dJointGetHinge2Anchor( dJointID j, dVector3 result ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Hinge2 ); + if ( joint->flags & dJOINT_REVERSE ) + getAnchor2( joint, result, joint->anchor2 ); + else + getAnchor( joint, result, joint->anchor1 ); +} + + +void dJointGetHinge2Anchor2( dJointID j, dVector3 result ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Hinge2 ); + if ( joint->flags & dJOINT_REVERSE ) + getAnchor( joint, result, joint->anchor1 ); + else + getAnchor2( joint, result, joint->anchor2 ); +} + + +void dJointGetHinge2Axis1( dJointID j, dVector3 result ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Hinge2 ); + if ( joint->node[0].body ) + { + dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis1 ); + } + else + { + dZeroVector3(result); + dUASSERT( false, "the joint does not have first body attached" ); + } +} + + +void dJointGetHinge2Axis2( dJointID j, dVector3 result ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Hinge2 ); + if ( joint->node[1].body ) + { + dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 ); + } + else + { + dZeroVector3(result); + dUASSERT( false, "the joint does not have second body attached" ); + } +} + + +dReal dJointGetHinge2Param( dJointID j, int parameter ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + if (( parameter & 0xff00 ) == 0x100 ) + { + return joint->limot2.get( parameter & 0xff ); + } + else + { + if ( parameter == dParamSuspensionERP ) return joint->susp_erp; + else if ( parameter == dParamSuspensionCFM ) return joint->susp_cfm; + else return joint->limot1.get( parameter ); + } +} + + +dReal dJointGetHinge2Angle1( dJointID j ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + return joint->measureAngle1(); +} + + +dReal dJointGetHinge2Angle2( dJointID j ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + return joint->measureAngle2(); +} + + + +dReal dJointGetHinge2Angle1Rate( dJointID j ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + if ( joint->node[0].body ) + { + dVector3 axis; + dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 ); + dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) + rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + else return 0; +} + + +dReal dJointGetHinge2Angle2Rate( dJointID j ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + if ( joint->node[0].body && joint->node[1].body ) + { + dVector3 axis; + dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 ); + dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) + rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + else return 0; +} + + +void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 ) +{ + dxJointHinge2* joint = ( dxJointHinge2* )j; + dVector3 axis1, axis2; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Hinge2 ); + + if ( joint->node[0].body && joint->node[1].body ) + { + dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 ); + dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 ); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] ); + dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] ); + } +} + + +dJointType +dxJointHinge2::type() const +{ + return dJointTypeHinge2; +} + + +sizeint +dxJointHinge2::size() const +{ + return sizeof( *this ); +} + + +void +dxJointHinge2::setRelativeValues() +{ + dVector3 anchor; + dJointGetHinge2Anchor(this, anchor); + setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 ); + + dVector3 axis; + + if ( node[0].body ) + { + dJointGetHinge2Axis1(this, axis); + setAxes( this, axis[0],axis[1],axis[2], axis1, NULL ); + } + + if ( node[0].body ) + { + dJointGetHinge2Axis2(this, axis); + setAxes( this, axis[0],axis[1],axis[2], NULL, axis2 ); + } + + dVector3 ax1, ax2; + getAxisInfo( ax1, ax2, axis, s0, c0 ); + + makeV1andV2(); + makeW1andW2(); +} diff --git a/libs/ode-0.16.1/ode/src/joints/hinge2.h b/libs/ode-0.16.1/ode/src/joints/hinge2.h new file mode 100644 index 0000000..06ce240 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/hinge2.h @@ -0,0 +1,71 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_HINGE2_H_ +#define _ODE_JOINT_HINGE2_H_ + +#include "joint.h" + + +// hinge 2 + +struct dxJointHinge2 : public dxJoint +{ + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis 1 w.r.t first body + dVector3 axis2; // axis 2 w.r.t second body + dReal c0, s0; // cos,sin of desired angle between axis 1,2 + dVector3 v1, v2; // angle ref vectors embedded in first body + dVector3 w1, w2; // angle ref vectors embedded in second body + dxJointLimitMotor limot1; // limit+motor info for axis 1 + dxJointLimitMotor limot2; // limit+motor info for axis 2 + dReal susp_erp, susp_cfm; // suspension parameters (erp,cfm) + + + dReal measureAngle1() const; + dReal measureAngle2() const; + void makeV1andV2(); + void makeW1andW2(); + + void getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axis, + dReal &sin_angle, dReal &cos_Angle) const; + + + + dxJointHinge2( dxWorld *w ); + + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + virtual void setRelativeValues(); +}; + + + +#endif diff --git a/libs/ode-0.16.1/ode/src/joints/joint.cpp b/libs/ode-0.16.1/ode/src/joints/joint.cpp new file mode 100644 index 0000000..1b7de7a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/joint.cpp @@ -0,0 +1,931 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/*
+
+design note: the general principle for giving a joint the option of connecting
+to the static environment (i.e. the absolute frame) is to check the second
+body (joint->node[1].body), and if it is zero then behave as if its body
+transform is the identity.
+
+*/
+
+#include <ode/ode.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "joint.h"
+#include "joint_internal.h"
+#include "util.h"
+
+extern void addObjectToList( dObject *obj, dObject **first );
+
+dxJoint::dxJoint( dxWorld *w ) :
+ dObject( w )
+{
+ //printf("constructing %p\n", this);
+ dIASSERT( w );
+ flags = 0;
+ node[0].joint = this;
+ node[0].body = 0;
+ node[0].next = 0;
+ node[1].joint = this;
+ node[1].body = 0;
+ node[1].next = 0;
+ dSetZero( lambda, 6 );
+
+ addObjectToList( this, ( dObject ** ) &w->firstjoint );
+
+ w->nj++;
+ feedback = 0;
+}
+
+dxJoint::~dxJoint()
+{ }
+
+
+/*virtual */
+void dxJoint::setRelativeValues()
+{
+ // Do nothing
+}
+
+bool dxJoint::isEnabled() const
+{
+ return ( (flags & dJOINT_DISABLED) == 0 &&
+ (node[0].body->invMass > 0 ||
+ (node[1].body && node[1].body->invMass > 0)) );
+}
+
+
+sizeint dxJointGroup::exportJoints(dxJoint **jlist)
+{
+ sizeint i=0;
+ dxJoint *j = (dxJoint*) m_stack.rewind();
+ while (j != NULL) {
+ jlist[i++] = j;
+ j = (dxJoint*) (m_stack.next (j->size()));
+ }
+ return i;
+}
+
+void dxJointGroup::freeAll()
+{
+ m_num = 0;
+ m_stack.freeAll();
+}
+
+
+//****************************************************************************
+// externs
+
+// extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
+// extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
+
+//****************************************************************************
+// utility
+
+// set three "ball-and-socket" rows in the constraint equation, and the
+// corresponding right hand side.
+
+void setBall( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dVector3 anchor1, dVector3 anchor2 )
+{
+ // anchor points in global coordinates with respect to body PORs.
+ dVector3 a1, a2;
+
+ // set Jacobian
+ J1[dxJoint::GI2_JLX] = 1;
+ J1[rowskip + dxJoint::GI2_JLY] = 1;
+ J1[2 * rowskip + dxJoint::GI2_JLZ] = 1;
+ dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
+ dSetCrossMatrixMinus( J1 + dxJoint::GI2__JA_MIN, a1, rowskip );
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ J2[dxJoint::GI2_JLX] = -1;
+ J2[rowskip + dxJoint::GI2_JLY] = -1;
+ J2[2 * rowskip + dxJoint::GI2_JLZ] = -1;
+ dMultiply0_331( a2, b1->posr.R, anchor2 );
+ dSetCrossMatrixPlus( J2 + dxJoint::GI2__JA_MIN, a2, rowskip );
+ }
+
+ // set right hand side
+ dReal k = fps * erp;
+ dxBody *b0 = joint->node[0].body;
+ if ( b1 )
+ {
+ dReal *currRhsCfm = pairRhsCfm;
+ for ( int j = dSA__MIN; j != dSA__MAX; j++ )
+ {
+ currRhsCfm[dxJoint::GI2_RHS] = k * ( a2[j] + b1->posr.pos[j] - a1[j] - b0->posr.pos[j] );
+ currRhsCfm += pairskip;
+ }
+ }
+ else
+ {
+ dReal *currRhsCfm = pairRhsCfm;
+ for ( int j = dSA__MIN; j != dSA__MAX; j++ )
+ {
+ currRhsCfm[dxJoint::GI2_RHS] = k * ( anchor2[j] - a1[j] - b0->posr.pos[j] );
+ currRhsCfm += pairskip;
+ }
+ }
+}
+
+
+// this is like setBall(), except that `axis' is a unit length vector
+// (in global coordinates) that should be used for the first jacobian
+// position row (the other two row vectors will be derived from this).
+// `erp1' is the erp value to use along the axis.
+
+void setBall2( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dVector3 anchor1, dVector3 anchor2,
+ dVector3 axis, dReal erp1 )
+{
+ // anchor points in global coordinates with respect to body PORs.
+ dVector3 a1, a2;
+
+ // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
+ // [0 1 0] and [0 0 1], which makes everything much easier.
+ dVector3 q1, q2;
+ dPlaneSpace( axis, q1, q2 );
+
+ // set Jacobian
+ dCopyVector3(J1 + dxJoint::GI2__JL_MIN, axis);
+ dCopyVector3(J1 + rowskip + dxJoint::GI2__JL_MIN, q1);
+ dCopyVector3(J1 + 2 * rowskip + dxJoint::GI2__JL_MIN, q2);
+ dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
+ dCalcVectorCross3( J1 + dxJoint::GI2__JA_MIN, a1, axis );
+ dCalcVectorCross3( J1 + rowskip + dxJoint::GI2__JA_MIN, a1, q1 );
+ dCalcVectorCross3( J1 + 2 * rowskip + dxJoint::GI2__JA_MIN, a1, q2 );
+
+ dxBody *b0 = joint->node[0].body;
+ dAddVectors3(a1, a1, b0->posr.pos);
+
+ // set right hand side - measure error along (axis,q1,q2)
+ dReal k1 = fps * erp1;
+ dReal k = fps * erp;
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ dCopyNegatedVector3(J2 + dxJoint::GI2__JL_MIN, axis);
+ dCopyNegatedVector3(J2 + rowskip + dxJoint::GI2__JL_MIN, q1);
+ dCopyNegatedVector3(J2 + 2 * rowskip + dxJoint::GI2__JL_MIN, q2);
+ dMultiply0_331( a2, b1->posr.R, anchor2 );
+ dCalcVectorCross3( J2 + dxJoint::GI2__JA_MIN, axis, a2 ); //== dCalcVectorCross3( J2 + dxJoint::GI2__J2A_MIN, a2, axis ); dNegateVector3( J2 + dxJoint::GI2__J2A_MIN );
+ dCalcVectorCross3( J2 + rowskip + dxJoint::GI2__JA_MIN, q1, a2 ); //== dCalcVectorCross3( J2 + rowskip + dxJoint::GI2__J2A_MIN, a2, q1 ); dNegateVector3( J2 + rowskip + dxJoint::GI2__J2A_MIN );
+ dCalcVectorCross3( J2 + 2 * rowskip + dxJoint::GI2__JA_MIN, q2, a2 ); //== dCalcVectorCross3( J2 + 2 * rowskip + dxJoint::GI2__J2A_MIN, a2, q2 ); dNegateVector3( J2 + 2 * rowskip + dxJoint::GI2__J2A_MIN );
+
+ dAddVectors3(a2, a2, b1->posr.pos);
+
+ dVector3 a2_minus_a1;
+ dSubtractVectors3(a2_minus_a1, a2, a1);
+ pairRhsCfm[dxJoint::GI2_RHS] = k1 * dCalcVectorDot3( axis, a2_minus_a1 );
+ pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q1, a2_minus_a1 );
+ pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q2, a2_minus_a1 );
+ }
+ else
+ {
+ dVector3 anchor2_minus_a1;
+ dSubtractVectors3(anchor2_minus_a1, anchor2, a1);
+ pairRhsCfm[dxJoint::GI2_RHS] = k1 * dCalcVectorDot3( axis, anchor2_minus_a1 );
+ pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q1, anchor2_minus_a1 );
+ pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q2, anchor2_minus_a1 );
+ }
+}
+
+
+// set three orientation rows in the constraint equation, and the
+// corresponding right hand side.
+
+void setFixedOrientation( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dQuaternion qrel )
+{
+ // 3 rows to make body rotations equal
+ J1[dxJoint::GI2_JAX] = 1;
+ J1[rowskip + dxJoint::GI2_JAY] = 1;
+ J1[2 * rowskip + dxJoint::GI2_JAZ] = 1;
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ J2[dxJoint::GI2_JAX] = -1;
+ J2[rowskip + dxJoint::GI2_JAY] = -1;
+ J2[2 * rowskip + dxJoint::GI2_JAZ] = -1;
+ }
+
+ // compute the right hand side. the first three elements will result in
+ // relative angular velocity of the two bodies - this is set to bring them
+ // back into alignment. the correcting angular velocity is
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * u
+ // = (erp*fps) * theta * u
+ // where rotation along unit length axis u by theta brings body 2's frame
+ // to qrel with respect to body 1's frame. using a small angle approximation
+ // for sin(), this gives
+ // angular_velocity = (erp*fps) * 2 * v
+ // where the quaternion of the relative rotation between the two bodies is
+ // q = [cos(theta/2) sin(theta/2)*u] = [s v]
+
+ // get qerr = relative rotation (rotation error) between two bodies
+ dQuaternion qerr, e;
+ dxBody *b0 = joint->node[0].body;
+ if ( b1 )
+ {
+ dQuaternion qq;
+ dQMultiply1( qq, b0->q, b1->q );
+ dQMultiply2( qerr, qq, qrel );
+ }
+ else
+ {
+ dQMultiply3( qerr, b0->q, qrel );
+ }
+ if ( qerr[0] < 0 )
+ {
+ qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
+ qerr[2] = -qerr[2];
+ qerr[3] = -qerr[3];
+ }
+ dMultiply0_331( e, b0->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
+ dReal k_mul_2 = fps * erp * REAL(2.0);
+ pairRhsCfm[dxJoint::GI2_RHS] = k_mul_2 * e[dSA_X];
+ pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k_mul_2 * e[dSA_Y];
+ pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k_mul_2 * e[dSA_Z];
+}
+
+
+// compute anchor points relative to bodies
+
+void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
+ dVector3 anchor1, dVector3 anchor2 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dReal q[4];
+ q[0] = x - b0->posr.pos[0];
+ q[1] = y - b0->posr.pos[1];
+ q[2] = z - b0->posr.pos[2];
+ q[3] = 0;
+ dMultiply1_331( anchor1, b0->posr.R, q );
+
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ q[0] = x - b1->posr.pos[0];
+ q[1] = y - b1->posr.pos[1];
+ q[2] = z - b1->posr.pos[2];
+ q[3] = 0;
+ dMultiply1_331( anchor2, b1->posr.R, q );
+ }
+ else
+ {
+ anchor2[0] = x;
+ anchor2[1] = y;
+ anchor2[2] = z;
+ }
+ }
+ anchor1[3] = 0;
+ anchor2[3] = 0;
+}
+
+
+// compute axes relative to bodies. either axis1 or axis2 can be 0.
+
+void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
+ dVector3 axis1, dVector3 axis2 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dReal q[4];
+ q[0] = x;
+ q[1] = y;
+ q[2] = z;
+ q[3] = 0;
+ dNormalize3( q );
+
+ if ( axis1 )
+ {
+ dMultiply1_331( axis1, b0->posr.R, q );
+ axis1[3] = 0;
+ }
+
+ if ( axis2 )
+ {
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ dMultiply1_331( axis2, b1->posr.R, q );
+ }
+ else
+ {
+ axis2[0] = x;
+ axis2[1] = y;
+ axis2[2] = z;
+ }
+ axis2[3] = 0;
+ }
+ }
+}
+
+
+void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dMultiply0_331( result, b0->posr.R, anchor1 );
+ result[0] += b0->posr.pos[0];
+ result[1] += b0->posr.pos[1];
+ result[2] += b0->posr.pos[2];
+ }
+}
+
+
+void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 )
+{
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ dMultiply0_331( result, b1->posr.R, anchor2 );
+ result[0] += b1->posr.pos[0];
+ result[1] += b1->posr.pos[1];
+ result[2] += b1->posr.pos[2];
+ }
+ else
+ {
+ result[0] = anchor2[0];
+ result[1] = anchor2[1];
+ result[2] = anchor2[2];
+ }
+}
+
+
+void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dMultiply0_331( result, b0->posr.R, axis1 );
+ }
+}
+
+
+void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 )
+{
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ dMultiply0_331( result, b1->posr.R, axis2 );
+ }
+ else
+ {
+ result[0] = axis2[0];
+ result[1] = axis2[1];
+ result[2] = axis2[2];
+ }
+}
+
+
+dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )
+{
+ // the angle between the two bodies is extracted from the quaternion that
+ // represents the relative rotation between them. recall that a quaternion
+ // q is:
+ // [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
+ // where s is a scalar and v is a 3-vector. u is a unit length axis and
+ // theta is a rotation along that axis. we can get theta/2 by:
+ // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
+ // but we can't get sin(theta/2) directly, only its absolute value, i.e.:
+ // |v| = |sin(theta/2)| * |u|
+ // = |sin(theta/2)|
+ // using this value will have a strange effect. recall that there are two
+ // quaternion representations of a given rotation, q and -q. typically as
+ // a body rotates along the axis it will go through a complete cycle using
+ // one representation and then the next cycle will use the other
+ // representation. this corresponds to u pointing in the direction of the
+ // hinge axis and then in the opposite direction. the result is that theta
+ // will appear to go "backwards" every other cycle. here is a fix: if u
+ // points "away" from the direction of the hinge (motor) axis (i.e. more
+ // than 90 degrees) then use -q instead of q. this represents the same
+ // rotation, but results in the cos(theta/2) value being sign inverted.
+
+ // extract the angle from the quaternion. cost2 = cos(theta/2),
+ // sint2 = |sin(theta/2)|
+ dReal cost2 = qrel[0];
+ dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );
+ dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumptions
+ ( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direction of axis
+ ( 2 * dAtan2( sint2, -cost2 ) ); // if u points in opposite direction
+
+ // the angle we get will be between 0..2*pi, but we want to return angles
+ // between -pi..pi
+ if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );
+
+ // the angle we've just extracted has the wrong sign
+ theta = -theta;
+
+ return theta;
+}
+
+
+// given two bodies (body1,body2), the hinge axis that they are connected by
+// w.r.t. body1 (axis), and the initial relative orientation between them
+// (q_initial), return the relative rotation angle. the initial relative
+// orientation corresponds to an angle of zero. if body2 is 0 then measure the
+// angle between body1 and the static frame.
+//
+// this will not return the correct angle if the bodies rotate along any axis
+// other than the given hinge axis.
+
+dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis,
+ dQuaternion q_initial )
+{
+ // get qrel = relative rotation between the two bodies
+ dQuaternion qrel;
+ if ( body2 )
+ {
+ dQuaternion qq;
+ dQMultiply1( qq, body1->q, body2->q );
+ dQMultiply2( qrel, qq, q_initial );
+ }
+ else
+ {
+ // pretend body2->q is the identity
+ dQMultiply3( qrel, body1->q, q_initial );
+ }
+
+ return getHingeAngleFromRelativeQuat( qrel, axis );
+}
+
+//****************************************************************************
+// dxJointLimitMotor
+
+void dxJointLimitMotor::init( dxWorld *world )
+{
+ vel = 0;
+ fmax = 0;
+ lostop = -dInfinity;
+ histop = dInfinity;
+ fudge_factor = 1;
+ normal_cfm = world->global_cfm;
+ stop_erp = world->global_erp;
+ stop_cfm = world->global_cfm;
+ bounce = 0;
+ limit = 0;
+ limit_err = 0;
+}
+
+
+void dxJointLimitMotor::set( int num, dReal value )
+{
+ switch ( num )
+ {
+ case dParamLoStop:
+ lostop = value;
+ break;
+ case dParamHiStop:
+ histop = value;
+ break;
+ case dParamVel:
+ vel = value;
+ break;
+ case dParamFMax:
+ if ( value >= 0 ) fmax = value;
+ break;
+ case dParamFudgeFactor:
+ if ( value >= 0 && value <= 1 ) fudge_factor = value;
+ break;
+ case dParamBounce:
+ bounce = value;
+ break;
+ case dParamCFM:
+ normal_cfm = value;
+ break;
+ case dParamStopERP:
+ stop_erp = value;
+ break;
+ case dParamStopCFM:
+ stop_cfm = value;
+ break;
+ }
+}
+
+
+dReal dxJointLimitMotor::get( int num ) const
+{
+ switch ( num )
+ {
+ case dParamLoStop:
+ return lostop;
+ case dParamHiStop:
+ return histop;
+ case dParamVel:
+ return vel;
+ case dParamFMax:
+ return fmax;
+ case dParamFudgeFactor:
+ return fudge_factor;
+ case dParamBounce:
+ return bounce;
+ case dParamCFM:
+ return normal_cfm;
+ case dParamStopERP:
+ return stop_erp;
+ case dParamStopCFM:
+ return stop_cfm;
+ default:
+ return 0;
+ }
+}
+
+
+bool dxJointLimitMotor::testRotationalLimit( dReal angle )
+{
+ if ( angle <= lostop )
+ {
+ limit = 1;
+ limit_err = angle - lostop;
+ return true;
+ }
+ else if ( angle >= histop )
+ {
+ limit = 2;
+ limit_err = angle - histop;
+ return true;
+ }
+ else
+ {
+ limit = 0;
+ return false;
+ }
+}
+
+
+bool dxJointLimitMotor::addLimot( dxJoint *joint,
+ dReal fps, dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+ const dVector3 ax1, int rotational )
+{
+ // if the joint is powered, or has joint limits, add in the extra row
+ int powered = fmax > 0;
+ if ( powered || limit )
+ {
+ dReal *J1Used = rotational ? J1 + GI2__JA_MIN : J1 + GI2__JL_MIN;
+ dReal *J2Used = rotational ? J2 + GI2__JA_MIN : J2 + GI2__JL_MIN;
+
+ dCopyVector3(J1Used, ax1);
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ dCopyNegatedVector3(J2Used, ax1);
+ }
+
+ // linear limot torque decoupling step:
+ //
+ // if this is a linear limot (e.g. from a slider), we have to be careful
+ // that the linear constraint forces (+/- ax1) applied to the two bodies
+ // do not create a torque couple. in other words, the points that the
+ // constraint force is applied at must lie along the same ax1 axis.
+ // a torque couple will result in powered or limited slider-jointed free
+ // bodies from gaining angular momentum.
+ // the solution used here is to apply the constraint forces at the point
+ // halfway between the body centers. there is no penalty (other than an
+ // extra tiny bit of computation) in doing this adjustment. note that we
+ // only need to do this if the constraint connects two bodies.
+
+ dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
+ if ( !rotational && b1 )
+ {
+ dxBody *b0 = joint->node[0].body;
+ dVector3 c;
+ c[0] = REAL( 0.5 ) * ( b1->posr.pos[0] - b0->posr.pos[0] );
+ c[1] = REAL( 0.5 ) * ( b1->posr.pos[1] - b0->posr.pos[1] );
+ c[2] = REAL( 0.5 ) * ( b1->posr.pos[2] - b0->posr.pos[2] );
+ dCalcVectorCross3( ltd, c, ax1 );
+ dCopyVector3(J1 + dxJoint::GI2__JA_MIN, ltd);
+ dCopyVector3(J2 + dxJoint::GI2__JA_MIN, ltd);
+ }
+
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if ( limit && ( lostop == histop ) ) powered = 0;
+
+ if ( powered )
+ {
+ pairRhsCfm[GI2_CFM] = normal_cfm;
+ if ( ! limit )
+ {
+ pairRhsCfm[GI2_RHS] = vel;
+ pairLoHi[GI2_LO] = -fmax;
+ pairLoHi[GI2_HI] = fmax;
+ }
+ else
+ {
+ // the joint is at a limit, AND is being powered. if the joint is
+ // being powered into the limit then we apply the maximum motor force
+ // in that direction, because the motor is working against the
+ // immovable limit. if the joint is being powered away from the limit
+ // then we have problems because actually we need *two* lcp
+ // constraints to handle this case. so we fake it and apply some
+ // fraction of the maximum force. the fraction to use can be set as
+ // a fudge factor.
+
+ dReal fm = fmax;
+ if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
+
+ // if we're powering away from the limit, apply the fudge factor
+ if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
+
+
+ dReal fm_ax1_0 = fm*ax1[0], fm_ax1_1 = fm*ax1[1], fm_ax1_2 = fm*ax1[2];
+
+ dxBody *b0 = joint->node[0].body;
+ dxWorldProcessContext *world_process_context = b0->world->unsafeGetWorldProcessingContext();
+
+ world_process_context->LockForAddLimotSerialization();
+
+ if ( rotational )
+ {
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 != NULL )
+ {
+ dBodyAddTorque( b1, fm_ax1_0, fm_ax1_1, fm_ax1_2 );
+ }
+
+ dBodyAddTorque( b0, -fm_ax1_0, -fm_ax1_1, -fm_ax1_2 );
+ }
+ else
+ {
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 != NULL )
+ {
+ // linear limot torque decoupling step: refer to above discussion
+ dReal neg_fm_ltd_0 = -fm*ltd[0], neg_fm_ltd_1 = -fm*ltd[1], neg_fm_ltd_2 = -fm*ltd[2];
+ dBodyAddTorque( b0, neg_fm_ltd_0, neg_fm_ltd_1, neg_fm_ltd_2 );
+ dBodyAddTorque( b1, neg_fm_ltd_0, neg_fm_ltd_1, neg_fm_ltd_2 );
+
+ dBodyAddForce( b1, fm_ax1_0, fm_ax1_1, fm_ax1_2 );
+ }
+
+ dBodyAddForce( b0, -fm_ax1_0, -fm_ax1_1, -fm_ax1_2 );
+ }
+
+ world_process_context->UnlockForAddLimotSerialization();
+ }
+ }
+
+ if ( limit )
+ {
+ dReal k = fps * stop_erp;
+ pairRhsCfm[GI2_RHS] = -k * limit_err;
+ pairRhsCfm[GI2_CFM] = stop_cfm;
+
+ if ( lostop == histop )
+ {
+ // limited low and high simultaneously
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ if ( limit == 1 )
+ {
+ // low limit
+ pairLoHi[GI2_LO] = 0;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ // high limit
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = 0;
+ }
+
+ // deal with bounce
+ if ( bounce > 0 )
+ {
+ // calculate joint velocity
+ dReal vel;
+ if ( rotational )
+ {
+ vel = dCalcVectorDot3( joint->node[0].body->avel, ax1 );
+ if ( joint->node[1].body )
+ vel -= dCalcVectorDot3( joint->node[1].body->avel, ax1 );
+ }
+ else
+ {
+ vel = dCalcVectorDot3( joint->node[0].body->lvel, ax1 );
+ if ( joint->node[1].body )
+ vel -= dCalcVectorDot3( joint->node[1].body->lvel, ax1 );
+ }
+
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if ( limit == 1 )
+ {
+ // low limit
+ if ( vel < 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc > pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ else
+ {
+ // high limit - all those computations are reversed
+ if ( vel > 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc < pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ }
+ }
+ }
+ return true;
+ }
+ return false;
+}
+
+/**
+ This function generalizes the "linear limot torque decoupling"
+ in addLimot to use anchor points provided by the caller.
+
+ This makes it so that the appropriate torques are applied to
+ a body when it's being linearly motored or limited using anchor points
+ that aren't at the center of mass.
+
+ pt1 and pt2 are centered in body coordinates but use global directions.
+ I.e., they are conveniently found within joint code with:
+ getAxis(joint,pt1,anchor1);
+ getAxis2(joint,pt2,anchor2);
+*/
+bool dxJointLimitMotor::addTwoPointLimot( dxJoint *joint, dReal fps,
+ dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+ const dVector3 ax1, const dVector3 pt1, const dVector3 pt2 )
+{
+ // if the joint is powered, or has joint limits, add in the extra row
+ int powered = fmax > 0;
+ if ( powered || limit )
+ {
+ // Set the linear portion
+ dCopyVector3(J1 + GI2__JL_MIN, ax1);
+ // Set the angular portion (to move the linear constraint
+ // away from the center of mass).
+ dCalcVectorCross3(J1 + GI2__JA_MIN, pt1, ax1);
+ // Set the constraints for the second body
+ if ( joint->node[1].body ) {
+ dCopyNegatedVector3(J2 + GI2__JL_MIN, ax1);
+ dCalcVectorCross3(J2 + GI2__JA_MIN, pt2, J2 + GI2__JL_MIN);
+ }
+
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if ( limit && ( lostop == histop ) ) powered = 0;
+
+ if ( powered )
+ {
+ pairRhsCfm[GI2_CFM] = normal_cfm;
+ if ( ! limit )
+ {
+ pairRhsCfm[GI2_RHS] = vel;
+ pairLoHi[GI2_LO] = -fmax;
+ pairLoHi[GI2_HI] = fmax;
+ }
+ else
+ {
+ // the joint is at a limit, AND is being powered. if the joint is
+ // being powered into the limit then we apply the maximum motor force
+ // in that direction, because the motor is working against the
+ // immovable limit. if the joint is being powered away from the limit
+ // then we have problems because actually we need *two* lcp
+ // constraints to handle this case. so we fake it and apply some
+ // fraction of the maximum force. the fraction to use can be set as
+ // a fudge factor.
+
+ dReal fm = fmax;
+ if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
+
+ // if we're powering away from the limit, apply the fudge factor
+ if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
+
+
+ const dReal* tAx1 = J1 + GI2__JA_MIN;
+ dBodyAddForce( joint->node[0].body, -fm*ax1[dSA_X], -fm*ax1[dSA_Y], -fm*ax1[dSA_Z] );
+ dBodyAddTorque( joint->node[0].body, -fm*tAx1[dSA_X], -fm*tAx1[dSA_Y], -fm*tAx1[dSA_Z] );
+
+ if ( joint->node[1].body )
+ {
+ const dReal* tAx2 = J2 + GI2__JA_MIN;
+ dBodyAddForce( joint->node[1].body, fm*ax1[dSA_X], fm*ax1[dSA_Y], fm*ax1[dSA_Z] );
+ dBodyAddTorque( joint->node[1].body, -fm*tAx2[dSA_X], -fm*tAx2[dSA_Y], -fm*tAx2[dSA_Z] );
+ }
+
+ }
+ }
+
+ if ( limit )
+ {
+ dReal k = fps * stop_erp;
+ pairRhsCfm[GI2_RHS] = -k * limit_err;
+ pairRhsCfm[GI2_CFM] = stop_cfm;
+
+ if ( lostop == histop )
+ {
+ // limited low and high simultaneously
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ if ( limit == 1 )
+ {
+ // low limit
+ pairLoHi[GI2_LO] = 0;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ // high limit
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = 0;
+ }
+
+ // deal with bounce
+ if ( bounce > 0 )
+ {
+ // calculate relative velocity of the two anchor points
+ dReal vel =
+ dCalcVectorDot3( joint->node[0].body->lvel, J1 + GI2__JL_MIN ) +
+ dCalcVectorDot3( joint->node[0].body->avel, J1 + GI2__JA_MIN );
+ if (joint->node[1].body) {
+ vel +=
+ dCalcVectorDot3( joint->node[1].body->lvel, J2 + GI2__JL_MIN ) +
+ dCalcVectorDot3( joint->node[1].body->avel, J2 + GI2__JA_MIN );
+ }
+
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if ( limit == 1 )
+ {
+ // low limit
+ if ( vel < 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc > pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ else
+ {
+ // high limit - all those computations are reversed
+ if ( vel > 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc < pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ }
+ }
+ }
+ return true;
+ }
+ return false;
+}
+
+
+// Local Variables:
+// mode:c++
+// c-basic-offset:4
+// End:
diff --git a/libs/ode-0.16.1/ode/src/joints/joint.h b/libs/ode-0.16.1/ode/src/joints/joint.h new file mode 100644 index 0000000..b6aa81e --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/joint.h @@ -0,0 +1,326 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_H_ +#define _ODE_JOINT_H_ + + +#include <ode/contact.h> +#include "../common.h" +#include "../objects.h" +#include "../obstack.h" + + +// joint flags +enum +{ + // if this flag is set, the joint was allocated in a joint group + dJOINT_INGROUP = 1, + + // if this flag is set, the joint was attached with arguments (0,body). + // our convention is to treat all attaches as (body,0), i.e. so node[0].body + // is always nonzero, so this flag records the fact that the arguments were + // swapped. + dJOINT_REVERSE = 2, + + // if this flag is set, the joint can not have just one body attached to it, + // it must have either zero or two bodies attached. + dJOINT_TWOBODIES = 4, + + dJOINT_DISABLED = 8 +}; + + +enum dJointConnectedBody +{ + dJCB__MIN, + + dJCB_FIRST_BODY = dJCB__MIN, + dJCB_SECOND_BODY, + + dJCB__MAX, + +}; + +static inline +dJointConnectedBody EncodeJointOtherConnectedBody(dJointConnectedBody cbBodyKind) +{ + dIASSERT(dIN_RANGE(cbBodyKind, dJCB__MIN, dJCB__MAX)); + dSASSERT(dJCB__MAX == 2); + + return (dJointConnectedBody)(dJCB_FIRST_BODY + dJCB_SECOND_BODY - cbBodyKind); +} + +/* joint body relativity enumeration */ +enum dJointBodyRelativity +{ + dJBR__MIN, + + dJBR_GLOBAL = dJBR__MIN, + + dJBR__BODIES_MIN, + + dJBR_BODY1 = dJBR__BODIES_MIN + dJCB_FIRST_BODY, + dJBR_BODY2 = dJBR__BODIES_MIN + dJCB_SECOND_BODY, + + dJBR__BODIES_MAX = dJBR__BODIES_MIN + dJCB__MAX, + + dJBR__MAX, + + dJBR__DEFAULT = dJBR_GLOBAL, + dJBR__BODIES_COUNT = dJBR__BODIES_MAX - dJBR__BODIES_MIN, + +}; + +ODE_PURE_INLINE int dJBREncodeBodyRelativityStatus(int relativity) +{ + return dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX); +} + +ODE_PURE_INLINE dJointBodyRelativity dJBRSwapBodyRelativity(int relativity) +{ + dIASSERT(dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX)); + return (dJointBodyRelativity)(dJBR_BODY1 + dJBR_BODY2 - relativity); +} + + + + +// there are two of these nodes in the joint, one for each connection to a +// body. these are node of a linked list kept by each body of it's connecting +// joints. but note that the body pointer in each node points to the body that +// makes use of the *other* node, not this node. this trick makes it a bit +// easier to traverse the body/joint graph. + +struct dxJointNode +{ + dxJoint *joint; // pointer to enclosing dxJoint object + dxBody *body; // *other* body this joint is connected to + dxJointNode *next; // next node in body's list of connected joints +}; + + +struct dxJoint : public dObject +{ + // naming convention: the "first" body this is connected to is node[0].body, + // and the "second" body is node[1].body. if this joint is only connected + // to one body then the second body is 0. + + // info returned by getInfo1 function. the constraint dimension is m (<=6). + // i.e. that is the total number of rows in the jacobian. `nub' is the + // number of unbounded variables (which have lo,hi = -/+ infinity). + + struct Info1 + { + // Structure size should not exceed sizeof(pointer) bytes to have + // to have good memory pattern in dxQuickStepper() + uint8 m, nub; + }; + + // info returned by getInfo2 function + + enum + { + GI2__J_MIN, + GI2__JL_MIN = GI2__J_MIN + dDA__L_MIN, + + GI2_JLX = GI2__J_MIN + dDA_LX, + GI2_JLY = GI2__J_MIN + dDA_LY, + GI2_JLZ = GI2__J_MIN + dDA_LZ, + + GI2__JL_MAX = GI2__J_MIN + dDA__L_MAX, + + GI2__JA_MIN = GI2__J_MIN + dDA__A_MIN, + + GI2_JAX = GI2__J_MIN + dDA_AX, + GI2_JAY = GI2__J_MIN + dDA_AY, + GI2_JAZ = GI2__J_MIN + dDA_AZ, + + GI2__JA_MAX = GI2__J_MIN + dDA__A_MAX, + GI2__J_MAX = GI2__J_MIN + dDA__MAX, + }; + + enum + { + GI2_RHS, + GI2_CFM, + GI2__RHS_CFM_MAX, + }; + + enum + { + GI2_LO, + GI2_HI, + GI2__LO_HI_MAX, + }; + + // info returned by getSureMaxInfo function. + // The information is used for memory reservation in calculations. + + struct SureMaxInfo + { + // The value of `max_m' must ALWAYS be not less than the value of `m' + // the getInfo1 call can generate in current joint state. Another + // requirement is that the value should be provided very quickly, + // without the excessive calculations. + // If it is hard/impossible to quickly predict the maximal value of `m' + // (which is the case for most joint types) the maximum for current + // joint type in general should be returned. If it can be known the `m' + // will be smaller, it can save a bit of memory from being reserved + // for calculations if that smaller value is returned. + + uint8 max_m; // Estimate of maximal `m' in Info1 + }; + + + unsigned flags; // dJOINT_xxx flags + dxJointNode node[2]; // connections to bodies. node[1].body can be 0 + dJointFeedback *feedback; // optional feedback structure + dReal lambda[6]; // lambda generated by last step + + + dxJoint( dxWorld *w ); + virtual ~dxJoint(); + + bool GetIsJointReverse() const { return (this->flags & dJOINT_REVERSE) != 0; } + + virtual void getInfo1( Info1* info ) = 0; + + // integrator parameters + virtual void getInfo2( + // fps=frames per second (1/stepsize), erp=default error reduction parameter (0..1) + dReal worldFPS, dReal worldERP, + // elements to jump from one row to the next in J's + int rowskip, + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + dReal *J1, dReal *J2, + // elements to jump from one pair of scalars to the next + int pairskip, + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + dReal *pairRhsCfm, + // lo and hi limits for variables (set to -/+ infinity on entry). + dReal *pairLoHi, + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex) = 0; + // This call quickly!!! estimates maximum value of "m" that could be returned by getInfo1() + // See comments at definition of SureMaxInfo for details. + virtual void getSureMaxInfo( SureMaxInfo* info ) = 0; + virtual dJointType type() const = 0; + virtual sizeint size() const = 0; + + /// Set values which are relative with respect to bodies. + /// Each dxJoint should redefine it if needed. + virtual void setRelativeValues(); + + // Test if this joint should be used in the simulation step + // (has the enabled flag set, and is attached to at least one dynamic body) + bool isEnabled() const; +}; + + +// joint group. NOTE: any joints in the group that have their world destroyed +// will have their world pointer set to 0. + +struct dxJointGroup : public dBase +{ + dxJointGroup(): m_num(0), m_stack() {} + + template<class T> + T *alloc(dWorldID w) + { + T *j = (T *)m_stack.alloc(sizeof(T)); + if (j != NULL) { + ++m_num; + new(j) T(w); + j->flags |= dJOINT_INGROUP; + } + return j; + } + + sizeint getJointCount() const { return m_num; } + sizeint exportJoints(dxJoint **jlist); + + void *beginEnum() { return m_stack.rewind(); } + void *continueEnum(sizeint num_bytes) { return m_stack.next(num_bytes); } + + void freeAll(); + +private: + sizeint m_num; // number of joints on the stack + dObStack m_stack; // a stack of (possibly differently sized) dxJoint objects. +}; + +// common limit and motor information for a single joint axis of movement +struct dxJointLimitMotor +{ + dReal vel, fmax; // powered joint: velocity, max force + dReal lostop, histop; // joint limits, relative to initial position + dReal fudge_factor; // when powering away from joint limits + dReal normal_cfm; // cfm to use when not at a stop + dReal stop_erp, stop_cfm; // erp and cfm for when at joint limit + dReal bounce; // restitution factor + // variables used between getInfo1() and getInfo2() + int limit; // 0=free, 1=at lo limit, 2=at hi limit + dReal limit_err; // if at limit, amount over limit + + void init( dxWorld * ); + void set( int num, dReal value ); + dReal get( int num ) const; + bool testRotationalLimit( dReal angle ); + + enum + { + GI2__JL_MIN = dxJoint::GI2__JL_MIN, + GI2__JA_MIN = dxJoint::GI2__JA_MIN, + GI2_JAX = dxJoint::GI2_JAX, + GI2_JAY = dxJoint::GI2_JAY, + GI2_JAZ = dxJoint::GI2_JAZ, + GI2_RHS = dxJoint::GI2_RHS, + GI2_CFM = dxJoint::GI2_CFM, + GI2_LO = dxJoint::GI2_LO, + GI2_HI = dxJoint::GI2_HI, + }; + + bool addLimot( dxJoint *joint, dReal fps, + dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi, + const dVector3 ax1, int rotational ); + bool addTwoPointLimot( dxJoint *joint, dReal fps, + dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi, + const dVector3 ax1, const dVector3 pt1, const dVector3 pt2 ); +}; + + +#endif + + +// Local Variables: +// mode:c++ +// c-basic-offset:4 +// End: diff --git a/libs/ode-0.16.1/ode/src/joints/joint_internal.h b/libs/ode-0.16.1/ode/src/joints/joint_internal.h new file mode 100644 index 0000000..30accb6 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/joint_internal.h @@ -0,0 +1,70 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#ifndef _ODE_JOINT_INTERNAL_H_
+#define _ODE_JOINT_INTERNAL_H_
+
+
+#include <ode/rotation.h>
+#include <ode/objects.h>
+#include "matrix.h"
+#include "odemath.h"
+
+
+#define checktype(j,t) dUASSERT(j->type() == dJointType##t, \
+ "joint type is not " #t)
+
+
+void setBall( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dVector3 anchor1, dVector3 anchor2 );
+void setBall2( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dVector3 anchor1, dVector3 anchor2,
+ dVector3 axis, dReal erp1 );
+
+void setFixedOrientation( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dQuaternion qrel );
+
+
+void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
+ dVector3 anchor1, dVector3 anchor2 );
+
+void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 );
+void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 );
+
+void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
+ dVector3 axis1, dVector3 axis2 );
+void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 );
+void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 );
+
+
+dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis, dQuaternion q_initial );
+dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis );
+
+#endif
+
diff --git a/libs/ode-0.16.1/ode/src/joints/joints.h b/libs/ode-0.16.1/ode/src/joints/joints.h new file mode 100644 index 0000000..d06af4d --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/joints.h @@ -0,0 +1,48 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINTS_H_ +#define _ODE_JOINTS_H_ + +#include <ode/common.h> + +#include "joint.h" + +#include "ball.h" +#include "dball.h" +#include "dhinge.h" +#include "transmission.h" +#include "hinge.h" +#include "slider.h" +#include "contact.h" +#include "universal.h" +#include "hinge2.h" +#include "fixed.h" +#include "null.h" +#include "amotor.h" +#include "lmotor.h" +#include "plane2d.h" +#include "pu.h" +#include "pr.h" +#include "piston.h" + +#endif diff --git a/libs/ode-0.16.1/ode/src/joints/lmotor.cpp b/libs/ode-0.16.1/ode/src/joints/lmotor.cpp new file mode 100644 index 0000000..8270188 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/lmotor.cpp @@ -0,0 +1,214 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "lmotor.h"
+#include "joint_internal.h"
+
+
+//****************************************************************************
+// lmotor joint
+dxJointLMotor::dxJointLMotor( dxWorld *w ) :
+ dxJoint( w )
+{
+ int i;
+ num = 0;
+ for ( i = 0;i < 3;i++ )
+ {
+ dSetZero( axis[i], 4 );
+ limot[i].init( world );
+ }
+}
+
+void
+dxJointLMotor::computeGlobalAxes( dVector3 ax[3] )
+{
+ for ( int i = 0; i < num; i++ )
+ {
+ if ( rel[i] == 1 )
+ {
+ dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
+ }
+ else if ( rel[i] == 2 )
+ {
+ if ( node[1].body ) // jds: don't assert, just ignore
+ {
+ dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
+ }
+ }
+ else
+ {
+ ax[i][0] = axis[i][0];
+ ax[i][1] = axis[i][1];
+ ax[i][2] = axis[i][2];
+ }
+ }
+}
+
+void
+dxJointLMotor::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = num;
+}
+
+void
+dxJointLMotor::getInfo1( dxJoint::Info1 *info )
+{
+ info->m = 0;
+ info->nub = 0;
+ for ( int i = 0; i < num; i++ )
+ {
+ if ( limot[i].fmax > 0 )
+ {
+ info->m++;
+ }
+ }
+}
+
+void
+dxJointLMotor::getInfo2( dReal worldFPS, dReal /*worldERP*/,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+{
+ dVector3 ax[3];
+ computeGlobalAxes( ax );
+
+ int currRowSkip = 0, currPairSkip = 0;
+ for ( int i = 0; i < num; ++i ) {
+ if (limot[i].addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax[i], 0 )) {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+}
+
+void dJointSetLMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ //for now we are ignoring rel!
+ dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
+ checktype( joint, LMotor );
+
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+
+ if ( !joint->node[1].body && rel == 2 ) rel = 1; //ref 1
+
+ joint->rel[anum] = rel;
+
+ dVector3 r;
+ r[0] = x;
+ r[1] = y;
+ r[2] = z;
+ r[3] = 0;
+ if ( rel > 0 )
+ {
+ if ( rel == 1 )
+ {
+ dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
+ }
+ else
+ {
+ //second body has to exists thanks to ref 1 line
+ dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
+ }
+ }
+ else
+ {
+ joint->axis[anum][0] = r[0];
+ joint->axis[anum][1] = r[1];
+ joint->axis[anum][2] = r[2];
+ }
+
+ dNormalize3( joint->axis[anum] );
+}
+
+void dJointSetLMotorNumAxes( dJointID j, int num )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint && num >= 0 && num <= 3 );
+ checktype( joint, LMotor );
+ if ( num < 0 ) num = 0;
+ if ( num > 3 ) num = 3;
+ joint->num = num;
+}
+
+void dJointSetLMotorParam( dJointID j, int parameter, dReal value )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint );
+ checktype( joint, LMotor );
+ int anum = parameter >> 8;
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+ parameter &= 0xff;
+ joint->limot[anum].set( parameter, value );
+}
+
+int dJointGetLMotorNumAxes( dJointID j )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint );
+ checktype( joint, LMotor );
+ return joint->num;
+}
+
+
+void dJointGetLMotorAxis( dJointID j, int anum, dVector3 result )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint && anum >= 0 && anum < 3 );
+ checktype( joint, LMotor );
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+ result[0] = joint->axis[anum][0];
+ result[1] = joint->axis[anum][1];
+ result[2] = joint->axis[anum][2];
+}
+
+dReal dJointGetLMotorParam( dJointID j, int parameter )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint );
+ checktype( joint, LMotor );
+ int anum = parameter >> 8;
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+ parameter &= 0xff;
+ return joint->limot[anum].get( parameter );
+}
+
+dJointType
+dxJointLMotor::type() const
+{
+ return dJointTypeLMotor;
+}
+
+
+sizeint
+dxJointLMotor::size() const
+{
+ return sizeof( *this );
+}
+
diff --git a/libs/ode-0.16.1/ode/src/joints/lmotor.h b/libs/ode-0.16.1/ode/src/joints/lmotor.h new file mode 100644 index 0000000..c819a47 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/lmotor.h @@ -0,0 +1,51 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_LMOTOR_H_ +#define _ODE_JOINT_LMOTOR_H_ + +#include "joint.h" + +struct dxJointLMotor : public dxJoint +{ + int num; + int rel[3]; + dVector3 axis[3]; + dxJointLimitMotor limot[3]; + + void computeGlobalAxes( dVector3 ax[3] ); + + + dxJointLMotor( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/null.cpp b/libs/ode-0.16.1/ode/src/joints/null.cpp new file mode 100644 index 0000000..315eea9 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/null.cpp @@ -0,0 +1,74 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "null.h" +#include "joint_internal.h" + + + +//**************************************************************************** +// null joint +dxJointNull::dxJointNull( dxWorld *w ) : + dxJoint( w ) +{ +} + +void +dxJointNull::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 0; +} + + +void +dxJointNull::getInfo1( dxJoint::Info1 *info ) +{ + info->m = 0; + info->nub = 0; +} + + +void +dxJointNull::getInfo2( dReal /*worldFPS*/, dReal /*worldERP*/, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + dDebug( 0, "this should never get called" ); +} + +dJointType +dxJointNull::type() const +{ + return dJointTypeNull; +} + +sizeint +dxJointNull::size() const +{ + return sizeof( *this ); +} + + diff --git a/libs/ode-0.16.1/ode/src/joints/null.h b/libs/ode-0.16.1/ode/src/joints/null.h new file mode 100644 index 0000000..fb3f629 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/null.h @@ -0,0 +1,46 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_NULL_H_ +#define _ODE_JOINT_NULL_H_ + +#include "joint.h" + + + +// null joint, for testing only + +struct dxJointNull : public dxJoint +{ + dxJointNull( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; +}; + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/piston.cpp b/libs/ode-0.16.1/ode/src/joints/piston.cpp new file mode 100644 index 0000000..3bd7fd0 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/piston.cpp @@ -0,0 +1,729 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "piston.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// Piston
+//
+
+dxJointPiston::dxJointPiston ( dxWorld *w ) :
+ dxJoint ( w )
+{
+ dSetZero ( axis1, 4 );
+ dSetZero ( axis2, 4 );
+
+ axis1[0] = 1;
+ axis2[0] = 1;
+
+ dSetZero ( qrel, 4 );
+
+ dSetZero ( anchor1, 4 );
+ dSetZero ( anchor2, 4 );
+
+ limotP.init ( world );
+
+ limotR.init ( world );
+}
+
+
+dReal dJointGetPistonPosition ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( joint->node[0].body )
+ {
+ dVector3 q;
+ // get the anchor (or offset) in global coordinates
+ dMultiply0_331 ( q, joint->node[0].body->posr.R, joint->anchor1 );
+
+ if ( joint->node[1].body )
+ {
+ dVector3 anchor2;
+ // get the anchor2 in global coordinates
+ dMultiply0_331 ( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
+
+ q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
+ ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
+ q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
+ ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
+ q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
+ ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
+ }
+ else
+ {
+ // N.B. When there is no body 2 the joint->anchor2 is already in
+ // global coordinates
+ q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
+ ( joint->anchor2[0] ) );
+ q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
+ ( joint->anchor2[1] ) );
+ q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
+ ( joint->anchor2[2] ) );
+
+ if ( joint->flags & dJOINT_REVERSE )
+ {
+ q[0] = -q[0];
+ q[1] = -q[1];
+ q[2] = -q[2];
+ }
+ }
+
+ // get axis in global coordinates
+ dVector3 ax;
+ dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
+
+ return dCalcVectorDot3 ( ax, q );
+ }
+
+ dDEBUGMSG ( "The function always return 0 since no body are attached" );
+ return 0;
+}
+
+
+dReal dJointGetPistonPositionRate ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ // get axis in global coordinates
+ dVector3 ax;
+ dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
+
+ // The linear velocity created by the rotation can be discarded since
+ // the rotation is along the prismatic axis and this rotation don't create
+ // linear velocity in the direction of the prismatic axis.
+ if ( joint->node[1].body )
+ {
+ return ( dCalcVectorDot3 ( ax, joint->node[0].body->lvel ) -
+ dCalcVectorDot3 ( ax, joint->node[1].body->lvel ) );
+ }
+ else
+ {
+ dReal rate = dCalcVectorDot3 ( ax, joint->node[0].body->lvel );
+ return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
+ }
+}
+
+
+dReal dJointGetPistonAngle ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston * ) j;
+ dAASSERT ( joint );
+ checktype ( joint, Piston );
+
+ if ( joint->node[0].body )
+ {
+ dReal ang = getHingeAngle ( joint->node[0].body, joint->node[1].body, joint->axis1,
+ joint->qrel );
+ if ( joint->flags & dJOINT_REVERSE )
+ return -ang;
+ else
+ return ang;
+ }
+ else return 0;
+}
+
+
+dReal dJointGetPistonAngleRate ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dAASSERT ( joint );
+ checktype ( joint, Piston );
+
+ if ( joint->node[0].body )
+ {
+ dVector3 axis;
+ dMultiply0_331 ( axis, joint->node[0].body->posr.R, joint->axis1 );
+ dReal rate = dCalcVectorDot3 ( axis, joint->node[0].body->avel );
+ if ( joint->node[1].body ) rate -= dCalcVectorDot3 ( axis, joint->node[1].body->avel );
+ if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
+ return rate;
+ }
+ else return 0;
+}
+
+
+void
+dxJointPiston::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = 6;
+}
+
+
+void
+dxJointPiston::getInfo1 ( dxJoint::Info1 *info )
+{
+ info->nub = 4; // Number of unbound variables
+ // The only bound variable is one linear displacement
+
+ info->m = 4; // Default number of constraint row
+
+ // see if we're at a joint limit.
+ limotP.limit = 0;
+ if ( ( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
+ limotP.lostop <= limotP.histop )
+ {
+ // measure joint position
+ dReal pos = dJointGetPistonPosition ( this );
+ limotP.testRotationalLimit ( pos ); // N.B. The fucntion is ill named
+ }
+
+ // powered Piston or at limits needs an extra constraint row
+ if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+
+
+ // see if we're at a joint limit.
+ limotR.limit = 0;
+ if ( ( limotR.lostop > -dInfinity || limotR.histop < dInfinity ) &&
+ limotR.lostop <= limotR.histop )
+ {
+ // measure joint position
+ dReal angle = getHingeAngle ( node[0].body, node[1].body, axis1,
+ qrel );
+ limotR.testRotationalLimit ( angle );
+ }
+
+ // powered Piston or at limits needs an extra constraint row
+ if ( limotR.limit || limotR.fmax > 0 ) info->m++;
+
+}
+
+
+void
+dxJointPiston::getInfo2 ( dReal worldFPS, dReal worldERP,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+{
+ const dReal k = worldFPS * worldERP;
+
+
+ // Pull out pos and R for both bodies. also get the `connection'
+ // vector pos2-pos1.
+
+ dVector3 dist; // Current position of body_1 w.r.t "anchor"
+ // 2 bodies anchor is center of body 2
+ // 1 bodies anchor is origin
+ dVector3 lanchor2 = { 0,0,0 };
+
+ dReal *pos1 = node[0].body->posr.pos;
+ dReal *R1 = node[0].body->posr.R;
+ dReal *R2 = NULL;
+
+ dxBody *body1 = node[1].body;
+
+ if ( body1 )
+ {
+ dReal *pos2 = body1->posr.pos;
+ R2 = body1->posr.R;
+
+ dMultiply0_331 ( lanchor2, R2, anchor2 );
+ dist[0] = lanchor2[0] + pos2[0] - pos1[0];
+ dist[1] = lanchor2[1] + pos2[1] - pos1[1];
+ dist[2] = lanchor2[2] + pos2[2] - pos1[2];
+ }
+ else
+ {
+ // pos2 = 0; // N.B. We can do that to be safe but it is no necessary
+ // R2 = 0; // N.B. We can do that to be safe but it is no necessary
+ if ( (flags & dJOINT_REVERSE) != 0 )
+ {
+ dSubtractVectors3(dist, pos1, anchor2); // Invert the value
+ }
+ else
+ {
+ dSubtractVectors3(dist, anchor2, pos1);
+ }
+ }
+
+ // ======================================================================
+ // Work on the angular part (i.e. row 0, 1)
+ // Set the two orientation rows. The rotoide axis should be the only
+ // unconstrained rotational axis, the angular velocity of the two bodies
+ // perpendicular to the rotoide axis should be equal.
+ // Thus the constraint equations are:
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ // Since the rotoide axis is the same as the prismatic axis.
+ //
+ //
+ // Also, compute the right hand side (RHS) of the rotation constraint equation set.
+ // The first 2 element will result in the relative angular velocity of the two
+ // bodies along axis p and q. This is set to bring the rotoide back into alignment.
+ // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * u
+ // = (erp*fps) * theta * u
+ // where rotation along unit length axis u by theta brings body 2's frame
+ //
+ // if theta is smallish, sin(theta) ~= theta and cos(theta) ~= 1
+ // where the quaternion of the relative rotation between the two bodies is
+ // quat = [cos(theta/2) sin(theta/2)*u]
+ // quat = [1 theta/2*u]
+ // => q[0] ~= 1
+ // 2 * q[1+i] = theta * u[i]
+ //
+ // Since there is no constraint along the rotoide axis
+ // only along p and q that we want the same angular velocity and need to reduce
+ // the error
+ dVector3 b, ax1, p, q;
+ dMultiply0_331 ( ax1, node[0].body->posr.R, axis1 );
+
+ // Find the 2 axis perpendicular to the rotoide axis.
+ dPlaneSpace ( ax1, p, q );
+
+ // LHS
+ dCopyVector3 ( J1 + GI2__JA_MIN, p );
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3 ( J2 + GI2__JA_MIN, p );
+ }
+
+ dCopyVector3 ( J1 + rowskip + GI2__JA_MIN, q );
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3 ( J2 + rowskip + GI2__JA_MIN, q );
+
+ // Some math for the RHS
+ dVector3 ax2;
+ dMultiply0_331 ( ax2, R2, axis2 );
+ dCalcVectorCross3( b, ax1, ax2 );
+ }
+ else
+ {
+ // Some math for the RHS
+ dCalcVectorCross3( b, ax1, axis2 );
+ }
+
+ // RHS
+ pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3 ( p, b );
+ pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3 ( q, b );
+
+
+ // ======================================================================
+ // Work on the linear part (i.e row 2,3)
+ // p2 + R2 anchor2' = p1 + R1 dist'
+ // v2 + w2 R2 anchor2' + R2 d(anchor2')/dt = v1 + w1 R1 dist' + R1 d(dist')/dt
+ // v2 + w2 x anchor2 = v1 + w1 x dist + v_p
+ // v_p is speed of prismatic joint (i.e. elongation rate)
+ // Since the constraints are perpendicular to v_p we have:
+ // p . v_p = 0 and q . v_p = 0
+ // Along p and q we have (since sliding along the prismatic axis is disregarded):
+ // u . ( v2 + w2 x anchor2 = v1 + w1 x dist + v_p) ( where u is p or q )
+ // Simplify
+ // u . v2 + u. w2 x anchor2 = u . v1 + u . w1 x dist
+ // or
+ // u . v1 - u . v2 + u . w1 x dist - u2 . w2 x anchor2 = 0
+ // using the fact that (a x b = - b x a)
+ // u . v1 - u . v2 - u . dist x w1 + u . anchor2 x w2 = 0
+ // With the help of the triple product:
+ // i.e. a . b x c = b . c x a = c . a x b or a . b x c = a x b . c
+ // Ref: http://mathworld.wolfram.com/ScalarTripleProduct.html
+ // u . v1 - u . v2 - u x dist . w1 + u x anchor2 . w2 = 0
+ // u . v1 - u . v2 + dist x u . w1 - u x anchor2 . w2 = 0
+ //
+ // Coeff for 1er line of: J1l => p, J2l => -p
+ // Coeff for 2er line of: J1l => q, J2l => -q
+ // Coeff for 1er line of: J1a => dist x p, J2a => p x anchor2
+ // Coeff for 2er line of: J1a => dist x q, J2a => q x anchor2
+
+ int currRowSkip = 2 * rowskip;
+ {
+ dCopyVector3 ( J1 + currRowSkip + GI2__JL_MIN, p );
+ dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, p );
+
+ if ( body1 )
+ {
+ // info->J2l[s2+i] = -p[i];
+ dCopyNegatedVector3 ( J2 + currRowSkip + GI2__JL_MIN, p );
+ // q x anchor2 instead of anchor2 x q since we want the negative value
+ dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, p, lanchor2 );
+ }
+ }
+
+ currRowSkip += rowskip;
+ {
+ dCopyVector3 ( J1 + currRowSkip + GI2__JL_MIN, q );
+ dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, q );
+
+ if ( body1 )
+ {
+ // info->J2l[s3+i] = -q[i];
+ dCopyNegatedVector3 ( J2 + currRowSkip + GI2__JL_MIN, q );
+ // The cross product is in reverse order since we want the negative value
+ dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, lanchor2 );
+ }
+ }
+
+ // We want to make correction for motion not in the line of the axis
+ // We calculate the displacement w.r.t. the "anchor" pt.
+ // i.e. Find the difference between the current position and the initial
+ // position along the constrained axies (i.e. axis p and q).
+ // The bodies can move w.r.t each other only along the prismatic axis
+ //
+ // Compute the RHS of rows 2 and 3
+ dVector3 err;
+ dMultiply0_331 ( err, R1, anchor1 );
+ dSubtractVectors3( err, dist, err );
+
+ int currPairSkip = 2 * pairskip;
+ {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( p, err );
+ }
+
+ currPairSkip += pairskip;
+ {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( q, err );
+ }
+
+ currRowSkip += rowskip; currPairSkip += pairskip;
+
+ if ( body1 || (flags & dJOINT_REVERSE) == 0 )
+ {
+ if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 0 ))
+ {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+ else
+ {
+ dVector3 rAx1;
+ dCopyNegatedVector3(rAx1, ax1);
+
+ if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAx1, 0 ))
+ {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+
+ limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
+}
+
+void dJointSetPistonAnchor ( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+ setAnchors ( joint, x, y, z, joint->anchor1, joint->anchor2 );
+ joint->computeInitialRelativeRotation();
+
+}
+
+void dJointSetPistonAnchorOffset (dJointID j, dReal x, dReal y, dReal z,
+ dReal dx, dReal dy, dReal dz)
+{
+ dxJointPiston* joint = (dxJointPiston*) j;
+ dUASSERT (joint,"bad joint argument");
+ checktype ( joint, Piston );
+
+ if (joint->flags & dJOINT_REVERSE)
+ {
+ dx = -dx;
+ dy = -dy;
+ dz = -dz;
+ }
+
+ if (joint->node[0].body)
+ {
+ joint->node[0].body->posr.pos[0] -= dx;
+ joint->node[0].body->posr.pos[1] -= dy;
+ joint->node[0].body->posr.pos[2] -= dz;
+ }
+
+ setAnchors (joint,x ,y, z, joint->anchor1, joint->anchor2);
+
+ if (joint->node[0].body)
+ {
+ joint->node[0].body->posr.pos[0] += dx;
+ joint->node[0].body->posr.pos[1] += dy;
+ joint->node[0].body->posr.pos[2] += dz;
+ }
+
+ joint->computeInitialRelativeRotation();
+}
+
+
+
+void dJointGetPistonAnchor ( dJointID j, dVector3 result )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ dUASSERT ( result, "bad result argument" );
+ checktype ( joint, Piston );
+ if ( joint->flags & dJOINT_REVERSE )
+ getAnchor2 ( joint, result, joint->anchor2 );
+ else
+ getAnchor ( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetPistonAnchor2 ( dJointID j, dVector3 result )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ dUASSERT ( result, "bad result argument" );
+ checktype ( joint, Piston );
+ if ( joint->flags & dJOINT_REVERSE )
+ getAnchor ( joint, result, joint->anchor1 );
+ else
+ getAnchor2 ( joint, result, joint->anchor2 );
+}
+
+
+
+void dJointSetPistonAxis ( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
+
+ joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetPistonAxisDelta ( dJointID j, dReal x, dReal y, dReal z,
+ dReal dx, dReal dy, dReal dz )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
+
+ joint->computeInitialRelativeRotation();
+
+ dVector3 c = {0,0,0};
+ if ( joint->node[1].body )
+ {
+ c[0] = ( joint->node[0].body->posr.pos[0] -
+ joint->node[1].body->posr.pos[0] - dx );
+ c[1] = ( joint->node[0].body->posr.pos[1] -
+ joint->node[1].body->posr.pos[1] - dy );
+ c[2] = ( joint->node[0].body->posr.pos[2] -
+ joint->node[1].body->posr.pos[2] - dz );
+ }
+ else /*if ( joint->node[0].body )*/ // -- body[0] should always be present -- there is a matrix multiplication below
+ {
+ c[0] = joint->node[0].body->posr.pos[0] - dx;
+ c[1] = joint->node[0].body->posr.pos[1] - dy;
+ c[2] = joint->node[0].body->posr.pos[2] - dz;
+ }
+
+ // Convert into frame of body 1
+ dMultiply1_331 ( joint->anchor1, joint->node[0].body->posr.R, c );
+}
+
+
+
+void dJointGetPistonAxis ( dJointID j, dVector3 result )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ dUASSERT ( result, "bad result argument" );
+ checktype ( joint, Piston );
+
+ getAxis ( joint, result, joint->axis1 );
+}
+
+void dJointSetPistonParam ( dJointID j, int parameter, dReal value )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( ( parameter & 0xff00 ) == 0x100 )
+ {
+ joint->limotR.set ( parameter & 0xff, value );
+ }
+ else
+ {
+ joint->limotP.set ( parameter, value );
+ }
+}
+
+
+dReal dJointGetPistonParam ( dJointID j, int parameter )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( ( parameter & 0xff00 ) == 0x100 )
+ {
+ return joint->limotR.get ( parameter & 0xff );
+ }
+ else
+ {
+ return joint->limotP.get ( parameter );
+ }
+}
+
+
+void dJointAddPistonForce ( dJointID j, dReal force )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( joint->flags & dJOINT_REVERSE )
+ force -= force;
+
+ dVector3 axis;
+ getAxis ( joint, axis, joint->axis1 );
+ // axis[i] *= force
+ dScaleVector3( axis, force );
+
+
+ if ( joint->node[0].body != 0 )
+ dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
+ if ( joint->node[1].body != 0 )
+ dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
+
+ if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
+ {
+ // Case where we don't need ltd since center of mass of both bodies
+ // pass by the anchor point '*' when travelling along the prismatic axis.
+ // Body_2
+ // Body_1 -----
+ // --- |-- | |
+ // | |---------------*-------------| | ---> prismatic axis
+ // --- |-- | |
+ // -----
+ // Body_2
+ // Case where we need ltd
+ // Body_1
+ // ---
+ // | |---------
+ // --- |
+ // | |--
+ // -----*----- ---> prismatic axis
+ // |-- |
+ // |
+ // |
+ // | -----
+ // | | |
+ // -------| |
+ // | |
+ // -----
+ // Body_2
+ //
+ // In real life force apply at the '*' point
+ // But in ODE the force are applied on the center of mass of Body_1 and Body_2
+ // So we have to add torques on both bodies to compensate for that when there
+ // is an offset between the anchor point and the center of mass of both bodies.
+ //
+ // We need to add to each body T = r x F
+ // Where r is the distance between the cm and '*'
+
+ dVector3 ltd; // Linear Torque Decoupling vector (a torque)
+ dVector3 c; // Distance of the body w.r.t the anchor
+ // N.B. The distance along the prismatic axis might not
+ // not be included in this variable since it won't add
+ // anything to the ltd.
+
+ // Calculate the distance of the body w.r.t the anchor
+
+ // The anchor1 of body1 can be used since:
+ // Real anchor = Position of body 1 + anchor + d* axis1 = anchor in world frame
+ // d is the position of the prismatic joint (i.e. elongation)
+ // Since axis1 x axis1 == 0
+ // We can do the following.
+ dMultiply0_331 ( c, joint->node[0].body->posr.R, joint->anchor1 );
+ dCalcVectorCross3( ltd, c, axis );
+ dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
+
+
+ dMultiply0_331 ( c, joint->node[1].body->posr.R, joint->anchor2 );
+ dCalcVectorCross3( ltd, c, axis );
+ dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
+ }
+}
+
+
+dJointType
+dxJointPiston::type() const
+{
+ return dJointTypePiston;
+}
+
+
+sizeint
+dxJointPiston::size() const
+{
+ return sizeof ( *this );
+}
+
+
+
+void
+dxJointPiston::setRelativeValues()
+{
+ dVector3 vec;
+ dJointGetPistonAnchor(this, vec);
+ setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 );
+
+ dJointGetPistonAxis(this, vec);
+ setAxes( this, vec[0], vec[1], vec[2], axis1, axis2 );
+
+ computeInitialRelativeRotation();
+}
+
+
+
+
+void
+dxJointPiston::computeInitialRelativeRotation()
+{
+ if ( node[0].body )
+ {
+ if ( node[1].body )
+ {
+ dQMultiply1 ( qrel, node[0].body->q, node[1].body->q );
+ }
+ else
+ {
+ // set joint->qrel to the transpose of the first body q
+ qrel[0] = node[0].body->q[0];
+ for ( int i = 1; i < 4; i++ )
+ qrel[i] = -node[0].body->q[i];
+ // WARNING do we need the - in -joint->node[0].body->q[i]; or not
+ }
+ }
+}
diff --git a/libs/ode-0.16.1/ode/src/joints/piston.h b/libs/ode-0.16.1/ode/src/joints/piston.h new file mode 100644 index 0000000..c202c20 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/piston.h @@ -0,0 +1,112 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_PISTON_H_ +#define _ODE_JOINT_PISTON_H_ + +#include "joint.h" + + +//////////////////////////////////////////////////////////////////////////////// +/// Component of a Piston joint +/// <PRE> +/// |- Anchor point +/// Body_1 | Body_2 +/// +---------------+ V +------------------+ +/// / /| / /| +/// / / + |-- ______ / / + +/// / x /./........x.......(_____()..../ x /.......> axis +/// +---------------+ / |-- +------------------+ / +/// | |/ | |/ +/// +---------------+ +------------------+ +/// | | +/// | | +/// |------------------> <----------------------------| +/// anchor1 anchor2 +/// +/// +/// </PRE> +/// +/// When the prismatic joint as been elongated (i.e. dJointGetPistonPosition) +/// return a value > 0 +/// <PRE> +/// |- Anchor point +/// Body_1 | Body_2 +/// +---------------+ V +------------------+ +/// / /| / /| +/// / / + |-- ______ / / + +/// / x /./........_____x.......(_____()..../ x /.......> axis +/// +---------------+ / |-- +------------------+ / +/// | |/ | |/ +/// +---------------+ +------------------+ +/// | | +/// | | +/// |------------------> <----------------------------| +/// anchor1 |----| anchor2 +/// ^ +/// |-- This is what dJointGetPistonPosition will +/// return +/// </PRE> +//////////////////////////////////////////////////////////////////////////////// +struct dxJointPiston : public dxJoint +{ + dVector3 axis1; ///< Axis of the prismatic and rotoide w.r.t first body + dVector3 axis2; ///< Axis of the prismatic and rotoide w.r.t second body + + + dQuaternion qrel; ///< Initial relative rotation body1 -> body2 + + /// Anchor w.r.t first body. + /// This is the same as the offset for the Slider joint + /// @note To find the position of the anchor when the body 1 has moved + /// you must add the position of the prismatic joint + /// i.e anchor = R1 * anchor1 + dJointGetPistonPosition() * (R1 * axis1) + dVector3 anchor1; + dVector3 anchor2; //< anchor w.r.t second body + + /// limit and motor information for the prismatic + /// part of the joint + dxJointLimitMotor limotP; + + /// limit and motor information for the rotoide + /// part of the joint + dxJointLimitMotor limotR; + + dxJointPiston( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + virtual void setRelativeValues(); + + void computeInitialRelativeRotation(); +}; + + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/plane2d.cpp b/libs/ode-0.16.1/ode/src/joints/plane2d.cpp new file mode 100644 index 0000000..0caecb3 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/plane2d.cpp @@ -0,0 +1,195 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "plane2d.h" +#include "joint_internal.h" + + + +//**************************************************************************** +// Plane2D +/* +This code is part of the Plane2D ODE joint +by psero@gmx.de +Wed Apr 23 18:53:43 CEST 2003 +*/ + + +static const dReal Midentity[3][3] = +{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1, } +}; + + +dxJointPlane2D::dxJointPlane2D( dxWorld *w ) : + dxJoint( w ) +{ + motor_x.init( world ); + motor_y.init( world ); + motor_angle.init( world ); +} + + +void +dxJointPlane2D::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + +void +dxJointPlane2D::getInfo1( dxJoint::Info1 *info ) +{ + info->nub = 3; + info->m = 3; + + if ( motor_x.fmax > 0 ) + row_motor_x = info->m++; + else + row_motor_x = 0; + + if ( motor_y.fmax > 0 ) + row_motor_y = info->m++; + else + row_motor_y = 0; + + if ( motor_angle.fmax > 0 ) + row_motor_angle = info->m++; + else + row_motor_angle = 0; +} + + + +void +dxJointPlane2D::getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + dReal eps = worldFPS * worldERP; + + /* + v = v1, w = omega1 + (v2, omega2 not important (== static environment)) + + constraint equations: + vz = 0 + wx = 0 + wy = 0 + + <=> ( 0 0 1 ) (vx) ( 0 0 0 ) (wx) ( 0 ) + ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 ) + ( 0 0 0 ) (vz) ( 0 1 0 ) (wz) ( 0 ) + J1/J1l Omega1/J1a + */ + + // fill in linear and angular coeff. for left hand side: + + J1[GI2_JLZ] = 1; + J1[rowskip + GI2_JAX] = 1; + J1[2 * rowskip + GI2_JAY] = 1; + + // error correction (against drift): + + // a) linear vz, so that z (== pos[2]) == 0 + pairRhsCfm[GI2_RHS] = eps * -node[0].body->posr.pos[2]; + +# if 0 + // b) angular correction? -> left to application !!! + dReal *body_z_axis = &node[0].body->R[8]; + pairRhsCfm[pairskip + GI2_RHS] = eps * + atan2( body_z_axis[1], body_z_axis[2] ); // wx error + pairRhsCfm[2 * pairskip + GI2_RHS] = eps * -atan2( body_z_axis[0], body_z_axis[2] ); // wy error +# endif + + // if the slider is powered, or has joint limits, add in the extra row: + + if ( row_motor_x > 0 ) + { + int currRowSkip = row_motor_x * rowskip, currPairSkip = row_motor_x * pairskip; + motor_x.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[0], 0 ); + } + + if ( row_motor_y > 0 ) + { + int currRowSkip = row_motor_y * rowskip, currPairSkip = row_motor_y * pairskip; + motor_y.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[1], 0 ); + } + + if ( row_motor_angle > 0 ) + { + int currRowSkip = row_motor_angle * rowskip, currPairSkip = row_motor_angle * pairskip; + motor_angle.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[2], 1 ); + } +} + + +dJointType +dxJointPlane2D::type() const +{ + return dJointTypePlane2D; +} + + +sizeint +dxJointPlane2D::size() const +{ + return sizeof( *this ); +} + + + +void dJointSetPlane2DXParam( dxJoint *joint, + int parameter, dReal value ) +{ + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Plane2D ); + dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint ); + joint2d->motor_x.set( parameter, value ); +} + + +void dJointSetPlane2DYParam( dxJoint *joint, + int parameter, dReal value ) +{ + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Plane2D ); + dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint ); + joint2d->motor_y.set( parameter, value ); +} + + + +void dJointSetPlane2DAngleParam( dxJoint *joint, + int parameter, dReal value ) +{ + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Plane2D ); + dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint ); + joint2d->motor_angle.set( parameter, value ); +} + diff --git a/libs/ode-0.16.1/ode/src/joints/plane2d.h b/libs/ode-0.16.1/ode/src/joints/plane2d.h new file mode 100644 index 0000000..a9ccab6 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/plane2d.h @@ -0,0 +1,54 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_PLANE2D_H_ +#define _ODE_JOINT_PLANE2D_H_ + +#include "joint.h" + + +// 2d joint, constrains to z == 0 + +struct dxJointPlane2D : public dxJoint +{ + int row_motor_x; + int row_motor_y; + int row_motor_angle; + dxJointLimitMotor motor_x; + dxJointLimitMotor motor_y; + dxJointLimitMotor motor_angle; + + + dxJointPlane2D( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/pr.cpp b/libs/ode-0.16.1/ode/src/joints/pr.cpp new file mode 100644 index 0000000..7d34ebe --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/pr.cpp @@ -0,0 +1,613 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "pr.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// Prismatic and Rotoide
+
+dxJointPR::dxJointPR( dxWorld *w ) :
+ dxJoint( w )
+{
+ // Default Position
+ // Z^
+ // | Body 1 P R Body2
+ // |+---------+ _ _ +-----------+
+ // || |----|----(_)--------+ |
+ // |+---------+ - +-----------+
+ // |
+ // X.-----------------------------------------> Y
+ // N.B. X is comming out of the page
+ dSetZero( anchor2, 4 );
+
+ dSetZero( axisR1, 4 );
+ axisR1[0] = 1;
+ dSetZero( axisR2, 4 );
+ axisR2[0] = 1;
+
+ dSetZero( axisP1, 4 );
+ axisP1[1] = 1;
+ dSetZero( qrel, 4 );
+ dSetZero( offset, 4 );
+
+ limotR.init( world );
+ limotP.init( world );
+}
+
+
+dReal dJointGetPRPosition( dJointID j )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, PR );
+
+ dVector3 q;
+ // get the offset in global coordinates
+ dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );
+
+ if ( joint->node[1].body )
+ {
+ dVector3 anchor2;
+
+ // get the anchor2 in global coordinates
+ dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
+
+ q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
+ ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
+ q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
+ ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
+ q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
+ ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
+
+ }
+ else
+ {
+ //N.B. When there is no body 2 the joint->anchor2 is already in
+ // global coordinates
+
+ q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
+ ( joint->anchor2[0] ) );
+ q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
+ ( joint->anchor2[1] ) );
+ q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
+ ( joint->anchor2[2] ) );
+
+ if ( joint->flags & dJOINT_REVERSE )
+ {
+ q[0] = -q[0];
+ q[1] = -q[1];
+ q[2] = -q[2];
+ }
+ }
+
+ dVector3 axP;
+ // get prismatic axis in global coordinates
+ dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );
+
+ return dCalcVectorDot3( axP, q );
+}
+
+dReal dJointGetPRPositionRate( dJointID j )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, PR );
+ // get axis1 in global coordinates
+ dVector3 ax1;
+ dMultiply0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 );
+
+ if ( joint->node[1].body )
+ {
+ dVector3 lv2;
+ dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 );
+ return dCalcVectorDot3( ax1, joint->node[0].body->lvel ) - dCalcVectorDot3( ax1, lv2 );
+ }
+ else
+ {
+ dReal rate = dCalcVectorDot3( ax1, joint->node[0].body->lvel );
+ return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
+ }
+}
+
+
+
+dReal dJointGetPRAngle( dJointID j )
+{
+ dxJointPR* joint = ( dxJointPR* )j;
+ dAASSERT( joint );
+ checktype( joint, PR );
+ if ( joint->node[0].body )
+ {
+ dReal ang = getHingeAngle( joint->node[0].body,
+ joint->node[1].body,
+ joint->axisR1,
+ joint->qrel );
+ if ( joint->flags & dJOINT_REVERSE )
+ return -ang;
+ else
+ return ang;
+ }
+ else return 0;
+}
+
+
+
+dReal dJointGetPRAngleRate( dJointID j )
+{
+ dxJointPR* joint = ( dxJointPR* )j;
+ dAASSERT( joint );
+ checktype( joint, PR );
+ if ( joint->node[0].body )
+ {
+ dVector3 axis;
+ dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axisR1 );
+ dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+ if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+ if ( joint->flags & dJOINT_REVERSE ) rate = -rate;
+ return rate;
+ }
+ else return 0;
+}
+
+
+
+
+void
+dxJointPR::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = 6;
+}
+
+
+
+void
+dxJointPR::getInfo1( dxJoint::Info1 *info )
+{
+ info->nub = 4;
+ info->m = 4;
+
+
+ // see if we're at a joint limit.
+ limotP.limit = 0;
+ if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
+ limotP.lostop <= limotP.histop )
+ {
+ // measure joint position
+ dReal pos = dJointGetPRPosition( this );
+ limotP.testRotationalLimit( pos ); // N.B. The function is ill named
+ }
+
+ // powered needs an extra constraint row
+ if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+
+
+ // see if we're at a joint limit.
+ limotR.limit = 0;
+ if (( limotR.lostop >= -M_PI || limotR.histop <= M_PI ) &&
+ limotR.lostop <= limotR.histop )
+ {
+ dReal angle = getHingeAngle( node[0].body,
+ node[1].body,
+ axisR1, qrel );
+ limotR.testRotationalLimit( angle );
+ }
+
+ // powered morit or at limits needs an extra constraint row
+ if ( limotR.limit || limotR.fmax > 0 ) info->m++;
+
+}
+
+
+
+void
+dxJointPR::getInfo2( dReal worldFPS, dReal worldERP,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+{
+ dReal k = worldFPS * worldERP;
+
+
+ dVector3 q; // plane space of axP and after that axR
+
+ // pull out pos and R for both bodies. also get the `connection'
+ // vector pos2-pos1.
+
+ dReal *pos2 = NULL, *R2 = NULL;
+
+ dReal *pos1 = node[0].body->posr.pos;
+ dReal *R1 = node[0].body->posr.R;
+
+ dxBody *body1 = node[1].body;
+
+ if ( body1 )
+ {
+ pos2 = body1->posr.pos;
+ R2 = body1->posr.R;
+ }
+
+
+ dVector3 axP; // Axis of the prismatic joint in global frame
+ dMultiply0_331( axP, R1, axisP1 );
+
+ // distance between the body1 and the anchor2 in global frame
+ // Calculated in the same way as the offset
+ dVector3 wanchor2 = {0, 0, 0}, dist;
+
+ if ( body1 )
+ {
+ // Calculate anchor2 in world coordinate
+ dMultiply0_331( wanchor2, R2, anchor2 );
+ dist[0] = wanchor2[0] + pos2[0] - pos1[0];
+ dist[1] = wanchor2[1] + pos2[1] - pos1[1];
+ dist[2] = wanchor2[2] + pos2[2] - pos1[2];
+ }
+ else
+ {
+ if ( (flags & dJOINT_REVERSE) != 0 )
+ {
+ dSubtractVectors3(dist, pos1, anchor2); // Invert the value
+ }
+ else
+ {
+ dSubtractVectors3(dist, anchor2, pos1); // Invert the value
+ }
+ }
+
+
+ // ======================================================================
+ // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered
+
+ // Set the two rotoide rows. The rotoide axis should be the only unconstrained
+ // rotational axis, the angular velocity of the two bodies perpendicular to
+ // the rotoide axis should be equal. Thus the constraint equations are
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ dVector3 ax2;
+ dVector3 ax1;
+ dMultiply0_331( ax1, R1, axisR1 );
+ dCalcVectorCross3( q , ax1, axP );
+
+ dCopyVector3(J1 + GI2__JA_MIN, axP);
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3(J2 + GI2__JA_MIN, axP);
+ }
+
+ dCopyVector3(J1 + rowskip + GI2__JA_MIN, q);
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, q);
+ }
+
+ // Compute the right hand side of the constraint equation set. Relative
+ // body velocities along p and q to bring the rotoide back into alignment.
+ // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame.
+ // We need to rotate both bodies along the axis u = (ax1 x ax2).
+ // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+ // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+ // ...as ax1 and ax2 are unit length. if theta is smallish,
+ // theta ~= sin(theta), so
+ // angular_velocity = (erp*fps) * (ax1 x ax2)
+ // ax1 x ax2 is in the plane space of ax1, so we project the angular
+ // velocity to p and q to find the right hand side.
+
+ if ( body1 )
+ {
+ dMultiply0_331( ax2, R2, axisR2 );
+ }
+ else
+ {
+ dCopyVector3(ax2, axisR2);
+ }
+
+ dVector3 b;
+ dCalcVectorCross3( b, ax1, ax2 );
+ pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3( b, axP );
+ pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( b, q );
+
+
+
+ // ==========================
+ // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered
+ // or 5 if rotoide and prismatic powered
+
+ // two rows. we want: vel2 = vel1 + w1 x c ... but this would
+ // result in three equations, so we project along the planespace vectors
+ // so that sliding along the prismatic axis is disregarded. for symmetry we
+ // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
+
+ // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist'
+ // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p
+ // v_p is speed of prismatic joint (i.e. elongation rate)
+ // Since the constraints are perpendicular to v_p we have:
+ // p dot v_p = 0 and q dot v_p = 0
+ // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
+ // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
+ // ==
+ // ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist
+ // since a . (b x c) = - b . (a x c) = - (a x c) . b
+ // and a x b = - b x a
+ // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0
+ // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0
+ // Coeff for 1er line of: J1l => ax1, J2l => -ax1
+ // Coeff for 2er line of: J1l => q, J2l => -q
+ // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1
+ // Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q
+
+ int currRowSkip = 2 * rowskip;
+ {
+ dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, ax1 );
+ dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, ax1 );
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, ax1 );
+ // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
+ dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, ax2, wanchor2 ); // since ax1 == ax2
+ }
+ }
+
+ currRowSkip += rowskip;
+ {
+ dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, q );
+ dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, dist, q );
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, q);
+ // The cross product is in reverse order since we want the negative value
+ dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, wanchor2 );
+ }
+ }
+
+ // We want to make correction for motion not in the line of the axisP
+ // We calculate the displacement w.r.t. the anchor pt.
+ //
+ // compute the elements 2 and 3 of right hand side.
+ // we want to align the offset point (in body 2's frame) with the center of body 1.
+ // The position should be the same when we are not along the prismatic axis
+ dVector3 err;
+ dMultiply0_331( err, R1, offset );
+ dSubtractVectors3(err, dist, err);
+
+ int currPairSkip = 2 * pairskip;
+ {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( ax1, err );
+ }
+
+ currPairSkip += pairskip;
+ {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( q, err );
+ }
+
+ currRowSkip += rowskip; currPairSkip += pairskip;
+
+ if ( body1 || (flags & dJOINT_REVERSE) == 0 )
+ {
+ if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, 0 ))
+ {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+ else
+ {
+ dVector3 rAxP;
+ dCopyNegatedVector3(rAxP, axP);
+
+ if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAxP, 0 ))
+ {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+
+ limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
+}
+
+
+// compute initial relative rotation body1 -> body2, or env -> body1
+void
+dxJointPR::computeInitialRelativeRotation()
+{
+ if ( node[0].body )
+ {
+ if ( node[1].body )
+ {
+ dQMultiply1( qrel, node[0].body->q, node[1].body->q );
+ }
+ else
+ {
+ // set joint->qrel to the transpose of the first body q
+ qrel[0] = node[0].body->q[0];
+ for ( int i = 1; i < 4; i++ )
+ qrel[i] = -node[0].body->q[i];
+ // WARNING do we need the - in -joint->node[0].body->q[i]; or not
+ }
+ }
+}
+
+void dJointSetPRAnchor( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, PR );
+ setAnchors( joint, x, y, z, joint->offset, joint->anchor2 );
+}
+
+
+void dJointSetPRAxis1( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, PR );
+
+ setAxes( joint, x, y, z, joint->axisP1, 0 );
+
+ joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetPRAxis2( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, PR );
+ setAxes( joint, x, y, z, joint->axisR1, joint->axisR2 );
+ joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetPRParam( dJointID j, int parameter, dReal value )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, PR );
+ if (( parameter & 0xff00 ) == 0x100 )
+ {
+ joint->limotR.set( parameter & 0xff, value ); // Take only lower part of the
+ } // parameter alue
+ else
+ {
+ joint->limotP.set( parameter, value );
+ }
+}
+
+void dJointGetPRAnchor( dJointID j, dVector3 result )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+ checktype( joint, PR );
+
+ if ( joint->node[1].body )
+ getAnchor2( joint, result, joint->anchor2 );
+ else
+ {
+ result[0] = joint->anchor2[0];
+ result[1] = joint->anchor2[1];
+ result[2] = joint->anchor2[2];
+ }
+}
+
+void dJointGetPRAxis1( dJointID j, dVector3 result )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+ checktype( joint, PR );
+ getAxis( joint, result, joint->axisP1 );
+}
+
+void dJointGetPRAxis2( dJointID j, dVector3 result )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+ checktype( joint, PR );
+ getAxis( joint, result, joint->axisR1 );
+}
+
+dReal dJointGetPRParam( dJointID j, int parameter )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dUASSERT( joint, "bad joint argument" );
+ checktype( joint, PR );
+ if (( parameter & 0xff00 ) == 0x100 )
+ {
+ return joint->limotR.get( parameter & 0xff );
+ }
+ else
+ {
+ return joint->limotP.get( parameter );
+ }
+}
+
+void dJointAddPRTorque( dJointID j, dReal torque )
+{
+ dxJointPR* joint = ( dxJointPR* ) j;
+ dVector3 axis;
+ dAASSERT( joint );
+ checktype( joint, PR );
+
+ if ( joint->flags & dJOINT_REVERSE )
+ torque = -torque;
+
+ getAxis( joint, axis, joint->axisR1 );
+ axis[0] *= torque;
+ axis[1] *= torque;
+ axis[2] *= torque;
+
+ if ( joint->node[0].body != 0 )
+ dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] );
+ if ( joint->node[1].body != 0 )
+ dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
+}
+
+
+dJointType
+dxJointPR::type() const
+{
+ return dJointTypePR;
+}
+
+sizeint
+dxJointPR::size() const
+{
+ return sizeof( *this );
+}
+
+
+void
+dxJointPR::setRelativeValues()
+{
+ dVector3 anchor;
+ dJointGetPRAnchor(this, anchor);
+ setAnchors( this, anchor[0], anchor[1], anchor[2], offset, anchor2 );
+
+ dVector3 axis;
+ dJointGetPRAxis1(this, axis);
+ setAxes( this, axis[0], axis[1], axis[2], axisP1, 0 );
+
+ dJointGetPRAxis2(this, axis);
+ setAxes( this, axis[0], axis[1], axis[2], axisR1, axisR2 );
+
+ computeInitialRelativeRotation();
+}
+
+
+
+
+
diff --git a/libs/ode-0.16.1/ode/src/joints/pr.h b/libs/ode-0.16.1/ode/src/joints/pr.h new file mode 100644 index 0000000..930c0cd --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/pr.h @@ -0,0 +1,100 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_PR_H_ +#define _ODE_JOINT_PR_H_ + +#include "joint.h" + + + +/** + * The axisP must be perpendicular to axis2 + * <PRE> + * +-------------+ + * | x | + * +------------\+ + * Prismatic articulation .. .. + * | .. .. + * \/ .. .. + * +--------------+ --| __.. .. anchor2 + * | x | .....|.......(__) .. + * +--------------+ --| ^ < + * |----------------------->| + * Offset |--- Rotoide articulation + * </PRE> + */ +struct dxJointPR : public dxJoint +{ + + /// @brief Position of the rotoide articulation w.r.t second body. + /// @note Position of body 2 in world frame + anchor2 in world frame give + /// the position of the rotoide articulation + dVector3 anchor2; + + + /// axis of the rotoide articulation w.r.t first body. + /// @note This is considered as axis1 from the parameter view. + dVector3 axisR1; + + /// axis of the rotoide articulation w.r.t second body. + /// @note This is considered also as axis1 from the parameter view + dVector3 axisR2; + + /// axis for the prismatic articulation w.r.t first body. + /// @note This is considered as axis2 in from the parameter view + dVector3 axisP1; + + + dQuaternion qrel; ///< initial relative rotation body1 -> body2. + + + /// @brief vector between the body1 and the rotoide articulation. + /// + /// Going from the first to the second in the frame of body1. + /// That should be aligned with body1 center along axisP. + /// This is calculated when the axis are set. + dVector3 offset; + dxJointLimitMotor limotR; ///< limit and motor information for the rotoide articulation. + dxJointLimitMotor limotP; ///< limit and motor information for the prismatic articulation. + + + void computeInitialRelativeRotation(); + + + dxJointPR( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + virtual void setRelativeValues(); +}; + + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/pu.cpp b/libs/ode-0.16.1/ode/src/joints/pu.cpp new file mode 100644 index 0000000..42eaf4b --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/pu.cpp @@ -0,0 +1,756 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "pu.h" +#include "joint_internal.h" + + +//**************************************************************************** +// Prismatic and Universal + +dxJointPU::dxJointPU( dxWorld *w ) : + dxJointUniversal( w ) +{ + // Default Position + // Y ^ Axis2 + // ^ | + // / | ^ Axis1 + // Z^ / | / + // | / Body 2 | / Body 1 + // | / +---------+ | / +-----------+ + // | / / /| | / / /| + // | / / / + _/ - / / + + // | / / /-/--------(_)----|--- /-----------/-------> AxisP + // | / +---------+ / - +-----------+ / + // | / | |/ | |/ + // | / +---------+ +-----------+ + // |/ + // .-----------------------------------------> X + // |-----------------> + // Anchor2 <--------------| + // Anchor1 + // + + // Setting member variables which are w.r.t body2 + dSetZero( axis1, 4 ); + axis1[1] = 1; + + // Setting member variables which are w.r.t body2 + dSetZero( anchor2, 4 ); + dSetZero( axis2, 4 ); + axis2[2] = 1; + + dSetZero( axisP1, 4 ); + axisP1[0] = 1; + + dSetZero( qrel1, 4 ); + dSetZero( qrel2, 4 ); + + + limotP.init( world ); + limot1.init( world ); + limot2.init( world ); +} + + +dReal dJointGetPUPosition( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + dVector3 q; + // get the offset in global coordinates + dMultiply0_331( q, joint->node[0].body->posr.R, joint->anchor1 ); + + if ( joint->node[1].body ) + { + dVector3 anchor2; + + // get the anchor2 in global coordinates + dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 ); + + q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) - + ( joint->node[1].body->posr.pos[0] + anchor2[0] ) ); + q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) - + ( joint->node[1].body->posr.pos[1] + anchor2[1] ) ); + q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) - + ( joint->node[1].body->posr.pos[2] + anchor2[2] ) ); + } + else + { + //N.B. When there is no body 2 the joint->anchor2 is already in + // global coordinates + + q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) - + ( joint->anchor2[0] ) ); + q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) - + ( joint->anchor2[1] ) ); + q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) - + ( joint->anchor2[2] ) ); + + if ( joint->flags & dJOINT_REVERSE ) + { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + } + } + + dVector3 axP; + // get prismatic axis in global coordinates + dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 ); + + return dCalcVectorDot3( axP, q ); +} + + +dReal dJointGetPUPositionRate( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if ( joint->node[0].body ) + { + // We want to find the rate of change of the prismatic part of the joint + // We can find it by looking at the speed difference between body1 and the + // anchor point. + + // r will be used to find the distance between body1 and the anchor point + dVector3 r; + dVector3 anchor2 = {0,0,0}; + if ( joint->node[1].body ) + { + // Find joint->anchor2 in global coordinates + dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 ); + + r[0] = ( joint->node[0].body->posr.pos[0] - + ( anchor2[0] + joint->node[1].body->posr.pos[0] ) ); + r[1] = ( joint->node[0].body->posr.pos[1] - + ( anchor2[1] + joint->node[1].body->posr.pos[1] ) ); + r[2] = ( joint->node[0].body->posr.pos[2] - + ( anchor2[2] + joint->node[1].body->posr.pos[2] ) ); + } + else + { + //N.B. When there is no body 2 the joint->anchor2 is already in + // global coordinates + // r = joint->node[0].body->posr.pos - joint->anchor2; + dSubtractVectors3( r, joint->node[0].body->posr.pos, joint->anchor2 ); + } + + // The body1 can have velocity coming from the rotation of + // the rotoide axis. We need to remove this. + + // N.B. We do vel = r X w instead of vel = w x r to have vel negative + // since we want to remove it from the linear velocity of the body + dVector3 lvel1; + dCalcVectorCross3( lvel1, r, joint->node[0].body->avel ); + + // lvel1 += joint->node[0].body->lvel; + dAddVectors3( lvel1, lvel1, joint->node[0].body->lvel ); + + // Since we want rate of change along the prismatic axis + // get axisP1 in global coordinates and get the component + // along this axis only + dVector3 axP1; + dMultiply0_331( axP1, joint->node[0].body->posr.R, joint->axisP1 ); + + if ( joint->node[1].body ) + { + // Find the contribution of the angular rotation to the linear speed + // N.B. We do vel = r X w instead of vel = w x r to have vel negative + // since we want to remove it from the linear velocity of the body + dVector3 lvel2; + dCalcVectorCross3( lvel2, anchor2, joint->node[1].body->avel ); + + // lvel1 -= lvel2 + joint->node[1].body->lvel; + dVector3 tmp; + dAddVectors3( tmp, lvel2, joint->node[1].body->lvel ); + dSubtractVectors3( lvel1, lvel1, tmp ); + + return dCalcVectorDot3( axP1, lvel1 ); + } + else + { + dReal rate = dCalcVectorDot3( axP1, lvel1 ); + return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate); + } + } + + return 0.0; +} + + + +void +dxJointPU::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + + +void +dxJointPU::getInfo1( dxJoint::Info1 *info ) +{ + info->m = 3; + info->nub = 3; + + // powered needs an extra constraint row + + // see if we're at a joint limit. + limotP.limit = 0; + if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) && + limotP.lostop <= limotP.histop ) + { + // measure joint position + dReal pos = dJointGetPUPosition( this ); + limotP.testRotationalLimit( pos ); // N.B. The function is ill named + } + + if ( limotP.limit || limotP.fmax > 0 ) info->m++; + + + bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) && + limot1.lostop <= limot1.histop; + bool limiting2 = ( limot2.lostop >= -M_PI || limot2.histop <= M_PI ) && + limot2.lostop <= limot2.histop; + + // We need to call testRotationLimit() even if we're motored, since it + // records the result. + limot1.limit = 0; + limot2.limit = 0; + if ( limiting1 || limiting2 ) + { + dReal angle1, angle2; + getAngles( &angle1, &angle2 ); + if ( limiting1 ) + limot1.testRotationalLimit( angle1 ); + if ( limiting2 ) + limot2.testRotationalLimit( angle2 ); + } + + if ( limot1.limit || limot1.fmax > 0 ) info->m++; + if ( limot2.limit || limot2.fmax > 0 ) info->m++; +} + + + +void +dxJointPU::getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + const dReal k = worldFPS * worldERP; + + // ====================================================================== + // The angular constraint + // + dVector3 ax1, ax2; // Global axes of rotation + getAxis(this, ax1, axis1); + getAxis2(this,ax2, axis2); + + dVector3 uniPerp; // Axis perpendicular to axes of rotation + dCalcVectorCross3(uniPerp,ax1,ax2); + dNormalize3( uniPerp ); + + dCopyVector3( J1 + GI2__JA_MIN, uniPerp ); + + dxBody *body1 = node[1].body; + + if ( body1 ) { + dCopyNegatedVector3( J2 + GI2__JA_MIN , uniPerp ); + } + // Corrective velocity attempting to keep uni axes perpendicular + dReal val = dCalcVectorDot3( ax1, ax2 ); + // Small angle approximation : + // theta = asin(val) + // theta is approximately val when val is near zero. + pairRhsCfm[GI2_RHS] = -k * val; + + // ========================================================================== + // Handle axes orthogonal to the prismatic + dVector3 an1, an2; // Global anchor positions + dVector3 axP, sep; // Prismatic axis and separation vector + getAnchor(this, an1, anchor1); + getAnchor2(this, an2, anchor2); + + if (flags & dJOINT_REVERSE) { + getAxis2(this, axP, axisP1); + } else { + getAxis(this, axP, axisP1); + } + dSubtractVectors3(sep, an2, an1); + + dVector3 p, q; + dPlaneSpace(axP, p, q); + + dCopyVector3( J1 + rowskip + GI2__JL_MIN, p ); + dCopyVector3( J1 + 2 * rowskip + GI2__JL_MIN, q ); + // Make the anchors be body local + // Aliasing isn't a problem here. + dSubtractVectors3(an1, an1, node[0].body->posr.pos); + dCalcVectorCross3( J1 + rowskip + GI2__JA_MIN, an1, p ); + dCalcVectorCross3( J1 + 2 * rowskip + GI2__JA_MIN, an1, q ); + + if (body1) { + dCopyNegatedVector3( J2 + rowskip + GI2__JL_MIN, p ); + dCopyNegatedVector3( J2 + 2 * rowskip + GI2__JL_MIN, q ); + dSubtractVectors3(an2, an2, body1->posr.pos); + dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, p, an2 ); + dCalcVectorCross3( J2 + 2 * rowskip + GI2__JA_MIN, q, an2 ); + } + + pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( p, sep ); + pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( q, sep ); + + // ========================================================================== + // Handle the limits/motors + int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip; + + if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) { + currRowSkip += rowskip; currPairSkip += pairskip; + } + + if (limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 )) { + currRowSkip += rowskip; currPairSkip += pairskip; + } + + if ( body1 || (flags & dJOINT_REVERSE) == 0 ) { + limotP.addTwoPointLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 ); + } else { + dNegateVector3(axP); + limotP.addTwoPointLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 ); + } +} + +void dJointSetPUAnchor( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + joint->computeInitialRelativeRotations(); +} + +/** + * This function initialize the anchor and the relative position of each body + * as if body2 was at its current position + [dx,dy,dy]. + * Ex: + * <PRE> + * dReal offset = 1; + * dVector3 dir; + * dJointGetPUAxis3(jId, dir); + * dJointSetPUAnchor(jId, 0, 0, 0); + * // If you request the position you will have: dJointGetPUPosition(jId) == 0 + * dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset); + * // If you request the position you will have: dJointGetPUPosition(jId) == -offset + * </PRE> + + * @param j The PU joint for which the anchor point will be set + * @param x The X position of the anchor point in world frame + * @param y The Y position of the anchor point in world frame + * @param z The Z position of the anchor point in world frame + * @param dx A delta to be added to the X position as if the anchor was set + * when body1 was at current_position[X] + dx + * @param dx A delta to be added to the Y position as if the anchor was set + * when body1 was at current_position[Y] + dy + * @param dx A delta to be added to the Z position as if the anchor was set + * when body1 was at current_position[Z] + dz + * @note Should have the same meaning as dJointSetSliderAxisDelta + */ +void dJointSetPUAnchorDelta( dJointID j, dReal x, dReal y, dReal z, + dReal dx, dReal dy, dReal dz ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] += dx; + joint->node[0].body->posr.pos[1] += dy; + joint->node[0].body->posr.pos[2] += dz; + } + + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] -= dx; + joint->node[0].body->posr.pos[1] -= dy; + joint->node[0].body->posr.pos[2] -= dz; + } + + joint->computeInitialRelativeRotations(); +} + +/** + * \brief This function initialize the anchor and the relative position of each body + * such that dJointGetPUPosition will return the dot product of axis and [dx,dy,dy]. + * + * The body 1 is moved to [-dx, -dy, -dx] then the anchor is set. This will be the + * position 0 for the prismatic part of the joint. Then the body 1 is moved to its + * original position. + * + * Ex: + * <PRE> + * dReal offset = 1; + * dVector3 dir; + * dJointGetPUAxis3(jId, dir); + * dJointSetPUAnchor(jId, 0, 0, 0); + * // If you request the position you will have: dJointGetPUPosition(jId) == 0 + * dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset); + * // If you request the position you will have: dJointGetPUPosition(jId) == offset + * </PRE> + + * @param j The PU joint for which the anchor point will be set + * @param x The X position of the anchor point in world frame + * @param y The Y position of the anchor point in world frame + * @param z The Z position of the anchor point in world frame + * @param dx A delta to be added to the X position as if the anchor was set + * when body1 was at current_position[X] + dx + * @param dx A delta to be added to the Y position as if the anchor was set + * when body1 was at current_position[Y] + dy + * @param dx A delta to be added to the Z position as if the anchor was set + * when body1 was at current_position[Z] + dz + * @note Should have the same meaning as dJointSetSliderAxisDelta + */ +void dJointSetPUAnchorOffset( dJointID j, dReal x, dReal y, dReal z, + dReal dx, dReal dy, dReal dz ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if (joint->flags & dJOINT_REVERSE) + { + dx = -dx; + dy = -dy; + dz = -dz; + } + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] -= dx; + joint->node[0].body->posr.pos[1] -= dy; + joint->node[0].body->posr.pos[2] -= dz; + } + + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] += dx; + joint->node[0].body->posr.pos[1] += dy; + joint->node[0].body->posr.pos[2] += dz; + } + + joint->computeInitialRelativeRotations(); +} + + + + + +void dJointSetPUAxis1( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + setAxes( joint, x, y, z, NULL, joint->axis2 ); + else + setAxes( joint, x, y, z, joint->axis1, NULL ); + joint->computeInitialRelativeRotations(); +} + +void dJointSetPUAxis2( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + setAxes( joint, x, y, z, joint->axis1, NULL ); + else + setAxes( joint, x, y, z, NULL, joint->axis2 ); + joint->computeInitialRelativeRotations(); +} + + +void dJointSetPUAxisP( dJointID id, dReal x, dReal y, dReal z ) +{ + dJointSetPUAxis3( id, x, y, z ); +} + + + +void dJointSetPUAxis3( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + setAxes( joint, x, y, z, joint->axisP1, 0 ); + + joint->computeInitialRelativeRotations(); +} + + + + +void dJointGetPUAngles( dJointID j, dReal *angle1, dReal *angle2 ) +{ + dxJointUniversal* joint = ( dxJointUniversal* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + joint->getAngles( angle2, angle1 ); + else + joint->getAngles( angle1, angle2 ); +} + + +dReal dJointGetPUAngle1( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + return joint->getAngle2(); + else + return joint->getAngle1(); +} + + +dReal dJointGetPUAngle2( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + return joint->getAngle1(); + else + return joint->getAngle2(); +} + + +dReal dJointGetPUAngle1Rate( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if ( joint->node[0].body ) + { + dVector3 axis; + + if ( joint->flags & dJOINT_REVERSE ) + getAxis2( joint, axis, joint->axis2 ); + else + getAxis( joint, axis, joint->axis1 ); + + dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + return 0; +} + + +dReal dJointGetPUAngle2Rate( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if ( joint->node[0].body ) + { + dVector3 axis; + + if ( joint->flags & dJOINT_REVERSE ) + getAxis( joint, axis, joint->axis1 ); + else + getAxis2( joint, axis, joint->axis2 ); + + dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + return 0; +} + + +void dJointSetPUParam( dJointID j, int parameter, dReal value ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + switch ( parameter & 0xff00 ) + { + case dParamGroup1: + joint->limot1.set( parameter, value ); + break; + case dParamGroup2: + joint->limot2.set( parameter & 0xff, value ); + break; + case dParamGroup3: + joint->limotP.set( parameter & 0xff, value ); + break; + } +} + +void dJointGetPUAnchor( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + + if ( joint->node[1].body ) + getAnchor2( joint, result, joint->anchor2 ); + else + { + // result[i] = joint->anchor2[i]; + dCopyVector3( result, joint->anchor2 ); + } +} + +void dJointGetPUAxis1( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + getAxis2( joint, result, joint->axis2 ); + else + getAxis( joint, result, joint->axis1 ); +} + +void dJointGetPUAxis2( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + getAxis( joint, result, joint->axis1 ); + else + getAxis2( joint, result, joint->axis2 ); +} + +/** + * @brief Get the prismatic axis + * @ingroup joints + * + * @note This function was added for convenience it is the same as + * dJointGetPUAxis3 + */ +void dJointGetPUAxisP( dJointID id, dVector3 result ) +{ + dJointGetPUAxis3( id, result ); +} + + +void dJointGetPUAxis3( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + getAxis( joint, result, joint->axisP1 ); +} + +dReal dJointGetPUParam( dJointID j, int parameter ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + switch ( parameter & 0xff00 ) + { + case dParamGroup1: + return joint->limot1.get( parameter ); + break; + case dParamGroup2: + return joint->limot2.get( parameter & 0xff ); + break; + case dParamGroup3: + return joint->limotP.get( parameter & 0xff ); + break; + } + + return 0; +} + + +dJointType +dxJointPU::type() const +{ + return dJointTypePU; +} + + +sizeint +dxJointPU::size() const +{ + return sizeof( *this ); +} + + +void +dxJointPU::setRelativeValues() +{ + dVector3 anchor; + dJointGetPUAnchor(this, anchor); + setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 ); + + dVector3 ax1, ax2, ax3; + dJointGetPUAxis1(this, ax1); + dJointGetPUAxis2(this, ax2); + dJointGetPUAxis3(this, ax3); + + if ( flags & dJOINT_REVERSE ) + { + setAxes( this, ax1[0], ax1[1], ax1[2], NULL, axis2 ); + setAxes( this, ax2[0], ax2[1], ax2[2], axis1, NULL ); + } + else + { + setAxes( this, ax1[0], ax1[1], ax1[2], axis1, NULL ); + setAxes( this, ax2[0], ax2[1], ax2[2], NULL, axis2 ); + } + + + setAxes( this, ax3[0], ax3[1], ax3[2], axisP1, NULL ); + + computeInitialRelativeRotations(); +} + diff --git a/libs/ode-0.16.1/ode/src/joints/pu.h b/libs/ode-0.16.1/ode/src/joints/pu.h new file mode 100644 index 0000000..34f7392 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/pu.h @@ -0,0 +1,88 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_PU_H_ +#define _ODE_JOINT_PU_H_ + +#include "universal.h" + + + +/** + * Component of a Prismatic -- Universal joint. + * The axisP must be perpendicular to axis1. + * The second axis of the universal joint is perpendicular to axis1. + * + * Since the PU joint is derived from the Universal joint. Some variable + * are reused. + * + * anchor1: Vector from body1 to the anchor point + * This vector is calculated when the body are attached or + * when the anchor point is set. It is like the offset of the Slider + * joint. Since their is a prismatic between the anchor and the body1 + * the distance might change as the simulation goes on. + * anchor2: Vector from body2 to the anchor point. + * <PRE> + * Body 2 + * +-------------+ + * | x | + * +------------\+ + * Prismatic articulation .. .. + * | .. .. + * Body 1 v .. .. + * +--------------+ --| __.. .. anchor2 + * <--------| x | .....|.......(__) .. + * axisP +--------------+ --| ^ < + * |----------------------->| + * anchor1 |--- Universal articulation + * axis1 going out of the plane + * axis2 is perpendicular to axis1 + * (i.e. 2 rotoides) + * </PRE> + */ +struct dxJointPU : public dxJointUniversal +{ + + /// @brief Axis for the prismatic articulation w.r.t first body. + /// @note This is considered as axis2 from the parameter view + dVector3 axisP1; + + dxJointLimitMotor limotP; ///< limit and motor information for the prismatic articulation. + + + dxJointPU( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + + virtual void setRelativeValues(); +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/slider.cpp b/libs/ode-0.16.1/ode/src/joints/slider.cpp new file mode 100644 index 0000000..2c9b008 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/slider.cpp @@ -0,0 +1,423 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "slider.h" +#include "joint_internal.h" + + + +//**************************************************************************** +// slider + +dxJointSlider::dxJointSlider ( dxWorld *w ) : + dxJoint ( w ) +{ + dSetZero ( axis1, 4 ); + axis1[0] = 1; + dSetZero ( qrel, 4 ); + dSetZero ( offset, 4 ); + limot.init ( world ); +} + + +dReal dJointGetSliderPosition ( dJointID j ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Slider ); + + // get axis1 in global coordinates + dVector3 ax1, q; + dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 ); + + if ( joint->node[1].body ) + { + // get body2 + offset point in global coordinates + dMultiply0_331 ( q, joint->node[1].body->posr.R, joint->offset ); + for ( int i = 0; i < 3; i++ ) + q[i] = joint->node[0].body->posr.pos[i] + - q[i] + - joint->node[1].body->posr.pos[i]; + } + else + { + q[0] = joint->node[0].body->posr.pos[0] - joint->offset[0]; + q[1] = joint->node[0].body->posr.pos[1] - joint->offset[1]; + q[2] = joint->node[0].body->posr.pos[2] - joint->offset[2]; + + if ( joint->flags & dJOINT_REVERSE ) + { + // N.B. it could have been simplier to only inverse the sign of + // the dCalcVectorDot3 result but this case is exceptional and doing + // the check for all case can decrease the performance. + ax1[0] = -ax1[0]; + ax1[1] = -ax1[1]; + ax1[2] = -ax1[2]; + } + } + + return dCalcVectorDot3 ( ax1, q ); +} + + +dReal dJointGetSliderPositionRate ( dJointID j ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Slider ); + + // get axis1 in global coordinates + dVector3 ax1; + dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 ); + + if ( joint->node[1].body ) + { + return dCalcVectorDot3 ( ax1, joint->node[0].body->lvel ) - + dCalcVectorDot3 ( ax1, joint->node[1].body->lvel ); + } + else + { + dReal rate = dCalcVectorDot3 ( ax1, joint->node[0].body->lvel ); + if ( joint->flags & dJOINT_REVERSE ) rate = - rate; + return rate; + } +} + + +void +dxJointSlider::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + +void +dxJointSlider::getInfo1 ( dxJoint::Info1 *info ) +{ + info->nub = 5; + + // see if joint is powered + if ( limot.fmax > 0 ) + info->m = 6; // powered slider needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + limot.limit = 0; + if ( ( limot.lostop > -dInfinity || limot.histop < dInfinity ) && + limot.lostop <= limot.histop ) + { + // measure joint position + dReal pos = dJointGetSliderPosition ( this ); + if ( pos <= limot.lostop ) + { + limot.limit = 1; + limot.limit_err = pos - limot.lostop; + info->m = 6; + } + else if ( pos >= limot.histop ) + { + limot.limit = 2; + limot.limit_err = pos - limot.histop; + info->m = 6; + } + } +} + + +void +dxJointSlider::getInfo2 ( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + // 3 rows to make body rotations equal + setFixedOrientation ( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, qrel ); + + // pull out pos and R for both bodies. also get the `connection' + // vector pos2-pos1. + dVector3 c; + dReal *pos2 = NULL, *R2 = NULL; + + dReal *pos1 = node[0].body->posr.pos; + dReal *R1 = node[0].body->posr.R; + + dVector3 ax1; // joint axis in global coordinates (unit length) + dVector3 p, q; // plane space of ax1 + dMultiply0_331 ( ax1, R1, axis1 ); + dPlaneSpace ( ax1, p, q ); + + dxBody *body1 = node[1].body; + + if ( body1 ) + { + R2 = body1->posr.R; + pos2 = body1->posr.pos; + dSubtractVectors3( c, pos2, pos1 ); + } + + // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. + int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip; + { + dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, p ); + + if ( body1 ) + { + dVector3 tmp; + + dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, p); + + dCalcVectorCross3( tmp, c, p ); + dCopyScaledVector3( J1 + currRowSkip + GI2__JA_MIN, tmp, REAL(0.5) ); + dCopyVector3( J2 + currRowSkip + GI2__JA_MIN, J1 + currRowSkip + GI2__JA_MIN ); + } + } + + currRowSkip += rowskip; + { + dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, q ); + + if ( body1 ) + { + dVector3 tmp; + + dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, q); + + dCalcVectorCross3( tmp, c, q ); + dCopyScaledVector3( J1 + currRowSkip + GI2__JA_MIN, tmp, REAL(0.5) ); + dCopyVector3( J2 + currRowSkip + GI2__JA_MIN, J1 + currRowSkip + GI2__JA_MIN ); + } + } + + // compute last two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + dReal k = worldFPS * worldERP; + + if ( body1 ) + { + dVector3 ofs; // offset point in global coordinates + dMultiply0_331 ( ofs, R2, offset ); + dAddVectors3(c, c, ofs); + + pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( p, c ); + + currPairSkip += pairskip; + pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( q, c ); + } + else + { + dVector3 ofs; // offset point in global coordinates + dSubtractVectors3(ofs, offset, pos1); + + pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( p, ofs ); + + currPairSkip += pairskip; + pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( q, ofs ); + + if ( (flags & dJOINT_REVERSE) != 0 ) + { + dNegateVector3(ax1); + } + } + + // if the slider is powered, or has joint limits, add in the extra row + currRowSkip += rowskip; currPairSkip += pairskip; + limot.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 0 ); +} + + +void dJointSetSliderAxis ( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Slider ); + setAxes ( joint, x, y, z, joint->axis1, 0 ); + + joint->computeOffset(); + + joint->computeInitialRelativeRotation(); +} + + +void dJointSetSliderAxisDelta ( dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Slider ); + setAxes ( joint, x, y, z, joint->axis1, 0 ); + + joint->computeOffset(); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if ( !(joint->node[1].body) ) + { + joint->offset[0] += dx; + joint->offset[1] += dy; + joint->offset[2] += dz; + } + + joint->computeInitialRelativeRotation(); +} + + + +void dJointGetSliderAxis ( dJointID j, dVector3 result ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dUASSERT ( joint, "bad joint argument" ); + dUASSERT ( result, "bad result argument" ); + checktype ( joint, Slider ); + getAxis ( joint, result, joint->axis1 ); +} + + +void dJointSetSliderParam ( dJointID j, int parameter, dReal value ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Slider ); + joint->limot.set ( parameter, value ); +} + + +dReal dJointGetSliderParam ( dJointID j, int parameter ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Slider ); + return joint->limot.get ( parameter ); +} + + +void dJointAddSliderForce ( dJointID j, dReal force ) +{ + dxJointSlider* joint = ( dxJointSlider* ) j; + dVector3 axis; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Slider ); + + if ( joint->flags & dJOINT_REVERSE ) + force = -force; + + getAxis ( joint, axis, joint->axis1 ); + axis[0] *= force; + axis[1] *= force; + axis[2] *= force; + + if ( joint->node[0].body != 0 ) + dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] ); + if ( joint->node[1].body != 0 ) + dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] ); + + if ( joint->node[0].body != 0 && joint->node[1].body != 0 ) + { + // linear torque decoupling: + // we have to compensate the torque, that this slider force may generate + // if body centers are not aligned along the slider axis + + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + + dVector3 c; + c[0] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] ); + c[1] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] ); + c[2] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] ); + dCalcVectorCross3( ltd, c, axis ); + + dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] ); + dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] ); + } +} + + +dJointType +dxJointSlider::type() const +{ + return dJointTypeSlider; +} + + +sizeint +dxJointSlider::size() const +{ + return sizeof ( *this ); +} + + +void +dxJointSlider::setRelativeValues() +{ + computeOffset(); + computeInitialRelativeRotation(); +} + + + +/// Compute initial relative rotation body1 -> body2, or env -> body1 +void +dxJointSlider::computeInitialRelativeRotation() +{ + if ( node[0].body ) + { + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if ( node[1].body ) + { + dQMultiply1 ( qrel, node[0].body->q, node[1].body->q ); + } + else + { + // set qrel to the transpose of the first body's q + qrel[0] = node[0].body->q[0]; + qrel[1] = -node[0].body->q[1]; + qrel[2] = -node[0].body->q[2]; + qrel[3] = -node[0].body->q[3]; + } + } +} + + +/// Compute center of body1 w.r.t body 2 +void +dxJointSlider::computeOffset() +{ + if ( node[1].body ) + { + dVector3 c; + c[0] = node[0].body->posr.pos[0] - node[1].body->posr.pos[0]; + c[1] = node[0].body->posr.pos[1] - node[1].body->posr.pos[1]; + c[2] = node[0].body->posr.pos[2] - node[1].body->posr.pos[2]; + + dMultiply1_331 ( offset, node[1].body->posr.R, c ); + } + else if ( node[0].body ) + { + offset[0] = node[0].body->posr.pos[0]; + offset[1] = node[0].body->posr.pos[1]; + offset[2] = node[0].body->posr.pos[2]; + } +} diff --git a/libs/ode-0.16.1/ode/src/joints/slider.h b/libs/ode-0.16.1/ode/src/joints/slider.h new file mode 100644 index 0000000..de6201e --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/slider.h @@ -0,0 +1,59 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_SLIDER_H_ +#define _ODE_JOINT_SLIDER_H_ + +#include "joint.h" + + +// slider. if body2 is 0 then qrel is the absolute rotation of body1 and +// offset is the position of body1 center along axis1. + +struct dxJointSlider : public dxJoint +{ + dVector3 axis1; // axis w.r.t first body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // point relative to body2 that should be + // aligned with body1 center along axis1 + dxJointLimitMotor limot; // limit and motor information + + dxJointSlider ( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1 ( Info1* info ); + virtual void getInfo2 ( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; + + virtual void setRelativeValues(); + + void computeInitialRelativeRotation(); + + void computeOffset(); +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/joints/transmission.cpp b/libs/ode-0.16.1/ode/src/joints/transmission.cpp new file mode 100644 index 0000000..825b9e2 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/transmission.cpp @@ -0,0 +1,698 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "transmission.h"
+#include "joint_internal.h"
+
+namespace {
+ static inline dReal clamp(dReal x, dReal minX, dReal maxX)
+ {
+ return x < minX ? minX : (x > maxX ? maxX : x);
+ }
+}
+
+/*
+ * Transmission joint
+ */
+
+dxJointTransmission::dxJointTransmission(dxWorld* w) :
+ dxJoint(w)
+{
+ int i;
+
+ flags |= dJOINT_TWOBODIES;
+ mode = dTransmissionParallelAxes;
+
+ cfm = world->global_cfm;
+ erp = world->global_erp;
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dSetZero( anchors[i], 4 );
+ dSetZero( axes[i], 4 );
+ axes[i][0] = 1;
+
+ radii[i] = 0;
+ }
+
+ backlash = 0;
+ ratio = 1;
+ update = 1;
+}
+
+void
+dxJointTransmission::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = 1;
+}
+
+void
+dxJointTransmission::getInfo1( dxJoint::Info1* info )
+{
+ // If there's backlash in the gears then constraint must be
+ // unilateral, that is the driving gear can only push the driven
+ // gear in one direction. In order to push it in the other it
+ // first needs to traverse the backlash gap.
+
+ info->m = 1;
+ info->nub = backlash > 0 ? 0 : 1;
+}
+
+void
+dxJointTransmission::getInfo2( dReal worldFPS, dReal /*worldERP*/,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+ {
+ dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v;
+ dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m;
+ const dReal *p[2], *omega[2];
+ int i;
+
+ // Transform all needed quantities to the global frame.
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dBodyGetRelPointPos(node[i].body,
+ anchors[i][0], anchors[i][1], anchors[i][2],
+ a[i]);
+
+ dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2],
+ n[i]);
+
+ p[i] = dBodyGetPosition(node[i].body);
+ omega[i] = dBodyGetAngularVel(node[i].body);
+ }
+
+ if (update) {
+ // Make sure both gear reference frames end up with the same
+ // handedness.
+
+ if (dCalcVectorDot3(n[0], n[1]) < 0) {
+ dNegateVector3(axes[0]);
+ dNegateVector3(n[0]);
+ }
+ }
+
+ // Calculate the mesh geometry based on the current mode.
+
+ switch (mode) {
+ case dTransmissionParallelAxes:
+ // Simply calculate the contact point as the point on the
+ // baseline that will yield the correct ratio.
+
+ dIASSERT (ratio > 0);
+
+ dSubtractVectors3(d, a[1], a[0]);
+ dAddVectorScaledVector3(c[0], a[0], d, ratio / (1 + ratio));
+ dCopyVector3(c[1], c[0]);
+
+ dNormalize3(d);
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dCalcVectorCross3(l[i], d, n[i]);
+ }
+
+ break;
+ case dTransmissionIntersectingAxes:
+ // Calculate the line of intersection between the planes of the
+ // gears.
+
+ dCalcVectorCross3(l[0], n[0], n[1]);
+ dCopyVector3(l[1], l[0]);
+
+ nn = dCalcVectorDot3(n[0], n[1]);
+ dIASSERT(fabs(nn) != 1);
+
+ na_0 = dCalcVectorDot3(n[0], a[0]);
+ na_1 = dCalcVectorDot3(n[1], a[1]);
+
+ dAddScaledVectors3(O, n[0], n[1],
+ (na_0 - na_1 * nn) / (1 - nn * nn),
+ (na_1 - na_0 * nn) / (1 - nn * nn));
+
+ // Find the contact point as:
+ //
+ // c = ((r_a - O) . l) l + O
+ //
+ // where r_a the anchor point of either gear and l, O the tangent
+ // line direction and origin.
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dSubtractVectors3(d, a[i], O);
+ m = dCalcVectorDot3(d, l[i]);
+ dAddVectorScaledVector3(c[i], O, l[i], m);
+ }
+
+ break;
+ case dTransmissionChainDrive:
+ dSubtractVectors3(d, a[0], a[1]);
+ m = dCalcVectorLength3(d);
+
+ dIASSERT(m > 0);
+
+ // Caclulate the angle of the contact point relative to the
+ // baseline.
+
+ cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors
+ sinphi = dSqrt (REAL(1.0) - cosphi * cosphi);
+
+ dNormalize3(d);
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ // Calculate the contact radius in the local reference
+ // frame of the chain. This has axis x pointing along the
+ // baseline, axis y pointing along the sprocket axis and
+ // the remaining axis normal to both.
+
+ u[0] = radii[i] * cosphi;
+ u[1] = 0;
+ u[2] = radii[i] * sinphi;
+
+ // Transform the contact radius into the global frame.
+
+ dCalcVectorCross3(z, d, n[i]);
+
+ v[0] = dCalcVectorDot3(d, u);
+ v[1] = dCalcVectorDot3(n[i], u);
+ v[2] = dCalcVectorDot3(z, u);
+
+ // Finally calculate contact points and l.
+
+ dAddVectors3(c[i], a[i], v);
+ dCalcVectorCross3(l[i], v, n[i]);
+ dNormalize3(l[i]);
+
+ // printf ("%d: %f, %f, %f\n",
+ // i, l[i][0], l[i][1], l[i][2]);
+ }
+
+ break;
+ }
+
+ if (update) {
+ // We need to calculate an initial reference frame for each
+ // wheel which we can measure the current phase against. This
+ // frame will have the initial contact radius as the x axis,
+ // the wheel axis as the z axis and their cross product as the
+ // y axis.
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dSubtractVectors3 (r[i], c[i], a[i]);
+ radii[i] = dCalcVectorLength3(r[i]);
+ dIASSERT(radii[i] > 0);
+
+ dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2],
+ reference[i]);
+ dNormalize3(reference[i]);
+ dCopyVector3(reference[i] + 8, axes[i]);
+ dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]);
+
+ // printf ("%f\n", dDOT(r[i], n[i]));
+ // printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n",
+ // reference[i][0],reference[i][1],reference[i][2],
+ // reference[i][4],reference[i][5],reference[i][6],
+ // reference[i][8],reference[i][9],reference[i][10]);
+
+ phase[i] = 0;
+ }
+
+ ratio = radii[0] / radii[1];
+ update = 0;
+ }
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dReal phase_hat;
+
+ dSubtractVectors3 (r[i], c[i], a[i]);
+
+ // Transform the (global) contact radius into the gear's
+ // reference frame.
+
+ dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s);
+ dMultiply0_331(t, reference[i], s);
+
+ // Now simply calculate its angle on the plane relative to the
+ // x-axis which is the initial contact radius. This will be
+ // an angle between -pi and pi that is coterminal with the
+ // actual phase of the wheel. To find the real phase we
+ // estimate it by adding omega * dt to the old phase and then
+ // find the closest angle to that, that is coterminal to
+ // theta.
+
+ theta = atan2(t[1], t[0]);
+ phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS;
+
+ if (phase_hat > M_PI_2) {
+ if (theta < 0) {
+ theta += (dReal)(2 * M_PI);
+ }
+
+ theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI));
+ } else if (phase_hat < -M_PI_2) {
+ if (theta > 0) {
+ theta -= (dReal)(2 * M_PI);
+ }
+
+ theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI));
+ }
+
+ if (phase_hat - theta > M_PI) {
+ phase[i] = theta + (dReal)(2 * M_PI);
+ } else if (phase_hat - theta < -M_PI) {
+ phase[i] = theta - (dReal)(2 * M_PI);
+ } else {
+ phase[i] = theta;
+ }
+
+ dIASSERT(fabs(phase_hat - phase[i]) < M_PI);
+ }
+
+ // Calculate the phase error. Depending on the mode the condition
+ // is that the distances traveled by each contact point must be
+ // either equal (chain and sprockets) or opposite (gears).
+
+ if (mode == dTransmissionChainDrive) {
+ delta = (dCalcVectorLength3(r[0]) * phase[0] -
+ dCalcVectorLength3(r[1]) * phase[1]);
+ } else {
+ delta = (dCalcVectorLength3(r[0]) * phase[0] +
+ dCalcVectorLength3(r[1]) * phase[1]);
+ }
+
+ // When in chain mode a torque reversal, signified by the change
+ // in sign of the wheel phase difference, has the added effect of
+ // switching the active chain branch. We must therefore reflect
+ // the contact points and tangents across the baseline.
+
+ if (mode == dTransmissionChainDrive && delta < 0) {
+ dVector3 d;
+
+ dSubtractVectors3(d, a[0], a[1]);
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dVector3 nn;
+ dReal a;
+
+ dCalcVectorCross3(nn, n[i], d);
+ a = dCalcVectorDot3(nn, nn);
+ dIASSERT(a > 0);
+
+ dAddScaledVectors3(c[i], c[i], nn,
+ 1, -2 * dCalcVectorDot3(c[i], nn) / a);
+ dAddScaledVectors3(l[i], l[i], nn,
+ -1, 2 * dCalcVectorDot3(l[i], nn) / a);
+ }
+ }
+
+ // Do not add the constraint if there's backlash and we're in the
+ // backlash gap.
+
+ if (backlash == 0 || fabs(delta) > backlash) {
+ // The constraint is satisfied if the absolute velocity of the
+ // contact point projected onto the tangent of the wheels is equal
+ // for both gears. This velocity can be calculated as:
+ //
+ // u = v + omega x r_c
+ //
+ // The constraint therefore becomes:
+ // (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=>
+ // (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ dSubtractVectors3 (r[i], c[i], p[i]);
+ }
+
+ dCopyVector3(J1 + GI2__JL_MIN, l[0]);
+ dCalcVectorCross3(J1 + GI2__JA_MIN, r[0], l[0]);
+
+ dCopyNegatedVector3(J2 + GI2__JL_MIN, l[1]);
+ dCalcVectorCross3(J2 + GI2__JA_MIN, l[1], r[1]);
+
+ if (delta > 0) {
+ if (backlash > 0) {
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = 0;
+ }
+
+ pairRhsCfm[GI2_RHS] = -worldFPS * erp * (delta - backlash);
+ } else {
+ if (backlash > 0) {
+ pairLoHi[GI2_LO] = 0;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+
+ pairRhsCfm[GI2_RHS] = -worldFPS * erp * (delta + backlash);
+ }
+ }
+
+ pairRhsCfm[GI2_CFM] = cfm;
+
+ // printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio);
+
+ // Cache the contact point (in world coordinates) to avoid
+ // recalculation if requested by the user.
+
+ dCopyVector3(contacts[0], c[0]);
+ dCopyVector3(contacts[1], c[1]);
+}
+
+void dJointSetTransmissionAxis( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ int i;
+
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT(joint->mode == dTransmissionParallelAxes ||
+ joint->mode == dTransmissionChainDrive ,
+ "axes must be set individualy in current mode" );
+
+ for (i = 0 ; i < 2 ; i += 1) {
+ if (joint->node[i].body) {
+ dBodyVectorFromWorld(joint->node[i].body, x, y, z, joint->axes[i]);
+ dNormalize3(joint->axes[i]);
+ }
+ }
+
+ joint->update = 1;
+}
+
+void dJointSetTransmissionAxis1( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT(joint->mode == dTransmissionIntersectingAxes,
+ "can't set individual axes in current mode" );
+
+ if (joint->node[0].body) {
+ dBodyVectorFromWorld(joint->node[0].body, x, y, z, joint->axes[0]);
+ dNormalize3(joint->axes[0]);
+ }
+
+ joint->update = 1;
+}
+
+void dJointSetTransmissionAxis2( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT(joint->mode == dTransmissionIntersectingAxes,
+ "can't set individual axes in current mode" );
+
+ if (joint->node[1].body) {
+ dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]);
+ dNormalize3(joint->axes[1]);
+ }
+
+ joint->update = 1;
+}
+
+void dJointSetTransmissionAnchor1( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ if (joint->node[0].body) {
+ dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchors[0]);
+ }
+
+ joint->update = 1;
+}
+
+void dJointSetTransmissionAnchor2( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ if (joint->node[1].body) {
+ dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchors[1]);
+ }
+
+ joint->update = 1;
+}
+
+void dJointGetTransmissionContactPoint1( dJointID j, dVector3 result )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+
+ dCopyVector3(result, joint->contacts[0]);
+}
+
+void dJointGetTransmissionContactPoint2( dJointID j, dVector3 result )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+
+ dCopyVector3(result, joint->contacts[1]);
+}
+
+void dJointGetTransmissionAxis( dJointID j, dVector3 result )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+ dUASSERT(joint->mode == dTransmissionParallelAxes,
+ "axes must be queried individualy in current mode" );
+
+ if (joint->node[0].body) {
+ dBodyVectorToWorld(joint->node[0].body,
+ joint->axes[0][0],
+ joint->axes[0][1],
+ joint->axes[0][2],
+ result);
+ }
+}
+
+void dJointGetTransmissionAxis1( dJointID j, dVector3 result )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+
+ if (joint->node[0].body) {
+ dBodyVectorToWorld(joint->node[0].body,
+ joint->axes[0][0],
+ joint->axes[0][1],
+ joint->axes[0][2],
+ result);
+ }
+}
+
+void dJointGetTransmissionAxis2( dJointID j, dVector3 result )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+
+ if (joint->node[1].body) {
+ dBodyVectorToWorld(joint->node[1].body,
+ joint->axes[1][0],
+ joint->axes[1][1],
+ joint->axes[1][2],
+ result);
+ }
+}
+
+void dJointGetTransmissionAnchor1( dJointID j, dVector3 result )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+
+ if (joint->node[0].body) {
+ dBodyGetRelPointPos(joint->node[0].body,
+ joint->anchors[0][0],
+ joint->anchors[0][1],
+ joint->anchors[0][2],
+ result);
+ }
+}
+
+void dJointGetTransmissionAnchor2( dJointID j, dVector3 result )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( result, "bad result argument" );
+
+ if (joint->node[1].body) {
+ dBodyGetRelPointPos(joint->node[1].body,
+ joint->anchors[1][0],
+ joint->anchors[1][1],
+ joint->anchors[1][2],
+ result);
+ }
+}
+
+void dJointSetTransmissionParam( dJointID j, int parameter, dReal value )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ switch ( parameter ) {
+ case dParamCFM:
+ joint->cfm = value;
+ break;
+ case dParamERP:
+ joint->erp = value;
+ break;
+ }
+}
+
+
+dReal dJointGetTransmissionParam( dJointID j, int parameter )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ switch ( parameter ) {
+ case dParamCFM:
+ return joint->cfm;
+ case dParamERP:
+ return joint->erp;
+ default:
+ return 0;
+ }
+}
+
+void dJointSetTransmissionMode( dJointID j, int mode )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( mode == dTransmissionParallelAxes ||
+ mode == dTransmissionIntersectingAxes ||
+ mode == dTransmissionChainDrive, "invalid joint mode" );
+
+ joint->mode = mode;
+}
+
+
+int dJointGetTransmissionMode( dJointID j )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ return joint->mode;
+}
+
+void dJointSetTransmissionRatio( dJointID j, dReal ratio )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( joint->mode == dTransmissionParallelAxes,
+ "can't set ratio explicitly in current mode" );
+ dUASSERT( ratio > 0, "ratio must be positive" );
+
+ joint->ratio = ratio;
+}
+
+
+dReal dJointGetTransmissionRatio( dJointID j )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ return joint->ratio;
+}
+
+dReal dJointGetTransmissionAngle1( dJointID j )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ return joint->phase[0];
+}
+
+dReal dJointGetTransmissionAngle2( dJointID j )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ return joint->phase[1];
+}
+
+dReal dJointGetTransmissionRadius1( dJointID j )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ return joint->radii[0];
+}
+
+dReal dJointGetTransmissionRadius2( dJointID j )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ return joint->radii[1];
+}
+
+void dJointSetTransmissionRadius1( dJointID j, dReal radius )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( joint->mode == dTransmissionChainDrive,
+ "can't set wheel radius explicitly in current mode" );
+
+ joint->radii[0] = radius;
+}
+
+void dJointSetTransmissionRadius2( dJointID j, dReal radius )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+ dUASSERT( joint->mode == dTransmissionChainDrive,
+ "can't set wheel radius explicitly in current mode" );
+
+ joint->radii[1] = radius;
+}
+
+dReal dJointGetTransmissionBacklash( dJointID j )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ return joint->backlash;
+}
+
+void dJointSetTransmissionBacklash( dJointID j, dReal backlash )
+{
+ dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
+ dUASSERT( joint, "bad joint argument" );
+
+ joint->backlash = backlash;
+}
+
+dJointType
+dxJointTransmission::type() const
+{
+ return dJointTypeTransmission;
+}
+
+sizeint
+dxJointTransmission::size() const
+{
+ return sizeof( *this );
+}
diff --git a/libs/ode-0.16.1/ode/src/joints/transmission.h b/libs/ode-0.16.1/ode/src/joints/transmission.h new file mode 100644 index 0000000..fae3f4c --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/transmission.h @@ -0,0 +1,51 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_TRANSMISSION_ +#define _ODE_JOINT_TRANSMISSION_ + +#include "joint.h" + +struct dxJointTransmission : public dxJoint +{ + int mode, update; + dVector3 contacts[2], axes[2], anchors[2]; + dMatrix3 reference[2]; + dReal phase[2], radii[2], backlash; + dReal ratio; // transmission ratio + dReal erp; // error reduction + dReal cfm; // constraint force mix in + + dxJointTransmission(dxWorld *w); + + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ); + virtual dJointType type() const; + virtual sizeint size() const; +}; + + +#endif diff --git a/libs/ode-0.16.1/ode/src/joints/universal.cpp b/libs/ode-0.16.1/ode/src/joints/universal.cpp new file mode 100644 index 0000000..1ef00a7 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/universal.cpp @@ -0,0 +1,803 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#include <ode/odeconfig.h> +#include "config.h" +#include "universal.h" +#include "joint_internal.h" + + + +//**************************************************************************** +// universal + +// I just realized that the universal joint is equivalent to a hinge 2 joint with +// perfectly stiff suspension. By comparing the hinge 2 implementation to +// the universal implementation, you may be able to improve this +// implementation (or, less likely, the hinge2 implementation). + +dxJointUniversal::dxJointUniversal( dxWorld *w ) : + dxJoint( w ) +{ + dSetZero( anchor1, 4 ); + dSetZero( anchor2, 4 ); + dSetZero( axis1, 4 ); + axis1[0] = 1; + dSetZero( axis2, 4 ); + axis2[1] = 1; + dSetZero( qrel1, 4 ); + dSetZero( qrel2, 4 ); + limot1.init( world ); + limot2.init( world ); +} + + +void +dxJointUniversal::getAxes( dVector3 ax1, dVector3 ax2 ) +{ + // This says "ax1 = joint->node[0].body->posr.R * joint->axis1" + dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); + + if ( node[1].body ) + { + dMultiply0_331( ax2, node[1].body->posr.R, axis2 ); + } + else + { + ax2[0] = axis2[0]; + ax2[1] = axis2[1]; + ax2[2] = axis2[2]; + } +} + +void +dxJointUniversal::getAngles( dReal *angle1, dReal *angle2 ) +{ + if ( node[0].body ) + { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getAxes( ax1, ax2 ); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] ); + + dRtoQ( R, qcross ); + + + // This code is essentialy the same as getHingeAngle(), see the comments + // there for details. + + // get qrel = relative rotation between node[0] and the cross + dQMultiply1( qq, node[0].body->q, qcross ); + dQMultiply2( qrel, qq, qrel1 ); + + *angle1 = getHingeAngleFromRelativeQuat( qrel, axis1 ); + + // This is equivalent to + // dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + // You see that the R is constructed from the same 2 axis as for angle1 + // but the first and second axis are swapped. + // So we can take the first R and rapply a rotation to it. + // The rotation is around the axis between the 2 axes (ax1 and ax2). + // We do a rotation of 180deg. + + dQuaternion qcross2; + // Find the vector between ax1 and ax2 (i.e. in the middle) + // We need to turn around this vector by 180deg + + // The 2 axes should be normalize so to find the vector between the 2. + // Add and devide by 2 then normalize or simply normalize + // ax2 + // ^ + // | + // | + /// *------------> ax1 + // We want the vector a 45deg + // + // N.B. We don't need to normalize the ax1 and ax2 since there are + // normalized when we set them. + + // We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z] + qrel[0] = 0; // equivalent to cos(Pi/2) + qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1 + qrel[2] = ax1[1] + ax2[1]; + qrel[3] = ax1[2] + ax2[2]; + + dReal l = dRecip( sqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] ) ); + qrel[1] *= l; + qrel[2] *= l; + qrel[3] *= l; + + dQMultiply0( qcross2, qrel, qcross ); + + if ( node[1].body ) + { + dQMultiply1( qq, node[1].body->q, qcross2 ); + dQMultiply2( qrel, qq, qrel2 ); + } + else + { + // pretend joint->node[1].body->q is the identity + dQMultiply2( qrel, qcross2, qrel2 ); + } + + *angle2 = - getHingeAngleFromRelativeQuat( qrel, axis2 ); + } + else + { + *angle1 = 0; + *angle2 = 0; + } +} + +dReal +dxJointUniversal::getAngle1() +{ + if ( node[0].body ) + { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getAxes( ax1, ax2 ); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] ); + dRtoQ( R, qcross ); + + // This code is essential the same as getHingeAngle(), see the comments + // there for details. + + // get qrel = relative rotation between node[0] and the cross + dQMultiply1( qq, node[0].body->q, qcross ); + dQMultiply2( qrel, qq, qrel1 ); + + return getHingeAngleFromRelativeQuat( qrel, axis1 ); + } + return 0; +} + + +dReal +dxJointUniversal::getAngle2() +{ + if ( node[0].body ) + { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getAxes( ax1, ax2 ); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] ); + dRtoQ( R, qcross ); + + if ( node[1].body ) + { + dQMultiply1( qq, node[1].body->q, qcross ); + dQMultiply2( qrel, qq, qrel2 ); + } + else + { + // pretend joint->node[1].body->q is the identity + dQMultiply2( qrel, qcross, qrel2 ); + } + + return - getHingeAngleFromRelativeQuat( qrel, axis2 ); + } + return 0; +} + + +void +dxJointUniversal::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + +void +dxJointUniversal::getInfo1( dxJoint::Info1 *info ) +{ + info->nub = 4; + info->m = 4; + + bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) && + limot1.lostop <= limot1.histop; + bool limiting2 = ( limot2.lostop >= -M_PI || limot2.histop <= M_PI ) && + limot2.lostop <= limot2.histop; + + // We need to call testRotationLimit() even if we're motored, since it + // records the result. + limot1.limit = 0; + limot2.limit = 0; + + if ( limiting1 || limiting2 ) + { + dReal angle1, angle2; + getAngles( &angle1, &angle2 ); + if ( limiting1 ) + limot1.testRotationalLimit( angle1 ); + if ( limiting2 ) + limot2.testRotationalLimit( angle2 ); + } + + if ( limot1.limit || limot1.fmax > 0 ) info->m++; + if ( limot2.limit || limot2.fmax > 0 ) info->m++; +} + + +void +dxJointUniversal::getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + // set the three ball-and-socket rows + setBall( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 ); + + // set the universal joint row. the angular velocity about an axis + // perpendicular to both joint axes should be equal. thus the constraint + // equation is + // p*w1 - p*w2 = 0 + // where p is a vector normal to both joint axes, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate + // about this. + dVector3 p; + + // Since axis1 and axis2 may not be perpendicular + // we find a axis2_tmp which is really perpendicular to axis1 + // and in the plane of axis1 and axis2 + getAxes( ax1, ax2 ); + + dReal k = dCalcVectorDot3( ax1, ax2 ); + + dVector3 ax2_temp; + dAddVectorScaledVector3(ax2_temp, ax2, ax1, -k); + dCalcVectorCross3( p, ax1, ax2_temp ); + dNormalize3( p ); + + int currRowSkip = 3 * rowskip; + { + dCopyVector3( J1 + currRowSkip + GI2__JA_MIN, p); + + if ( node[1].body ) + { + dCopyNegatedVector3( J2 + currRowSkip + GI2__JA_MIN, p); + } + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p to bring the axes back to perpendicular. + // If ax1, ax2 are unit length joint axes as computed from body1 and + // body2, we need to rotate both bodies along the axis p. If theta + // is the angle between ax1 and ax2, we need an angular velocity + // along p to cover the angle erp * (theta - Pi/2) in one step: + // + // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize + // = (erp*fps) * (theta - Pi/2) + // + // if theta is close to Pi/2, + // theta - Pi/2 ~= cos(theta), so + // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) + + int currPairSkip = 3 * pairskip; + { + pairRhsCfm[currPairSkip + GI2_RHS] = worldFPS * worldERP * (-k); + } + + currRowSkip += rowskip; currPairSkip += pairskip; + + // if the first angle is powered, or has joint limits, add in the stuff + if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) + { + currRowSkip += rowskip; currPairSkip += pairskip; + } + + // if the second angle is powered, or has joint limits, add in more stuff + limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 ); +} + + +void +dxJointUniversal::computeInitialRelativeRotations() +{ + if ( node[0].body ) + { + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross; + + getAxes( ax1, ax2 ); + + // Axis 1. + dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] ); + dRtoQ( R, qcross ); + dQMultiply1( qrel1, node[0].body->q, qcross ); + + // Axis 2. + dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] ); + dRtoQ( R, qcross ); + if ( node[1].body ) + { + dQMultiply1( qrel2, node[1].body->q, qcross ); + } + else + { + // set joint->qrel to qcross + for ( int i = 0; i < 4; i++ ) qrel2[i] = qcross[i]; + } + } +} + + +void dJointSetUniversalAnchor( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + joint->computeInitialRelativeRotations(); +} + + +void dJointSetUniversalAxis1( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + setAxes( joint, x, y, z, NULL, joint->axis2 ); + else + setAxes( joint, x, y, z, joint->axis1, NULL ); + joint->computeInitialRelativeRotations(); +} + +void dJointSetUniversalAxis1Offset( dJointID j, dReal x, dReal y, dReal z, + dReal offset1, dReal offset2 ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + { + setAxes( joint, x, y, z, NULL, joint->axis2 ); + offset1 = -offset1; + offset2 = -offset2; + } + else + setAxes( joint, x, y, z, joint->axis1, NULL ); + + joint->computeInitialRelativeRotations(); + + + dVector3 ax2; + getAxis2( joint, ax2, joint->axis2 ); + + { + dVector3 ax1; + joint->getAxes(ax1, ax2); + } + + + + dQuaternion qAngle; + dQFromAxisAndAngle(qAngle, x, y, z, offset1); + + dMatrix3 R; + dRFrom2Axes( R, x, y, z, ax2[0], ax2[1], ax2[2] ); + + dQuaternion qcross; + dRtoQ( R, qcross ); + + dQuaternion qOffset; + dQMultiply0(qOffset, qAngle, qcross); + + dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset ); + + // Calculating the second offset + dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2); + + dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], x, y, z ); + dRtoQ( R, qcross ); + + dQMultiply1(qOffset, qAngle, qcross); + if ( joint->node[1].body ) + { + dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset ); + } + else + { + joint->qrel2[0] = qcross[0]; + joint->qrel2[1] = qcross[1]; + joint->qrel2[2] = qcross[2]; + joint->qrel2[3] = qcross[3]; + } +} + + +void dJointSetUniversalAxis2( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + setAxes( joint, x, y, z, joint->axis1, NULL ); + else + setAxes( joint, x, y, z, NULL, joint->axis2 ); + joint->computeInitialRelativeRotations(); +} + +void dJointSetUniversalAxis2Offset( dJointID j, dReal x, dReal y, dReal z, + dReal offset1, dReal offset2 ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + + if ( joint->flags & dJOINT_REVERSE ) + { + setAxes( joint, x, y, z, joint->axis1, NULL ); + offset1 = -offset2; + offset2 = -offset1; + } + else + setAxes( joint, x, y, z, NULL, joint->axis2 ); + + + joint->computeInitialRelativeRotations(); + + // It is easier to retreive the 2 axes here since + // when there is only one body B2 (the axes switch position) + // Doing this way eliminate the need to write the code differently + // for both case. + dVector3 ax1, ax2; + joint->getAxes(ax1, ax2 ); + + + + dQuaternion qAngle; + dQFromAxisAndAngle(qAngle, ax1[0], ax1[1], ax1[2], offset1); + + dMatrix3 R; + dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + + dQuaternion qcross; + dRtoQ( R, qcross ); + + dQuaternion qOffset; + dQMultiply0(qOffset, qAngle, qcross); + + + + dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset ); + + + // Calculating the second offset + dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2); + + dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ( R, qcross ); + + dQMultiply1(qOffset, qAngle, qcross); + if ( joint->node[1].body ) + { + dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset ); + } + else + { + joint->qrel2[0] = qcross[0]; + joint->qrel2[1] = qcross[1]; + joint->qrel2[2] = qcross[2]; + joint->qrel2[3] = qcross[3]; + } +} + + +void dJointGetUniversalAnchor( dJointID j, dVector3 result ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + getAnchor2( joint, result, joint->anchor2 ); + else + getAnchor( joint, result, joint->anchor1 ); +} + + +void dJointGetUniversalAnchor2( dJointID j, dVector3 result ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + getAnchor( joint, result, joint->anchor1 ); + else + getAnchor2( joint, result, joint->anchor2 ); +} + + +void dJointGetUniversalAxis1( dJointID j, dVector3 result ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + getAxis2( joint, result, joint->axis2 ); + else + getAxis( joint, result, joint->axis1 ); +} + + +void dJointGetUniversalAxis2( dJointID j, dVector3 result ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + getAxis( joint, result, joint->axis1 ); + else + getAxis2( joint, result, joint->axis2 ); +} + + +void dJointSetUniversalParam( dJointID j, int parameter, dReal value ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if (( parameter & 0xff00 ) == 0x100 ) + { + joint->limot2.set( parameter & 0xff, value ); + } + else + { + joint->limot1.set( parameter, value ); + } +} + + +dReal dJointGetUniversalParam( dJointID j, int parameter ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if (( parameter & 0xff00 ) == 0x100 ) + { + return joint->limot2.get( parameter & 0xff ); + } + else + { + return joint->limot1.get( parameter ); + } +} + +void dJointGetUniversalAngles( dJointID j, dReal *angle1, dReal *angle2 ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + { + joint->getAngles( angle2, angle1 ); + *angle2 = -(*angle2); + return; + } + else + return joint->getAngles( angle1, angle2 ); +} + + +dReal dJointGetUniversalAngle1( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + return joint->getAngle2(); + else + return joint->getAngle1(); +} + + +dReal dJointGetUniversalAngle2( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + if ( joint->flags & dJOINT_REVERSE ) + return -joint->getAngle1(); + else + return joint->getAngle2(); +} + + +dReal dJointGetUniversalAngle1Rate( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + + if ( joint->node[0].body ) + { + dVector3 axis; + + if ( joint->flags & dJOINT_REVERSE ) + getAxis2( joint, axis, joint->axis2 ); + else + getAxis( joint, axis, joint->axis1 ); + + dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) + rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + return 0; +} + + +dReal dJointGetUniversalAngle2Rate( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, Universal ); + + if ( joint->node[0].body ) + { + dVector3 axis; + + if ( joint->flags & dJOINT_REVERSE ) + getAxis( joint, axis, joint->axis1 ); + else + getAxis2( joint, axis, joint->axis2 ); + + dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + return 0; +} + + +void dJointAddUniversalTorques( dJointID j, dReal torque1, dReal torque2 ) +{ + dxJointUniversal* joint = ( dxJointUniversal* )j; + dVector3 axis1, axis2; + dAASSERT( joint ); + checktype( joint, Universal ); + + if ( joint->flags & dJOINT_REVERSE ) + { + dReal temp = torque1; + torque1 = - torque2; + torque2 = - temp; + } + + getAxis( joint, axis1, joint->axis1 ); + getAxis2( joint, axis2, joint->axis2 ); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + + if ( joint->node[0].body != 0 ) + dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] ); + if ( joint->node[1].body != 0 ) + dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] ); +} + + +dJointType +dxJointUniversal::type() const +{ + return dJointTypeUniversal; +} + + +sizeint +dxJointUniversal::size() const +{ + return sizeof( *this ); +} + + + +void +dxJointUniversal::setRelativeValues() +{ + dVector3 anchor; + dJointGetUniversalAnchor(this, anchor); + setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 ); + + dVector3 ax1,ax2; + dJointGetUniversalAxis1(this, ax1); + dJointGetUniversalAxis2(this, ax2); + + if ( flags & dJOINT_REVERSE ) + { + setAxes( this, ax1[0],ax1[1],ax1[2], NULL, axis2 ); + setAxes( this, ax2[0],ax2[1],ax2[2], axis1, NULL ); + } + else + { + setAxes( this, ax1[0],ax1[1],ax1[2], axis1, NULL ); + setAxes( this, ax2[0],ax2[1],ax2[2], NULL, axis2 ); + } + + computeInitialRelativeRotations(); +} + diff --git a/libs/ode-0.16.1/ode/src/joints/universal.h b/libs/ode-0.16.1/ode/src/joints/universal.h new file mode 100644 index 0000000..98e5468 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/universal.h @@ -0,0 +1,64 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_UNIVERSAL_H_ +#define _ODE_JOINT_UNIVERSAL_H_ + +#include "joint.h" + +// universal + +struct dxJointUniversal : public dxJoint +{ + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece + dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2 + dxJointLimitMotor limot1; // limit and motor information for axis1 + dxJointLimitMotor limot2; // limit and motor information for axis2 + + + void getAxes( dVector3 ax1, dVector3 ax2 ); + void getAngles( dReal *angle1, dReal *angle2 ); + dReal getAngle1(); + dReal getAngle2(); + void computeInitialRelativeRotations(); + + + dxJointUniversal( dxWorld *w ); + virtual void getSureMaxInfo( SureMaxInfo* info ); + virtual void getInfo1( Info1* info ); + virtual void getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex); + virtual dJointType type() const; + virtual sizeint size() const; + + virtual void setRelativeValues(); +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/lcp.cpp b/libs/ode-0.16.1/ode/src/lcp.cpp new file mode 100644 index 0000000..58db0bd --- /dev/null +++ b/libs/ode-0.16.1/ode/src/lcp.cpp @@ -0,0 +1,1317 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/*
+
+
+THE ALGORITHM
+-------------
+
+solve A*x = b+w, with x and w subject to certain LCP conditions.
+each x(i),w(i) must lie on one of the three line segments in the following
+diagram. each line segment corresponds to one index set :
+
+ w(i)
+ /|\ | :
+ | | :
+ | |i in N :
+ w>0 | |state[i]=0 :
+ | | :
+ | | : i in C
+ w=0 + +-----------------------+
+ | : |
+ | : |
+ w<0 | : |i in N
+ | : |state[i]=1
+ | : |
+ | : |
+ +-------|-----------|-----------|----------> x(i)
+ lo 0 hi
+
+the Dantzig algorithm proceeds as follows:
+ for i=1:n
+ * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or
+ negative towards the line. as this is done, the other (x(j),w(j))
+ for j<i are constrained to be on the line. if any (x,w) reaches the
+ end of a line segment then it is switched between index sets.
+ * i is added to the appropriate index set depending on what line segment
+ it hits.
+
+we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit
+simpler, because the starting point for x(i),w(i) is always on the dotted
+line x=0 and x will only ever increase in one direction, so it can only hit
+two out of the three line segments.
+
+
+NOTES
+-----
+
+this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m".
+the implementation is split into an LCP problem object (dLCP) and an LCP
+driver function. most optimization occurs in the dLCP object.
+
+a naive implementation of the algorithm requires either a lot of data motion
+or a lot of permutation-array lookup, because we are constantly re-ordering
+rows and columns. to avoid this and make a more optimized algorithm, a
+non-trivial data structure is used to represent the matrix A (this is
+implemented in the fast version of the dLCP object).
+
+during execution of this algorithm, some indexes in A are clamped (set C),
+some are non-clamped (set N), and some are "don't care" (where x=0).
+A,x,b,w (and other problem vectors) are permuted such that the clamped
+indexes are first, the unclamped indexes are next, and the don't-care
+indexes are last. this permutation is recorded in the array `p'.
+initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped,
+the corresponding elements of p are swapped.
+
+because the C and N elements are grouped together in the rows of A, we can do
+lots of work with a fast dot product function. if A,x,etc were not permuted
+and we only had a permutation array, then those dot products would be much
+slower as we would have a permutation array lookup in some inner loops.
+
+A is accessed through an array of row pointers, so that element (i,j) of the
+permuted matrix is A[i][j]. this makes row swapping fast. for column swapping
+we still have to actually move the data.
+
+during execution of this algorithm we maintain an L*D*L' factorization of
+the clamped submatrix of A (call it `AC') which is the top left nC*nC
+submatrix of A. there are two ways we could arrange the rows/columns in AC.
+
+(1) AC is always permuted such that L*D*L' = AC. this causes a problem
+when a row/column is removed from C, because then all the rows/columns of A
+between the deleted index and the end of C need to be rotated downward.
+this results in a lot of data motion and slows things down.
+(2) L*D*L' is actually a factorization of a *permutation* of AC (which is
+itself a permutation of the underlying A). this is what we do - the
+permutation is recorded in the vector C. call this permutation A[C,C].
+when a row/column is removed from C, all we have to do is swap two
+rows/columns and manipulate C.
+
+*/
+
+#include <ode/common.h>
+#include <ode/misc.h>
+#include <ode/timer.h> // for testing
+#include "config.h"
+#include "lcp.h"
+#include "util.h"
+#include "matrix.h"
+#include "mat.h" // for testing
+#include "threaded_solver_ldlt.h"
+
+#include "fastdot_impl.h"
+#include "fastldltfactor_impl.h"
+#include "fastldltsolve_impl.h"
+
+
+//***************************************************************************
+// code generation parameters
+
+// LCP debugging (mostly for fast dLCP) - this slows things down a lot
+//#define DEBUG_LCP
+
+#define dLCP_FAST // use fast dLCP object
+
+#define NUB_OPTIMIZATIONS // use NUB optimizations
+
+
+// option 1 : matrix row pointers (less data copying)
+#define ROWPTRS
+#define ATYPE dReal **
+#define AROW(i) (m_A[i])
+
+// option 2 : no matrix row pointers (slightly faster inner loops)
+//#define NOROWPTRS
+//#define ATYPE dReal *
+//#define AROW(i) (m_A+(i)*m_nskip)
+
+
+//***************************************************************************
+
+#define dMIN(A,B) ((A)>(B) ? (B) : (A))
+#define dMAX(A,B) ((B)>(A) ? (B) : (A))
+
+
+#define LMATRIX_ALIGNMENT dMAX(64, EFFICIENT_ALIGNMENT)
+
+//***************************************************************************
+
+
+// transfer b-values to x-values
+template<bool zero_b>
+inline
+void transfer_b_to_x(dReal pairsbx[PBX__MAX], unsigned n)
+{
+ dReal *const endbx = pairsbx + (sizeint)n * PBX__MAX;
+ for (dReal *currbx = pairsbx; currbx != endbx; currbx += PBX__MAX) {
+ currbx[PBX_X] = currbx[PBX_B];
+ if (zero_b) {
+ currbx[PBX_B] = REAL(0.0);
+ }
+ }
+}
+
+// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
+// A is nskip. this only references and swaps the lower triangle.
+// if `do_fast_row_swaps' is nonzero and row pointers are being used, then
+// rows will be swapped by exchanging row pointers. otherwise the data will
+// be copied.
+
+static
+void swapRowsAndCols (ATYPE A, unsigned n, unsigned i1, unsigned i2, unsigned nskip,
+ int do_fast_row_swaps)
+{
+ dAASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
+ nskip >= n && i1 < i2);
+
+# ifdef ROWPTRS
+ dReal *A_i1 = A[i1];
+ dReal *A_i2 = A[i2];
+ for (unsigned i=i1+1; i<i2; ++i) {
+ dReal *A_i_i1 = A[i] + i1;
+ A_i1[i] = *A_i_i1;
+ *A_i_i1 = A_i2[i];
+ }
+ A_i1[i2] = A_i1[i1];
+ A_i1[i1] = A_i2[i1];
+ A_i2[i1] = A_i2[i2];
+ // swap rows, by swapping row pointers
+ if (do_fast_row_swaps) {
+ A[i1] = A_i2;
+ A[i2] = A_i1;
+ }
+ else {
+ // Only swap till i2 column to match A plain storage variant.
+ for (unsigned k = 0; k <= i2; ++k) {
+ dxSwap(A_i1[k], A_i2[k]);
+ }
+ }
+ // swap columns the hard way
+ for (unsigned j = i2 + 1; j < n; ++j) {
+ dReal *A_j = A[j];
+ dxSwap(A_j[i1], A_j[i2]);
+ }
+# else
+ dReal *A_i1 = A + (sizeint)nskip * i1;
+ dReal *A_i2 = A + (sizeint)nskip * i2;
+
+ for (unsigned k = 0; k < i1; ++k) {
+ dxSwap(A_i1[k], A_i2[k]);
+ }
+
+ dReal *A_i = A_i1 + nskip;
+ for (unsigned i= i1 + 1; i < i2; A_i += nskip, ++i) {
+ dxSwap(A_i2[i], A_i[i1]);
+ }
+
+ dxSwap(A_i1[i1], A_i2[i2]);
+
+ dReal *A_j = A_i2 + nskip;
+ for (unsigned j = i2 + 1; j < n; A_j += nskip, ++j) {
+ dxSwap(A_j[i1], A_j[i2]);
+ }
+# endif
+}
+
+
+// swap two indexes in the n*n LCP problem. i1 must be <= i2.
+
+static
+void swapProblem (ATYPE A, dReal pairsbx[PBX__MAX], dReal *w, dReal pairslh[PLH__MAX],
+ unsigned *p, bool *state, int *findex,
+ unsigned n, unsigned i1, unsigned i2, unsigned nskip,
+ int do_fast_row_swaps)
+{
+ dIASSERT (n>0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
+
+ if (i1 != i2) {
+ swapRowsAndCols (A, n, i1, i2, nskip, do_fast_row_swaps);
+
+ dxSwap((pairsbx + (sizeint)i1 * PBX__MAX)[PBX_B], (pairsbx + (sizeint)i2 * PBX__MAX)[PBX_B]);
+ dxSwap((pairsbx + (sizeint)i1 * PBX__MAX)[PBX_X], (pairsbx + (sizeint)i2 * PBX__MAX)[PBX_X]);
+ dSASSERT(PBX__MAX == 2);
+
+ dxSwap(w[i1], w[i2]);
+
+ dxSwap((pairslh + (sizeint)i1 * PLH__MAX)[PLH_LO], (pairslh + (sizeint)i2 * PLH__MAX)[PLH_LO]);
+ dxSwap((pairslh + (sizeint)i1 * PLH__MAX)[PLH_HI], (pairslh + (sizeint)i2 * PLH__MAX)[PLH_HI]);
+ dSASSERT(PLH__MAX == 2);
+
+ dxSwap(p[i1], p[i2]);
+ dxSwap(state[i1], state[i2]);
+
+ if (findex != NULL) {
+ dxSwap(findex[i1], findex[i2]);
+ }
+ }
+}
+
+
+// for debugging - check that L,d is the factorization of A[C,C].
+// A[C,C] has size nC*nC and leading dimension nskip.
+// L has size nC*nC and leading dimension nskip.
+// d has size nC.
+
+#ifdef DEBUG_LCP
+
+static
+void checkFactorization (ATYPE A, dReal *_L, dReal *_d,
+ unsigned nC, unsigned *C, unsigned nskip)
+{
+ unsigned i, j;
+ if (nC == 0) return;
+
+ // get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C]
+ dMatrix A1 (nC, nC);
+ for (i=0; i < nC; i++) {
+ for (j = 0; j <= i; j++) A1(i, j) = A1(j, i) = AROW(i)[j];
+ }
+ dMatrix A2 = A1.select (nC, C, nC, C);
+
+ // printf ("A1=\n"); A1.print(); printf ("\n");
+ // printf ("A2=\n"); A2.print(); printf ("\n");
+
+ // compute A3 = L*D*L'
+ dMatrix L (nC, nC, _L, nskip, 1);
+ dMatrix D (nC, nC);
+ for (i = 0; i < nC; i++) D(i, i) = 1.0 / _d[i];
+ L.clearUpperTriangle();
+ for (i = 0; i < nC; i++) L(i, i) = 1;
+ dMatrix A3 = L * D * L.transpose();
+
+ // printf ("L=\n"); L.print(); printf ("\n");
+ // printf ("D=\n"); D.print(); printf ("\n");
+ // printf ("A3=\n"); A2.print(); printf ("\n");
+
+ // compare A2 and A3
+ dReal diff = A2.maxDifference (A3);
+ if (diff > 1e-8)
+ dDebug (0, "L*D*L' check, maximum difference = %.6e\n", diff);
+}
+
+#endif
+
+
+// for debugging
+
+#ifdef DEBUG_LCP
+
+static
+void checkPermutations (unsigned i, unsigned n, unsigned nC, unsigned nN, unsigned *p, unsigned *C)
+{
+ unsigned j,k;
+ dIASSERT (/*nC >= 0 && nN >= 0 && */(nC + nN) == i && i < n);
+ for (k=0; k<i; k++) dIASSERT (p[k] >= 0 && p[k] < i);
+ for (k=i; k<n; k++) dIASSERT (p[k] == k);
+ for (j=0; j<nC; j++) {
+ int C_is_bad = 1;
+ for (k=0; k<nC; k++) if (C[k]==j) C_is_bad = 0;
+ dIASSERT (C_is_bad==0);
+ }
+}
+
+#endif
+
+//***************************************************************************
+// dLCP manipulator object. this represents an n*n LCP problem.
+//
+// two index sets C and N are kept. each set holds a subset of
+// the variable indexes 0..n-1. an index can only be in one set.
+// initially both sets are empty.
+//
+// the index set C is special: solutions to A(C,C)\A(C,i) can be generated.
+
+//***************************************************************************
+// fast implementation of dLCP. see the above definition of dLCP for
+// interface comments.
+//
+// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is
+// permuted as the other vectors/matrices are permuted.
+//
+// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have
+// contiguous indexes. the don't-care indexes follow N.
+//
+// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are
+// added or removed from the set C the factorization is updated.
+// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A.
+// the leading dimension of the matrix L is always `nskip'.
+//
+// at the start there may be other indexes that are unbounded but are not
+// included in `nub'. dLCP will permute the matrix so that absolutely all
+// unbounded vectors are at the start. thus there may be some initial
+// permutation.
+//
+// the algorithms here assume certain patterns, particularly with respect to
+// index transfer.
+
+#ifdef dLCP_FAST
+
+struct dLCP {
+ const unsigned m_n;
+ const unsigned m_nskip;
+ unsigned m_nub;
+ unsigned m_nC, m_nN; // size of each index set
+ ATYPE const m_A; // A rows
+ dReal *const m_pairsbx, *const m_w, *const m_pairslh; // permuted LCP problem data
+ dReal *const m_L, *const m_d; // L*D*L' factorization of set C
+ dReal *const m_Dell, *const m_ell, *const m_tmp;
+ bool *const m_state;
+ int *const m_findex;
+ unsigned *const m_p, *const m_C;
+
+ dLCP (unsigned _n, unsigned _nskip, unsigned _nub, dReal *_Adata, dReal *_pairsbx, dReal *_w,
+ dReal *_pairslh, dReal *_L, dReal *_d,
+ dReal *_Dell, dReal *_ell, dReal *_tmp,
+ bool *_state, int *_findex, unsigned *_p, unsigned *_C, dReal **Arows);
+ unsigned getNub() const { return m_nub; }
+ void transfer_i_to_C (unsigned i);
+ void transfer_i_to_N (unsigned /*i*/) { m_nN++; } // because we can assume C and N span 1:i-1
+ void transfer_i_from_N_to_C (unsigned i);
+ void transfer_i_from_C_to_N (unsigned i, void *tmpbuf);
+ static sizeint estimate_transfer_i_from_C_to_N_mem_req(unsigned nC, unsigned nskip) { return dEstimateLDLTRemoveTmpbufSize(nC, nskip); }
+ unsigned numC() const { return m_nC; }
+ unsigned numN() const { return m_nN; }
+ unsigned indexC (unsigned i) const { return i; }
+ unsigned indexN (unsigned i) const { return i+m_nC; }
+ dReal Aii (unsigned i) const { return AROW(i)[i]; }
+ template<unsigned q_stride>
+ dReal AiC_times_qC (unsigned i, dReal *q) const { return calculateLargeVectorDot<q_stride> (AROW(i), q, m_nC); }
+ template<unsigned q_stride>
+ dReal AiN_times_qN (unsigned i, dReal *q) const { return calculateLargeVectorDot<q_stride> (AROW(i) + m_nC, q + (sizeint)m_nC * q_stride, m_nN); }
+ void pN_equals_ANC_times_qC (dReal *p, dReal *q);
+ void pN_plusequals_ANi (dReal *p, unsigned i, bool dir_positive);
+ template<unsigned p_stride>
+ void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q);
+ void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q);
+ void solve1 (dReal *a, unsigned i, bool dir_positive, int only_transfer=0);
+ void unpermute_X();
+ void unpermute_W();
+};
+
+
+dLCP::dLCP (unsigned _n, unsigned _nskip, unsigned _nub, dReal *_Adata, dReal *_pairsbx, dReal *_w,
+ dReal *_pairslh, dReal *_L, dReal *_d,
+ dReal *_Dell, dReal *_ell, dReal *_tmp,
+ bool *_state, int *_findex, unsigned *_p, unsigned *_C, dReal **Arows):
+ m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
+# ifdef ROWPTRS
+ m_A(Arows),
+#else
+ m_A(_Adata),
+#endif
+ m_pairsbx(_pairsbx), m_w(_w), m_pairslh(_pairslh),
+ m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
+ m_state(_state), m_findex(_findex), m_p(_p), m_C(_C)
+{
+ dxtSetZero<PBX__MAX>(m_pairsbx + PBX_X, m_n);
+
+ {
+# ifdef ROWPTRS
+ // make matrix row pointers
+ dReal *aptr = _Adata;
+ ATYPE A = m_A;
+ const unsigned n = m_n, nskip = m_nskip;
+ for (unsigned k=0; k<n; aptr+=nskip, ++k) A[k] = aptr;
+# endif
+ }
+
+ {
+ unsigned *p = m_p;
+ const unsigned n = m_n;
+ for (unsigned k=0; k != n; ++k) p[k] = k; // initially unpermutted
+ }
+
+ /*
+ // for testing, we can do some random swaps in the area i > nub
+ {
+ const unsigned n = m_n;
+ const unsigned nub = m_nub;
+ if (nub < n) {
+ for (unsigned k=0; k<100; k++) {
+ unsigned i1,i2;
+ do {
+ i1 = dRandInt(n-nub)+nub;
+ i2 = dRandInt(n-nub)+nub;
+ }
+ while (i1 > i2);
+ //printf ("--> %d %d\n",i1,i2);
+ swapProblem (m_A, m_pairsbx, m_w, m_pairslh, m_p, m_state, m_findex, n, i1, i2, m_nskip, 0);
+ }
+ }
+ */
+
+ // permute the problem so that *all* the unbounded variables are at the
+ // start, i.e. look for unbounded variables not included in `nub'. we can
+ // potentially push up `nub' this way and get a bigger initial factorization.
+ // note that when we swap rows/cols here we must not just swap row pointers,
+ // as the initial factorization relies on the data being all in one chunk.
+ // variables that have findex >= 0 are *not* considered to be unbounded even
+ // if lo=-inf and hi=inf - this is because these limits may change during the
+ // solution process.
+
+ {
+ int *findex = m_findex;
+ dReal *pairslh = m_pairslh;
+ const unsigned n = m_n;
+ for (unsigned k = m_nub; k < n; ++k) {
+ if (findex && findex[k] >= 0) continue;
+ if ((pairslh + (sizeint)k * PLH__MAX)[PLH_LO] == -dInfinity && (pairslh + (sizeint)k * PLH__MAX)[PLH_HI] == dInfinity) {
+ swapProblem (m_A, m_pairsbx, m_w, pairslh, m_p, m_state, findex, n, m_nub, k, m_nskip, 0);
+ m_nub++;
+ }
+ }
+ }
+
+ // if there are unbounded variables at the start, factorize A up to that
+ // point and solve for x. this puts all indexes 0..nub-1 into C.
+ if (m_nub > 0) {
+ const unsigned nub = m_nub;
+ {
+ dReal *Lrow = m_L;
+ const unsigned nskip = m_nskip;
+ for (unsigned j = 0; j < nub; Lrow += nskip, ++j) memcpy(Lrow, AROW(j), (j + 1) * sizeof(dReal));
+ }
+ transfer_b_to_x<false> (m_pairsbx, nub);
+ factorMatrixAsLDLT<1> (m_L, m_d, nub, m_nskip);
+ solveEquationSystemWithLDLT<1, PBX__MAX> (m_L, m_d, m_pairsbx + PBX_X, nub, m_nskip);
+ dSetZero (m_w, nub);
+ {
+ unsigned *C = m_C;
+ for (unsigned k = 0; k < nub; ++k) C[k] = k;
+ }
+ m_nC = nub;
+ }
+
+ // permute the indexes > nub such that all findex variables are at the end
+ if (m_findex) {
+ const unsigned nub = m_nub;
+ int *findex = m_findex;
+ unsigned num_at_end = 0;
+ for (unsigned k = m_n; k > nub; ) {
+ --k;
+ if (findex[k] >= 0) {
+ swapProblem (m_A, m_pairsbx, m_w, m_pairslh, m_p, m_state, findex, m_n, k, m_n - 1 - num_at_end, m_nskip, 1);
+ num_at_end++;
+ }
+ }
+ }
+
+ // print info about indexes
+ /*
+ {
+ const unsigned n = m_n;
+ const unsigned nub = m_nub;
+ for (unsigned k=0; k<n; k++) {
+ if (k<nub) printf ("C");
+ else if ((m_pairslh + (sizeint)k * PLH__MAX)[PLH_LO] == -dInfinity && (m_pairslh + (sizeint)k * PLH__MAX)[PLH_HI] == dInfinity) printf ("c");
+ else printf (".");
+ }
+ printf ("\n");
+ }
+ */
+}
+
+
+void dLCP::transfer_i_to_C (unsigned i)
+{
+ {
+ const unsigned nC = m_nC;
+
+ if (nC > 0) {
+ // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
+ dReal *const Ltgt = m_L + (sizeint)m_nskip * nC, *ell = m_ell;
+ memcpy(Ltgt, ell, nC * sizeof(dReal));
+
+ dReal ell_Dell_dot = dxDot(m_ell, m_Dell, nC);
+ dReal AROW_i_i = AROW(i)[i] != ell_Dell_dot ? AROW(i)[i] : dNextAfter(AROW(i)[i], dInfinity); // A hack to avoid getting a zero in the denominator
+ m_d[nC] = dRecip (AROW_i_i - ell_Dell_dot);
+ }
+ else {
+ m_d[0] = dRecip (AROW(i)[i]);
+ }
+
+ swapProblem (m_A, m_pairsbx, m_w, m_pairslh, m_p, m_state, m_findex, m_n, nC, i, m_nskip, 1);
+
+ m_C[nC] = nC;
+ m_nC = nC + 1; // nC value is outdated after this line
+ }
+
+# ifdef DEBUG_LCP
+ checkFactorization (m_A, m_L, m_d, m_nC, m_C, m_nskip);
+ if (i < (m_n-1)) checkPermutations (i+1, m_n, m_nC, m_nN, m_p, m_C);
+# endif
+}
+
+
+void dLCP::transfer_i_from_N_to_C (unsigned i)
+{
+ {
+ const unsigned nC = m_nC;
+ if (nC > 0) {
+ {
+ dReal *const aptr = AROW(i);
+ dReal *Dell = m_Dell;
+ const unsigned *C = m_C;
+# ifdef NUB_OPTIMIZATIONS
+ // if nub>0, initial part of aptr unpermuted
+ const unsigned nub = m_nub;
+ unsigned j=0;
+ for ( ; j<nub; ++j) Dell[j] = aptr[j];
+ for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
+# else
+ for (unsigned j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
+# endif
+ }
+ solveL1Straight<1>(m_L, m_Dell, nC, m_nskip);
+
+ dReal ell_Dell_dot = REAL(0.0);
+ dReal *const Ltgt = m_L + (sizeint)m_nskip * nC;
+ dReal *ell = m_ell, *Dell = m_Dell, *d = m_d;
+ for (unsigned j = 0; j < nC; ++j) {
+ dReal ell_j, Dell_j = Dell[j];
+ Ltgt[j] = ell[j] = ell_j = Dell_j * d[j];
+ ell_Dell_dot += ell_j * Dell_j;
+ }
+
+ dReal AROW_i_i = AROW(i)[i] != ell_Dell_dot ? AROW(i)[i] : dNextAfter(AROW(i)[i], dInfinity); // A hack to avoid getting a zero in the denominator
+ m_d[nC] = dRecip (AROW_i_i - ell_Dell_dot);
+ }
+ else {
+ m_d[0] = dRecip (AROW(i)[i]);
+ }
+
+ swapProblem (m_A, m_pairsbx, m_w, m_pairslh, m_p, m_state, m_findex, m_n, nC, i, m_nskip, 1);
+
+ m_C[nC] = nC;
+ m_nN--;
+ m_nC = nC + 1; // nC value is outdated after this line
+ }
+
+ // @@@ TO DO LATER
+ // if we just finish here then we'll go back and re-solve for
+ // delta_x. but actually we can be more efficient and incrementally
+ // update delta_x here. but if we do this, we wont have ell and Dell
+ // to use in updating the factorization later.
+
+# ifdef DEBUG_LCP
+ checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
+# endif
+}
+
+
+void dLCP::transfer_i_from_C_to_N (unsigned i, void *tmpbuf)
+{
+ {
+ unsigned *C = m_C;
+ // remove a row/column from the factorization, and adjust the
+ // indexes (black magic!)
+ int last_idx = -1;
+ const unsigned nC = m_nC;
+ unsigned j = 0;
+ for ( ; j < nC; ++j) {
+ if (C[j] == nC - 1) {
+ last_idx = j;
+ }
+ if (C[j] == i) {
+ dxLDLTRemove (m_A, C, m_L, m_d, m_n, nC, j, m_nskip, tmpbuf);
+ unsigned k;
+ if (last_idx == -1) {
+ for (k = j + 1 ; k < nC; ++k) {
+ if (C[k] == nC - 1) {
+ break;
+ }
+ }
+ dIASSERT (k < nC);
+ }
+ else {
+ k = last_idx;
+ }
+ C[k] = C[j];
+ if (j != (nC - 1)) memmove (C + j, C + j + 1, (nC - j - 1) * sizeof(C[0]));
+ break;
+ }
+ }
+ dIASSERT (j < nC);
+
+ swapProblem (m_A, m_pairsbx, m_w, m_pairslh, m_p, m_state, m_findex, m_n, i, nC - 1, m_nskip, 1);
+
+ m_nN++;
+ m_nC = nC - 1; // nC value is outdated after this line
+ }
+
+# ifdef DEBUG_LCP
+ checkFactorization (m_A, m_L, m_d, m_nC, m_C, m_nskip);
+# endif
+}
+
+
+void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
+{
+ // we could try to make this matrix-vector multiplication faster using
+ // outer product matrix tricks, e.g. with the dMultidotX() functions.
+ // but i tried it and it actually made things slower on random 100x100
+ // problems because of the overhead involved. so we'll stick with the
+ // simple method for now.
+ const unsigned nC = m_nC;
+ dReal *ptgt = p + nC;
+ const unsigned nN = m_nN;
+ for (unsigned i = 0; i < nN; ++i) {
+ ptgt[i] = dxDot (AROW(i + nC), q, nC);
+ }
+}
+
+
+void dLCP::pN_plusequals_ANi (dReal *p, unsigned i, bool dir_positive)
+{
+ const unsigned nC = m_nC;
+ dReal *aptr = AROW(i) + nC;
+ dReal *ptgt = p + nC;
+ if (dir_positive) {
+ const unsigned nN = m_nN;
+ for (unsigned j=0; j < nN; ++j) ptgt[j] += aptr[j];
+ }
+ else {
+ const unsigned nN = m_nN;
+ for (unsigned j=0; j < nN; ++j) ptgt[j] -= aptr[j];
+ }
+}
+
+template<unsigned p_stride>
+void dLCP::pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
+{
+ const unsigned nC = m_nC;
+ dReal *q_end = q + nC;
+ for (; q != q_end; p += p_stride, ++q) {
+ *p += s * (*q);
+ }
+}
+
+void dLCP::pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
+{
+ const unsigned nC = m_nC;
+ dReal *ptgt = p + nC, *qsrc = q + nC;
+ const unsigned nN = m_nN;
+ for (unsigned i = 0; i < nN; ++i) {
+ ptgt[i] += s * qsrc[i];
+ }
+}
+
+void dLCP::solve1 (dReal *a, unsigned i, bool dir_positive, int only_transfer)
+{
+ // the `Dell' and `ell' that are computed here are saved. if index i is
+ // later added to the factorization then they can be reused.
+ //
+ // @@@ question: do we need to solve for entire delta_x??? yes, but
+ // only if an x goes below 0 during the step.
+
+ const unsigned nC = m_nC;
+ if (nC > 0) {
+ {
+ dReal *Dell = m_Dell;
+ unsigned *C = m_C;
+ dReal *aptr = AROW(i);
+# ifdef NUB_OPTIMIZATIONS
+ // if nub>0, initial part of aptr[] is guaranteed unpermuted
+ const unsigned nub = m_nub;
+ unsigned j = 0;
+ for ( ; j < nub; ++j) Dell[j] = aptr[j];
+ for ( ; j < nC; ++j) Dell[j] = aptr[C[j]];
+# else
+ for (unsigned j = 0; j < nC; ++j) Dell[j] = aptr[C[j]];
+# endif
+ }
+ solveL1Straight<1>(m_L, m_Dell, nC, m_nskip);
+ {
+ dReal *ell = m_ell, *Dell = m_Dell, *d = m_d;
+ for (unsigned j = 0; j < nC; ++j) ell[j] = Dell[j] * d[j];
+ }
+
+ if (!only_transfer) {
+ dReal *tmp = m_tmp, *ell = m_ell;
+ {
+ for (unsigned j = 0; j < nC; ++j) tmp[j] = ell[j];
+ }
+ solveL1Transposed<1>(m_L, tmp, nC, m_nskip);
+ if (dir_positive) {
+ unsigned *C = m_C;
+ dReal *tmp = m_tmp;
+ for (unsigned j = 0; j < nC; ++j) a[C[j]] = -tmp[j];
+ } else {
+ unsigned *C = m_C;
+ dReal *tmp = m_tmp;
+ for (unsigned j = 0; j < nC; ++j) a[C[j]] = tmp[j];
+ }
+ }
+ }
+}
+
+
+void dLCP::unpermute_X()
+{
+ unsigned *p = m_p;
+ dReal *pairsbx = m_pairsbx;
+ const unsigned n = m_n;
+ for (unsigned j = 0; j < n; ++j) {
+ unsigned k = p[j];
+ if (k != j) {
+ // p[j] = j; -- not going to be checked anymore anyway
+ dReal x_j = (pairsbx + (sizeint)j * PBX__MAX)[PBX_X];
+ for (;;) {
+ dxSwap(x_j, (pairsbx + (sizeint)k * PBX__MAX)[PBX_X]);
+
+ unsigned orig_k = p[k];
+ p[k] = k;
+ if (orig_k == j) {
+ break;
+ }
+ k = orig_k;
+ }
+ (pairsbx + (sizeint)j * PBX__MAX)[PBX_X] = x_j;
+ }
+ }
+}
+
+void dLCP::unpermute_W()
+{
+ memcpy (m_tmp, m_w, m_n * sizeof(dReal));
+
+ const unsigned *p = m_p;
+ dReal *w = m_w, *tmp = m_tmp;
+ const unsigned n = m_n;
+ for (unsigned j = 0; j < n; ++j) {
+ unsigned k = p[j];
+ w[k] = tmp[j];
+ }
+}
+
+#endif // dLCP_FAST
+
+
+static void dxSolveLCP_AllUnbounded (dxWorldProcessMemArena *memarena, unsigned n, dReal *A, dReal pairsbx[PBX__MAX]);
+static void dxSolveLCP_Generic (dxWorldProcessMemArena *memarena, unsigned n, dReal *A, dReal pairsbx[PBX__MAX],
+ dReal *outer_w/*=NULL*/, unsigned nub, dReal pairslh[PLH__MAX], int *findex);
+
+/*extern */
+void dxSolveLCP (dxWorldProcessMemArena *memarena, unsigned n, dReal *A, dReal pairsbx[PBX__MAX],
+ dReal *outer_w/*=NULL*/, unsigned nub, dReal pairslh[PLH__MAX], int *findex)
+{
+ if (nub >= n)
+ {
+ dxSolveLCP_AllUnbounded (memarena, n, A, pairsbx);
+ }
+ else
+ {
+ dxSolveLCP_Generic (memarena, n, A, pairsbx, outer_w, nub, pairslh, findex);
+ }
+}
+
+//***************************************************************************
+// if all the variables are unbounded then we can just factor, solve, and return
+
+static
+void dxSolveLCP_AllUnbounded (dxWorldProcessMemArena *memarena, unsigned n, dReal *A, dReal pairsbx[PBX__MAX])
+{
+ dAASSERT(A != NULL);
+ dAASSERT(pairsbx != NULL);
+ dAASSERT(n != 0);
+
+ transfer_b_to_x<true>(pairsbx, n);
+
+ unsigned nskip = dPAD(n);
+ factorMatrixAsLDLT<PBX__MAX> (A, pairsbx + PBX_B, n, nskip);
+ solveEquationSystemWithLDLT<PBX__MAX, PBX__MAX> (A, pairsbx + PBX_B, pairsbx + PBX_X, n, nskip);
+}
+
+//***************************************************************************
+// an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
+
+static
+void dxSolveLCP_Generic (dxWorldProcessMemArena *memarena, unsigned n, dReal *A, dReal pairsbx[PBX__MAX],
+ dReal *outer_w/*=NULL*/, unsigned nub, dReal pairslh[PLH__MAX], int *findex)
+{
+ dAASSERT (n > 0 && A && pairsbx && pairslh && nub >= 0 && nub < n);
+# ifndef dNODEBUG
+ {
+ // check restrictions on lo and hi
+ dReal *endlh = pairslh + (sizeint)n * PLH__MAX;
+ for (dReal *currlh = pairslh; currlh != endlh; currlh += PLH__MAX) dIASSERT (currlh[PLH_LO] <= 0 && currlh[PLH_HI] >= 0);
+ }
+# endif
+
+ const unsigned nskip = dPAD(n);
+ dReal *L = memarena->AllocateOveralignedArray<dReal> ((sizeint)nskip * n, LMATRIX_ALIGNMENT);
+ dReal *d = memarena->AllocateArray<dReal> (n);
+ dReal *w = outer_w != NULL ? outer_w : memarena->AllocateArray<dReal> (n);
+ dReal *delta_w = memarena->AllocateArray<dReal> (n);
+ dReal *delta_x = memarena->AllocateArray<dReal> (n);
+ dReal *Dell = memarena->AllocateArray<dReal> (n);
+ dReal *ell = memarena->AllocateArray<dReal> (n);
+#ifdef ROWPTRS
+ dReal **Arows = memarena->AllocateArray<dReal *> (n);
+#else
+ dReal **Arows = NULL;
+#endif
+ unsigned *p = memarena->AllocateArray<unsigned> (n);
+ unsigned *C = memarena->AllocateArray<unsigned> (n);
+
+ // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
+ bool *state = memarena->AllocateArray<bool> (n);
+
+ // create LCP object. note that tmp is set to delta_w to save space, this
+ // optimization relies on knowledge of how tmp is used, so be careful!
+ dLCP lcp(n, nskip, nub, A, pairsbx, w, pairslh, L, d, Dell, ell, delta_w, state, findex, p, C, Arows);
+ unsigned adj_nub = lcp.getNub();
+
+ // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
+ // LCP conditions then i is added to the appropriate index set. otherwise
+ // x(i),w(i) is driven either +ve or -ve to force it to the valid region.
+ // as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
+ // while driving x(i) we maintain the LCP conditions on the other variables
+ // 0..i-1. we do this by watching out for other x(i),w(i) values going
+ // outside the valid region, and then switching them between index sets
+ // when that happens.
+
+ bool hit_first_friction_index = false;
+ for (unsigned i = adj_nub; i < n; ++i) {
+ bool s_error = false;
+ // the index i is the driving index and indexes i+1..n-1 are "dont care",
+ // i.e. when we make changes to the system those x's will be zero and we
+ // don't care what happens to those w's. in other words, we only consider
+ // an (i+1)*(i+1) sub-problem of A*x=b+w.
+
+ // if we've hit the first friction index, we have to compute the lo and
+ // hi values based on the values of x already computed. we have been
+ // permuting the indexes, so the values stored in the findex vector are
+ // no longer valid. thus we have to temporarily unpermute the x vector.
+ // for the purposes of this computation, 0*infinity = 0 ... so if the
+ // contact constraint's normal force is 0, there should be no tangential
+ // force applied.
+
+ if (!hit_first_friction_index && findex && findex[i] >= 0) {
+ // un-permute x into delta_w, which is not being used at the moment
+ for (unsigned j = 0; j < n; ++j) delta_w[p[j]] = (pairsbx + (sizeint)j * PBX__MAX)[PBX_X];
+
+ // set lo and hi values
+ for (unsigned k = i; k < n; ++k) {
+ dReal *currlh = pairslh + (sizeint)k * PLH__MAX;
+ dReal wfk = delta_w[findex[k]];
+ if (wfk == 0) {
+ currlh[PLH_HI] = 0;
+ currlh[PLH_LO] = 0;
+ }
+ else {
+ currlh[PLH_HI] = dFabs (currlh[PLH_HI] * wfk);
+ currlh[PLH_LO] = -currlh[PLH_HI];
+ }
+ }
+ hit_first_friction_index = true;
+ }
+
+ // thus far we have not even been computing the w values for indexes
+ // greater than i, so compute w[i] now.
+ dReal wPrep = lcp.AiC_times_qC<PBX__MAX> (i, pairsbx + PBX_X) + lcp.AiN_times_qN<PBX__MAX> (i, pairsbx + PBX_X);
+
+ dReal *currbx = pairsbx + (sizeint)i * PBX__MAX;
+
+ w[i] = wPrep - currbx[PBX_B];
+
+ // if lo=hi=0 (which can happen for tangential friction when normals are
+ // 0) then the index will be assigned to set N with some state. however,
+ // set C's line has zero size, so the index will always remain in set N.
+ // with the "normal" switching logic, if w changed sign then the index
+ // would have to switch to set C and then back to set N with an inverted
+ // state. this is pointless, and also computationally expensive. to
+ // prevent this from happening, we use the rule that indexes with lo=hi=0
+ // will never be checked for set changes. this means that the state for
+ // these indexes may be incorrect, but that doesn't matter.
+
+ dReal *currlh = pairslh + (sizeint)i * PLH__MAX;
+
+ // see if x(i),w(i) is in a valid region
+ if (currlh[PLH_LO] == 0 && w[i] >= 0) {
+ lcp.transfer_i_to_N (i);
+ state[i] = false;
+ }
+ else if (currlh[PLH_HI] == 0 && w[i] <= 0) {
+ lcp.transfer_i_to_N (i);
+ state[i] = true;
+ }
+ else if (w[i] == 0) {
+ // this is a degenerate case. by the time we get to this test we know
+ // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
+ // and similarly that hi > 0. this means that the line segment
+ // corresponding to set C is at least finite in extent, and we are on it.
+ // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
+ lcp.solve1 (delta_x, i, false, 1);
+
+ lcp.transfer_i_to_C (i);
+ }
+ else {
+ // we must push x(i) and w(i)
+ for (;;) {
+ // find direction to push on x(i)
+ bool dir_positive = (w[i] <= 0);
+
+ // compute: delta_x(C) = -dir*A(C,C)\A(C,i)
+ lcp.solve1 (delta_x, i, dir_positive);
+
+ // note that delta_x[i] = (dir_positive ? 1 : -1), but we wont bother to set it
+
+ // compute: delta_w = A*delta_x ... note we only care about
+ // delta_w(N) and delta_w(i), the rest is ignored
+ lcp.pN_equals_ANC_times_qC (delta_w, delta_x);
+ lcp.pN_plusequals_ANi (delta_w, i, dir_positive);
+ delta_w[i] = dir_positive
+ ? lcp.AiC_times_qC<1> (i, delta_x) + lcp.Aii(i)
+ : lcp.AiC_times_qC<1> (i, delta_x) - lcp.Aii(i);
+
+ // find largest step we can take (size=s), either to drive x(i),w(i)
+ // to the valid LCP region or to drive an already-valid variable
+ // outside the valid region.
+
+ int cmd = 1; // index switching command
+ unsigned si = 0; // si = index to switch if cmd>3
+
+ dReal s = delta_w[i] != REAL(0.0)
+ ? -w[i] / delta_w[i]
+ : (w[i] != REAL(0.0) ? dCopySign(dInfinity, -w[i]) : REAL(0.0));
+
+ if (dir_positive) {
+ if (currlh[PLH_HI] < dInfinity) {
+ dReal s2 = (currlh[PLH_HI] - currbx[PBX_X]); // was (hi[i]-x[i])/dirf // step to x(i)=hi(i)
+ if (s2 < s) {
+ s = s2;
+ cmd = 3;
+ }
+ }
+ }
+ else {
+ if (currlh[PLH_LO] > -dInfinity) {
+ dReal s2 = (currbx[PBX_X] - currlh[PLH_LO]); // was (lo[i]-x[i])/dirf // step to x(i)=lo(i)
+ if (s2 < s) {
+ s = s2;
+ cmd = 2;
+ }
+ }
+ }
+
+ {
+ const unsigned numN = lcp.numN();
+ for (unsigned k = 0; k < numN; ++k) {
+ const unsigned indexN_k = lcp.indexN(k);
+ if (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) {
+ // don't bother checking if lo=hi=0
+ dReal *indexlh = pairslh + (sizeint)indexN_k * PLH__MAX;
+ if (indexlh[PLH_LO] == 0 && indexlh[PLH_HI] == 0) continue;
+ dReal s2 = -w[indexN_k] / delta_w[indexN_k];
+ if (s2 < s) {
+ s = s2;
+ cmd = 4;
+ si = indexN_k;
+ }
+ }
+ }
+ }
+
+ {
+ const unsigned numC = lcp.numC();
+ for (unsigned k = adj_nub; k < numC; ++k) {
+ const unsigned indexC_k = lcp.indexC(k);
+ dReal *indexlh = pairslh + (sizeint)indexC_k * PLH__MAX;
+ if (delta_x[indexC_k] < 0 && indexlh[PLH_LO] > -dInfinity) {
+ dReal s2 = (indexlh[PLH_LO] - (pairsbx + (sizeint)indexC_k * PBX__MAX)[PBX_X]) / delta_x[indexC_k];
+ if (s2 < s) {
+ s = s2;
+ cmd = 5;
+ si = indexC_k;
+ }
+ }
+ if (delta_x[indexC_k] > 0 && indexlh[PLH_HI] < dInfinity) {
+ dReal s2 = (indexlh[PLH_HI] - (pairsbx + (sizeint)indexC_k * PBX__MAX)[PBX_X]) / delta_x[indexC_k];
+ if (s2 < s) {
+ s = s2;
+ cmd = 6;
+ si = indexC_k;
+ }
+ }
+ }
+ }
+
+ //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
+ // "C->NL","C->NH"};
+ //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
+
+ // if s <= 0 then we've got a problem. if we just keep going then
+ // we're going to get stuck in an infinite loop. instead, just cross
+ // our fingers and exit with the current solution.
+ if (s <= REAL(0.0)) {
+ dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",(double)s);
+ if (i < n) {
+ dxtSetZero<PBX__MAX>(currbx + PBX_X, n - i);
+ dxSetZero (w + i, n - i);
+ }
+ s_error = true;
+ break;
+ }
+
+ // apply x = x + s * delta_x
+ lcp.pC_plusequals_s_times_qC<PBX__MAX> (pairsbx + PBX_X, s, delta_x);
+ currbx[PBX_X] = dir_positive
+ ? currbx[PBX_X] + s
+ : currbx[PBX_X] - s;
+
+ // apply w = w + s * delta_w
+ lcp.pN_plusequals_s_times_qN (w, s, delta_w);
+ w[i] += s * delta_w[i];
+
+ void *tmpbuf;
+ // switch indexes between sets if necessary
+ switch (cmd) {
+ case 1: // done
+ w[i] = 0;
+ lcp.transfer_i_to_C (i);
+ break;
+ case 2: // done
+ currbx[PBX_X] = currlh[PLH_LO];
+ state[i] = false;
+ lcp.transfer_i_to_N (i);
+ break;
+ case 3: // done
+ currbx[PBX_X] = currlh[PLH_HI];
+ state[i] = true;
+ lcp.transfer_i_to_N (i);
+ break;
+ case 4: // keep going
+ w[si] = 0;
+ lcp.transfer_i_from_N_to_C (si);
+ break;
+ case 5: // keep going
+ (pairsbx + (sizeint)si * PBX__MAX)[PBX_X] = (pairslh + (sizeint)si * PLH__MAX)[PLH_LO];
+ state[si] = false;
+ tmpbuf = memarena->PeekBufferRemainder();
+ lcp.transfer_i_from_C_to_N (si, tmpbuf);
+ break;
+ case 6: // keep going
+ (pairsbx + (sizeint)si * PBX__MAX)[PBX_X] = (pairslh + (sizeint)si * PLH__MAX)[PLH_HI];
+ state[si] = true;
+ tmpbuf = memarena->PeekBufferRemainder();
+ lcp.transfer_i_from_C_to_N (si, tmpbuf);
+ break;
+ }
+
+ if (cmd <= 3) break;
+ } // for (;;)
+ } // else
+
+ if (s_error) {
+ break;
+ }
+ } // for (unsigned i = adj_nub; i < n; ++i)
+
+ // now we have to un-permute x and w
+ if (outer_w != NULL) {
+ lcp.unpermute_W();
+ }
+ lcp.unpermute_X(); // This destroys p[] and must be done last
+}
+
+sizeint dxEstimateSolveLCPMemoryReq(unsigned n, bool outer_w_avail)
+{
+ const unsigned nskip = dPAD(n);
+
+ sizeint res = 0;
+
+ res += dOVERALIGNED_SIZE(sizeof(dReal) * ((sizeint)n * nskip), LMATRIX_ALIGNMENT); // for L
+ res += 5 * dEFFICIENT_SIZE(sizeof(dReal) * n); // for d, delta_w, delta_x, Dell, ell
+ if (!outer_w_avail) {
+ res += dEFFICIENT_SIZE(sizeof(dReal) * n); // for w
+ }
+#ifdef ROWPTRS
+ res += dEFFICIENT_SIZE(sizeof(dReal *) * n); // for Arows
+#endif
+ res += 2 * dEFFICIENT_SIZE(sizeof(unsigned) * n); // for p, C
+ res += dEFFICIENT_SIZE(sizeof(bool) * n); // for state
+
+ // Use n instead of nC as nC varies at runtime while n is greater or equal to nC
+ sizeint lcp_transfer_req = dLCP::estimate_transfer_i_from_C_to_N_mem_req(n, nskip);
+ res += dEFFICIENT_SIZE(lcp_transfer_req); // for dLCP::transfer_i_from_C_to_N
+
+ return res;
+}
+
+
+//***************************************************************************
+// accuracy and timing test
+
+static sizeint EstimateTestSolveLCPMemoryReq(unsigned n)
+{
+ const unsigned nskip = dPAD(n);
+
+ sizeint res = 0;
+
+ res += 2 * dEFFICIENT_SIZE(sizeof(dReal) * ((sizeint)n * nskip)); // for A, A2
+ res += 7 * dEFFICIENT_SIZE(sizeof(dReal) * n); // for x, b, w, lo, hi, tmp1, tmp2
+ res += dEFFICIENT_SIZE(sizeof(dReal) * PBX__MAX * n); // for pairsbx,
+ res += dEFFICIENT_SIZE(sizeof(dReal) * PLH__MAX * n); // for pairslh
+
+ res += dxEstimateSolveLCPMemoryReq(n, true);
+
+ return res;
+}
+
+extern "C" ODE_API int dTestSolveLCP()
+{
+ const unsigned n = 100;
+
+ sizeint memreq = EstimateTestSolveLCPMemoryReq(n);
+ dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, NULL, NULL);
+ if (arena == NULL) {
+ return 0;
+ }
+ arena->ResetState();
+
+ unsigned i,nskip = dPAD(n);
+#ifdef dDOUBLE
+ const dReal tol = REAL(1e-9);
+#endif
+#ifdef dSINGLE
+ const dReal tol = REAL(1e-4);
+#endif
+ printf ("dTestSolveLCP()\n");
+
+ dReal *A = arena->AllocateArray<dReal> (n*nskip);
+ dReal *x = arena->AllocateArray<dReal> (n);
+ dReal *b = arena->AllocateArray<dReal> (n);
+ dReal *w = arena->AllocateArray<dReal> (n);
+ dReal *lo = arena->AllocateArray<dReal> (n);
+ dReal *hi = arena->AllocateArray<dReal> (n);
+
+ dReal *A2 = arena->AllocateArray<dReal> (n*nskip);
+ dReal *pairsbx = arena->AllocateArray<dReal> (n * PBX__MAX);
+ dReal *pairslh = arena->AllocateArray<dReal> (n * PLH__MAX);
+
+ dReal *tmp1 = arena->AllocateArray<dReal> (n);
+ dReal *tmp2 = arena->AllocateArray<dReal> (n);
+
+ double total_time = 0;
+ for (unsigned count=0; count < 1000; count++) {
+ BEGIN_STATE_SAVE(arena, saveInner) {
+
+ // form (A,b) = a random positive definite LCP problem
+ dMakeRandomMatrix (A2,n,n,1.0);
+ dMultiply2 (A,A2,A2,n,n,n);
+ dMakeRandomMatrix (x,n,1,1.0);
+ dMultiply0 (b,A,x,n,n,1);
+ for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
+
+ // choose `nub' in the range 0..n-1
+ unsigned nub = 50; //dRandInt (n);
+
+ // make limits
+ for (i=0; i<nub; i++) lo[i] = -dInfinity;
+ for (i=0; i<nub; i++) hi[i] = dInfinity;
+ //for (i=nub; i<n; i++) lo[i] = 0;
+ //for (i=nub; i<n; i++) hi[i] = dInfinity;
+ //for (i=nub; i<n; i++) lo[i] = -dInfinity;
+ //for (i=nub; i<n; i++) hi[i] = 0;
+ for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
+ for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
+
+ // set a few limits to lo=hi=0
+ /*
+ for (i=0; i<10; i++) {
+ unsigned j = dRandInt (n-nub) + nub;
+ lo[j] = 0;
+ hi[j] = 0;
+ }
+ */
+
+ // solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
+ // SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
+ // ensure that it doesn't get referenced (if it does, the answer will be
+ // wrong).
+
+ memcpy (A2, A, n * nskip * sizeof(dReal));
+ dClearUpperTriangle (A2, n);
+ for (i = 0; i != n; ++i) {
+ dReal *currbx = pairsbx + i * PBX__MAX;
+ currbx[PBX_B] = b[i];
+ currbx[PBX_X] = 0;
+ }
+ for (i = 0; i != n; ++i) {
+ dReal *currlh = pairslh + i * PLH__MAX;
+ currlh[PLH_LO] = lo[i];
+ currlh[PLH_HI] = hi[i];
+ }
+ dSetZero (w,n);
+
+ dStopwatch sw;
+ dStopwatchReset (&sw);
+ dStopwatchStart (&sw);
+
+ dxSolveLCP (arena,n,A2,pairsbx,w,nub,pairslh,0);
+
+ dStopwatchStop (&sw);
+ double time = dStopwatchTime(&sw);
+ total_time += time;
+ double average = total_time / double(count+1) * 1000.0;
+
+ for (i = 0; i != n; ++i) {
+ const dReal *currbx = pairsbx + i * PBX__MAX;
+ x[i] = currbx[PBX_X];
+ }
+
+ // check the solution
+
+ dMultiply0 (tmp1,A,x,n,n,1);
+ for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
+ dReal diff = dMaxDifference (tmp1,tmp2,n,1);
+ // printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
+ // diff > tol ? "FAILED" : "passed");
+ if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
+ unsigned n1=0,n2=0,n3=0;
+ for (i=0; i<n; i++) {
+ if (x[i]==lo[i] && w[i] >= 0) {
+ n1++; // ok
+ }
+ else if (x[i]==hi[i] && w[i] <= 0) {
+ n2++; // ok
+ }
+ else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
+ n3++; // ok
+ }
+ else {
+ dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
+ x[i],w[i],lo[i],hi[i]);
+ }
+ }
+
+ // pacifier
+ printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
+ printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
+
+ } END_STATE_SAVE(arena, saveInner);
+ }
+
+ dxFreeTemporaryWorldProcessMemArena(arena);
+ return 1;
+}
diff --git a/libs/ode-0.16.1/ode/src/lcp.h b/libs/ode-0.16.1/ode/src/lcp.h new file mode 100644 index 0000000..da65d6f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/lcp.h @@ -0,0 +1,81 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i) +satisfies one of + (1) x = lo, w >= 0 + (2) x = hi, w <= 0 + (3) lo < x < hi, w = 0 +A is a matrix of dimension n*n, everything else is a vector of size n*1. +lo and hi can be +/- dInfinity as needed. the first `nub' variables are +unbounded, i.e. hi and lo are assumed to be +/- dInfinity. + +we restrict lo(i) <= 0 and hi(i) >= 0. + +the original data (A,b) may be modified by this function. + +if the `findex' (friction index) parameter is nonzero, it points to an array +of index values. in this case constraints that have findex[i] >= 0 are +special. all non-special constraints are solved for, then the lo and hi values +for the special constraints are set: + hi[i] = abs( hi[i] * x[findex[i]] ) + lo[i] = -hi[i] +and the solution continues. this mechanism allows a friction approximation +to be implemented. the first `nub' variables are assumed to have findex < 0. + +*/ + + +#ifndef _ODE_LCP_H_ +#define _ODE_LCP_H_ + +class dxWorldProcessMemArena; + +enum dxLCPBXElement +{ + PBX__MIN, + + PBX_B = PBX__MIN, + PBX_X, + + PBX__MAX, +}; + +enum dxLCPLHElement +{ + PLH__MIN, + + PLH_LO = PLH__MIN, + PLH_HI, + + PLH__MAX, +}; + +void dxSolveLCP (dxWorldProcessMemArena *memarena, + unsigned n, dReal *A, dReal pairsbx[PBX__MAX], dReal *w, + unsigned nub, dReal pairslh[PLH__MAX], int *findex); + +sizeint dxEstimateSolveLCPMemoryReq(unsigned n, bool outer_w_avail); + +#endif diff --git a/libs/ode-0.16.1/ode/src/mass.cpp b/libs/ode-0.16.1/ode/src/mass.cpp new file mode 100644 index 0000000..961b2da --- /dev/null +++ b/libs/ode-0.16.1/ode/src/mass.cpp @@ -0,0 +1,554 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/mass.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + +// Local dependencies +#include "collision_kernel.h" + +#if dTRIMESH_ENABLED +#include "collision_trimesh_internal.h" +#endif // dTRIMESH_ENABLED + +#define SQR(x) ((x)*(x)) //!< Returns x square +#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube + +#define _I(i,j) I[(i)*4+(j)] + + +// return 1 if ok, 0 if bad + +int dMassCheck (const dMass *m) +{ + int i; + + if (m->mass <= 0) { + dDEBUGMSG ("mass must be > 0"); + return 0; + } + if (!dIsPositiveDefinite (m->I,3,NULL)) { + dDEBUGMSG ("inertia must be positive definite"); + return 0; + } + + // verify that the center of mass position is consistent with the mass + // and inertia matrix. this is done by checking that the inertia around + // the center of mass is also positive definite. from the comment in + // dMassTranslate(), if the body is translated so that its center of mass + // is at the point of reference, then the new inertia is: + // I + mass*crossmat(c)^2 + // note that requiring this to be positive definite is exactly equivalent + // to requiring that the spatial inertia matrix + // [ mass*eye(3,3) M*crossmat(c)^T ] + // [ M*crossmat(c) I ] + // is positive definite, given that I is PD and mass>0. see the theorem + // about partitioned PD matrices for proof. + + dMatrix3 I2,chat; + dSetZero (chat,12); + dSetCrossMatrixPlus (chat,m->c,4); + dMultiply0_333 (I2,chat,chat); + for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i]; + for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i]; + for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i]; + if (!dIsPositiveDefinite (I2,3,NULL)) { + dDEBUGMSG ("center of mass inconsistent with mass parameters"); + return 0; + } + return 1; +} + + +void dMassSetZero (dMass *m) +{ + dAASSERT (m); + m->mass = REAL(0.0); + dSetZero (m->c,sizeof(m->c) / sizeof(dReal)); + dSetZero (m->I,sizeof(m->I) / sizeof(dReal)); +} + + +void dMassSetParameters (dMass *m, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal I11, dReal I22, dReal I33, + dReal I12, dReal I13, dReal I23) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = themass; + m->c[0] = cgx; + m->c[1] = cgy; + m->c[2] = cgz; + m->_I(0,0) = I11; + m->_I(1,1) = I22; + m->_I(2,2) = I33; + m->_I(0,1) = I12; + m->_I(0,2) = I13; + m->_I(1,2) = I23; + m->_I(1,0) = I12; + m->_I(2,0) = I13; + m->_I(2,1) = I23; + dMassCheck (m); +} + + +void dMassSetSphere (dMass *m, dReal density, dReal radius) +{ + dMassSetSphereTotal (m, (dReal) ((REAL(4.0)/REAL(3.0)) * M_PI * + radius*radius*radius * density), radius); +} + + +void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = total_mass; + dReal II = REAL(0.4) * total_mass * radius*radius; + m->_I(0,0) = II; + m->_I(1,1) = II; + m->_I(2,2) = II; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassSetCapsule (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dReal M1,M2,Ia,Ib; + dAASSERT (m); + dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); + dMassSetZero (m); + M1 = (dReal) (M_PI*radius*radius*length*density); // cylinder mass + M2 = (dReal) ((REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density); // total cap mass + m->mass = M1+M2; + Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) + + M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length); + Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius; + m->_I(0,0) = Ia; + m->_I(1,1) = Ia; + m->_I(2,2) = Ia; + m->_I(direction-1,direction-1) = Ib; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction, + dReal a, dReal b) +{ + dMassSetCapsule (m, 1.0, direction, a, b); + dMassAdjust (m, total_mass); +} + + +void dMassSetCylinder (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dMassSetCylinderTotal (m, (dReal) (M_PI*radius*radius*length*density), + direction, radius, length); +} + +void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction, + dReal radius, dReal length) +{ + dReal r2,I; + dAASSERT (m); + dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); + dMassSetZero (m); + r2 = radius*radius; + m->mass = total_mass; + I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length); + m->_I(0,0) = I; + m->_I(1,1) = I; + m->_I(2,2) = I; + m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassSetBox (dMass *m, dReal density, + dReal lx, dReal ly, dReal lz) +{ + dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz); +} + + +void dMassSetBoxTotal (dMass *m, dReal total_mass, + dReal lx, dReal ly, dReal lz) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = total_mass; + m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz); + m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz); + m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly); + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + + + + + +/* +* dMassSetTrimesh, implementation by Gero Mueller. +* Based on Brian Mirtich, "Fast and Accurate Computation of +* Polyhedral Mass Properties," journal of graphics tools, volume 1, +* number 2, 1996. +*/ +void dMassSetTrimesh( dMass *m, dReal density, dGeomID g ) +{ + dAASSERT (m); + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dMassSetZero (m); + +#if dTRIMESH_ENABLED + + dxTriMesh *TriMesh = static_cast<dxTriMesh *>(g); + unsigned int triangles = TriMesh->getMeshTriangleCount(); + + dReal nx, ny, nz; + unsigned int i, A, B, C; + // face integrals + dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; + + // projection integrals + dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; + + dReal T0 = 0; + dReal T1[3] = {0., 0., 0.}; + dReal T2[3] = {0., 0., 0.}; + dReal TP[3] = {0., 0., 0.}; + + for( i = 0; i < triangles; i++ ) + { + dVector3 v[3]; + TriMesh->fetchMeshTransformedTriangle(v, i); + + dVector3 n, a, b; + dSubtractVectors3( a, v[1], v[0] ); + dSubtractVectors3( b, v[2], v[0] ); + dCalcVectorCross3( n, b, a ); + nx = fabs(n[0]); + ny = fabs(n[1]); + nz = fabs(n[2]); + + if( nx > ny && nx > nz ) + C = 0; + else + C = (ny > nz) ? 1 : 2; + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (n[C] != REAL(0.0)) + { + A = (C + 1) % 3; + B = (A + 1) % 3; + + // calculate face integrals + { + dReal w; + dReal k1, k2, k3, k4; + + //compProjectionIntegrals(f); + { + dReal a0=0, a1=0, da; + dReal b0=0, b1=0, db; + dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; + dReal a1_2, a1_3, b1_2, b1_3; + dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; + dReal Cab, Kab, Caab, Kaab, Cabb, Kabb; + + P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; + + for( int j = 0; j < 3; j++) + { + switch(j) + { + case 0: + a0 = v[0][A]; + b0 = v[0][B]; + a1 = v[1][A]; + b1 = v[1][B]; + break; + case 1: + a0 = v[1][A]; + b0 = v[1][B]; + a1 = v[2][A]; + b1 = v[2][B]; + break; + case 2: + a0 = v[2][A]; + b0 = v[2][B]; + a1 = v[0][A]; + b1 = v[0][B]; + break; + } + da = a1 - a0; + db = b1 - b0; + a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; + b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; + a1_2 = a1 * a1; a1_3 = a1_2 * a1; + b1_2 = b1 * b1; b1_3 = b1_2 * b1; + + C1 = a1 + a0; + Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; + Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; + Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; + Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; + Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; + Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; + + P1 += db*C1; + Pa += db*Ca; + Paa += db*Caa; + Paaa += db*Caaa; + Pb += da*Cb; + Pbb += da*Cbb; + Pbbb += da*Cbbb; + Pab += db*(b1*Cab + b0*Kab); + Paab += db*(b1*Caab + b0*Kaab); + Pabb += da*(a1*Cabb + a0*Kabb); + } + + P1 /= 2.0; + Pa /= 6.0; + Paa /= 12.0; + Paaa /= 20.0; + Pb /= -6.0; + Pbb /= -12.0; + Pbbb /= -20.0; + Pab /= 24.0; + Paab /= 60.0; + Pabb /= -60.0; + } + + w = - dCalcVectorDot3(n, v[0]); + + k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; + + Fa = k1 * Pa; + Fb = k1 * Pb; + Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); + + Faa = k1 * Paa; + Fbb = k1 * Pbb; + Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb + + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faaa = k1 * Paaa; + Fbbb = k1 * Pbbb; + Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab + + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb + + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) + + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faab = k1 * Paab; + Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); + Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb + + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); + } + + + T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc)); + + T1[A] += n[A] * Faa; + T1[B] += n[B] * Fbb; + T1[C] += n[C] * Fcc; + T2[A] += n[A] * Faaa; + T2[B] += n[B] * Fbbb; + T2[C] += n[C] * Fccc; + TP[A] += n[A] * Faab; + TP[B] += n[B] * Fbbc; + TP[C] += n[C] * Fcca; + } + } + + T1[0] /= 2; T1[1] /= 2; T1[2] /= 2; + T2[0] /= 3; T2[1] /= 3; T2[2] /= 3; + TP[0] /= 2; TP[1] /= 2; TP[2] /= 2; + + m->mass = density * T0; + m->_I(0,0) = density * (T2[1] + T2[2]); + m->_I(1,1) = density * (T2[2] + T2[0]); + m->_I(2,2) = density * (T2[0] + T2[1]); + m->_I(0,1) = - density * TP[0]; + m->_I(1,0) = - density * TP[0]; + m->_I(2,1) = - density * TP[1]; + m->_I(1,2) = - density * TP[1]; + m->_I(2,0) = - density * TP[2]; + m->_I(0,2) = - density * TP[2]; + + // Added to address SF bug 1729095 + dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 ); + +# ifndef dNODEBUG + dMassCheck (m); +# endif + +#endif // dTRIMESH_ENABLED +} + + +void dMassSetTrimeshTotal( dMass *m, dReal total_mass, dGeomID g) +{ + dAASSERT( m ); + dUASSERT( g && g->type == dTriMeshClass, "argument not a trimesh" ); + dMassSetTrimesh( m, 1.0, g ); + dMassAdjust( m, total_mass ); +} + + + + +void dMassAdjust (dMass *m, dReal newmass) +{ + dAASSERT (m); + dReal scale = newmass / m->mass; + m->mass = newmass; + for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassTranslate (dMass *m, dReal x, dReal y, dReal z) +{ + // if the body is translated by `a' relative to its point of reference, + // the new inertia about the point of reference is: + // + // I + mass*(crossmat(c)^2 - crossmat(c+a)^2) + // + // where c is the existing center of mass and I is the old inertia. + + int i,j; + dMatrix3 ahat,chat,t1,t2; + dReal a[3]; + + dAASSERT (m); + + // adjust inertia matrix + dSetZero (chat,12); + dSetCrossMatrixPlus (chat,m->c,4); + a[0] = x + m->c[0]; + a[1] = y + m->c[1]; + a[2] = z + m->c[2]; + dSetZero (ahat,12); + dSetCrossMatrixPlus (ahat,a,4); + dMultiply0_333 (t1,ahat,ahat); + dMultiply0_333 (t2,chat,chat); + for (i=0; i<3; i++) for (j=0; j<3; j++) + m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]); + + // ensure perfect symmetry + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); + + // adjust center of mass + m->c[0] += x; + m->c[1] += y; + m->c[2] += z; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassRotate (dMass *m, const dMatrix3 R) +{ + // if the body is rotated by `R' relative to its point of reference, + // the new inertia about the point of reference is: + // + // R * I * R' + // + // where I is the old inertia. + + dMatrix3 t1; + dReal t2[3]; + + dAASSERT (m); + + // rotate inertia matrix + dMultiply2_333 (t1,m->I,R); + dMultiply0_333 (m->I,R,t1); + + // ensure perfect symmetry + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); + + // rotate center of mass + dMultiply0_331 (t2,R,m->c); + m->c[0] = t2[0]; + m->c[1] = t2[1]; + m->c[2] = t2[2]; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassAdd (dMass *a, const dMass *b) +{ + int i; + dAASSERT (a && b); + dReal denom = dRecip (a->mass + b->mass); + for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom; + a->mass += b->mass; + for (i=0; i<12; i++) a->I[i] += b->I[i]; +} + + +// Backwards compatible API +void dMassSetCappedCylinder(dMass *a, dReal b, int c, dReal d, dReal e) +{ + return dMassSetCapsule(a,b,c,d,e); +} + +void dMassSetCappedCylinderTotal(dMass *a, dReal b, int c, dReal d, dReal e) +{ + return dMassSetCapsuleTotal(a,b,c,d,e); +} + diff --git a/libs/ode-0.16.1/ode/src/mat.cpp b/libs/ode-0.16.1/ode/src/mat.cpp new file mode 100644 index 0000000..67f5673 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/mat.cpp @@ -0,0 +1,231 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/misc.h> +#include <ode/error.h> +#include <ode/memory.h> +#include "config.h" +#include "matrix.h" +#include "mat.h" + + +dMatrix::dMatrix() +{ + n = 0; + m = 0; + data = 0; +} + + +dMatrix::dMatrix (int rows, int cols) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + dSetZero (data,n*m); +} + + +dMatrix::dMatrix (const dMatrix &a) +{ + n = a.n; + m = a.m; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); +} + + +dMatrix::dMatrix (int rows, int cols, + dReal *_data, int rowskip, int colskip) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip]; + } +} + + +dMatrix::~dMatrix() +{ + if (data) dFree (data,n*m*sizeof(dReal)); +} + + +dReal & dMatrix::operator () (int i, int j) +{ + if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)"); + return data [i*m+j]; +} + + +void dMatrix::operator= (const dMatrix &a) +{ + if (data) dFree (data,n*m*sizeof(dReal)); + n = a.n; + m = a.m; + if (n > 0 && m > 0) { + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); + } + else data = 0; +} + + +void dMatrix::operator= (dReal a) +{ + for (int i=0; i<n*m; i++) data[i] = a; +} + + +dMatrix dMatrix::transpose() +{ + dMatrix r (m,n); + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j]; + } + return r; +} + + +dMatrix dMatrix::select (int np, int *p, int nq, int *q) +{ + if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes"); + dMatrix r (np,nq); + for (int i=0; i<np; i++) { + for (int j=0; j<nq; j++) { + if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m) + dDebug (0,"Matrix select, bad index arrays"); + r.data[i*nq+j] = data[p[i]*m+q[j]]; + } + } + return r; +} + + +dMatrix dMatrix::operator + (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes"); + dMatrix r (n,m); + for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i]; + return r; +} + + +dMatrix dMatrix::operator - (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes"); + dMatrix r (n,m); + for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i]; + return r; +} + + +dMatrix dMatrix::operator - () +{ + dMatrix r (n,m); + for (int i=0; i<n*m; i++) r.data[i] = -data[i]; + return r; +} + + +dMatrix dMatrix::operator * (const dMatrix &a) +{ + if (m != a.n) dDebug (0,"matrix *, mismatched sizes"); + dMatrix r (n,a.m); + for (int i=0; i<n; i++) { + for (int j=0; j<a.m; j++) { + dReal sum = 0; + for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j]; + r.data [i*a.m+j] = sum; + } + } + return r; +} + + +void dMatrix::operator += (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes"); + for (int i=0; i<n*m; i++) data[i] += a.data[i]; +} + + +void dMatrix::operator -= (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes"); + for (int i=0; i<n*m; i++) data[i] -= a.data[i]; +} + + +void dMatrix::clearUpperTriangle() +{ + if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices"); + for (int i=0; i<n; i++) { + for (int j=i+1; j<m; j++) data[i*m+j] = 0; + } +} + + +void dMatrix::clearLowerTriangle() +{ + if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices"); + for (int i=0; i<n; i++) { + for (int j=0; j<i; j++) data[i*m+j] = 0; + } +} + + +void dMatrix::makeRandom (dReal range) +{ + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) + data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; + } +} + + +void dMatrix::print (const char *fmt, FILE *f) +{ + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]); + fprintf (f,"\n"); + } +} + + +dReal dMatrix::maxDifference (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes"); + dReal max = 0; + for (int i=0; i<n; i++) { + for (int j=0; j<m; j++) { + dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]); + if (diff > max) max = diff; + } + } + return max; +} diff --git a/libs/ode-0.16.1/ode/src/mat.h b/libs/ode-0.16.1/ode/src/mat.h new file mode 100644 index 0000000..920c348 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/mat.h @@ -0,0 +1,71 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// matrix class. this is mostly for convenience in the testing code, it is +// not optimized at all. correctness is much more importance here. + +#ifndef _ODE_MAT_H_ +#define _ODE_MAT_H_ + +#include <ode/common.h> + + +class dMatrix { + int n,m; // matrix dimension, n,m >= 0 + dReal *data; // if nonzero, n*m elements allocated on the heap + +public: + // constructors, destructors + dMatrix(); // make default 0x0 matrix + dMatrix (int rows, int cols); // construct zero matrix of given size + dMatrix (const dMatrix &); // construct copy of given matrix + // create copy of given data - element (i,j) is data[i*rowskip+j*colskip] + dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip); + ~dMatrix(); // destructor + + // data movement + dReal & operator () (int i, int j); // reference an element + void operator= (const dMatrix &); // matrix = matrix + void operator= (dReal); // matrix = scalar + dMatrix transpose(); // return transposed matrix + // return a permuted submatrix of this matrix, made up of the rows in p + // and the columns in q. p has np elements, q has nq elements. + dMatrix select (int np, int *p, int nq, int *q); + + // operators + dMatrix operator + (const dMatrix &); + dMatrix operator - (const dMatrix &); + dMatrix operator - (); + dMatrix operator * (const dMatrix &); + void operator += (const dMatrix &); + void operator -= (const dMatrix &); + + // utility + void clearUpperTriangle(); + void clearLowerTriangle(); + void makeRandom (dReal range); + void print (const char *fmt = "%10.4f ", FILE *f=stdout); + dReal maxDifference (const dMatrix &); +}; + + +#endif diff --git a/libs/ode-0.16.1/ode/src/matrix.cpp b/libs/ode-0.16.1/ode/src/matrix.cpp new file mode 100644 index 0000000..cdb77af --- /dev/null +++ b/libs/ode-0.16.1/ode/src/matrix.cpp @@ -0,0 +1,593 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/common.h> +#include "config.h" +#include "matrix.h" +#include "objects.h" +#include "threaded_solver_ldlt.h" + +#include <ode/memory.h> + + +// misc defines +#define ALLOCA dALLOCA16 +#define STACK_ALLOC_MAX 8192U + +/*extern */ +void dxMultiply0(dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r) +{ + dAASSERT (A && B && C && p>0 && q>0 && r>0); + const unsigned qskip = dPAD(q); + const unsigned rskip = dPAD(r); + dReal *aa = A; + const dReal *bb = B; + for (unsigned i = p; i != 0; aa+=rskip, bb+=qskip, --i) { + dReal *a = aa; + const dReal *cc = C, *ccend = C + r; + for (; cc != ccend; ++a, ++cc) { + dReal sum = REAL(0.0); + const dReal *c = cc; + const dReal *b = bb, *bend = bb + q; + for (; b != bend; c+=rskip, ++b) { + sum += (*b) * (*c); + } + (*a) = sum; + } + } +} + + +/*extern */ +void dxMultiply1(dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r) +{ + dAASSERT (A && B && C && p>0 && q>0 && r>0); + const unsigned pskip = dPAD(p); + const unsigned rskip = dPAD(r); + dReal *aa = A; + const dReal *bb = B, *bbend = B + p; + for (; bb != bbend; aa += rskip, ++bb) { + dReal *a = aa; + const dReal *cc = C, *ccend = C + r; + for (; cc != ccend; ++a, ++cc) { + dReal sum = REAL(0.0); + const dReal *b = bb, *c = cc; + for (unsigned k = q; k != 0; b += pskip, c += rskip, --k) { + sum += (*b) * (*c); + } + (*a) = sum; + } + } +} + + +/*extern */ +void dxMultiply2(dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r) +{ + dAASSERT (A && B && C && p>0 && q>0 && r>0); + const unsigned rskip = dPAD(r); + const unsigned qskip = dPAD(q); + dReal *aa = A; + const dReal *bb = B; + for (unsigned i = p; i != 0; aa += rskip, bb += qskip, --i) { + dReal *a = aa, *aend = aa + r; + const dReal *cc = C; + for (; a != aend; cc+=qskip, ++a) { + dReal sum = REAL(0.0); + const dReal *b = bb, *c = cc, *cend = cc + q; + for (; c != cend; ++b, ++c) { + sum += (*b) * (*c); + } + (*a) = sum; + } + } +} + + +/*extern */ +int dxFactorCholesky(dReal *A, unsigned n, void *tmpBuf/*[n]*/) +{ + dAASSERT (n > 0 && A); + bool failure = false; + + dReal *alloctedBuf = NULL; + sizeint allocatedSize; + + const unsigned nskip = dPAD (n); + + dReal *recip = (dReal *)tmpBuf; + if (tmpBuf == NULL) { + allocatedSize = n * sizeof(dReal); + alloctedBuf = allocatedSize > STACK_ALLOC_MAX ? (dReal *)dAlloc(allocatedSize) : NULL; + recip = alloctedBuf != NULL ? alloctedBuf : (dReal*)ALLOCA(allocatedSize); + } + + dReal *aa = A; + for (unsigned i = 0; i < n; aa += nskip, ++i) { + dReal *cc = aa; + { + const dReal *bb = A; + for (unsigned j = 0; j < i; bb += nskip, ++cc, ++j) { + dReal sum = *cc; + const dReal *a = aa, *b = bb, *bend = bb + j; + for (; b != bend; ++a, ++b) { + sum -= (*a) * (*b); + } + *cc = sum * recip[j]; + } + } + { + dReal sum = *cc; + dReal *a = aa, *aend = aa + i; + for (; a != aend; ++a) { + sum -= (*a)*(*a); + } + if (sum <= REAL(0.0)) { + failure = true; + break; + } + dReal sumsqrt = dSqrt(sum); + *cc = sumsqrt; + recip[i] = dRecip (sumsqrt); + } + } + + if (alloctedBuf != NULL) { + dFree(alloctedBuf, allocatedSize); + } + + return failure ? 0 : 1; +} + + +/*extern */ +void dxSolveCholesky(const dReal *L, dReal *b, unsigned n, void *tmpBuf/*[n]*/) +{ + dAASSERT (n > 0 && L && b); + + dReal *alloctedBuf = NULL; + sizeint allocatedSize; + + const unsigned nskip = dPAD (n); + + dReal *y = (dReal *)tmpBuf; + if (tmpBuf == NULL) { + allocatedSize = n * sizeof(dReal); + alloctedBuf = allocatedSize > STACK_ALLOC_MAX ? (dReal *)dAlloc(allocatedSize) : NULL; + y = alloctedBuf != NULL ? alloctedBuf : (dReal*)ALLOCA(allocatedSize); + } + + { + const dReal *ll = L; + for (unsigned i = 0; i < n; ll += nskip, ++i) { + dReal sum = REAL(0.0); + for (unsigned k = 0; k < i; ++k) { + sum += ll[k] * y[k]; + } + dIASSERT(ll[i] != dReal(0.0)); + y[i] = (b[i] - sum) / ll[i]; + } + } + { + const dReal *ll = L + (n - 1) * (nskip + 1); + for (unsigned i = n; i > 0; ll -= nskip + 1) { + --i; + dReal sum = REAL(0.0); + const dReal *l = ll + nskip; + for (unsigned k = i + 1; k < n; l += nskip, ++k) { + sum += (*l) * b[k]; + } + dIASSERT(*ll != dReal(0.0)); + b[i] = (y[i] - sum) / (*ll); + } + } + + if (alloctedBuf != NULL) { + dFree(alloctedBuf, allocatedSize); + } +} + + +/*extern */ +int dxInvertPDMatrix(const dReal *A, dReal *Ainv, unsigned n, void *tmpBuf/*[nskip*(n+2)]*/) +{ + dAASSERT (n > 0 && A && Ainv); + bool success = false; + + dReal *alloctedBuf = NULL; + sizeint allocatedSize; + + sizeint choleskyFactorSize = dxEstimateFactorCholeskyTmpbufSize(n); + sizeint choleskySolveSize = dxEstimateSolveCholeskyTmpbufSize(n); + sizeint choleskyMaxSize = dMACRO_MAX(choleskyFactorSize, choleskySolveSize); + dIASSERT(choleskyMaxSize % sizeof(dReal) == 0); + + const unsigned nskip = dPAD (n); + const sizeint nskip_mul_n = (sizeint)nskip * n; + + dReal *tmp = (dReal *)tmpBuf; + if (tmpBuf == NULL) { + allocatedSize = choleskyMaxSize + (nskip + nskip_mul_n) * sizeof(dReal); + alloctedBuf = allocatedSize > STACK_ALLOC_MAX ? (dReal *)dAlloc(allocatedSize) : NULL; + tmp = alloctedBuf != NULL ? alloctedBuf : (dReal*)ALLOCA(allocatedSize); + } + + dReal *X = (dReal *)((char *)tmp + choleskyMaxSize); + dReal *L = X + nskip; + memcpy (L, A, nskip_mul_n * sizeof(dReal)); + if (dxFactorCholesky(L, n, tmp)) { + dSetZero (Ainv, nskip_mul_n); // make sure all padding elements set to 0 + dReal *aa = Ainv, *xi = X, *xiend = X + n; + for (; xi != xiend; ++aa, ++xi) { + dSetZero(X, n); + *xi = REAL(1.0); + dxSolveCholesky(L, X, n, tmp); + dReal *a = aa; + const dReal *x = X, *xend = X + n; + for (; x != xend; a += nskip, ++x) { + *a = *x; + } + } + success = true; + } + + if (alloctedBuf != NULL) { + dFree(alloctedBuf, allocatedSize); + } + + return success ? 1 : 0; +} + + +/*extern */ +int dxIsPositiveDefinite(const dReal *A, unsigned n, void *tmpBuf/*[nskip*(n+1)]*/) +{ + dAASSERT (n > 0 && A); + + dReal *alloctedBuf = NULL; + sizeint allocatedSize; + + sizeint choleskyFactorSize = dxEstimateFactorCholeskyTmpbufSize(n); + dIASSERT(choleskyFactorSize % sizeof(dReal) == 0); + + const unsigned nskip = dPAD (n); + const sizeint nskip_mul_n = (sizeint)nskip * n; + + dReal *tmp = (dReal *)tmpBuf; + if (tmpBuf == NULL) { + allocatedSize = choleskyFactorSize + nskip_mul_n * sizeof(dReal); + alloctedBuf = allocatedSize > STACK_ALLOC_MAX ? (dReal *)dAlloc(allocatedSize) : NULL; + tmp = alloctedBuf != NULL ? alloctedBuf : (dReal*)ALLOCA(allocatedSize); + } + + dReal *Acopy = (dReal *)((char *)tmp + choleskyFactorSize); + memcpy(Acopy, A, nskip_mul_n * sizeof(dReal)); + int factorResult = dxFactorCholesky (Acopy, n, tmp); + + if (alloctedBuf != NULL) { + dFree(alloctedBuf, allocatedSize); + } + + return factorResult; +} + + +/*extern */ +void dxLDLTAddTL(dReal *L, dReal *d, const dReal *a, unsigned n, unsigned nskip, void *tmpBuf/*[2*nskip]*/) +{ + dAASSERT(L && d && a && n > 0 && nskip >= n); + + if (n < 2) return; + + dReal *alloctedBuf = NULL; + sizeint allocatedSize; + + dReal *W1 = (dReal *)tmpBuf; + if (tmpBuf == NULL) { + allocatedSize = nskip * (2 * sizeof(dReal)); + alloctedBuf = allocatedSize > STACK_ALLOC_MAX ? (dReal *)dAlloc(allocatedSize) : NULL; + W1 = alloctedBuf != NULL ? alloctedBuf : (dReal*)ALLOCA(allocatedSize); + } + + dReal *W2 = W1 + nskip; + + W1[0] = REAL(0.0); + W2[0] = REAL(0.0); + for (unsigned j = 1; j < n; ++j) { + W1[j] = W2[j] = (dReal) (a[j] * M_SQRT1_2); + } + dReal W11 = (dReal) ((REAL(0.5)*a[0]+1)*M_SQRT1_2); + dReal W21 = (dReal) ((REAL(0.5)*a[0]-1)*M_SQRT1_2); + + dReal alpha1 = REAL(1.0); + dReal alpha2 = REAL(1.0); + + { + dReal dee = d[0]; + dReal alphanew = alpha1 + (W11*W11)*dee; + dIASSERT(alphanew != dReal(0.0)); + dee /= alphanew; + dReal gamma1 = W11 * dee; + dee *= alpha1; + alpha1 = alphanew; + alphanew = alpha2 - (W21*W21)*dee; + dee /= alphanew; + //dReal gamma2 = W21 * dee; + alpha2 = alphanew; + dReal k1 = REAL(1.0) - W21*gamma1; + dReal k2 = W21*gamma1*W11 - W21; + dReal *ll = L + nskip; + for (unsigned p = 1; p < n; ll += nskip, ++p) { + dReal Wp = W1[p]; + dReal ell = *ll; + W1[p] = Wp - W11*ell; + W2[p] = k1*Wp + k2*ell; + } + } + + dReal *ll = L + (nskip + 1); + for (unsigned j = 1; j < n; ll += nskip + 1, ++j) { + dReal k1 = W1[j]; + dReal k2 = W2[j]; + + dReal dee = d[j]; + dReal alphanew = alpha1 + (k1*k1)*dee; + dIASSERT(alphanew != dReal(0.0)); + dee /= alphanew; + dReal gamma1 = k1 * dee; + dee *= alpha1; + alpha1 = alphanew; + alphanew = alpha2 - (k2*k2)*dee; + dee /= alphanew; + dReal gamma2 = k2 * dee; + dee *= alpha2; + d[j] = dee; + alpha2 = alphanew; + + dReal *l = ll + nskip; + for (unsigned p = j + 1; p < n; l += nskip, ++p) { + dReal ell = *l; + dReal Wp = W1[p] - k1 * ell; + ell += gamma1 * Wp; + W1[p] = Wp; + Wp = W2[p] - k2 * ell; + ell -= gamma2 * Wp; + W2[p] = Wp; + *l = ell; + } + } + + if (alloctedBuf != NULL) { + dFree(alloctedBuf, allocatedSize); + } +} + + +// macros for dLDLTRemove() for accessing A - either access the matrix +// directly or access it via row pointers. we are only supposed to reference +// the lower triangle of A (it is symmetric), but indexes i and j come from +// permutation vectors so they are not predictable. so do a test on the +// indexes - this should not slow things down too much, as we don't do this +// in an inner loop. + +#define _GETA(i,j) (A[i][j]) +//#define _GETA(i,j) (A[(i)*nskip+(j)]) +#define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i)) + + +/*extern */ +void dxLDLTRemove(dReal **A, const unsigned *p, dReal *L, dReal *d, + unsigned n1, unsigned n2, unsigned r, unsigned nskip, void *tmpBuf/*n2 + 2*nskip*/) +{ + dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 /*&& r >= 0 */&& r < n2 && + n1 >= n2 && nskip >= n1); +#ifndef dNODEBUG + for (unsigned i = 0; i < n2; ++i) dIASSERT(p[i] >= 0 && p[i] < n1); +#endif + + if (r == n2 - 1) { + return; // deleting the last row/col is easy + } + + dReal *alloctedBuf = NULL; + sizeint allocatedSize; + + sizeint LDLTAddTLSize = dxEstimateLDLTAddTLTmpbufSize(nskip); + dIASSERT(LDLTAddTLSize % sizeof(dReal) == 0); + + dReal *tmp = (dReal *)tmpBuf; + if (tmpBuf == NULL) { + allocatedSize = LDLTAddTLSize + n2 * sizeof(dReal); + alloctedBuf = allocatedSize > STACK_ALLOC_MAX ? (dReal *)dAlloc(allocatedSize) : NULL; + tmp = alloctedBuf != NULL ? alloctedBuf : (dReal*)ALLOCA(allocatedSize); + } + + if (r == 0) { + dReal *a = (dReal *)((char *)tmp + LDLTAddTLSize); + const unsigned p_0 = p[0]; + for (unsigned i = 0; i < n2; ++i) { + a[i] = -GETA(p[i],p_0); + } + a[0] += REAL(1.0); + dxLDLTAddTL (L, d, a, n2, nskip, tmp); + } + else { + dReal *t = (dReal *)((char *)tmp + LDLTAddTLSize); + { + dReal *Lcurr = L + r*nskip; + for (unsigned i = 0; i < r; ++Lcurr, ++i) { + dIASSERT(d[i] != dReal(0.0)); + t[i] = *Lcurr / d[i]; + } + } + dReal *a = t + r; + { + dReal *Lcurr = L + r * nskip; + const unsigned *pp_r = p + r, p_r = *pp_r; + const unsigned n2_minus_r = n2 - r; + for (unsigned i = 0; i < n2_minus_r; Lcurr += nskip, ++i) { + a[i] = dDot(Lcurr, t, r) - GETA(pp_r[i], p_r); + } + } + a[0] += REAL(1.0); + dxLDLTAddTL (L + (sizeint)(nskip + 1) * r, d + r, a, n2 - r, nskip, tmp); + } + + // snip out row/column r from L and d + dxRemoveRowCol (L, n2, nskip, r); + if (r < (n2 - 1)) memmove (d + r, d + r + 1, (n2 - r - 1) * sizeof(dReal)); + + if (alloctedBuf != NULL) { + dFree(alloctedBuf, allocatedSize); + } +} + + +/*extern */ +void dxRemoveRowCol(dReal *A, unsigned n, unsigned nskip, unsigned r) +{ + dAASSERT(A && n > 0 && nskip >= n && r >= 0 && r < n); + if (r >= n - 1) return; + if (r > 0) { + { + const sizeint move_size = (n - r - 1) * sizeof(dReal); + dReal *Adst = A + r; + for (unsigned i = 0; i < r; Adst += nskip, ++i) { + dReal *Asrc = Adst + 1; + memmove (Adst, Asrc, move_size); + } + } + { + const sizeint cpy_size = r * sizeof(dReal); + dReal *Adst = A + (sizeint)nskip * r; + unsigned n1 = n - 1; + for (unsigned i = r; i < n1; ++i) { + dReal *Asrc = Adst + nskip; + memcpy (Adst, Asrc, cpy_size); + Adst = Asrc; + } + } + } + { + const sizeint cpy_size = (n - r - 1) * sizeof(dReal); + dReal *Adst = A + (sizeint)(nskip + 1) * r; + unsigned n1 = n - 1; + for (unsigned i = r; i < n1; ++i) { + dReal *Asrc = Adst + (nskip + 1); + memcpy (Adst, Asrc, cpy_size); + Adst = Asrc - 1; + } + } +} + + +#undef dSetZero +#undef dSetValue +//#undef dDot +#undef dMultiply0 +#undef dMultiply1 +#undef dMultiply2 +#undef dFactorCholesky +#undef dSolveCholesky +#undef dInvertPDMatrix +#undef dIsPositiveDefinite +#undef dLDLTAddTL +#undef dLDLTRemove +#undef dRemoveRowCol + + +/*extern ODE_API */ +void dSetZero(dReal *a, int n) +{ + dxSetZero(a, n); +} + +/*extern ODE_API */ +void dSetValue(dReal *a, int n, dReal value) +{ + dxSetValue(a, n, value); +} + +// dReal dDot (const dReal *a, const dReal *b, int n); + +/*extern ODE_API */ +void dMultiply0(dReal *A, const dReal *B, const dReal *C, int p,int q,int r) +{ + dxMultiply0(A, B, C, p, q, r); +} + +/*extern ODE_API */ +void dMultiply1(dReal *A, const dReal *B, const dReal *C, int p,int q,int r) +{ + dxMultiply1(A, B, C, p, q, r); +} + +/*extern ODE_API */ +void dMultiply2(dReal *A, const dReal *B, const dReal *C, int p,int q,int r) +{ + dxMultiply2(A, B, C, p, q, r); +} + +/*extern ODE_API */ +int dFactorCholesky(dReal *A, int n) +{ + return dxFactorCholesky(A, n, NULL); +} + +/*extern ODE_API */ +void dSolveCholesky(const dReal *L, dReal *b, int n) +{ + dxSolveCholesky(L, b, n, NULL); +} + +/*extern ODE_API */ +int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n) +{ + return dxInvertPDMatrix(A, Ainv, n, NULL); +} + +/*extern ODE_API */ +int dIsPositiveDefinite(const dReal *A, int n) +{ + return dxIsPositiveDefinite(A, n, NULL); +} + + +/*extern ODE_API */ +void dLDLTAddTL(dReal *L, dReal *d, const dReal *a, int n, int nskip) +{ + dxLDLTAddTL(L, d, a, n, nskip, NULL); +} + +/*extern ODE_API */ +void dLDLTRemove(dReal **A, const int *p, dReal *L, dReal *d, int n1, int n2, int r, int nskip) +{ + dxLDLTRemove(A, (const unsigned *)p, L, d, n1, n2, r, nskip, NULL); + dSASSERT(sizeof(unsigned) == sizeof(*p)); +} + +/*extern ODE_API */ +void dRemoveRowCol(dReal *A, int n, int nskip, int r) +{ + dxRemoveRowCol(A, n, nskip, r); +} + diff --git a/libs/ode-0.16.1/ode/src/matrix.h b/libs/ode-0.16.1/ode/src/matrix.h new file mode 100644 index 0000000..b723722 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/matrix.h @@ -0,0 +1,160 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/*
+ * optimized and unoptimized vector and matrix functions
+ * (inlined private versions)
+ */
+
+#ifndef _ODE__PRIVATE_MATRIX_H_
+#define _ODE__PRIVATE_MATRIX_H_
+
+
+#include <ode/matrix.h>
+
+
+#ifdef __cplusplus
+
+template <unsigned a_stride, typename element_type>
+ODE_INLINE
+void dxtSetZero (element_type *a, sizeint n)
+{
+ element_type *const aend = a + n * a_stride;
+ for (element_type *acurr = a; acurr != aend; acurr += a_stride) {
+ *acurr = (element_type)0;
+ }
+}
+
+template <typename element_type>
+ODE_INLINE
+void dxSetZero (element_type *a, sizeint n)
+{
+ dxtSetZero<1>(a, n);
+}
+
+template <typename element_type>
+ODE_INLINE
+void dxSetValue (element_type *a, sizeint n, element_type value)
+{
+ element_type *const aend = a + n;
+ for (element_type *acurr = a; acurr != aend; ++acurr) {
+ *acurr = value;
+ }
+}
+
+
+#else // #ifndef __cplusplus
+
+ODE_PURE_INLINE
+void dxSetZero (dReal *a, sizeint n)
+{
+ dReal *const aend = a + n;
+ dReal *acurr;
+ for (acurr = a; acurr != aend; ++acurr) {
+ *acurr = 0;
+ }
+}
+
+ODE_PURE_INLINE
+void dxSetValue (dReal *a, sizeint n, dReal value)
+{
+ dReal *const aend = a + n;
+ dReal *acurr;
+ for (acurr = a; acurr != aend; ++acurr) {
+ *acurr = value;
+ }
+}
+
+
+#endif // #ifdef __cplusplus
+
+
+dReal dxDot (const dReal *a, const dReal *b, unsigned n);
+void dxMultiply0 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
+void dxMultiply1 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
+void dxMultiply2 (dReal *A, const dReal *B, const dReal *C, unsigned p, unsigned q, unsigned r);
+int dxFactorCholesky (dReal *A, unsigned n, void *tmpbuf);
+void dxSolveCholesky (const dReal *L, dReal *b, unsigned n, void *tmpbuf);
+int dxInvertPDMatrix (const dReal *A, dReal *Ainv, unsigned n, void *tmpbuf);
+int dxIsPositiveDefinite (const dReal *A, unsigned n, void *tmpbuf);
+void dxLDLTAddTL (dReal *L, dReal *d, const dReal *a, unsigned n, unsigned nskip, void *tmpbuf);
+void dxLDLTRemove (dReal **A, const unsigned *p, dReal *L, dReal *d, unsigned n1, unsigned n2, unsigned r, unsigned nskip, void *tmpbuf);
+void dxRemoveRowCol (dReal *A, unsigned n, unsigned nskip, unsigned r);
+
+ODE_PURE_INLINE sizeint dxEstimateFactorCholeskyTmpbufSize(unsigned n)
+{
+ return dPAD(n) * sizeof(dReal);
+}
+
+ODE_PURE_INLINE sizeint dxEstimateSolveCholeskyTmpbufSize(unsigned n)
+{
+ return dPAD(n) * sizeof(dReal);
+}
+
+ODE_PURE_INLINE sizeint dxEstimateInvertPDMatrixTmpbufSize(unsigned n)
+{
+ sizeint FactorCholesky_size = dxEstimateFactorCholeskyTmpbufSize(n);
+ sizeint SolveCholesky_size = dxEstimateSolveCholeskyTmpbufSize(n);
+ sizeint MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size;
+ return (sizeint)dPAD(n) * (n + 1) * sizeof(dReal) + MaxCholesky_size;
+}
+
+ODE_PURE_INLINE sizeint dxEstimateIsPositiveDefiniteTmpbufSize(unsigned n)
+{
+ return (sizeint)dPAD(n) * n * sizeof(dReal) + dxEstimateFactorCholeskyTmpbufSize(n);
+}
+
+ODE_PURE_INLINE sizeint dxEstimateLDLTAddTLTmpbufSize(unsigned nskip)
+{
+ return nskip * (2 * sizeof(dReal));
+}
+
+ODE_PURE_INLINE sizeint dxEstimateLDLTRemoveTmpbufSize(unsigned n2, unsigned nskip)
+{
+ return n2 * sizeof(dReal) + dxEstimateLDLTAddTLTmpbufSize(nskip);
+}
+
+/* For internal use */
+#define dSetZero(a, n) dxSetZero(a, n)
+#define dSetValue(a, n, value) dxSetValue(a, n, value)
+#define dDot(a, b, n) dxDot(a, b, n)
+#define dMultiply0(A, B, C, p, q, r) dxMultiply0(A, B, C, p, q, r)
+#define dMultiply1(A, B, C, p, q, r) dxMultiply1(A, B, C, p, q, r)
+#define dMultiply2(A, B, C, p, q, r) dxMultiply2(A, B, C, p, q, r)
+#define dFactorCholesky(A, n, tmpbuf) dxFactorCholesky(A, n, tmpbuf)
+#define dSolveCholesky(L, b, n, tmpbuf) dxSolveCholesky(L, b, n, tmpbuf)
+#define dInvertPDMatrix(A, Ainv, n, tmpbuf) dxInvertPDMatrix(A, Ainv, n, tmpbuf)
+#define dIsPositiveDefinite(A, n, tmpbuf) dxIsPositiveDefinite(A, n, tmpbuf)
+#define dLDLTAddTL(L, d, a, n, nskip, tmpbuf) dxLDLTAddTL(L, d, a, n, nskip, tmpbuf)
+#define dLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf) dxLDLTRemove(A, p, L, d, n1, n2, r, nskip, tmpbuf)
+#define dRemoveRowCol(A, n, nskip, r) dxRemoveRowCol(A, n, nskip, r)
+
+
+#define dEstimateFactorCholeskyTmpbufSize(n) dxEstimateFactorCholeskyTmpbufSize(n)
+#define dEstimateSolveCholeskyTmpbufSize(n) dxEstimateSolveCholeskyTmpbufSize(n)
+#define dEstimateInvertPDMatrixTmpbufSize(n) dxEstimateInvertPDMatrixTmpbufSize(n)
+#define dEstimateIsPositiveDefiniteTmpbufSize(n) dxEstimateIsPositiveDefiniteTmpbufSize(n)
+#define dEstimateLDLTAddTLTmpbufSize(nskip) dxEstimateLDLTAddTLTmpbufSize(nskip)
+#define dEstimateLDLTRemoveTmpbufSize(n2, nskip) dxEstimateLDLTRemoveTmpbufSize(n2, nskip)
+
+
+#endif // #ifndef _ODE__PRIVATE_MATRIX_H_
diff --git a/libs/ode-0.16.1/ode/src/memory.cpp b/libs/ode-0.16.1/ode/src/memory.cpp new file mode 100644 index 0000000..5a67448 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/memory.cpp @@ -0,0 +1,95 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/memory.h> +#include <ode/error.h> +#include "config.h" + + +static dAllocFunction *allocfn = 0; +static dReallocFunction *reallocfn = 0; +static dFreeFunction *freefn = 0; + +#ifdef __MINGW32__ +/* +this is a guard against AC_FUNC_MALLOC and AC_FUNC_REALLOC +which break cross compilation, no issues in native MSYS. +*/ +#undef malloc +#undef realloc +#endif + +void dSetAllocHandler (dAllocFunction *fn) +{ + allocfn = fn; +} + + +void dSetReallocHandler (dReallocFunction *fn) +{ + reallocfn = fn; +} + + +void dSetFreeHandler (dFreeFunction *fn) +{ + freefn = fn; +} + + +dAllocFunction *dGetAllocHandler() +{ + return allocfn; +} + + +dReallocFunction *dGetReallocHandler() +{ + return reallocfn; +} + + +dFreeFunction *dGetFreeHandler() +{ + return freefn; +} + + +void * dAlloc (sizeint size) +{ + if (allocfn) return allocfn (size); else return malloc (size); +} + + +void * dRealloc (void *ptr, sizeint oldsize, sizeint newsize) +{ + if (reallocfn) return reallocfn (ptr,oldsize,newsize); + else return realloc (ptr,newsize); +} + + +void dFree (void *ptr, sizeint size) +{ + if (!ptr) return; + if (freefn) freefn (ptr,size); else free (ptr); +} diff --git a/libs/ode-0.16.1/ode/src/misc.cpp b/libs/ode-0.16.1/ode/src/misc.cpp new file mode 100644 index 0000000..e63a029 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/misc.cpp @@ -0,0 +1,217 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/odeconfig.h> +#include <ode/misc.h> +#include "config.h" +#include "matrix.h" +#include "error.h" +#include "odeou.h" + +//**************************************************************************** +// random numbers + +static volatile duint32 seed = 0; + +unsigned long dRand() +{ + duint32 origSeed, newSeed; +#if !dTHREADING_INTF_DISABLED + do { +#endif + origSeed = seed; + newSeed = ((duint32)1664525 * origSeed + (duint32)1013904223) & (duint32)0xffffffff; +#if dTHREADING_INTF_DISABLED + seed = newSeed; +#else + } while (!AtomicCompareExchange((volatile atomicord32 *)&seed, origSeed, newSeed)); +#endif + return newSeed; +} + + +unsigned long dRandGetSeed() +{ + return seed; +} + + +void dRandSetSeed (unsigned long s) +{ + seed = s; +} + + +int dTestRand() +{ + unsigned long oldseed = seed; + int ret = 1; + seed = 0; + if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 || + dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 || + dRand() != 0x6252e503) ret = 0; + seed = oldseed; + return ret; +} + + +// adam's all-int straightforward(?) dRandInt (0..n-1) +int dRandInt (int n) +{ + int result; + // Since there is no memory barrier macro in ODE assign via volatile variable + // to prevent compiler reusing seed as value of `r' + volatile unsigned long raw_r = dRand(); + duint32 r = (duint32)raw_r; + + duint32 un = n; + dIASSERT(sizeof(n) == sizeof(un)); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + // if (un <= 0x00010000UL) { + // r ^= (r >> 16); + // if (un <= 0x00000100UL) { + // r ^= (r >> 8); + // if (un <= 0x00000010UL) { + // r ^= (r >> 4); + // if (un <= 0x00000004UL) { + // r ^= (r >> 2); + // if (un <= 0x00000002UL) { + // r ^= (r >> 1); + // } + // } + // } + // } + // } + // Optimized version of above + if (un <= (duint32)0x00000010) { + r ^= (r >> 16); + r ^= (r >> 8); + r ^= (r >> 4); + if (un <= (duint32)0x00000002) { + r ^= (r >> 2); + r ^= (r >> 1); + result = (r/* & (duint32)0x01*/) & (un >> 1); + } else { + if (un <= (duint32)0x00000004) { + r ^= (r >> 2); + result = ((r & (duint32)0x03) * un) >> 2; + } else { + result = ((r & (duint32)0x0F) * un) >> 4; + } + } + } else { + if (un <= (duint32)0x00000100) { + r ^= (r >> 16); + r ^= (r >> 8); + result = ((r & (duint32)0xFF) * un) >> 8; + } else { + if (un <= (duint32)0x00010000) { + r ^= (r >> 16); + result = ((r & (duint32)0xFFFF) * un) >> 16; + } else { + result = (int)(((duint64)r * un) >> 32); + } + } + } + + return result; +} + + +dReal dRandReal() +{ + return (dReal)(((double) dRand()) / ((double) 0xffffffff)); +} + +//**************************************************************************** +// matrix utility stuff + +void dPrintMatrix (const dReal *A, int n, int m, const char *fmt, FILE *f) +{ + int skip = dPAD(m); + const dReal *Arow = A; + for (int i=0; i<n; Arow+=skip, ++i) { + for (int j=0; j<m; ++j) fprintf (f,fmt,Arow[j]); + fprintf (f,"\n"); + } +} + + +void dMakeRandomVector (dReal *A, int n, dReal range) +{ + int i; + for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; +} + + +void dMakeRandomMatrix (dReal *A, int n, int m, dReal range) +{ + int skip = dPAD(m); + // dSetZero (A,n*skip); + dReal *Arow = A; + for (int i=0; i<n; Arow+=skip, ++i) { + for (int j=0; j<m; ++j) Arow[j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; + } +} + + +void dClearUpperTriangle (dReal *A, int n) +{ + int skip = dPAD(n); + dReal *Arow = A; + for (int i=0; i<n; Arow+=skip, ++i) { + for (int j=i+1; j<n; ++j) Arow[j] = 0; + } +} + + +dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m) +{ + int skip = dPAD(m); + dReal max = REAL(0.0); + const dReal *Arow = A, *Brow = B; + for (int i=0; i<n; Arow+=skip, Brow +=skip, ++i) { + for (int j=0; j<m; ++j) { + dReal diff = dFabs(Arow[j] - Brow[j]); + if (diff > max) max = diff; + } + } + return max; +} + + +dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n) +{ + int skip = dPAD(n); + dReal max = REAL(0.0); + const dReal *Arow = A, *Brow = B; + for (int i=0; i<n; Arow+=skip, Brow+=skip, ++i) { + for (int j=0; j<=i; ++j) { + dReal diff = dFabs(Arow[j] - Brow[j]); + if (diff > max) max = diff; + } + } + return max; +} + diff --git a/libs/ode-0.16.1/ode/src/nextafterf.c b/libs/ode-0.16.1/ode/src/nextafterf.c new file mode 100644 index 0000000..78fbe31 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/nextafterf.c @@ -0,0 +1,115 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* _nextafterf() implementation for MSVC */ + +#include <ode/common.h> +#include "config.h" + + +#if defined(_ODE__NEXTAFTERF_REQUIRED) + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +/* A union which permits us to convert between a float and a 32 bit int. */ + +typedef union +{ + float value; + uint32 word; +} ieee_float_shape_type; + +/* Get a 32 bit int from a float. */ + +#define GET_FLOAT_WORD(i,d) \ + do { \ + volatile ieee_float_shape_type gf_u; \ + gf_u.value = (d); \ + (i) = gf_u.word; \ + } while (0) + +/* Set a float from a 32 bit int. */ + +#define SET_FLOAT_WORD(d,i) \ + do { \ + volatile ieee_float_shape_type sf_u; \ + sf_u.word = (i); \ + (d) = sf_u.value; \ + } while (0) + + +#undef nextafterf +float _nextafterf(float x, float y) +{ + int32 hx,hy,ix,iy; + + GET_FLOAT_WORD(hx,x); + GET_FLOAT_WORD(hy,y); + ix = hx&0x7fffffff; /* |x| */ + iy = hy&0x7fffffff; /* |y| */ + + if((ix>0x7f800000) || /* x is nan */ + (iy>0x7f800000)) /* y is nan */ + return x+y; + if(x==y) return x; /* x=y, return x */ + if(ix==0) { /* x == 0 */ + SET_FLOAT_WORD(x,(hy&0x80000000)|1);/* return +-minsubnormal */ + y = x*x; + if(y==x) return y; else return x; /* raise underflow flag */ + } + if(hx>=0) { /* x > 0 */ + if(hx>hy) { /* x > y, x -= ulp */ + hx -= 1; + } else { /* x < y, x += ulp */ + hx += 1; + } + } else { /* x < 0 */ + if(hy>=0||hx>hy){ /* x < y, x -= ulp */ + hx -= 1; + } else { /* x > y, x += ulp */ + hx += 1; + } + } + hy = hx&0x7f800000; + if(hy>=0x7f800000) return x+x; /* overflow */ + if(hy<0x00800000) { /* underflow */ + y = x*x; + if(y!=x) { /* raise underflow flag */ + SET_FLOAT_WORD(y,hx); + return y; + } + } + SET_FLOAT_WORD(x,hx); + return x; +} + + +#endif /* #if defined(_ODE__NEXTAFTERF_REQUIRED) */ diff --git a/libs/ode-0.16.1/ode/src/objects.cpp b/libs/ode-0.16.1/ode/src/objects.cpp new file mode 100644 index 0000000..e024aca --- /dev/null +++ b/libs/ode-0.16.1/ode/src/objects.cpp @@ -0,0 +1,138 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Object, body, and world methods. + + +#include <ode/common.h> +#include <ode/threading_impl.h> +#include <ode/objects.h> +#include "config.h" +#include "objects.h" +#include "default_threading.h" +#include "threading_impl.h" +#include "matrix.h" +#include "util.h" + + +#define dWORLD_DEFAULT_GLOBAL_ERP REAL(0.2) + +#if defined(dSINGLE) +#define dWORLD_DEFAULT_GLOBAL_CFM REAL(1e-5) +#elif defined(dDOUBLE) +#define dWORLD_DEFAULT_GLOBAL_CFM REAL(1e-10) +#else +#error dSINGLE or dDOUBLE must be defined +#endif + + +dObject::~dObject() +{ + // Do nothing - a virtual destructor +} + + +dxAutoDisable::dxAutoDisable(void *): + idle_time(REAL(0.0)), + idle_steps(10), + average_samples(1), // Default is 1 sample => Instantaneous velocity + linear_average_threshold(REAL(0.01)*REAL(0.01)), // (magnitude squared) + angular_average_threshold(REAL(0.01)*REAL(0.01)) // (magnitude squared) +{ +} + +dxDampingParameters::dxDampingParameters(void *): + linear_scale(REAL(0.0)), + angular_scale(REAL(0.0)), + linear_threshold(REAL(0.01) * REAL(0.01)), + angular_threshold(REAL(0.01) * REAL(0.01)) +{ +} + +dxQuickStepParameters::dxQuickStepParameters(void *): + num_iterations(20), + w(REAL(1.3)) +{ +} + +dxContactParameters::dxContactParameters(void *): + max_vel(dInfinity), + min_depth(REAL(0.0)) +{ +} + +dxWorld::dxWorld(): + dBase(), + dxThreadingBase(), + firstbody(NULL), + firstjoint(NULL), + nb(0), + nj(0), + global_erp(dWORLD_DEFAULT_GLOBAL_ERP), + global_cfm(dWORLD_DEFAULT_GLOBAL_CFM), + adis(NULL), + body_flags(0), + islands_max_threads(dWORLDSTEP_THREADCOUNT_UNLIMITED), + wmem(NULL), + qs(NULL), + contactp(NULL), + dampingp(NULL), + max_angular_speed(dInfinity), + userdata(0) +{ + dxThreadingBase::setThreadingDefaultImplProvider(this); + + dSetZero (gravity, 4); +} + +dxWorld::~dxWorld() +{ + if (wmem) + { + wmem->CleanupWorldReferences(this); + wmem->Release(); + } +} + + +void dxWorld::assignThreadingImpl(const dxThreadingFunctionsInfo *functions_info, dThreadingImplementationID threading_impl) +{ + if (wmem != NULL) + { + // Free objects allocated with old threading + wmem->CleanupWorldReferences(this); + } + + dxThreadingBase::assignThreadingImpl(functions_info, threading_impl); +} + +dxWorldProcessContext *dxWorld::unsafeGetWorldProcessingContext() const +{ + return wmem->GetWorldProcessingContext(); +} + +const dxThreadingFunctionsInfo *dxWorld::retrieveThreadingDefaultImpl(dThreadingImplementationID &out_defaultImpl) +{ + out_defaultImpl = DefaultThreadingHolder::getDefaultThreadingImpl(); + return DefaultThreadingHolder::getDefaultThreadingFunctions(); +} + diff --git a/libs/ode-0.16.1/ode/src/objects.h b/libs/ode-0.16.1/ode/src/objects.h new file mode 100644 index 0000000..0e7d34f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/objects.h @@ -0,0 +1,206 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// object, body, and world structs. + + +#ifndef _ODE__PRIVATE_OBJECTS_H_ +#define _ODE__PRIVATE_OBJECTS_H_ + + +#include <ode/common.h> +#include <ode/memory.h> +#include <ode/mass.h> +#include "error.h" +#include "array.h" +#include "common.h" +#include "threading_base.h" + + +struct dxJointNode; +class dxStepWorkingMemory; +class dxWorldProcessContext; + + +// some body flags + +enum { + dxBodyFlagFiniteRotation = 1, // use finite rotations + dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis + dxBodyDisabled = 4, // body is disabled + dxBodyNoGravity = 8, // body is not influenced by gravity + dxBodyAutoDisable = 16, // enable auto-disable on body + dxBodyLinearDamping = 32, // use linear damping + dxBodyAngularDamping = 64, // use angular damping + dxBodyMaxAngularSpeed = 128,// use maximum angular speed + dxBodyGyroscopic = 256 // use gyroscopic term +}; + + +// base class that does correct object allocation / deallocation + +struct dBase { + void *operator new (size_t size) { return dAlloc (size); } + void *operator new (size_t, void *p) { return p; } + void operator delete (void *ptr, size_t size) { dFree (ptr,size); } + void *operator new[] (size_t size) { return dAlloc (size); } + void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); } +}; + + +// base class for bodies and joints + +struct dObject : public dBase { + dxWorld *world; // world this object is in + dObject *next; // next object of this type in list + dObject **tome; // pointer to previous object's next ptr + int tag; // used by dynamics algorithms + void *userdata; // user settable data + + explicit dObject(dxWorld *w): world(w), next(NULL), tome(NULL), tag(0), userdata(NULL) {} + virtual ~dObject(); +}; + + +// auto disable parameters +struct dxAutoDisable { + dReal idle_time; // time the body needs to be idle to auto-disable it + int idle_steps; // steps the body needs to be idle to auto-disable it + unsigned int average_samples; // size of the average_lvel and average_avel buffers + dReal linear_average_threshold; // linear (squared) average velocity threshold + dReal angular_average_threshold; // angular (squared) average velocity threshold + + dxAutoDisable() {} + explicit dxAutoDisable(void *); +}; + + +// damping parameters +struct dxDampingParameters { + dReal linear_scale; // multiply the linear velocity by (1 - scale) + dReal angular_scale; // multiply the angular velocity by (1 - scale) + dReal linear_threshold; // linear (squared) average speed threshold + dReal angular_threshold; // angular (squared) average speed threshold + + dxDampingParameters() {} + explicit dxDampingParameters(void *); +}; + + +// quick-step parameters +struct dxQuickStepParameters { + int num_iterations; // number of SOR iterations to perform + dReal w; // the SOR over-relaxation parameter + + dxQuickStepParameters() {} + explicit dxQuickStepParameters(void *); +}; + + +// contact generation parameters +struct dxContactParameters { + dReal max_vel; // maximum correcting velocity + dReal min_depth; // thickness of 'surface layer' + + dxContactParameters() {} + explicit dxContactParameters(void *); +}; + +// position vector and rotation matrix for geometry objects that are not +// connected to bodies. +struct dxPosR { + dVector3 pos; + dMatrix3 R; +}; + +struct dxBody : public dObject { + dxJointNode *firstjoint; // list of attached joints + unsigned flags; // some dxBodyFlagXXX flags + dGeomID geom; // first collision geom associated with body + dMass mass; // mass parameters about POR + dMatrix3 invI; // inverse of mass.I + dReal invMass; // 1 / mass.mass + dxPosR posr; // position and orientation of point of reference + dQuaternion q; // orientation quaternion + dVector3 lvel,avel; // linear and angular velocity of POR + dVector3 facc,tacc; // force and torque accumulators + dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none + + // auto-disable information + dxAutoDisable adis; // auto-disable parameters + dReal adis_timeleft; // time left to be idle + int adis_stepsleft; // steps left to be idle + dVector3* average_lvel_buffer; // buffer for the linear average velocity calculation + dVector3* average_avel_buffer; // buffer for the angular average velocity calculation + unsigned int average_counter; // counter/index to fill the average-buffers + int average_ready; // indicates ( with = 1 ), if the Body's buffers are ready for average-calculations + + void (*moved_callback)(dxBody*); // let the user know the body moved + dxDampingParameters dampingp; // damping parameters, depends on flags + dReal max_angular_speed; // limit the angular velocity to this magnitude + + dxBody(dxWorld *w); +}; + + +struct dxWorld : public dBase, public dxThreadingBase, private dxIThreadingDefaultImplProvider { + dxBody *firstbody; // body linked list + dxJoint *firstjoint; // joint linked list + int nb,nj; // number of bodies and joints in lists + dVector3 gravity; // gravity vector (m/s/s) + dReal global_erp; // global error reduction parameter + dReal global_cfm; // global constraint force mixing parameter + dxAutoDisable adis; // auto-disable parameters + int body_flags; // flags for new bodies + unsigned islands_max_threads; // maximum threads to allocate for island processing + dxStepWorkingMemory *wmem; // Working memory object for dWorldStep/dWorldQuickStep + + dxQuickStepParameters qs; + dxContactParameters contactp; + dxDampingParameters dampingp; // damping parameters + dReal max_angular_speed; // limit the angular velocity to this magnitude + + void* userdata; + + dxWorld(); + virtual ~dxWorld(); // Compilers issue warnings if a class with virtual methods does not have a virtual destructor :( + + void assignThreadingImpl(const dxThreadingFunctionsInfo *functions_info, dThreadingImplementationID threading_impl); + + unsigned calculateIslandProcessingMaxThreadCount(unsigned *ptrOut_activeThreadCount=NULL) const + { + unsigned activeThreadCount, *ptrActiveThreadCountToUse = ptrOut_activeThreadCount != NULL ? &activeThreadCount : NULL; + unsigned limitedCount = calculateThreadingLimitedThreadCount(islands_max_threads, false, ptrActiveThreadCountToUse); + if (ptrOut_activeThreadCount != NULL) { + *ptrOut_activeThreadCount = dMACRO_MAX(activeThreadCount, 1U); + } + return dMACRO_MAX(limitedCount, 1U); + } + + dxWorldProcessContext *unsafeGetWorldProcessingContext() const; + +private: // dxIThreadingDefaultImplProvider + virtual const dxThreadingFunctionsInfo *retrieveThreadingDefaultImpl(dThreadingImplementationID &out_defaultImpl); +}; + + +#endif // #ifndef _ODE__PRIVATE_OBJECTS_H_ diff --git a/libs/ode-0.16.1/ode/src/obstack.cpp b/libs/ode-0.16.1/ode/src/obstack.cpp new file mode 100644 index 0000000..541f0e3 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/obstack.cpp @@ -0,0 +1,157 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/common.h> +#include <ode/error.h> +#include <ode/memory.h> +#include "config.h" +#include "obstack.h" + + +//**************************************************************************** +// macros and constants + +#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \ + ofs = (sizeint) (dEFFICIENT_SIZE( ((sizeint)(arena)) + ofs ) - ((sizeint)(arena)) ) + +#define MAX_ALLOC_SIZE \ + ((sizeint)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1)) + +//**************************************************************************** +// dObStack + +dObStack::dObStack(): + m_first(NULL), m_last(NULL), + m_current_arena(NULL), m_current_ofs(0) +{ +} + + +dObStack::~dObStack() +{ + // free all arenas + Arena *a,*nexta; + a = m_first; + while (a) { + nexta = a->m_next; + dFree (a,dOBSTACK_ARENA_SIZE); + a = nexta; + } +} + + +void *dObStack::alloc (sizeint num_bytes) +{ + if (num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large"); + + bool last_alloc_needed = false, last_init_needed = false; + Arena **last_ptr = NULL; + + if (m_last != NULL) { + if ((m_last->m_used + num_bytes) > dOBSTACK_ARENA_SIZE) { + if (m_last->m_next != NULL) { + m_last = m_last->m_next; + last_init_needed = true; + } else { + last_ptr = &m_last->m_next; + last_alloc_needed = true; + } + } + } else { + last_ptr = &m_last; + last_alloc_needed = true; + } + + if (last_alloc_needed) { + Arena *new_last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE); + new_last->m_next = 0; + *last_ptr = new_last; + if (m_first == NULL) { + m_first = new_last; + } + m_last = new_last; + last_init_needed = true; + } + + if (last_init_needed) { + m_last->m_used = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (m_last,m_last->m_used); + } + + // allocate an area in the arena + char *c = ((char*) m_last) + m_last->m_used; + m_last->m_used += num_bytes; + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (m_last,m_last->m_used); + return c; +} + + +void dObStack::freeAll() +{ + Arena *current = m_first; + m_last = current; + // It is necessary to reset used sizes in whole arena chain + // otherwise enumeration may proceed to remains of old deleted joints in unused arenas + while (current) { + current->m_used = sizeof(Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current,current->m_used); + current = current->m_next; + } +} + + +void *dObStack::rewind() +{ + return switch_to_arena(m_first); +} + +void *dObStack::next (sizeint num_bytes) +{ + // this functions like alloc, except that no new storage is ever allocated + if (!m_current_arena) { + return 0; + } + + m_current_ofs += num_bytes; + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (m_current_arena,m_current_ofs); + + if (m_current_ofs < m_current_arena->m_used) { + return ((char*) m_current_arena) + m_current_ofs; + } + + return switch_to_arena(m_current_arena->m_next); +} + +void *dObStack::switch_to_arena(Arena *next_arena) +{ + m_current_arena = next_arena; + if (!next_arena) { + return 0; + } + m_current_ofs = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (next_arena, m_current_ofs); + // Check if end of allocation has been reached + if (m_current_ofs >= next_arena->m_used) { + return 0; + } + return ((char*) next_arena) + m_current_ofs; +} diff --git a/libs/ode-0.16.1/ode/src/obstack.h b/libs/ode-0.16.1/ode/src/obstack.h new file mode 100644 index 0000000..8d4f067 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/obstack.h @@ -0,0 +1,73 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_OBSTACK_H_ +#define _ODE_OBSTACK_H_ + +#include "objects.h" + +// each obstack Arena pointer points to a block of this many bytes +#define dOBSTACK_ARENA_SIZE 16384 + + +struct dObStack : public dBase { + dObStack(); + ~dObStack(); + + void *alloc (sizeint num_bytes); + // allocate a block in the last arena, allocating a new arena if necessary. + // it is a runtime error if num_bytes is larger than the arena size. + + void freeAll(); + // free all blocks in all arenas. this does not deallocate the arenas + // themselves, so future alloc()s will reuse them. + + void *rewind(); + // rewind the obstack iterator, and return the address of the first + // allocated block. return 0 if there are no allocated blocks. + + void *next (sizeint num_bytes); + // return the address of the next allocated block. 'num_bytes' is the size + // of the previous block. this returns null if there are no more arenas. + // the sequence of 'num_bytes' parameters passed to next() during a + // traversal of the list must exactly match the parameters passed to alloc(). + +private: + struct Arena { + Arena *m_next; // next arena in linked list + sizeint m_used; // total number of bytes used in this arena, counting + }; // this header + +private: + void *switch_to_arena(Arena *next_arena); + +private: + Arena *m_first; // head of the arena linked list. 0 if no arenas yet + Arena *m_last; // arena where blocks are currently being allocated + + // used for iterator + Arena *m_current_arena; + sizeint m_current_ofs; +}; + + +#endif diff --git a/libs/ode-0.16.1/ode/src/ode.cpp b/libs/ode-0.16.1/ode/src/ode.cpp new file mode 100644 index 0000000..40bfd6a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/ode.cpp @@ -0,0 +1,2325 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +// this source file is mostly concerned with the data structures, not the +// numerics. + +#include <ode/ode.h> +#include <ode/memory.h> +#include <ode/error.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "objects.h" +#include "joints/joints.h" +#include "step.h" +#include "quickstep.h" +#include "util.h" +#include "odetls.h" + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// utility + + +// add an object `obj' to the list who's head pointer is pointed to by `first'. + +void addObjectToList (dObject *obj, dObject **first) +{ + obj->next = *first; + obj->tome = first; + if (*first) (*first)->tome = &obj->next; + (*first) = obj; +} + + +// remove the object from the linked list + +static inline void removeObjectFromList (dObject *obj) +{ + if (obj->next) obj->next->tome = obj->tome; + *(obj->tome) = obj->next; + // safeguard + obj->next = NULL; + obj->tome = NULL; +} + + +// remove the joint from neighbour lists of all connected bodies + +static void removeJointReferencesFromAttachedBodies (dxJoint *j) +{ + for (int i=0; i<2; i++) { + dxBody *body = j->node[i].body; + if (body) { + dxJointNode *n = body->firstjoint; + dxJointNode *last = NULL; + while (n) { + if (n->joint == j) { + if (last) last->next = n->next; + else body->firstjoint = n->next; + break; + } + last = n; + n = n->next; + } + } + } + j->node[0].body = NULL; + j->node[0].next = NULL; + j->node[1].body = NULL; + j->node[1].next = NULL; +} + +//**************************************************************************** +// debugging + +// see if an object list loops on itself (if so, it's bad). + +static int listHasLoops (dObject *first) +{ + if (first==0 || first->next==0) return 0; + dObject *a=first,*b=first->next; + int skip=0; + while (b) { + if (a==b) return 1; + b = b->next; + if (skip) a = a->next; + skip ^= 1; + } + return 0; +} + + +// check the validity of the world data structures + +static int g_world_check_tag_generator = 0; + +static inline int generateWorldCheckTag() +{ + // Atomicity is not necessary here + return ++g_world_check_tag_generator; +} + +static void checkWorld (dxWorld *w) +{ + dxBody *b; + dxJoint *j; + + // check there are no loops + if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops"); + if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops"); + + // check lists are well formed (check `tome' pointers) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + if (b->next && b->next->tome != &b->next) + dDebug (0,"bad tome pointer in body list"); + } + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->next && j->next->tome != &j->next) + dDebug (0,"bad tome pointer in joint list"); + } + + // check counts + int n = 0; + for (b=w->firstbody; b; b=(dxBody*)b->next) n++; + if (w->nb != n) dDebug (0,"body count incorrect"); + n = 0; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++; + if (w->nj != n) dDebug (0,"joint count incorrect"); + + // set all tag values to a known value + int count = generateWorldCheckTag(); + for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count; + + // check all body/joint world pointers are ok + for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w) + dDebug (0,"bad world pointer in body list"); + for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w) + dDebug (0,"bad world pointer in joint list"); + + /* + // check for half-connected joints - actually now these are valid + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body || j->node[1].body) { + if (!(j->node[0].body && j->node[1].body)) + dDebug (0,"half connected joint found"); + } + } + */ + + // check that every joint node appears in the joint lists of both bodies it + // attaches + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + for (int i=0; i<2; i++) { + if (j->node[i].body) { + int ok = 0; + for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) { + if (n->joint == j) ok = 1; + } + if (ok==0) dDebug (0,"joint not in joint list of attached body"); + } + } + } + + // check all body joint lists (correct body ptrs) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (&n->joint->node[0] == n) { + if (n->joint->node[1].body != b) + dDebug (0,"bad body pointer in joint node of body list (1)"); + } + else { + if (n->joint->node[0].body != b) + dDebug (0,"bad body pointer in joint node of body list (2)"); + } + if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body"); + } + } + + // check all body pointers in joints, check they are distinct + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body && (j->node[0].body == j->node[1].body)) + dDebug (0,"non-distinct body pointers in joint"); + if ((j->node[0].body && j->node[0].body->tag != count) || + (j->node[1].body && j->node[1].body->tag != count)) + dDebug (0,"bad body pointer in joint"); + } +} + + +void dWorldCheck (dxWorld *w) +{ + checkWorld (w); +} + +//**************************************************************************** +// body + +dxBody::dxBody(dxWorld *w) : +dObject(w) +{ + +} + + +dxWorld* dBodyGetWorld (dxBody * b) +{ + dAASSERT (b); + return b->world; +} + +dxBody *dBodyCreate (dxWorld *w) +{ + dAASSERT (w); + dxBody *b = new dxBody(w); + b->firstjoint = NULL; + b->flags = 0; + b->geom = NULL; + b->average_lvel_buffer = NULL; + b->average_avel_buffer = NULL; + dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0); + dSetZero (b->invI,4*3); + b->invI[0] = 1; + b->invI[5] = 1; + b->invI[10] = 1; + b->invMass = 1; + dSetZero (b->posr.pos,4); + dSetZero (b->q,4); + b->q[0] = 1; + dRSetIdentity (b->posr.R); + dSetZero (b->lvel,4); + dSetZero (b->avel,4); + dSetZero (b->facc,4); + dSetZero (b->tacc,4); + dSetZero (b->finite_rot_axis,4); + addObjectToList (b,(dObject **) &w->firstbody); + w->nb++; + + // set auto-disable parameters + b->average_avel_buffer = b->average_lvel_buffer = NULL; // no buffer at beginning + dBodySetAutoDisableDefaults (b); // must do this after adding to world + b->adis_stepsleft = b->adis.idle_steps; + b->adis_timeleft = b->adis.idle_time; + b->average_counter = 0; + b->average_ready = 0; // average buffer not filled on the beginning + dBodySetAutoDisableAverageSamplesCount(b, b->adis.average_samples); + + b->moved_callback = NULL; + + dBodySetDampingDefaults(b); // must do this after adding to world + + b->flags |= w->body_flags & dxBodyMaxAngularSpeed; + b->max_angular_speed = w->max_angular_speed; + + b->flags |= dxBodyGyroscopic; + + return b; +} + + +void dBodyDestroy (dxBody *b) +{ + dAASSERT (b); + + // all geoms that link to this body must be notified that the body is about + // to disappear. note that the call to dGeomSetBody(geom,0) will result in + // dGeomGetBodyNext() returning 0 for the body, so we must get the next body + // before setting the body to 0. + dxGeom *next_geom = NULL; + for (dxGeom *geom = b->geom; geom; geom = next_geom) { + next_geom = dGeomGetBodyNext (geom); + dGeomSetBody (geom,0); + } + + // detach all neighbouring joints, then delete this body. + dxJointNode *n = b->firstjoint; + while (n) { + // sneaky trick to speed up removal of joint references (black magic) + n->joint->node[(n == n->joint->node)].body = NULL; + + dxJointNode *next = n->next; + n->next = NULL; + removeJointReferencesFromAttachedBodies (n->joint); + n = next; + } + removeObjectFromList (b); + b->world->nb--; + + // delete the average buffers + if(b->average_lvel_buffer) + { + delete[] (b->average_lvel_buffer); + b->average_lvel_buffer = NULL; + } + if(b->average_avel_buffer) + { + delete[] (b->average_avel_buffer); + b->average_avel_buffer = NULL; + } + + delete b; +} + + +void dBodySetData (dBodyID b, void *data) +{ + dAASSERT (b); + b->userdata = data; +} + + +void *dBodyGetData (dBodyID b) +{ + dAASSERT (b); + return b->userdata; +} + + +void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->posr.pos[0] = x; + b->posr.pos[1] = y; + b->posr.pos[2] = z; + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetRotation (dBodyID b, const dMatrix3 R) +{ + dAASSERT (b && R); + + memcpy(b->posr.R, R, sizeof(dMatrix3)); + + bool bOrthogonalizeResult = dxOrthogonalizeR(b->posr.R); + dAVERIFY(bOrthogonalizeResult); + + dRtoQ (R, b->q); + dNormalize4 (b->q); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) { + dGeomMoved (geom); + } +} + + +void dBodySetQuaternion (dBodyID b, const dQuaternion q) +{ + dAASSERT (b && q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dNormalize4 (b->q); + dQtoR (b->q,b->posr.R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->lvel[0] = x; + b->lvel[1] = y; + b->lvel[2] = z; +} + + +void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->avel[0] = x; + b->avel[1] = y; + b->avel[2] = z; +} + + +const dReal * dBodyGetPosition (dBodyID b) +{ + dAASSERT (b); + return b->posr.pos; +} + + +void dBodyCopyPosition (dBodyID b, dVector3 pos) +{ + dAASSERT (b); + dReal* src = b->posr.pos; + pos[0] = src[0]; + pos[1] = src[1]; + pos[2] = src[2]; +} + + +const dReal * dBodyGetRotation (dBodyID b) +{ + dAASSERT (b); + return b->posr.R; +} + + +void dBodyCopyRotation (dBodyID b, dMatrix3 R) +{ + dAASSERT (b); + const dReal* src = b->posr.R; + R[0] = src[0]; + R[1] = src[1]; + R[2] = src[2]; + R[3] = src[3]; + R[4] = src[4]; + R[5] = src[5]; + R[6] = src[6]; + R[7] = src[7]; + R[8] = src[8]; + R[9] = src[9]; + R[10] = src[10]; + R[11] = src[11]; +} + + +const dReal * dBodyGetQuaternion (dBodyID b) +{ + dAASSERT (b); + return b->q; +} + + +void dBodyCopyQuaternion (dBodyID b, dQuaternion quat) +{ + dAASSERT (b); + dReal* src = b->q; + quat[0] = src[0]; + quat[1] = src[1]; + quat[2] = src[2]; + quat[3] = src[3]; +} + + +const dReal * dBodyGetLinearVel (dBodyID b) +{ + dAASSERT (b); + return b->lvel; +} + + +const dReal * dBodyGetAngularVel (dBodyID b) +{ + dAASSERT (b); + return b->avel; +} + + +void dBodySetMass (dBodyID b, const dMass *mass) +{ + dAASSERT (b && mass ); + dIASSERT(dMassCheck(mass)); + + // The centre of mass must be at the origin. + // Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it. + dUASSERT( fabs( mass->c[0] ) <= dEpsilon && + fabs( mass->c[1] ) <= dEpsilon && + fabs( mass->c[2] ) <= dEpsilon, "The centre of mass must be at the origin." ); + + b->mass = *mass; + if (dInvertPDMatrix (b->mass.I,b->invI,3,NULL)==0) { + dDEBUGMSG ("inertia must be positive definite!"); + dRSetIdentity (b->invI); + } + b->invMass = dRecip(b->mass.mass); +} + + +void dBodyGetMass (dBodyID b, dMass *mass) +{ + dAASSERT (b && mass); + *mass = b->mass; +} + + +void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; +} + + +void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->tacc[0] += fx; + b->tacc[1] += fy; + b->tacc[2] += fz; +} + + +void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMultiply0_331 (t2,b->posr.R,t1); + b->facc[0] += t2[0]; + b->facc[1] += t2[1]; + b->facc[2] += t2[2]; +} + + +void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMultiply0_331 (t2,b->posr.R,t1); + b->tacc[0] += t2[0]; + b->tacc[1] += t2[1]; + b->tacc[2] += t2[2]; +} + + +void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; + dVector3 f,q; + f[0] = fx; + f[1] = fy; + f[2] = fz; + q[0] = px - b->posr.pos[0]; + q[1] = py - b->posr.pos[1]; + q[2] = pz - b->posr.pos[2]; + dAddVectorCross3(b->tacc,q,f); +} + + +void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 prel,f,p; + f[0] = fx; + f[1] = fy; + f[2] = fz; + f[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMultiply0_331 (p,b->posr.R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dAddVectorCross3(b->tacc,p,f); +} + + +void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,f; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + dMultiply0_331 (f,b->posr.R,frel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dVector3 q; + q[0] = px - b->posr.pos[0]; + q[1] = py - b->posr.pos[1]; + q[2] = pz - b->posr.pos[2]; + dAddVectorCross3(b->tacc,q,f); +} + + +void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,prel,f,p; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMultiply0_331 (f,b->posr.R,frel); + dMultiply0_331 (p,b->posr.R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dAddVectorCross3(b->tacc,p,f); +} + + +const dReal * dBodyGetForce (dBodyID b) +{ + dAASSERT (b); + return b->facc; +} + + +const dReal * dBodyGetTorque (dBodyID b) +{ + dAASSERT (b); + return b->tacc; +} + + +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->facc[0] = x; + b->facc[1] = y; + b->facc[2] = z; +} + + +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->tacc[0] = x; + b->tacc[1] = y; + b->tacc[2] = z; +} + + +void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMultiply0_331 (p,b->posr.R,prel); + result[0] = p[0] + b->posr.pos[0]; + result[1] = p[1] + b->posr.pos[1]; + result[2] = p[2] + b->posr.pos[2]; +} + + +void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMultiply0_331 (p,b->posr.R,prel); + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dAddVectorCross3(result,b->avel,p); +} + + +void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px - b->posr.pos[0]; + p[1] = py - b->posr.pos[1]; + p[2] = pz - b->posr.pos[2]; + p[3] = 0; + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dAddVectorCross3(result,b->avel,p); +} + + +void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel; + prel[0] = px - b->posr.pos[0]; + prel[1] = py - b->posr.pos[1]; + prel[2] = pz - b->posr.pos[2]; + prel[3] = 0; + dMultiply1_331 (result,b->posr.R,prel); +} + + +void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMultiply0_331 (result,b->posr.R,p); +} + + +void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMultiply1_331 (result,b->posr.R,p); +} + + +void dBodySetFiniteRotationMode (dBodyID b, int mode) +{ + dAASSERT (b); + b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis); + if (mode) { + b->flags |= dxBodyFlagFiniteRotation; + if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 || + b->finite_rot_axis[2] != 0) { + b->flags |= dxBodyFlagFiniteRotationAxis; + } + } +} + + +void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->finite_rot_axis[0] = x; + b->finite_rot_axis[1] = y; + b->finite_rot_axis[2] = z; + if (x != 0 || y != 0 || z != 0) { + dNormalize3 (b->finite_rot_axis); + b->flags |= dxBodyFlagFiniteRotationAxis; + } + else { + b->flags &= ~dxBodyFlagFiniteRotationAxis; + } +} + + +int dBodyGetFiniteRotationMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyFlagFiniteRotation) != 0); +} + + +void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result) +{ + dAASSERT (b); + result[0] = b->finite_rot_axis[0]; + result[1] = b->finite_rot_axis[1]; + result[2] = b->finite_rot_axis[2]; +} + + +int dBodyGetNumJoints (dBodyID b) +{ + dAASSERT (b); + int count=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, count++); + return count; +} + + +dJointID dBodyGetJoint (dBodyID b, int index) +{ + dAASSERT (b); + int i=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) { + if (i == index) return n->joint; + } + return 0; +} + +void dBodySetDynamic (dBodyID b) +{ + dAASSERT (b); + + dBodySetMass(b,&b->mass); +} + +void dBodySetKinematic (dBodyID b) +{ + dAASSERT (b); + dSetZero (b->invI,4*3); + b->invMass = 0; +} + +int dBodyIsKinematic (dBodyID b) +{ + dAASSERT (b); + return b->invMass == 0; +} + +void dBodyEnable (dBodyID b) +{ + dAASSERT (b); + b->flags &= ~dxBodyDisabled; + b->adis_stepsleft = b->adis.idle_steps; + b->adis_timeleft = b->adis.idle_time; + // no code for average-processing needed here +} + + +void dBodyDisable (dBodyID b) +{ + dAASSERT (b); + b->flags |= dxBodyDisabled; +} + + +int dBodyIsEnabled (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyDisabled) == 0); +} + + +void dBodySetGravityMode (dBodyID b, int mode) +{ + dAASSERT (b); + if (mode) b->flags &= ~dxBodyNoGravity; + else b->flags |= dxBodyNoGravity; +} + + +int dBodyGetGravityMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyNoGravity) == 0); +} + + +// body auto-disable functions + +dReal dBodyGetAutoDisableLinearThreshold (dBodyID b) +{ + dAASSERT(b); + return dSqrt (b->adis.linear_average_threshold); +} + + +void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_average_threshold) +{ + dAASSERT(b); + b->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold; +} + + +dReal dBodyGetAutoDisableAngularThreshold (dBodyID b) +{ + dAASSERT(b); + return dSqrt (b->adis.angular_average_threshold); +} + + +void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_average_threshold) +{ + dAASSERT(b); + b->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold; +} + + +int dBodyGetAutoDisableAverageSamplesCount (dBodyID b) +{ + dAASSERT(b); + return b->adis.average_samples; +} + + +void dBodySetAutoDisableAverageSamplesCount (dBodyID b, unsigned int average_samples_count) +{ + dAASSERT(b); + b->adis.average_samples = average_samples_count; + // update the average buffers + if(b->average_lvel_buffer) + { + delete[] b->average_lvel_buffer; + b->average_lvel_buffer = NULL; + } + if(b->average_avel_buffer) + { + delete[] b->average_avel_buffer; + b->average_avel_buffer = NULL; + } + if(b->adis.average_samples > 0) + { + b->average_lvel_buffer = new dVector3[b->adis.average_samples]; + b->average_avel_buffer = new dVector3[b->adis.average_samples]; + } + else + { + b->average_lvel_buffer = NULL; + b->average_avel_buffer = NULL; + } + // new buffer is empty + b->average_counter = 0; + b->average_ready = 0; +} + + +int dBodyGetAutoDisableSteps (dBodyID b) +{ + dAASSERT(b); + return b->adis.idle_steps; +} + + +void dBodySetAutoDisableSteps (dBodyID b, int steps) +{ + dAASSERT(b); + b->adis.idle_steps = steps; +} + + +dReal dBodyGetAutoDisableTime (dBodyID b) +{ + dAASSERT(b); + return b->adis.idle_time; +} + + +void dBodySetAutoDisableTime (dBodyID b, dReal time) +{ + dAASSERT(b); + b->adis.idle_time = time; +} + + +int dBodyGetAutoDisableFlag (dBodyID b) +{ + dAASSERT(b); + return ((b->flags & dxBodyAutoDisable) != 0); +} + + +void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable) +{ + dAASSERT(b); + if (!do_auto_disable) + { + b->flags &= ~dxBodyAutoDisable; + // (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag + b->flags &= ~dxBodyDisabled; + b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world); + b->adis.idle_time = dWorldGetAutoDisableTime(b->world); + // resetting the average calculations too + dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) ); + } + else + { + b->flags |= dxBodyAutoDisable; + } +} + + +void dBodySetAutoDisableDefaults (dBodyID b) +{ + dAASSERT(b); + dWorldID w = b->world; + dAASSERT(w); + b->adis = w->adis; + dBodySetAutoDisableFlag (b, w->body_flags & dxBodyAutoDisable); +} + + +// body damping functions + +dReal dBodyGetLinearDamping(dBodyID b) +{ + dAASSERT(b); + return b->dampingp.linear_scale; +} + +void dBodySetLinearDamping(dBodyID b, dReal scale) +{ + dAASSERT(b); + if (scale) + b->flags |= dxBodyLinearDamping; + else + b->flags &= ~dxBodyLinearDamping; + b->dampingp.linear_scale = scale; +} + +dReal dBodyGetAngularDamping(dBodyID b) +{ + dAASSERT(b); + return b->dampingp.angular_scale; +} + +void dBodySetAngularDamping(dBodyID b, dReal scale) +{ + dAASSERT(b); + if (scale) + b->flags |= dxBodyAngularDamping; + else + b->flags &= ~dxBodyAngularDamping; + b->dampingp.angular_scale = scale; +} + +void dBodySetDamping(dBodyID b, dReal linear_scale, dReal angular_scale) +{ + dAASSERT(b); + dBodySetLinearDamping(b, linear_scale); + dBodySetAngularDamping(b, angular_scale); +} + +dReal dBodyGetLinearDampingThreshold(dBodyID b) +{ + dAASSERT(b); + return dSqrt(b->dampingp.linear_threshold); +} + +void dBodySetLinearDampingThreshold(dBodyID b, dReal threshold) +{ + dAASSERT(b); + b->dampingp.linear_threshold = threshold*threshold; +} + + +dReal dBodyGetAngularDampingThreshold(dBodyID b) +{ + dAASSERT(b); + return dSqrt(b->dampingp.angular_threshold); +} + +void dBodySetAngularDampingThreshold(dBodyID b, dReal threshold) +{ + dAASSERT(b); + b->dampingp.angular_threshold = threshold*threshold; +} + +void dBodySetDampingDefaults(dBodyID b) +{ + dAASSERT(b); + dWorldID w = b->world; + dAASSERT(w); + b->dampingp = w->dampingp; + const unsigned mask = dxBodyLinearDamping | dxBodyAngularDamping; + b->flags &= ~mask; // zero them + b->flags |= w->body_flags & mask; +} + +dReal dBodyGetMaxAngularSpeed(dBodyID b) +{ + dAASSERT(b); + return b->max_angular_speed; +} + +void dBodySetMaxAngularSpeed(dBodyID b, dReal max_speed) +{ + dAASSERT(b); + if (max_speed < dInfinity) + b->flags |= dxBodyMaxAngularSpeed; + else + b->flags &= ~dxBodyMaxAngularSpeed; + b->max_angular_speed = max_speed; +} + +void dBodySetMovedCallback(dBodyID b, void (*callback)(dBodyID)) +{ + dAASSERT(b); + b->moved_callback = callback; +} + + +dGeomID dBodyGetFirstGeom(dBodyID b) +{ + dAASSERT(b); + return b->geom; +} + + +dGeomID dBodyGetNextGeom(dGeomID geom) +{ + dAASSERT(geom); + return dGeomGetBodyNext(geom); +} + + +int dBodyGetGyroscopicMode(dBodyID b) +{ + dAASSERT(b); + return b->flags & dxBodyGyroscopic; +} + +void dBodySetGyroscopicMode(dBodyID b, int enabled) +{ + dAASSERT(b); + if (enabled) + b->flags |= dxBodyGyroscopic; + else + b->flags &= ~dxBodyGyroscopic; +} + + + +//**************************************************************************** +// joints + + + +template<class T> +dxJoint* createJoint(dWorldID w, dJointGroupID group) +{ + dxJoint *j; + if (group) { + j = group->alloc<T>(w); + } else { + j = new T(w); + } + return j; +} + + +dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointBall>(w,group); +} + + +dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointHinge>(w,group); +} + + +dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointSlider>(w,group); +} + + +dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group, + const dContact *c) +{ + dAASSERT (w && c); + dxJointContact *j = (dxJointContact *) + createJoint<dxJointContact> (w,group); + j->contact = *c; + return j; +} + + +dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointHinge2> (w,group); +} + + +dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointUniversal> (w,group); +} + +dxJoint * dJointCreatePR (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointPR> (w,group); +} + +dxJoint * dJointCreatePU (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointPU> (w,group); +} + +dxJoint * dJointCreatePiston (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointPiston> (w,group); +} + +dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointFixed> (w,group); +} + + +dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointNull> (w,group); +} + + +dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointAMotor> (w,group); +} + +dxJoint * dJointCreateLMotor (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointLMotor> (w,group); +} + +dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointPlane2D> (w,group); +} + +dxJoint * dJointCreateDBall (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointDBall> (w,group); +} + +dxJoint * dJointCreateDHinge (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointDHinge> (w,group); +} + + +dxJoint * dJointCreateTransmission (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint<dxJointTransmission> (w,group); +} + +static void FinalizeAndDestroyJointInstance(dxJoint *j, bool delete_it) +{ + // if any group joints have their world pointer set to 0, their world was + // previously destroyed. no special handling is required for these joints. + if (j->world != NULL) { + removeJointReferencesFromAttachedBodies (j); + removeObjectFromList (j); + j->world->nj--; + } + if (delete_it) { + delete j; + } else { + j->~dxJoint(); + } +} + +void dJointDestroy (dxJoint *j) +{ + dAASSERT (j); + if (!(j->flags & dJOINT_INGROUP)) { + FinalizeAndDestroyJointInstance(j, true); + } +} + + +dJointGroupID dJointGroupCreate (int /*max_size*/) +{ + // not any more ... dUASSERT (max_size > 0,"max size must be > 0"); + dxJointGroup *group = new dxJointGroup(); + return group; +} + + +void dJointGroupDestroy (dJointGroupID group) +{ + dAASSERT (group); + dJointGroupEmpty (group); + delete group; +} + +void dJointGroupEmpty (dJointGroupID group) +{ + dAASSERT (group); + + const sizeint num_joints = group->getJointCount(); + if (num_joints != 0) { + // Local array is used since ALLOCA leads to mysterious NULL values in first array element and crashes under VS2005 :) + const sizeint max_stack_jlist_size = 1024; + dxJoint *stack_jlist[max_stack_jlist_size]; + + const sizeint jlist_size = num_joints * sizeof(dxJoint*); + dxJoint **jlist = num_joints <= max_stack_jlist_size ? stack_jlist : (dxJoint **)dAlloc(jlist_size); + + if (jlist != NULL) { + // the joints in this group are detached starting from the most recently + // added (at the top of the stack). this helps ensure that the various + // linked lists are not traversed too much, as the joints will hopefully + // be at the start of those lists. + sizeint num_exported = group->exportJoints(jlist); + dIVERIFY(num_exported == num_joints); + + for (sizeint i = num_joints; i != 0; ) { + --i; + dxJoint *j = jlist[i]; + FinalizeAndDestroyJointInstance(j, false); + } + } else { + // ...else if there is no memory, go on detaching the way it is possible + sizeint joint_bytes; + for (dxJoint *j = (dxJoint *)group->beginEnum(); j != NULL; j = (dxJoint *)group->continueEnum(joint_bytes)) { + joint_bytes = j->size(); // Get size before object is destroyed! + FinalizeAndDestroyJointInstance(j, false); + } + } + + group->freeAll(); + + if (jlist != stack_jlist && jlist != NULL) { + dFree(jlist, jlist_size); + } + } +} + + +int dJointGetNumBodies(dxJoint *joint) +{ + // check arguments + dUASSERT (joint,"bad joint argument"); + + if ( !joint->node[0].body ) + return 0; + else if ( !joint->node[1].body ) + return 1; + else + return 2; +} + + +void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2) +{ + // check arguments + dUASSERT (joint,"bad joint argument"); + dUASSERT (body1 == NULL || body1 != body2, "can't have body1==body2"); + dxWorld *world = joint->world; + dUASSERT ( (body1 == NULL || body1->world == world) && + (body2 == NULL || body2->world == world), + "joint and bodies must be in same world"); + + // check if the joint can not be attached to just one body + dUASSERT (!((joint->flags & dJOINT_TWOBODIES) && + ((body1 != NULL) != (body2 != NULL))), + "joint can not be attached to just one body"); + + // remove any existing body attachments + if (joint->node[0].body != NULL || joint->node[1].body != NULL) { + removeJointReferencesFromAttachedBodies (joint); + } + + // if a body is zero, make sure that it is body2, so 0 --> node[1].body + if (body1 == NULL) { + body1 = body2; + body2 = NULL; + joint->flags |= dJOINT_REVERSE; + } + else { + joint->flags &= (~dJOINT_REVERSE); + } + + // attach to new bodies + joint->node[0].body = body1; + joint->node[1].body = body2; + + if (body1 != NULL) { + joint->node[1].next = body1->firstjoint; + body1->firstjoint = &joint->node[1]; + } + else { + joint->node[1].next = NULL; + } + + if (body2 != NULL) { + joint->node[0].next = body2->firstjoint; + body2->firstjoint = &joint->node[0]; + } + else { + joint->node[0].next = NULL; + } + + // Since the bodies are now set. + // Calculate the values depending on the bodies. + // Only need to calculate relative value if a body exist + if (body1 != NULL || body2 != NULL) { + joint->setRelativeValues(); + } +} + +void dJointEnable (dxJoint *joint) +{ + dAASSERT (joint); + joint->flags &= ~dJOINT_DISABLED; +} + +void dJointDisable (dxJoint *joint) +{ + dAASSERT (joint); + joint->flags |= dJOINT_DISABLED; +} + +int dJointIsEnabled (dxJoint *joint) +{ + dAASSERT (joint); + return (joint->flags & dJOINT_DISABLED) == 0; +} + +void dJointSetData (dxJoint *joint, void *data) +{ + dAASSERT (joint); + joint->userdata = data; +} + + +void *dJointGetData (dxJoint *joint) +{ + dAASSERT (joint); + return joint->userdata; +} + + +dJointType dJointGetType (dxJoint *joint) +{ + dAASSERT (joint); + return joint->type(); +} + + +dBodyID dJointGetBody (dxJoint *joint, int index) +{ + dAASSERT (joint); + if (index == 0 || index == 1) { + if (joint->flags & dJOINT_REVERSE) return joint->node[1-index].body; + else return joint->node[index].body; + } + else return 0; +} + + +void dJointSetFeedback (dxJoint *joint, dJointFeedback *f) +{ + dAASSERT (joint); + joint->feedback = f; +} + + +dJointFeedback *dJointGetFeedback (dxJoint *joint) +{ + dAASSERT (joint); + return joint->feedback; +} + + + +dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2) +{ + dAASSERT (in_b1 || in_b2); + + dBodyID b1, b2; + + if (in_b1 == 0) { + b1 = in_b2; + b2 = in_b1; + } + else { + b1 = in_b1; + b2 = in_b2; + } + + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return n->joint; + } + + return 0; +} + + + +int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list) +{ + dAASSERT (in_b1 || in_b2); + + + dBodyID b1, b2; + + if (in_b1 == 0) { + b1 = in_b2; + b2 = in_b1; + } + else { + b1 = in_b1; + b2 = in_b2; + } + + // look through b1's neighbour list for b2 + int numConnectingJoints = 0; + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) + out_list[numConnectingJoints++] = n->joint; + } + + return numConnectingJoints; +} + + +int dAreConnected (dBodyID b1, dBodyID b2) +{ + dAASSERT (b1/* && b2*/); // b2 can be NULL to test for connection to environment + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return 1; + } + return 0; +} + + +int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type) +{ + dAASSERT (b1/* && b2*/); // b2 can be NULL to test for connection to environment + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1; + } + return 0; +} + +//**************************************************************************** +// world + +dxWorld * dWorldCreate() +{ + dxWorld *w = new dxWorld(); + + return w; +} + + +void dWorldDestroy (dxWorld *w) +{ + // delete all bodies and joints + dAASSERT (w); + dxBody *nextb, *b = w->firstbody; + while (b) { + nextb = (dxBody*) b->next; + dBodyDestroy(b); // calling here dBodyDestroy for correct destroying! (i.e. the average buffers) + b = nextb; + } + + dxJoint *nextj, *j = w->firstjoint; + while (j) { + nextj = (dxJoint*)j->next; + if (j->flags & dJOINT_INGROUP) { + // the joint is part of a group, so "deactivate" it instead + j->world = NULL; + j->node[0].body = NULL; + j->node[0].next = NULL; + j->node[1].body = NULL; + j->node[1].next = NULL; + dMessage (0,"warning: destroying world containing grouped joints"); + } + else { + // TODO: shouldn't we call dJointDestroy()? + sizeint sz = j->size(); + j->~dxJoint(); + dFree (j,sz); + } + j = nextj; + } + + delete w; +} + + +void dWorldSetData (dWorldID w, void *data) +{ + dAASSERT (w); + w->userdata = data; +} + + +void* dWorldGetData (dWorldID w) +{ + dAASSERT (w); + return w->userdata; +} + + +void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z) +{ + dAASSERT (w); + w->gravity[0] = x; + w->gravity[1] = y; + w->gravity[2] = z; +} + + +void dWorldGetGravity (dWorldID w, dVector3 g) +{ + dAASSERT (w); + g[0] = w->gravity[0]; + g[1] = w->gravity[1]; + g[2] = w->gravity[2]; +} + + +void dWorldSetERP (dWorldID w, dReal erp) +{ + dAASSERT (w); + w->global_erp = erp; +} + + +dReal dWorldGetERP (dWorldID w) +{ + dAASSERT (w); + return w->global_erp; +} + + +void dWorldSetCFM (dWorldID w, dReal cfm) +{ + dAASSERT (w); + w->global_cfm = cfm; +} + + +dReal dWorldGetCFM (dWorldID w) +{ + dAASSERT (w); + return w->global_cfm; +} + + +void dWorldSetStepIslandsProcessingMaxThreadCount(dWorldID w, unsigned count) +{ + dAASSERT (w); + w->islands_max_threads = count; +} + +unsigned dWorldGetStepIslandsProcessingMaxThreadCount(dWorldID w) +{ + dAASSERT (w); + return w->islands_max_threads; +} + +int dWorldUseSharedWorkingMemory(dWorldID w, dWorldID from_world) +{ + dUASSERT (w,"bad world argument"); + + bool result = false; + + if (from_world) + { + dUASSERT (!w->wmem, "world does already have working memory allocated"); // Prevent replacement of one memory object with another to avoid cases when smaller buffer replaces a larger one or memory manager changes. + + dxStepWorkingMemory *wmem = AllocateOnDemand(from_world->wmem); + + if (wmem) + { + // Even though there is an assertion check on entry still release existing + // memory object for extra safety. + if (w->wmem) + { + w->wmem->Release(); + w->wmem = NULL; + } + + wmem->Addref(); + w->wmem = wmem; + + result = true; + } + } + else + { + dxStepWorkingMemory *wmem = w->wmem; + + if (wmem) + { + wmem->Release(); + w->wmem = NULL; + } + + result = true; + } + + return result; +} + +void dWorldCleanupWorkingMemory(dWorldID w) +{ + dUASSERT (w,"bad world argument"); + + dxStepWorkingMemory *wmem = w->wmem; + + if (wmem) + { + wmem->CleanupMemory(); + } +} + +int dWorldSetStepMemoryReservationPolicy(dWorldID w, const dWorldStepReserveInfo *policyinfo) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (!policyinfo || (policyinfo->struct_size >= sizeof(*policyinfo) && policyinfo->reserve_factor >= 1.0f), "Bad policy info"); + + bool result = false; + + dxStepWorkingMemory *wmem = policyinfo ? AllocateOnDemand(w->wmem) : w->wmem; + + if (wmem) + { + if (policyinfo) + { + wmem->SetMemoryReserveInfo(policyinfo->reserve_factor, policyinfo->reserve_minimum); + result = wmem->GetMemoryReserveInfo() != NULL; + } + else + { + wmem->ResetMemoryReserveInfoToDefault(); + result = true; + } + } + else if (!policyinfo) + { + result = true; + } + + return result; +} + +int dWorldSetStepMemoryManager(dWorldID w, const dWorldStepMemoryFunctionsInfo *memfuncs) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (!memfuncs || memfuncs->struct_size >= sizeof(*memfuncs), "Bad memory functions info"); + + bool result = false; + + dxStepWorkingMemory *wmem = memfuncs ? AllocateOnDemand(w->wmem) : w->wmem; + + if (wmem) + { + if (memfuncs) + { + wmem->SetMemoryManager(memfuncs->alloc_block, memfuncs->shrink_block, memfuncs->free_block); + result = wmem->GetMemoryManager() != NULL; + } + else + { + wmem->ResetMemoryManagerToDefault(); + result = true; + } + } + else if (!memfuncs) + { + result = true; + } + + return result; +} + +void dWorldSetStepThreadingImplementation(dWorldID w, + const dxThreadingFunctionsInfo *functions_info, dThreadingImplementationID threading_impl) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (!functions_info || functions_info->struct_size >= sizeof(*functions_info), "Bad threading functions info"); + +#if dTHREADING_INTF_DISABLED + dUASSERT(functions_info == NULL && threading_impl == NULL, "Threading interface is not available"); +#else + w->assignThreadingImpl(functions_info, threading_impl); +#endif +} + + +int dWorldStep (dWorldID w, dReal stepsize) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + + bool result = false; + + dxWorldProcessIslandsInfo islandsinfo; + if (dxReallocateWorldProcessContext (w, islandsinfo, stepsize, &dxEstimateStepMemoryRequirements)) + { + if (dxProcessIslands (w, islandsinfo, stepsize, &dxStepIsland, &dxEstimateStepMaxCallCount)) + { + result = true; + } + } + + return result; +} + +int dWorldQuickStep (dWorldID w, dReal stepsize) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + + bool result = false; + + dxWorldProcessIslandsInfo islandsinfo; + if (dxReallocateWorldProcessContext (w, islandsinfo, stepsize, &dxEstimateQuickStepMemoryRequirements)) + { + if (dxProcessIslands (w, islandsinfo, stepsize, &dxQuickStepIsland, &dxEstimateQuickStepMaxCallCount)) + { + result = true; + } + } + + return result; +} + + +void dWorldImpulseToForce (dWorldID w, dReal stepsize, + dReal ix, dReal iy, dReal iz, + dVector3 force) +{ + dAASSERT (w); + stepsize = dRecip(stepsize); + force[0] = stepsize * ix; + force[1] = stepsize * iy; + force[2] = stepsize * iz; + // @@@ force[3] = 0; +} + + +// world auto-disable functions + +dReal dWorldGetAutoDisableLinearThreshold (dWorldID w) +{ + dAASSERT(w); + return dSqrt (w->adis.linear_average_threshold); +} + + +void dWorldSetAutoDisableLinearThreshold (dWorldID w, dReal linear_average_threshold) +{ + dAASSERT(w); + w->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold; +} + + +dReal dWorldGetAutoDisableAngularThreshold (dWorldID w) +{ + dAASSERT(w); + return dSqrt (w->adis.angular_average_threshold); +} + + +void dWorldSetAutoDisableAngularThreshold (dWorldID w, dReal angular_average_threshold) +{ + dAASSERT(w); + w->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold; +} + + +int dWorldGetAutoDisableAverageSamplesCount (dWorldID w) +{ + dAASSERT(w); + return w->adis.average_samples; +} + + +void dWorldSetAutoDisableAverageSamplesCount (dWorldID w, unsigned int average_samples_count) +{ + dAASSERT(w); + w->adis.average_samples = average_samples_count; +} + + +int dWorldGetAutoDisableSteps (dWorldID w) +{ + dAASSERT(w); + return w->adis.idle_steps; +} + + +void dWorldSetAutoDisableSteps (dWorldID w, int steps) +{ + dAASSERT(w); + w->adis.idle_steps = steps; +} + + +dReal dWorldGetAutoDisableTime (dWorldID w) +{ + dAASSERT(w); + return w->adis.idle_time; +} + + +void dWorldSetAutoDisableTime (dWorldID w, dReal time) +{ + dAASSERT(w); + w->adis.idle_time = time; +} + + +int dWorldGetAutoDisableFlag (dWorldID w) +{ + dAASSERT(w); + return w->body_flags & dxBodyAutoDisable; +} + + +void dWorldSetAutoDisableFlag (dWorldID w, int do_auto_disable) +{ + dAASSERT(w); + if (do_auto_disable) + w->body_flags |= dxBodyAutoDisable; + else + w->body_flags &= ~dxBodyAutoDisable; +} + + +// world damping functions + +dReal dWorldGetLinearDampingThreshold(dWorldID w) +{ + dAASSERT(w); + return dSqrt(w->dampingp.linear_threshold); +} + +void dWorldSetLinearDampingThreshold(dWorldID w, dReal threshold) +{ + dAASSERT(w); + w->dampingp.linear_threshold = threshold*threshold; +} + +dReal dWorldGetAngularDampingThreshold(dWorldID w) +{ + dAASSERT(w); + return dSqrt(w->dampingp.angular_threshold); +} + +void dWorldSetAngularDampingThreshold(dWorldID w, dReal threshold) +{ + dAASSERT(w); + w->dampingp.angular_threshold = threshold*threshold; +} + +dReal dWorldGetLinearDamping(dWorldID w) +{ + dAASSERT(w); + return w->dampingp.linear_scale; +} + +void dWorldSetLinearDamping(dWorldID w, dReal scale) +{ + dAASSERT(w); + if (scale) + w->body_flags |= dxBodyLinearDamping; + else + w->body_flags &= ~dxBodyLinearDamping; + w->dampingp.linear_scale = scale; +} + +dReal dWorldGetAngularDamping(dWorldID w) +{ + dAASSERT(w); + return w->dampingp.angular_scale; +} + +void dWorldSetAngularDamping(dWorldID w, dReal scale) +{ + dAASSERT(w); + if (scale) + w->body_flags |= dxBodyAngularDamping; + else + w->body_flags &= ~dxBodyAngularDamping; + w->dampingp.angular_scale = scale; +} + +void dWorldSetDamping(dWorldID w, dReal linear_scale, dReal angular_scale) +{ + dAASSERT(w); + dWorldSetLinearDamping(w, linear_scale); + dWorldSetAngularDamping(w, angular_scale); +} + +dReal dWorldGetMaxAngularSpeed(dWorldID w) +{ + dAASSERT(w); + return w->max_angular_speed; +} + +void dWorldSetMaxAngularSpeed(dWorldID w, dReal max_speed) +{ + dAASSERT(w); + if (max_speed < dInfinity) + w->body_flags |= dxBodyMaxAngularSpeed; + else + w->body_flags &= ~dxBodyMaxAngularSpeed; + w->max_angular_speed = max_speed; +} + + +void dWorldSetQuickStepNumIterations (dWorldID w, int num) +{ + dAASSERT(w); + w->qs.num_iterations = num; +} + + +int dWorldGetQuickStepNumIterations (dWorldID w) +{ + dAASSERT(w); + return w->qs.num_iterations; +} + + +void dWorldSetQuickStepW (dWorldID w, dReal param) +{ + dAASSERT(w); + w->qs.w = param; +} + + +dReal dWorldGetQuickStepW (dWorldID w) +{ + dAASSERT(w); + return w->qs.w; +} + + +void dWorldSetContactMaxCorrectingVel (dWorldID w, dReal vel) +{ + dAASSERT(w); + w->contactp.max_vel = vel; +} + + +dReal dWorldGetContactMaxCorrectingVel (dWorldID w) +{ + dAASSERT(w); + return w->contactp.max_vel; +} + + +void dWorldSetContactSurfaceLayer (dWorldID w, dReal depth) +{ + dAASSERT(w); + w->contactp.min_depth = depth; +} + + +dReal dWorldGetContactSurfaceLayer (dWorldID w) +{ + dAASSERT(w); + return w->contactp.min_depth; +} + +//**************************************************************************** +// testing + +#define NUM 100 + +#define DO(x) + + +extern "C" void dTestDataStructures() +{ + int i; + DO(printf ("testDynamicsStuff()\n")); + + dBodyID body [NUM]; + int nb = 0; + dJointID joint [NUM]; + int nj = 0; + + for (i=0; i<NUM; i++) body[i] = NULL; + for (i=0; i<NUM; i++) joint[i] = NULL; + + DO(printf ("creating world\n")); + dWorldID w = dWorldCreate(); + checkWorld (w); + + for (;;) { + if (nb < NUM && dRandReal() > 0.5) { + DO(printf ("creating body\n")); + body[nb] = dBodyCreate (w); + DO(printf ("\t--> %p\n",body[nb])); + nb++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj < NUM && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + DO(printf ("creating joint, attaching to %p,%p\n",b1,b2)); + joint[nj] = dJointCreateBall (w,0); + DO(printf ("\t-->%p\n",joint[nj])); + checkWorld (w); + dJointAttach (joint[nj],b1,b2); + nj++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nj > 0 && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + int k = dRand() % nj; + DO(printf ("reattaching joint %p\n",joint[k])); + dJointAttach (joint[k],b1,b2); + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nb > 0 && dRandReal() > 0.5) { + int k = dRand() % nb; + DO(printf ("destroying body %p\n",body[k])); + dBodyDestroy (body[k]); + checkWorld (w); + for (; k < (NUM-1); k++) body[k] = body[k+1]; + nb--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj > 0 && dRandReal() > 0.5) { + int k = dRand() % nj; + DO(printf ("destroying joint %p\n",joint[k])); + dJointDestroy (joint[k]); + checkWorld (w); + for (; k < (NUM-1); k++) joint[k] = joint[k+1]; + nj--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + + /* + printf ("creating world\n"); + dWorldID w = dWorldCreate(); + checkWorld (w); + printf ("creating body\n"); + dBodyID b1 = dBodyCreate (w); + checkWorld (w); + printf ("creating body\n"); + dBodyID b2 = dBodyCreate (w); + checkWorld (w); + printf ("creating joint\n"); + dJointID j = dJointCreateBall (w); + checkWorld (w); + printf ("attaching joint\n"); + dJointAttach (j,b1,b2); + checkWorld (w); + printf ("destroying joint\n"); + dJointDestroy (j); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b1); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b2); + checkWorld (w); + printf ("destroying world\n"); + dWorldDestroy (w); + */ +} + +//**************************************************************************** +// configuration +#if 1 +#define REGISTER_EXTENSION( __a ) #__a " " +#else +#define REGISTER_EXTENSION( __a ) "__a " +#endif +static const char ode_configuration[] = "ODE " + +// EXTENSION LIST BEGIN +//********************************** + +#ifdef dNODEBUG +REGISTER_EXTENSION( ODE_EXT_no_debug ) +#endif // dNODEBUG + +#if dTRIMESH_ENABLED +REGISTER_EXTENSION( ODE_EXT_trimesh ) + +// tri-mesh extensions +#if dTRIMESH_OPCODE +REGISTER_EXTENSION( ODE_EXT_opcode ) + +// opcode extensions +#if dTRIMESH_16BIT_INDICES +REGISTER_EXTENSION( ODE_OPC_16bit_indices ) +#endif + +#if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER +REGISTER_EXTENSION( ODE_OPC_new_collider ) +#endif + +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +REGISTER_EXTENSION( ODE_EXT_gimpact ) + +// gimpact extensions +#endif + +#endif // dTRIMESH_ENABLED + +#if dTLS_ENABLED +REGISTER_EXTENSION( ODE_EXT_mt_collisions ) +#endif // dTLS_ENABLED + +#if !dTHREADING_INTF_DISABLED +REGISTER_EXTENSION( ODE_EXT_threading ) + +#if dBUILTIN_THREADING_IMPL_ENABLED +REGISTER_EXTENSION( ODE_THR_builtin_impl ) +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED +#endif // #if !dTHREADING_INTF_DISABLED + +//********************************** +// EXTENSION LIST END + +// These tokens are mutually exclusive, and always present +#ifdef dSINGLE +"ODE_single_precision" +#else +"ODE_double_precision" +#endif // dDOUBLE + +; // END + +const char* dGetConfiguration (void) +{ + return ode_configuration; +} + + +// Helper to check for a feature of ODE +int dCheckConfiguration( const char* extension ) +{ + const char *start; + char *where, *terminator; + + /* Feature names should not have spaces. */ + where = (char*)strchr(extension, ' '); + if ( where || *extension == '\0') + return 1; + + const char* config = dGetConfiguration(); + + const sizeint ext_length = strlen(extension); + + /* It takes a bit of care to be fool-proof. Don't be fooled by sub-strings, etc. */ + start = config; + for ( ; ; ) + { + where = (char*)strstr((const char *) start, extension); + if (!where) + break; + + terminator = where + ext_length; + + if ( (where == start || *(where - 1) == ' ') && + (*terminator == ' ' || *terminator == '\0') ) + { + return 1; + } + + start = terminator; + } + + return 0; +} + + +// Local Variables: +// c-basic-offset:4 +// End: diff --git a/libs/ode-0.16.1/ode/src/odeinit.cpp b/libs/ode-0.16.1/ode/src/odeinit.cpp new file mode 100644 index 0000000..25cc302 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odeinit.cpp @@ -0,0 +1,575 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +ODE initialization/finalization code + +*/ + +#include <ode/common.h> +#include <ode/odeinit.h> +// <ode/objects.h> included for dWorldQuickStepCleanup() +#include <ode/objects.h> +#include "config.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_trimesh_internal.h" +#include "odetls.h" +#include "odeou.h" +#include "default_threading.h" + + +//**************************************************************************** +// Initialization tracking variables + +static unsigned int g_uiODEInitCounter = 0; +static unsigned int g_uiODEInitModes = 0; + + +#if dTRIMESH_ENABLED && dTRIMESH_OPCODE + +static +void OPCODEAbort() +{ + dICHECK(!"OPCODE Library Abort"); +} + + +#endif // #if dTRIMESH_ENABLED && dTRIMESH_OPCODE + + +enum EODEINITMODE +{ + OIM__MIN, + + OIM_AUTOTLSCLEANUP = OIM__MIN, + OIM_MANUALTLSCLEANUP, + + OIM__MAX +}; + +#if dTLS_ENABLED +static const EODETLSKIND g_atkTLSKindsByInitMode[OIM__MAX] = +{ + OTK_AUTOCLEANUP, // OIM_AUTOTLSCLEANUP, + OTK_MANUALCLEANUP, // OIM_MANUALTLSCLEANUP, +}; +#endif // #if dTLS_ENABLED + +static inline bool IsODEModeInitialized(EODEINITMODE imInitMode) +{ + return (g_uiODEInitModes & (1U << imInitMode)) != 0; +} + +static inline void SetODEModeInitialized(EODEINITMODE imInitMode) +{ + g_uiODEInitModes |= (1U << imInitMode); +} + +static inline void ResetODEModeInitialized(EODEINITMODE imInitMode) +{ + g_uiODEInitModes &= ~(1U << imInitMode); +} + +static inline bool IsODEAnyModeInitialized() +{ + return g_uiODEInitModes != 0; +} + + +enum +{ + TLD_INTERNAL_COLLISIONDATA_ALLOCATED = 0x00000001 +}; + +static bool AllocateThreadBasicDataIfNecessary(EODEINITMODE imInitMode) +{ + bool bResult = false; + + do + { +#if dTLS_ENABLED + EODETLSKIND tkTlsKind = g_atkTLSKindsByInitMode[imInitMode]; + + const unsigned uDataAllocationFlags = COdeTls::GetDataAllocationFlags(tkTlsKind); + + // If no flags are set it may mean that TLS slot is not allocated yet + if (uDataAllocationFlags == 0) + { + // Assign zero flags to make sure that TLS slot has been allocated + if (!COdeTls::AssignDataAllocationFlags(tkTlsKind, 0)) + { + break; + } + } +#else + (void)imInitMode; // unused +#endif // #if dTLS_ENABLED + + bResult = true; + } + while (false); + + return bResult; +} + +static void FreeThreadBasicDataOnFailureIfNecessary(EODEINITMODE imInitMode) +{ +#if dTLS_ENABLED + + if (imInitMode == OIM_MANUALTLSCLEANUP) + { + EODETLSKIND tkTlsKind = g_atkTLSKindsByInitMode[imInitMode]; + + const unsigned uDataAllocationFlags = COdeTls::GetDataAllocationFlags(tkTlsKind); + + if (uDataAllocationFlags == 0) + { + // So far, only free TLS slot, if no subsystems have data allocated + COdeTls::CleanupForThread(); + } + } +#else + (void)imInitMode; // unused +#endif // #if dTLS_ENABLED +} + +#if dTLS_ENABLED +static bool AllocateThreadCollisionData(EODETLSKIND tkTlsKind) +{ + bool bResult = false; + + do + { + dIASSERT(!(COdeTls::GetDataAllocationFlags(tkTlsKind) & TLD_INTERNAL_COLLISIONDATA_ALLOCATED)); + +#if dTRIMESH_ENABLED + + TrimeshCollidersCache *pccColliderCache = new TrimeshCollidersCache(); + if (!COdeTls::AssignTrimeshCollidersCache(tkTlsKind, pccColliderCache)) + { + delete pccColliderCache; + break; + } + +#endif // dTRIMESH_ENABLED + + COdeTls::SignalDataAllocationFlags(tkTlsKind, TLD_INTERNAL_COLLISIONDATA_ALLOCATED); + + bResult = true; + } + while (false); + + return bResult; +} +#endif // dTLS_ENABLED + +static bool AllocateThreadCollisionDataIfNecessary(EODEINITMODE imInitMode, bool &bOutDataAllocated) +{ + bool bResult = false; + bOutDataAllocated = false; + + do + { +#if dTLS_ENABLED + EODETLSKIND tkTlsKind = g_atkTLSKindsByInitMode[imInitMode]; + + const unsigned uDataAllocationFlags = COdeTls::GetDataAllocationFlags(tkTlsKind); + + if ((uDataAllocationFlags & TLD_INTERNAL_COLLISIONDATA_ALLOCATED) == 0) + { + if (!AllocateThreadCollisionData(tkTlsKind)) + { + break; + } + + bOutDataAllocated = true; + } +#else + (void)imInitMode; // unused +#endif // #if dTLS_ENABLED + + bResult = true; + } + while (false); + + return bResult; +} + +static void FreeThreadCollisionData(EODEINITMODE imInitMode) +{ +#if dTLS_ENABLED + + EODETLSKIND tkTlsKind = g_atkTLSKindsByInitMode[imInitMode]; + + COdeTls::DestroyTrimeshCollidersCache(tkTlsKind); + + COdeTls::DropDataAllocationFlags(tkTlsKind, TLD_INTERNAL_COLLISIONDATA_ALLOCATED); +#else + (void)imInitMode; // unused +#endif // dTLS_ENABLED +} + + +static bool InitODEForMode(EODEINITMODE imInitMode) +{ + bool bResult = false; + +#if dOU_ENABLED + bool bOUCustomizationsDone = false; +#endif +#if dATOMICS_ENABLED + bool bAtomicsInitialized = false; +#endif +#if dTLS_ENABLED + EODETLSKIND tkTLSKindToInit = g_atkTLSKindsByInitMode[imInitMode]; + bool bTlsInitialized = false; +#else + (void)imInitMode; // unused +#endif + + bool bWorldThreadingInitialized = false; + + do + { + bool bAnyModeAlreadyInitialized = IsODEAnyModeInitialized(); + + if (!bAnyModeAlreadyInitialized) + { +#if dOU_ENABLED + if (!COdeOu::DoOUCustomizations()) + { + break; + } + + bOUCustomizationsDone = true; +#endif + +#if dATOMICS_ENABLED + if (!COdeOu::InitializeAtomics()) + { + break; + } + + bAtomicsInitialized = true; +#endif + } + +#if dTLS_ENABLED + if (!COdeTls::Initialize(tkTLSKindToInit)) + { + break; + } + + bTlsInitialized = true; +#endif + + if (!bAnyModeAlreadyInitialized) + { + if (!DefaultThreadingHolder::initializeDefaultThreading()) + { + break; + } + + bWorldThreadingInitialized = true; + +#if dTRIMESH_ENABLED && dTRIMESH_OPCODE + if (!Opcode::InitOpcode(&OPCODEAbort)) + { + break; + } +#endif + +#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + gimpact_init(); +#endif + + dInitColliders(); + } + + bResult = true; + } + while (false); + + if (!bResult) + { + if (bWorldThreadingInitialized) + { + DefaultThreadingHolder::finalizeDefaultThreading(); + } + +#if dTLS_ENABLED + if (bTlsInitialized) + { + COdeTls::Finalize(tkTLSKindToInit); + } +#endif + +#if dATOMICS_ENABLED + if (bAtomicsInitialized) + { + COdeOu::FinalizeAtomics(); + } +#endif + +#if dOU_ENABLED + if (bOUCustomizationsDone) + { + COdeOu::UndoOUCustomizations(); + } +#endif + } + + return bResult; +} + + +static bool AllocateODEDataForThreadForMode(EODEINITMODE imInitMode, unsigned int uiAllocateFlags) +{ + bool bResult = false; + + bool bCollisionDataAllocated = false; + + do + { + if (!AllocateThreadBasicDataIfNecessary(imInitMode)) + { + break; + } + + if (uiAllocateFlags & dAllocateFlagCollisionData) + { + if (!AllocateThreadCollisionDataIfNecessary(imInitMode, bCollisionDataAllocated)) + { + break; + } + } + + bResult = true; + } + while (false); + + if (!bResult) + { + if (bCollisionDataAllocated) + { + FreeThreadCollisionData(imInitMode); + } + + FreeThreadBasicDataOnFailureIfNecessary(imInitMode); + } + + return bResult; +} + + +static void CloseODEForMode(EODEINITMODE imInitMode) +{ + bool bAnyModeStillInitialized = IsODEAnyModeInitialized(); + + if (!bAnyModeStillInitialized) + { + dClearPosrCache(); + dFinitUserClasses(); + dFinitColliders(); + +#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + gimpact_terminate(); +#endif + +#if dTRIMESH_ENABLED && dTRIMESH_OPCODE + extern void opcode_collider_cleanup(); + // Free up static allocations in opcode + opcode_collider_cleanup(); + + Opcode::CloseOpcode(); +#endif + + DefaultThreadingHolder::finalizeDefaultThreading(); + } + +#if dTLS_ENABLED + EODETLSKIND tkTLSKindToFinalize = g_atkTLSKindsByInitMode[imInitMode]; + COdeTls::Finalize(tkTLSKindToFinalize); +#else + (void)imInitMode; // unused +#endif + + if (!bAnyModeStillInitialized) + { +#if dATOMICS_ENABLED + COdeOu::FinalizeAtomics(); +#endif + +#if dOU_ENABLED + COdeOu::UndoOUCustomizations(); +#endif + } +} + + +//**************************************************************************** +// internal initialization and close routine implementations + +static bool InternalInitODE(unsigned int uiInitFlags) +{ + bool bResult = false; + + do + { + EODEINITMODE imInitMode = (uiInitFlags & dInitFlagManualThreadCleanup) ? OIM_MANUALTLSCLEANUP : OIM_AUTOTLSCLEANUP; + + if (!IsODEModeInitialized(imInitMode)) + { + if (!InitODEForMode(imInitMode)) + { + break; + } + + SetODEModeInitialized(imInitMode); + } + + ++g_uiODEInitCounter; + bResult = true; + } + while (false); + + return bResult; +} + +static void InternalCloseODE() +{ + unsigned int uiCurrentMode = (--g_uiODEInitCounter == 0) ? OIM__MIN : OIM__MAX; + for (; uiCurrentMode != OIM__MAX; ++uiCurrentMode) + { + if (IsODEModeInitialized((EODEINITMODE)uiCurrentMode)) + { + // Must be called before CloseODEForMode() + ResetODEModeInitialized((EODEINITMODE)uiCurrentMode); + + // Must be called after ResetODEModeInitialized() + CloseODEForMode((EODEINITMODE)uiCurrentMode); + } + } +} + +static bool InternalAllocateODEDataForThread(unsigned int uiAllocateFlags) +{ + bool bAnyFailure = false; + + for (unsigned uiCurrentMode = OIM__MIN; uiCurrentMode != OIM__MAX; ++uiCurrentMode) + { + if (IsODEModeInitialized((EODEINITMODE)uiCurrentMode)) + { + if (!AllocateODEDataForThreadForMode((EODEINITMODE)uiCurrentMode, uiAllocateFlags)) + { + bAnyFailure = true; + break; + } + } + } + + bool bResult = !bAnyFailure; + return bResult; +} + +static void InternalCleanupODEAllDataForThread() +{ +#if dTLS_ENABLED + COdeTls::CleanupForThread(); +#endif +} + +//**************************************************************************** +// initialization and shutdown routines - allocate and initialize data, +// cleanup before exiting + +void dInitODE() +{ + int bInitResult = InternalInitODE(0); + dIVERIFY(bInitResult); + + int ibAllocResult = InternalAllocateODEDataForThread(dAllocateMaskAll); + dIVERIFY(ibAllocResult); +} + +int dInitODE2(unsigned int uiInitFlags/*=0*/) +{ + bool bResult = false; + + bool bODEInitialized = false; + + do + { + if (!InternalInitODE(uiInitFlags)) + { + break; + } + + bODEInitialized = true; + + if (!InternalAllocateODEDataForThread(dAllocateFlagBasicData)) + { + break; + } + + bResult = true; + } + while (false); + + if (!bResult) + { + if (bODEInitialized) + { + InternalCloseODE(); + } + } + + return bResult; +} + + +int dAllocateODEDataForThread(unsigned int uiAllocateFlags) +{ + dUASSERT(g_uiODEInitCounter != 0, "Call dInitODE2 first"); + + bool bResult = InternalAllocateODEDataForThread(uiAllocateFlags); + return bResult; +} + + +void dCleanupODEAllDataForThread() +{ + dUASSERT(g_uiODEInitCounter != 0, "Call dInitODE2 first or delay dCloseODE until all threads exit"); + + InternalCleanupODEAllDataForThread(); +} + + +void dCloseODE() +{ + dUASSERT(g_uiODEInitCounter != 0, "dCloseODE must not be called without dInitODE2 or if dInitODE2 fails"); // dCloseODE must not be called without dInitODE2 or if dInitODE2 fails + + InternalCloseODE(); +} + diff --git a/libs/ode-0.16.1/ode/src/odemath.cpp b/libs/ode-0.16.1/ode/src/odemath.cpp new file mode 100644 index 0000000..5e69b9b --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odemath.cpp @@ -0,0 +1,312 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/common.h> +#include "config.h" +#include "odemath.h" + + +#undef dSafeNormalize3 +#undef dSafeNormalize4 +#undef dNormalize3 +#undef dNormalize4 + +#undef dPlaneSpace +#undef dOrthogonalizeR + + +int dSafeNormalize3 (dVector3 a) +{ + return dxSafeNormalize3(a); +} + +int dSafeNormalize4 (dVector4 a) +{ + return dxSafeNormalize4(a); +} + +void dNormalize3(dVector3 a) +{ + dxNormalize3(a); +} + +void dNormalize4(dVector4 a) +{ + dxNormalize4(a); +} + + +void dPlaneSpace(const dVector3 n, dVector3 p, dVector3 q) +{ + return dxPlaneSpace(n, p, q); +} + +int dOrthogonalizeR(dMatrix3 m) +{ + return dxOrthogonalizeR(m); +} + + +/*extern */ +bool dxCouldBeNormalized3(const dVector3 a) +{ + dAASSERT (a); + + bool ret = false; + + for (unsigned axis = dV3E__AXES_MIN; axis != dV3E__AXES_MAX; ++axis) { + if (a[axis] != REAL(0.0)) { + ret = true; + break; + } + } + + return ret; +} + +// this may be called for vectors `a' with extremely small magnitude, for +// example the result of a cross product on two nearly perpendicular vectors. +// we must be robust to these small vectors. to prevent numerical error, +// first find the component a[i] with the largest magnitude and then scale +// all the components by 1/a[i]. then we can compute the length of `a' and +// scale the components by 1/l. this has been verified to work with vectors +// containing the smallest representable numbers. + +/*extern */ +bool dxSafeNormalize3 (dVector3 a) +{ + dAASSERT (a); + + bool ret = false; + + do { + dReal abs_a0 = dFabs(a[dV3E_X]); + dReal abs_a1 = dFabs(a[dV3E_Y]); + dReal abs_a2 = dFabs(a[dV3E_Z]); + + dVec3Element idx; + + if (abs_a1 > abs_a0) { + if (abs_a2 > abs_a1) { // abs_a2 is the largest + idx = dV3E_Z; + } + else { // abs_a1 is the largest + idx = dV3E_Y; + } + } + else if (abs_a2 > abs_a0) {// abs_a2 is the largest + idx = dV3E_Z; + } + else { // aa[0] might be the largest + if (!(abs_a0 > REAL(0.0))) { + // if all a's are zero, this is where we'll end up. + // return the vector unchanged. + break; + } + + // abs_a0 is the largest + idx = dV3E_X; + } + + if (idx == dV3E_X) { + dReal aa0_recip = dRecip(abs_a0); + dReal a1 = a[dV3E_Y] * aa0_recip; + dReal a2 = a[dV3E_Z] * aa0_recip; + dReal l = dRecipSqrt(REAL(1.0) + a1 * a1 + a2 * a2); + a[dV3E_Y] = a1 * l; + a[dV3E_Z] = a2 * l; + a[dV3E_X] = dCopySign(l, a[dV3E_X]); + } + else if (idx == dV3E_Y) { + dReal aa1_recip = dRecip(abs_a1); + dReal a0 = a[dV3E_X] * aa1_recip; + dReal a2 = a[dV3E_Z] * aa1_recip; + dReal l = dRecipSqrt(REAL(1.0) + a0 * a0 + a2 * a2); + a[dV3E_X] = a0 * l; + a[dV3E_Z] = a2 * l; + a[dV3E_Y] = dCopySign(l, a[dV3E_Y]); + } + else { + dReal aa2_recip = dRecip(abs_a2); + dReal a0 = a[dV3E_X] * aa2_recip; + dReal a1 = a[dV3E_Y] * aa2_recip; + dReal l = dRecipSqrt(REAL(1.0) + a0 * a0 + a1 * a1); + a[dV3E_X] = a0 * l; + a[dV3E_Y] = a1 * l; + a[dV3E_Z] = dCopySign(l, a[dV3E_Z]); + } + + ret = true; + } + while (false); + + return ret; +} + +/* OLD VERSION */ +/* +void dNormalize3 (dVector3 a) +{ + dIASSERT (a); + dReal l = dCalcVectorDot3(a,a); + if (l > 0) { + l = dRecipSqrt(l); + a[0] *= l; + a[1] *= l; + a[2] *= l; + } + else { + a[0] = 1; + a[1] = 0; + a[2] = 0; + } +} +*/ + +/*extern */ +bool dxCouldBeNormalized4(const dVector4 a) +{ + dAASSERT (a); + + bool ret = false; + + for (unsigned axis = dV4E__MIN; axis != dV4E__MAX; ++axis) { + if (a[axis] != REAL(0.0)) { + ret = true; + break; + } + } + + return ret; +} + +/*extern */ +bool dxSafeNormalize4 (dVector4 a) +{ + dAASSERT (a); + + bool ret = false; + + dReal l = a[dV4E_X] * a[dV4E_X] + a[dV4E_Y] * a[dV4E_Y] + a[dV4E_Z] * a[dV4E_Z] + a[dV4E_O] * a[dV4E_O]; + if (l > 0) { + l = dRecipSqrt(l); + a[dV4E_X] *= l; + a[dV4E_Y] *= l; + a[dV4E_Z] *= l; + a[dV4E_O] *= l; + + ret = true; + } + + return ret; +} + + +void dxPlaneSpace (const dVector3 n, dVector3 p, dVector3 q) +{ + dAASSERT (n && p && q); + if (dFabs(n[2]) > M_SQRT1_2) { + // choose p in y-z plane + dReal a = n[1]*n[1] + n[2]*n[2]; + dReal k = dRecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + dReal a = n[0]*n[0] + n[1]*n[1]; + dReal k = dRecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} + + +/* +* This takes what is supposed to be a rotation matrix, +* and make sure it is correct. +* Note: this operates on rows, not columns, because for rotations +* both ways give equivalent results. +*/ +bool dxOrthogonalizeR(dMatrix3 m) +{ + bool ret = false; + + do { + if (!dxCouldBeNormalized3(m + dM3E__X_MIN)) { + break; + } + + dReal n0 = dCalcVectorLengthSquare3(m + dM3E__X_MIN); + + dVector3 row2_store; + dReal *row2 = m + dM3E__Y_MIN; + // project row[0] on row[1], should be zero + dReal proj = dCalcVectorDot3(m + dM3E__X_MIN, m + dM3E__Y_MIN); + if (proj != 0) { + // Gram-Schmidt step on row[1] + dReal proj_div_n0 = proj / n0; + row2_store[dV3E_X] = m[dM3E__Y_MIN + dV3E_X] - proj_div_n0 * m[dM3E__X_MIN + dV3E_X] ; + row2_store[dV3E_Y] = m[dM3E__Y_MIN + dV3E_Y] - proj_div_n0 * m[dM3E__X_MIN + dV3E_Y]; + row2_store[dV3E_Z] = m[dM3E__Y_MIN + dV3E_Z] - proj_div_n0 * m[dM3E__X_MIN + dV3E_Z]; + row2 = row2_store; + } + + if (!dxCouldBeNormalized3(row2)) { + break; + } + + if (n0 != REAL(1.0)) { + bool row0_norm_fault = !dxSafeNormalize3(m + dM3E__X_MIN); + dIVERIFY(!row0_norm_fault); + } + + dReal n1 = dCalcVectorLengthSquare3(row2); + if (n1 != REAL(1.0)) { + bool row1_norm_fault = !dxSafeNormalize3(row2); + dICHECK(!row1_norm_fault); + } + + dIASSERT(dFabs(dCalcVectorDot3(m + dM3E__X_MIN, row2)) < 1e-6); + + /* just overwrite row[2], this makes sure the matrix is not + a reflection */ + dCalcVectorCross3(m + dM3E__Z_MIN, m + dM3E__X_MIN, row2); + + m[dM3E_XPAD] = m[dM3E_YPAD] = m[dM3E_ZPAD] = 0; + + ret = true; + } + while (false); + + return ret; +} diff --git a/libs/ode-0.16.1/ode/src/odemath.h b/libs/ode-0.16.1/ode/src/odemath.h new file mode 100644 index 0000000..becf284 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odemath.h @@ -0,0 +1,72 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE__PRIVATE_ODEMATH_H_ +#define _ODE__PRIVATE_ODEMATH_H_ + +#include <ode/odemath.h> +#include "error.h" + + +bool dxCouldBeNormalized3(const dVector3 a); +bool dxSafeNormalize3 (dVector3 a); +bool dxCouldBeNormalized4(const dVector4 a); +bool dxSafeNormalize4 (dVector4 a); + +ODE_PURE_INLINE +void dxNormalize3(dVector3 a) +{ + bool bSafeNormalize3Fault; + if ((bSafeNormalize3Fault = !dxSafeNormalize3(a))) + { + dIVERIFY(!bSafeNormalize3Fault); + + a[0] = REAL(1.0); a[2] = a[1] = REAL(0.0); + } +} + +ODE_PURE_INLINE +void dxNormalize4(dVector4 a) +{ + bool bSafeNormalize4Fault; + if ((bSafeNormalize4Fault = !dxSafeNormalize4(a))) + { + dIVERIFY(!bSafeNormalize4Fault); + + a[0] = REAL(1.0); a[3] = a[2] = a[1] = REAL(0.0); + } +} + +void dxPlaneSpace (const dVector3 n, dVector3 p, dVector3 q); +bool dxOrthogonalizeR(dMatrix3 m); + +// For internal use +#define dSafeNormalize3(a) dxSafeNormalize3(a) +#define dSafeNormalize4(a) dxSafeNormalize4(a) +#define dNormalize3(a) dxNormalize3(a) +#define dNormalize4(a) dxNormalize4(a) + +#define dPlaneSpace(n, p, q) dxPlaneSpace(n, p, q) +#define dOrthogonalizeR(m) dxOrthogonalizeR(m) + + +#endif diff --git a/libs/ode-0.16.1/ode/src/odeou.cpp b/libs/ode-0.16.1/ode/src/odeou.cpp new file mode 100644 index 0000000..e784c41 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odeou.cpp @@ -0,0 +1,107 @@ +/************************************************************************* + * * + * OU library interface file for Open Dynamics Engine, * + * Copyright (C) 2008-2019 Oleh Derevenko. All rights reserved. * + * Email: odar@eleks.com (change all "a" to "e") * + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +ODE interface to OU library implementation. + +*/ + + +#include <ode/common.h> +#include <ode/memory.h> +#include "config.h" +#include "odeou.h" + + + +#if dOU_ENABLED + + +using _OU_NAMESPACE::EASSERTIONFAILURESEVERITY; +using _OU_NAMESPACE::AFS__MAX; +using _OU_NAMESPACE::CMemoryManagerCustomization; +using _OU_NAMESPACE::CAssertionCheckCustomization; + + +BEGIN_NAMESPACE_OU(); +template<> +const char *const CEnumUnsortedElementArray<EASSERTIONFAILURESEVERITY, AFS__MAX, const char *>::m_aetElementArray[] = +{ + "assert", // AFS_ASSERT, + "check", // AFS_CHECK, +}; +END_NAMESPACE_OU(); + +static const CEnumUnsortedElementArray<EASSERTIONFAILURESEVERITY, AFS__MAX, const char *> g_aszAssertionFailureSeverityNames; + + +static void _OU_CONVENTION_CALLBACK ForwardOUAssertionFailure(EASSERTIONFAILURESEVERITY fsFailureSeverity, + const char *szAssertionExpression, const char *szAssertionFileName, unsigned int uiAssertionSourceLine) +{ + dDebug(d_ERR_IASSERT, "Assertion failure in OU Library. Kind: %s, expression: \"%s\", file: \"%s\", line: %u", + g_aszAssertionFailureSeverityNames.Encode(fsFailureSeverity), + szAssertionExpression, szAssertionFileName, uiAssertionSourceLine); +} + + +static void *_OU_CONVENTION_CALLBACK ForwardOUMemoryAlloc(size_t nBlockSize) +{ + return dAlloc(nBlockSize); +} + +static void *_OU_CONVENTION_CALLBACK ForwardOUMemoryRealloc(void *pv_ExistingBlock, size_t nBlockNewSize) +{ + return dRealloc(pv_ExistingBlock, 0, nBlockNewSize); +} + +static void _OU_CONVENTION_CALLBACK ForwardOUMemoryFree(void *pv_ExistingBlock) +{ + return dFree(pv_ExistingBlock, 0); +} + + +bool COdeOu::DoOUCustomizations() +{ + CMemoryManagerCustomization::CustomizeMemoryManager(&ForwardOUMemoryAlloc, + &ForwardOUMemoryRealloc, &ForwardOUMemoryFree); + + CAssertionCheckCustomization::CustomizeAssertionChecks(&ForwardOUAssertionFailure); + + return true; +} + +void COdeOu::UndoOUCustomizations() +{ + CAssertionCheckCustomization::CustomizeAssertionChecks(NULL); + + CMemoryManagerCustomization::CustomizeMemoryManager(NULL, NULL, NULL); +} + + +#endif // dOU_ENABLED + diff --git a/libs/ode-0.16.1/ode/src/odeou.h b/libs/ode-0.16.1/ode/src/odeou.h new file mode 100644 index 0000000..a06de8f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odeou.h @@ -0,0 +1,107 @@ +/************************************************************************* +* * +* OU library interface file for Open Dynamics Engine, * +* Copyright (C) 2008-2019 Oleh Derevenko. All rights reserved. * +* Email: odar@eleks.com (change all "a" to "e") * +* * +* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * +* All rights reserved. Email: russ@q12.org Web: www.q12.org * +* * +* * +* This library is free software; you can redistribute it and/or * +* modify it under the terms of EITHER: * +* (1) The GNU Lesser General Public License as published by the Free * +* Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. The text of the GNU Lesser * +* General Public License is included with this library in the * +* file LICENSE.TXT. * +* (2) The BSD-style license that is included with this library in * +* the file LICENSE-BSD.TXT. * +* * +* This library is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * +* LICENSE.TXT and LICENSE-BSD.TXT for more details. * +* * +*************************************************************************/ + +/* + +ODE interface to OU library functions. + +*/ + + +#ifndef _ODE_ODEOU_H_ +#define _ODE_ODEOU_H_ + + +#if dOU_ENABLED + +#include <ou/assert.h> +#include <ou/enumarrays.h> +#include <ou/macros.h> +#include <ou/templates.h> +#include <ou/typewrapper.h> +#include <ou/simpleflags.h> +#include <ou/customization.h> + +#if dATOMICS_ENABLED +#include <ou/atomic.h> +#include <ou/atomicflags.h> +#endif + +#if dTLS_ENABLED +#include <ou/threadlocalstorage.h> +#endif + + +using _OU_NAMESPACE::CEnumUnsortedElementArray; +using _OU_NAMESPACE::CEnumSortedElementArray; + +#if dATOMICS_ENABLED +using _OU_NAMESPACE::atomicord32; +using _OU_NAMESPACE::atomicptr; +using _OU_NAMESPACE::InitializeAtomicAPI; +using _OU_NAMESPACE::FinalizeAtomicAPI; +using _OU_NAMESPACE::AtomicIncrement; +using _OU_NAMESPACE::AtomicDecrement; +using _OU_NAMESPACE::AtomicCompareExchange; +using _OU_NAMESPACE::AtomicExchange; +using _OU_NAMESPACE::AtomicExchangeAddNoResult; +using _OU_NAMESPACE::AtomicExchangeAdd; +using _OU_NAMESPACE::AtomicCompareExchangePointer; +using _OU_NAMESPACE::AtomicExchangePointer; +using _OU_NAMESPACE::AtomicReadReorderBarrier; +using _OU_NAMESPACE::AtomicStore; +using _OU_NAMESPACE::AtomicStorePointer; +#endif + + +class COdeOu +{ +public: + static bool DoOUCustomizations(); + static void UndoOUCustomizations(); + +#if dATOMICS_ENABLED + static bool InitializeAtomics() { return InitializeAtomicAPI(); } + static void FinalizeAtomics() { FinalizeAtomicAPI(); } +#endif +}; + + +#endif + + +#if !dOU_ENABLED || !dATOMICS_ENABLED + +typedef unsigned int atomicord32; +typedef void *atomicptr; + + +#endif // dOU_ENABLED + + + +#endif // _ODE_ODEOU_H_ diff --git a/libs/ode-0.16.1/ode/src/odetls.cpp b/libs/ode-0.16.1/ode/src/odetls.cpp new file mode 100644 index 0000000..5df2845 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odetls.cpp @@ -0,0 +1,153 @@ +/************************************************************************* + * * + * Thread local storage access stub for Open Dynamics Engine, * + * Copyright (C) 2008-2019 Oleh Derevenko. All rights reserved. * + * Email: odar@eleks.com (change all "a" to "e") * + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +ODE Thread Local Storage access stub implementation. + +*/ + +#include <ode/common.h> +#include "config.h" +#include "odemath.h" +#include "odetls.h" +#include "collision_trimesh_internal.h" + + +#if dTLS_ENABLED + + +using _OU_NAMESPACE::CTLSInitialization; + + +////////////////////////////////////////////////////////////////////////// +// Class static fields + +HTLSKEY COdeTls::m_ahtkStorageKeys[OTK__MAX] = { 0 }; + + +////////////////////////////////////////////////////////////////////////// +// Initialization and finalization + +bool COdeTls::Initialize(EODETLSKIND tkTLSKind) +{ + dIASSERT(!m_ahtkStorageKeys[tkTLSKind]); + + bool bResult = false; + + unsigned uOUFlags = 0; + + if (tkTLSKind == OTK_MANUALCLEANUP) + { + uOUFlags |= CTLSInitialization::SIF_MANUAL_CLEANUP_ON_THREAD_EXIT; + } + + if (CTLSInitialization::InitializeTLSAPI(m_ahtkStorageKeys[tkTLSKind], OTI__MAX, uOUFlags)) + { + bResult = true; + } + + return bResult; +} + +void COdeTls::Finalize(EODETLSKIND tkTLSKind) +{ + CTLSInitialization::FinalizeTLSAPI(); + + m_ahtkStorageKeys[tkTLSKind] = 0; +} + + +void COdeTls::CleanupForThread() +{ + if (m_ahtkStorageKeys[OTK_MANUALCLEANUP]) + { + CTLSInitialization::CleanupOnThreadExit(); + } + else + { + dIASSERT(false); // The class is not intended to be cleaned up manually + } +} + + +////////////////////////////////////////////////////////////////////////// +// Value modifiers + +bool COdeTls::AssignDataAllocationFlags(EODETLSKIND tkTLSKind, unsigned uInitializationFlags) +{ + bool bResult = CThreadLocalStorage::SetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS, (tlsvaluetype)(sizeint)uInitializationFlags); + return bResult; +} + + +bool COdeTls::AssignTrimeshCollidersCache(EODETLSKIND tkTLSKind, TrimeshCollidersCache *pccInstance) +{ + dIASSERT(!CThreadLocalStorage::GetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_TRIMESH_TRIMESH_COLLIDER_CACHE)); + + bool bResult = CThreadLocalStorage::SetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_TRIMESH_TRIMESH_COLLIDER_CACHE, (tlsvaluetype)pccInstance, &COdeTls::FreeTrimeshCollidersCache_Callback); + return bResult; +} + +void COdeTls::DestroyTrimeshCollidersCache(EODETLSKIND tkTLSKind) +{ + TrimeshCollidersCache *pccCacheInstance = (TrimeshCollidersCache *)CThreadLocalStorage::GetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_TRIMESH_TRIMESH_COLLIDER_CACHE); + + if (pccCacheInstance != NULL) + { + FreeTrimeshCollidersCache(pccCacheInstance); + + CThreadLocalStorage::UnsafeSetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_TRIMESH_TRIMESH_COLLIDER_CACHE, (tlsvaluetype)NULL); + } +} + + +////////////////////////////////////////////////////////////////////////// +// Value type destructors + +void COdeTls::FreeTrimeshCollidersCache(TrimeshCollidersCache *pccCacheInstance) +{ +#if dTRIMESH_ENABLED + delete pccCacheInstance; +#else + dIASSERT(pccCacheInstance == NULL); // The cache is not being allocated if the library is configured without trimeshes +#endif +} + + +////////////////////////////////////////////////////////////////////////// +// Value type destructor callbacks + +void COdeTls::FreeTrimeshCollidersCache_Callback(tlsvaluetype vValueData) +{ + TrimeshCollidersCache *pccCacheInstance = (TrimeshCollidersCache *)vValueData; + FreeTrimeshCollidersCache(pccCacheInstance); +} + + +#endif // #if dTLS_ENABLED + diff --git a/libs/ode-0.16.1/ode/src/odetls.h b/libs/ode-0.16.1/ode/src/odetls.h new file mode 100644 index 0000000..db3306b --- /dev/null +++ b/libs/ode-0.16.1/ode/src/odetls.h @@ -0,0 +1,126 @@ +/************************************************************************* + * * + * Thread local storage access stub for Open Dynamics Engine, * + * Copyright (C) 2008-2019 Oleh Derevenko. All rights reserved. * + * Email: odar@eleks.com (change all "a" to "e") * + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +ODE Thread Local Storage access stub interface. + +*/ + + +#ifndef _ODE_ODETLS_H_ +#define _ODE_ODETLS_H_ + + +#include "odeou.h" + + +#if dTLS_ENABLED + + +using _OU_NAMESPACE::tlsvaluetype; +using _OU_NAMESPACE::HTLSKEY; +using _OU_NAMESPACE::CThreadLocalStorage; + + +struct TrimeshCollidersCache; + + +enum EODETLSKIND +{ + OTK__MIN, + + OTK_AUTOCLEANUP = OTK__MIN, + OTK_MANUALCLEANUP, + + OTK__MAX, + + OTK__DEFAULT = OTK_AUTOCLEANUP, +}; + +enum EODETLSITEM +{ + OTI_DATA_ALLOCATION_FLAGS, + OTI_TRIMESH_TRIMESH_COLLIDER_CACHE, + + OTI__MAX, +}; + + +class COdeTls +{ +public: + static bool Initialize(EODETLSKIND tkTLSKind); + static void Finalize(EODETLSKIND tkTLSKind); + + static void CleanupForThread(); + +public: + static unsigned GetDataAllocationFlags(EODETLSKIND tkTLSKind) + { + // Must be a safe call as it is used to test if TLS slot is allocated at all + return (unsigned)(sizeint)CThreadLocalStorage::GetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS); + } + + static void SignalDataAllocationFlags(EODETLSKIND tkTLSKind, unsigned uFlagsMask) + { + unsigned uCurrentFlags = (unsigned)(sizeint)CThreadLocalStorage::UnsafeGetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS); + CThreadLocalStorage::UnsafeSetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS, (tlsvaluetype)(sizeint)(uCurrentFlags | uFlagsMask)); + } + + static void DropDataAllocationFlags(EODETLSKIND tkTLSKind, unsigned uFlagsMask) + { + unsigned uCurrentFlags = (unsigned)(sizeint)CThreadLocalStorage::UnsafeGetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS); + CThreadLocalStorage::UnsafeSetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS, (tlsvaluetype)(sizeint)(uCurrentFlags & ~uFlagsMask)); + } + + static TrimeshCollidersCache *GetTrimeshCollidersCache(EODETLSKIND tkTLSKind) + { + return (TrimeshCollidersCache *)CThreadLocalStorage::UnsafeGetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_TRIMESH_TRIMESH_COLLIDER_CACHE); + } + +public: + static bool AssignDataAllocationFlags(EODETLSKIND tkTLSKind, unsigned uInitializationFlags); + + static bool AssignTrimeshCollidersCache(EODETLSKIND tkTLSKind, TrimeshCollidersCache *pccInstance); + static void DestroyTrimeshCollidersCache(EODETLSKIND tkTLSKind); + +private: + static void FreeTrimeshCollidersCache(TrimeshCollidersCache *pccCacheInstance); + +private: + static void _OU_CONVENTION_CALLBACK FreeTrimeshCollidersCache_Callback(tlsvaluetype vValueData); + +private: + static HTLSKEY m_ahtkStorageKeys[OTK__MAX]; +}; + + +#endif // dTLS_ENABLED + + +#endif // _ODE_ODETLS_H_ diff --git a/libs/ode-0.16.1/ode/src/plane.cpp b/libs/ode-0.16.1/ode/src/plane.cpp new file mode 100644 index 0000000..b54e894 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/plane.cpp @@ -0,0 +1,146 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// plane public API + +static void make_sure_plane_normal_has_unit_length (dxPlane *g) +{ + dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2]; + if (l > 0) { + l = dRecipSqrt(l); + g->p[0] *= l; + g->p[1] *= l; + g->p[2] *= l; + g->p[3] *= l; + } + else { + g->p[0] = 1; + g->p[1] = 0; + g->p[2] = 0; + g->p[3] = 0; + } +} + + +dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) : +dxGeom (space,0) +{ + type = dPlaneClass; + p[0] = a; + p[1] = b; + p[2] = c; + p[3] = d; + make_sure_plane_normal_has_unit_length (this); +} + + +void dxPlane::computeAABB() +{ + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + + // Planes that have normal vectors aligned along an axis can use a + // less comprehensive (half space) bounding box. + + if ( p[1] == 0.0f && p[2] == 0.0f ) { + // normal aligned with x-axis + aabb[0] = (p[0] > 0) ? -dInfinity : -p[3]; + aabb[1] = (p[0] > 0) ? p[3] : dInfinity; + } else + if ( p[0] == 0.0f && p[2] == 0.0f ) { + // normal aligned with y-axis + aabb[2] = (p[1] > 0) ? -dInfinity : -p[3]; + aabb[3] = (p[1] > 0) ? p[3] : dInfinity; + } else + if ( p[0] == 0.0f && p[1] == 0.0f ) { + // normal aligned with z-axis + aabb[4] = (p[2] > 0) ? -dInfinity : -p[3]; + aabb[5] = (p[2] > 0) ? p[3] : dInfinity; + } +} + + +dGeomID dCreatePlane (dSpaceID space, + dReal a, dReal b, dReal c, dReal d) +{ + return new dxPlane (space,a,b,c,d); +} + + +void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + p->p[0] = a; + p->p[1] = b; + p->p[2] = c; + p->p[3] = d; + make_sure_plane_normal_has_unit_length (p); + dGeomMoved (g); +} + + +void dGeomPlaneGetParams (dGeomID g, dVector4 result) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + result[0] = p->p[0]; + result[1] = p->p[1]; + result[2] = p->p[2]; + result[3] = p->p[3]; +} + + +dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + return p->p[3] - p->p[0]*x - p->p[1]*y - p->p[2]*z; +} diff --git a/libs/ode-0.16.1/ode/src/quickstep.cpp b/libs/ode-0.16.1/ode/src/quickstep.cpp new file mode 100644 index 0000000..046bc33 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/quickstep.cpp @@ -0,0 +1,3344 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#include <ode/common.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/misc.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "lcp.h"
+#include "util.h"
+#include "threadingutils.h"
+
+#include <new>
+
+
+//***************************************************************************
+// configuration
+
+// for the SOR and CG methods:
+// uncomment the following line to use warm starting. this definitely
+// help for motor-driven joints. unfortunately it appears to hurt
+// with high-friction contacts using the SOR method. use with care
+
+// #define WARM_STARTING 1
+
+
+#define REORDERING_METHOD__DONT_REORDER 0
+#define REORDERING_METHOD__BY_ERROR 1
+#define REORDERING_METHOD__RANDOMLY 2
+
+// for the SOR method:
+// uncomment the following line to determine a new constraint-solving
+// order for each iteration. however, the qsort per iteration is expensive,
+// and the optimal order is somewhat problem dependent.
+// @@@ try the leaf->root ordering.
+
+// #define CONSTRAINTS_REORDERING_METHOD REORDERING_METHOD__BY_ERROR
+
+
+// for the SOR method:
+// uncomment the following line to randomly reorder constraint rows
+// during the solution. depending on the situation, this can help a lot
+// or hardly at all, but it doesn't seem to hurt.
+
+#define CONSTRAINTS_REORDERING_METHOD REORDERING_METHOD__RANDOMLY
+
+
+#if !defined(CONSTRAINTS_REORDERING_METHOD)
+#define CONSTRAINTS_REORDERING_METHOD REORDERING_METHOD__DONT_REORDER
+#endif
+
+
+#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__RANDOMLY
+#if !defined(RANDOM_CONSTRAINTS_REORDERING_FREQUENCY)
+#define RANDOM_CONSTRAINTS_REORDERING_FREQUENCY 8U
+#endif
+dSASSERT(RANDOM_CONSTRAINTS_REORDERING_FREQUENCY != 0);
+#endif
+
+enum dxRandomReorderStage
+{
+ RRS__MIN,
+
+ RRS_REORDERING = RRS__MIN,
+
+ RRS__MAX,
+};
+
+
+//***************************************************************************
+// macros, typedefs, forwards and inlines
+
+struct IndexError;
+
+
+#define dMIN(A,B) ((A)>(B) ? (B) : (A))
+#define dMAX(A,B) ((B)>(A) ? (B) : (A))
+
+
+#define dxQUICKSTEPISLAND_STAGE2B_STEP 16U
+#define dxQUICKSTEPISLAND_STAGE2C_STEP 32U
+
+#ifdef WARM_STARTING
+#define dxQUICKSTEPISLAND_STAGE4A_STEP 256U
+#else
+#define dxQUICKSTEPISLAND_STAGE4A_STEP 512U
+#endif
+
+#define dxQUICKSTEPISLAND_STAGE4LCP_IMJ_STEP 8U
+#define dxQUICKSTEPISLAND_STAGE4LCP_AD_STEP 8U
+
+#ifdef WARM_STARTING
+#define dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP 128U
+#define dxQUICKSTEPISLAND_STAGE4LCP_FC_COMPLETE_TO_PREPARE_COMPLEXITY_DIVISOR 4
+#define dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP_PREPARE (dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP * dxQUICKSTEPISLAND_STAGE4LCP_FC_COMPLETE_TO_PREPARE_COMPLEXITY_DIVISOR)
+#define dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP_COMPLETE (dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP)
+#else
+#define dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP (dxQUICKSTEPISLAND_STAGE4A_STEP / 2) // Average info.m is 3 for stage4a, while there are 6 reals per index in fc
+#endif
+
+#define dxQUICKSTEPISLAND_STAGE4B_STEP 256U
+
+#define dxQUICKSTEPISLAND_STAGE6A_STEP 16U
+#define dxQUICKSTEPISLAND_STAGE6B_STEP 1U
+
+template<unsigned int step_size>
+inline unsigned int CalculateOptimalThreadsCount(unsigned int complexity, unsigned int max_threads)
+{
+ unsigned int raw_threads = dMAX(complexity, step_size) / step_size; // Round down on division
+ unsigned int optimum = dMIN(raw_threads, max_threads);
+ return optimum;
+}
+
+#define dxENCODE_INDEX(index) ((unsigned int)((index) + 1))
+#define dxDECODE_INDEX(code) ((unsigned int)((code) - 1))
+#define dxHEAD_INDEX 0
+
+//****************************************************************************
+// special matrix multipliers
+
+// multiply block of B matrix (q x 6) with 12 dReal per row with C vector (q)
+static inline void Multiply1_12q1 (dReal *A, const dReal *B, const dReal *C, unsigned int q)
+{
+ dIASSERT (q>0 && A && B && C);
+
+ dReal a = 0;
+ dReal b = 0;
+ dReal c = 0;
+ dReal d = 0;
+ dReal e = 0;
+ dReal f = 0;
+ dReal s;
+
+ for(unsigned int i=0, k = 0; i<q; k += 12, i++)
+ {
+ s = C[i]; //C[i] and B[n+k] cannot overlap because its value has been read into a temporary.
+
+ //For the rest of the loop, the only memory dependency (array) is from B[]
+ a += B[ k] * s;
+ b += B[1+k] * s;
+ c += B[2+k] * s;
+ d += B[3+k] * s;
+ e += B[4+k] * s;
+ f += B[5+k] * s;
+ }
+
+ A[0] = a;
+ A[1] = b;
+ A[2] = c;
+ A[3] = d;
+ A[4] = e;
+ A[5] = f;
+}
+
+//***************************************************************************
+// testing stuff
+
+#ifdef TIMING
+#define IFTIMING(x) x
+#else
+#define IFTIMING(x) ((void)0)
+#endif
+
+
+struct dJointWithInfo1
+{
+ dxJoint *joint;
+ dxJoint::Info1 info;
+};
+
+
+struct dxMIndexItem
+{
+ unsigned mIndex;
+ unsigned fbIndex;
+};
+
+struct dxJBodiesItem
+{
+ unsigned first;
+ int second; // The index is optional and can equal to -1
+};
+
+enum dxInvIRowElement
+{
+ IIE__MIN,
+
+ IIE__MATRIX_MIN = IIE__MIN,
+ IIE__MATRIX_MAX = IIE__MATRIX_MIN + dM3E__MAX,
+
+ IIE__MAX = IIE__MATRIX_MAX,
+};
+
+enum dxRHSCFMElement
+{
+ RCE_RHS = dxJoint::GI2_RHS,
+ RCE_CFM = dxJoint::GI2_CFM,
+
+ RCE__RHS_CFM_MAX = dxJoint::GI2__RHS_CFM_MAX,
+};
+
+enum dxLoHiElement
+{
+ LHE_LO = dxJoint::GI2_LO,
+ LHE_HI = dxJoint::GI2_HI,
+
+ LHE__LO_HI_MAX = dxJoint::GI2__LO_HI_MAX,
+};
+
+enum dxJacobiVectorElement
+{
+ JVE__MIN,
+
+ JVE__L_MIN = JVE__MIN + dDA__L_MIN,
+
+ JVE_LX = JVE__MIN + dDA_LX,
+ JVE_LY = JVE__MIN + dDA_LY,
+ JVE_LZ = JVE__MIN + dDA_LZ,
+
+ JVE__L_MAX = JVE__MIN + dDA__L_MAX,
+
+ JVE__A_MIN = JVE__MIN + dDA__A_MIN,
+
+ JVE_AX = JVE__MIN + dDA_AX,
+ JVE_AY = JVE__MIN + dDA_AY,
+ JVE_AZ = JVE__MIN + dDA_AZ,
+
+ JVE__A_MAX = JVE__MIN + dDA__A_MAX,
+
+ JVE__MAX = JVE__MIN + dDA__MAX,
+
+ JVE__L_COUNT = JVE__L_MAX - JVE__L_MIN,
+ JVE__A_COUNT = JVE__A_MAX - JVE__A_MIN,
+};
+
+enum dxJacobiMatrixElement
+{
+ JME__MIN,
+
+ JME__J1_MIN = JME__MIN,
+ JME__J1L_MIN = JME__J1_MIN + JVE__L_MIN,
+
+ JME_J1LX = JME__J1_MIN + JVE_LX,
+ JME_J1LY = JME__J1_MIN + JVE_LY,
+ JME_J1LZ = JME__J1_MIN + JVE_LZ,
+
+ JME__J1L_MAX = JME__J1_MIN + JVE__L_MAX,
+
+ JME__J1A_MIN = JME__J1_MIN + JVE__A_MIN,
+
+ JME_J1AX = JME__J1_MIN + JVE_AX,
+ JME_J1AY = JME__J1_MIN + JVE_AY,
+ JME_J1AZ = JME__J1_MIN + JVE_AZ,
+
+ JME__J1A_MAX = JME__J1_MIN + JVE__A_MAX,
+ JME__J1_MAX = JME__J1_MIN + JVE__MAX,
+
+ JME__RHS_CFM_MIN = JME__J1_MAX,
+ JME_RHS = JME__RHS_CFM_MIN + RCE_RHS,
+ JME_CFM = JME__RHS_CFM_MIN + RCE_CFM,
+ JME__RHS_CFM_MAX = JME__RHS_CFM_MIN + RCE__RHS_CFM_MAX,
+
+ JME__J2_MIN = JME__RHS_CFM_MAX,
+ JME__J2L_MIN = JME__J2_MIN + JVE__L_MIN,
+
+ JME_J2LX = JME__J2_MIN + JVE_LX,
+ JME_J2LY = JME__J2_MIN + JVE_LY,
+ JME_J2LZ = JME__J2_MIN + JVE_LZ,
+
+ JME__J2L_MAX = JME__J2_MIN + JVE__L_MAX,
+
+ JME__J2A_MIN = JME__J2_MIN + JVE__A_MIN,
+
+ JME_J2AX = JME__J2_MIN + JVE_AX,
+ JME_J2AY = JME__J2_MIN + JVE_AY,
+ JME_J2AZ = JME__J2_MIN + JVE_AZ,
+
+ JME__J2A_MAX = JME__J2_MIN + JVE__A_MAX,
+ JME__J2_MAX = JME__J2_MIN + JVE__MAX,
+
+ JME__LO_HI_MIN = JME__J2_MAX,
+ JME_LO = JME__LO_HI_MIN + LHE_LO,
+ JME_HI = JME__LO_HI_MIN + LHE_HI,
+ JME__LO_HI_MAX = JME__LO_HI_MIN + LHE__LO_HI_MAX,
+
+ JME__MAX = JME__LO_HI_MAX, // Is not that a luck to have 16 elements here? ;-)
+
+ JME__J1_COUNT = JME__J1_MAX - JME__J1_MIN,
+ JME__J2_COUNT = JME__J2_MAX - JME__J2_MIN,
+ JME__J_COUNT = JVE__MAX,
+};
+
+dSASSERT(JME__J_COUNT == JME__J1_COUNT);
+dSASSERT(JME__J_COUNT == JME__J2_COUNT);
+
+enum dxJacobiCopyElement
+{
+ JCE__MIN,
+
+ JCE__J1_MIN = JCE__MIN,
+ JCE__J1L_MIN = JCE__J1_MIN,
+
+ JCE_J1LX = JCE__J1L_MIN,
+ JCE_J1LY,
+ JCE_J1LZ,
+
+ JCE__J1L_MAX,
+
+ JCE__J1A_MIN = JCE__J1L_MAX,
+
+ JCE_J1AX = JCE__J1A_MIN,
+ JCE_J1AY,
+ JCE_J1AZ,
+
+ JCE__J1A_MAX,
+ JCE__J1_MAX = JCE__J1A_MAX,
+
+ JCE__J2_MIN = JCE__J1_MAX,
+ JCE__J2L_MIN = JCE__J2_MIN,
+
+ JCE_J2LX = JCE__J2L_MIN,
+ JCE_J2LY,
+ JCE_J2LZ,
+
+ JCE__J2L_MAX,
+
+ JCE__J2A_MIN = JCE__J2L_MAX,
+
+ JCE_J2AX = JCE__J2A_MIN,
+ JCE_J2AY,
+ JCE_J2AZ,
+
+ JCE__J2A_MAX,
+ JCE__J2_MAX = JCE__J2A_MAX,
+
+ JCE__MAX = JCE__J2_MAX,
+
+ JCE__J1_COUNT = JCE__J1_MAX - JCE__J1_MIN,
+ JCE__J2_COUNT = JCE__J2_MAX - JCE__J2_MIN,
+ JCE__JMAX_COUNT = dMAX(JCE__J1_COUNT, JCE__J2_COUNT),
+};
+
+enum dxInvMJTElement
+{
+ IMJ__MIN,
+
+ IMJ__1_MIN = IMJ__MIN,
+
+ IMJ__1L_MIN = IMJ__1_MIN + JVE__L_MIN,
+
+ IMJ_1LX = IMJ__1_MIN + JVE_LX,
+ IMJ_1LY = IMJ__1_MIN + JVE_LY,
+ IMJ_1LZ = IMJ__1_MIN + JVE_LZ,
+
+ IMJ__1L_MAX = IMJ__1_MIN + JVE__L_MAX,
+
+ IMJ__1A_MIN = IMJ__1_MIN + JVE__A_MIN,
+
+ IMJ_1AX = IMJ__1_MIN + JVE_AX,
+ IMJ_1AY = IMJ__1_MIN + JVE_AY,
+ IMJ_1AZ = IMJ__1_MIN + JVE_AZ,
+
+ IMJ__1A_MAX = IMJ__1_MIN + JVE__A_MAX,
+
+ IMJ__1_MAX = IMJ__1_MIN + JVE__MAX,
+
+ IMJ__2_MIN = IMJ__1_MAX,
+
+ IMJ__2L_MIN = IMJ__2_MIN + JVE__L_MIN,
+
+ IMJ_2LX = IMJ__2_MIN + JVE_LX,
+ IMJ_2LY = IMJ__2_MIN + JVE_LY,
+ IMJ_2LZ = IMJ__2_MIN + JVE_LZ,
+
+ IMJ__2L_MAX = IMJ__2_MIN + JVE__L_MAX,
+
+ IMJ__2A_MIN = IMJ__2_MIN + JVE__A_MIN,
+
+ IMJ_2AX = IMJ__2_MIN + JVE_AX,
+ IMJ_2AY = IMJ__2_MIN + JVE_AY,
+ IMJ_2AZ = IMJ__2_MIN + JVE_AZ,
+
+ IMJ__2A_MAX = IMJ__2_MIN + JVE__A_MAX,
+
+ IMJ__2_MAX = IMJ__2_MIN + JVE__MAX,
+
+ IMJ__MAX = IMJ__2_MAX,
+};
+
+enum dxContactForceElement
+{
+ CFE__MIN,
+
+ CFE__DYNAMICS_MIN = CFE__MIN,
+
+ CFE__L_MIN = CFE__DYNAMICS_MIN + dDA__L_MIN,
+
+ CFE_LX = CFE__DYNAMICS_MIN + dDA_LX,
+ CFE_LY = CFE__DYNAMICS_MIN + dDA_LY,
+ CFE_LZ = CFE__DYNAMICS_MIN + dDA_LZ,
+
+ CFE__L_MAX = CFE__DYNAMICS_MIN + dDA__L_MAX,
+
+ CFE__A_MIN = CFE__DYNAMICS_MIN + dDA__A_MIN,
+
+ CFE_AX = CFE__DYNAMICS_MIN + dDA_AX,
+ CFE_AY = CFE__DYNAMICS_MIN + dDA_AY,
+ CFE_AZ = CFE__DYNAMICS_MIN + dDA_AZ,
+
+ CFE__A_MAX = CFE__DYNAMICS_MIN + dDA__A_MAX,
+
+ CFE__DYNAMICS_MAX = CFE__DYNAMICS_MIN + dDA__MAX,
+
+ CFE__MAX = CFE__DYNAMICS_MAX,
+};
+
+enum dxRHSElement
+{
+ RHS__MIN,
+
+ RHS__DYNAMICS_MIN = RHS__MIN,
+
+ RHS__L_MIN = RHS__DYNAMICS_MIN + dDA__L_MIN,
+
+ RHS_LX = RHS__DYNAMICS_MIN + dDA_LX,
+ RHS_LY = RHS__DYNAMICS_MIN + dDA_LY,
+ RHS_LZ = RHS__DYNAMICS_MIN + dDA_LZ,
+
+ RHS__L_MAX = RHS__DYNAMICS_MIN + dDA__L_MAX,
+
+ RHS__A_MIN = RHS__DYNAMICS_MIN + dDA__A_MIN,
+
+ RHS_AX = RHS__DYNAMICS_MIN + dDA_AX,
+ RHS_AY = RHS__DYNAMICS_MIN + dDA_AY,
+ RHS_AZ = RHS__DYNAMICS_MIN + dDA_AZ,
+
+ RHS__A_MAX = RHS__DYNAMICS_MIN + dDA__A_MAX,
+
+ RHS__DYNAMICS_MAX = RHS__DYNAMICS_MIN + dDA__MAX,
+
+ RHS__MAX = RHS__DYNAMICS_MAX,
+};
+
+
+#define JACOBIAN_ALIGNMENT dMAX(JME__MAX * sizeof(dReal), EFFICIENT_ALIGNMENT)
+dSASSERT(((JME__MAX - 1) & JME__MAX) == 0); // Otherwise there is no reason to over-align the Jacobian
+
+#define JCOPY_ALIGNMENT dMAX(32, EFFICIENT_ALIGNMENT)
+#define INVI_ALIGNMENT dMAX(32, EFFICIENT_ALIGNMENT)
+#define INVMJ_ALIGNMENT dMAX(32, EFFICIENT_ALIGNMENT)
+
+
+struct dxQuickStepperStage0Outputs
+{
+ unsigned int nj;
+ unsigned int m;
+ unsigned int mfb;
+};
+
+struct dxQuickStepperStage1CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *stepperCallContext, void *stageMemArenaState, dReal *invI, dJointWithInfo1 *jointinfos)
+ {
+ m_stepperCallContext = stepperCallContext;
+ m_stageMemArenaState = stageMemArenaState;
+ m_invI = invI;
+ m_jointinfos = jointinfos;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ void *m_stageMemArenaState;
+ dReal *m_invI;
+ dJointWithInfo1 *m_jointinfos;
+ dxQuickStepperStage0Outputs m_stage0Outputs;
+};
+
+struct dxQuickStepperStage0BodiesCallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *stepperCallContext, dReal *invI)
+ {
+ m_stepperCallContext = stepperCallContext;
+ m_invI = invI;
+ m_tagsTaken = 0;
+ m_gravityTaken = 0;
+ m_inertiaBodyIndex = 0;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ dReal *m_invI;
+ atomicord32 m_tagsTaken;
+ atomicord32 m_gravityTaken;
+ volatile atomicord32 m_inertiaBodyIndex;
+};
+
+struct dxQuickStepperStage0JointsCallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *stepperCallContext, dJointWithInfo1 *jointinfos, dxQuickStepperStage0Outputs *stage0Outputs)
+ {
+ m_stepperCallContext = stepperCallContext;
+ m_jointinfos = jointinfos;
+ m_stage0Outputs = stage0Outputs;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ dJointWithInfo1 *m_jointinfos;
+ dxQuickStepperStage0Outputs *m_stage0Outputs;
+};
+
+static int dxQuickStepIsland_Stage0_Bodies_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage0_Joints_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage1_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+
+static void dxQuickStepIsland_Stage0_Bodies(dxQuickStepperStage0BodiesCallContext *callContext);
+static void dxQuickStepIsland_Stage0_Joints(dxQuickStepperStage0JointsCallContext *callContext);
+static void dxQuickStepIsland_Stage1(dxQuickStepperStage1CallContext *callContext);
+
+
+struct dxQuickStepperLocalContext
+{
+ void Initialize(dReal *invI, dJointWithInfo1 *jointinfos, unsigned int nj,
+ unsigned int m, unsigned int mfb, const dxMIndexItem *mindex, dxJBodiesItem *jb, int *findex,
+ dReal *J, dReal *Jcopy)
+ {
+ m_invI = invI;
+ m_jointinfos = jointinfos;
+ m_nj = nj;
+ m_m = m;
+ m_mfb = mfb;
+ m_valid_findices = 0;
+ m_mindex = mindex;
+ m_jb = jb;
+ m_findex = findex;
+ m_J = J;
+ m_Jcopy = Jcopy;
+ }
+
+ dReal *m_invI;
+ dJointWithInfo1 *m_jointinfos;
+ unsigned int m_nj;
+ unsigned int m_m;
+ unsigned int m_mfb;
+ volatile atomicord32 m_valid_findices;
+ const dxMIndexItem *m_mindex;
+ dxJBodiesItem *m_jb;
+ int *m_findex;
+ dReal *m_J;
+ dReal *m_Jcopy;
+};
+
+struct dxQuickStepperStage3CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, const dxQuickStepperLocalContext *localContext,
+ void *stage1MemArenaState)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ m_stage1MemArenaState = stage1MemArenaState;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ const dxQuickStepperLocalContext *m_localContext;
+ void *m_stage1MemArenaState;
+};
+
+struct dxQuickStepperStage2CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, dxQuickStepperLocalContext *localContext,
+ dReal *rhs_tmp)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ m_rhs_tmp = rhs_tmp;
+ m_ji_J = 0;
+ m_ji_jb = 0;
+ m_bi = 0;
+ m_Jrhsi = 0;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ dxQuickStepperLocalContext *m_localContext;
+ dReal *m_rhs_tmp;
+ volatile atomicord32 m_ji_J;
+ volatile atomicord32 m_ji_jb;
+ volatile atomicord32 m_bi;
+ volatile atomicord32 m_Jrhsi;
+};
+
+static int dxQuickStepIsland_Stage2a_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage2aSync_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage2b_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage2bSync_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage2c_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage3_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+
+static void dxQuickStepIsland_Stage2a(dxQuickStepperStage2CallContext *stage2CallContext);
+static void dxQuickStepIsland_Stage2b(dxQuickStepperStage2CallContext *stage2CallContext);
+static void dxQuickStepIsland_Stage2c(dxQuickStepperStage2CallContext *stage2CallContext);
+static void dxQuickStepIsland_Stage3(dxQuickStepperStage3CallContext *stage3CallContext);
+
+
+struct dxQuickStepperStage5CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, const dxQuickStepperLocalContext *localContext,
+ void *stage3MemArenaState)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ m_stage3MemArenaState = stage3MemArenaState;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ const dxQuickStepperLocalContext *m_localContext;
+ void *m_stage3MemArenaState;
+};
+
+struct dxQuickStepperStage4CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, const dxQuickStepperLocalContext *localContext,
+ dReal *lambda, dReal *cforce, dReal *iMJ, IndexError *order, dReal *last_lambda, atomicord32 *bi_links_or_mi_levels, atomicord32 *mi_links)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ m_lambda = lambda;
+ m_cforce = cforce;
+ m_iMJ = iMJ;
+ m_order = order;
+ m_last_lambda = last_lambda;
+ m_bi_links_or_mi_levels = bi_links_or_mi_levels;
+ m_mi_links = mi_links;
+ m_LCP_IterationSyncReleasee = NULL;
+ m_LCP_IterationAllowedThreads = 0;
+ m_LCP_fcStartReleasee = NULL;
+ m_ji_4a = 0;
+ m_mi_iMJ = 0;
+ m_mi_fc = 0;
+ m_mi_Ad = 0;
+ m_LCP_iteration = 0;
+ m_cf_4b = 0;
+ m_ji_4b = 0;
+ }
+
+ void AssignLCP_IterationData(dCallReleaseeID releaseeInstance, unsigned int iterationAllowedThreads)
+ {
+ m_LCP_IterationSyncReleasee = releaseeInstance;
+ m_LCP_IterationAllowedThreads = iterationAllowedThreads;
+ }
+
+ void AssignLCP_fcStartReleasee(dCallReleaseeID releaseeInstance)
+ {
+ m_LCP_fcStartReleasee = releaseeInstance;
+ }
+
+ void AssignLCP_fcAllowedThreads(unsigned int prepareThreads, unsigned int completeThreads)
+ {
+ m_LCP_fcPrepareThreadsRemaining = prepareThreads;
+ m_LCP_fcCompleteThreadsTotal = completeThreads;
+ }
+
+ void ResetLCP_fcComputationIndex()
+ {
+ m_mi_fc = 0;
+ }
+
+ void ResetSOR_ConstraintsReorderVariables(unsigned reorderThreads)
+ {
+ m_SOR_reorderHeadTaken = 0;
+ m_SOR_reorderTailTaken = 0;
+ m_SOR_bi_zeroHeadTaken = 0;
+ m_SOR_bi_zeroTailTaken = 0;
+ m_SOR_mi_zeroHeadTaken = 0;
+ m_SOR_mi_zeroTailTaken = 0;
+ m_SOR_reorderThreadsRemaining = reorderThreads;
+ }
+
+ void RecordLCP_IterationStart(unsigned int totalThreads, dCallReleaseeID nextReleasee)
+ {
+ m_LCP_iterationThreadsTotal = totalThreads;
+ m_LCP_iterationThreadsRemaining = totalThreads;
+ m_LCP_iterationNextReleasee = nextReleasee;
+ }
+
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ const dxQuickStepperLocalContext *m_localContext;
+ dReal *m_lambda;
+ dReal *m_cforce;
+ dReal *m_iMJ;
+ IndexError *m_order;
+ dReal *m_last_lambda;
+ atomicord32 *m_bi_links_or_mi_levels;
+ atomicord32 *m_mi_links;
+ dCallReleaseeID m_LCP_IterationSyncReleasee;
+ unsigned int m_LCP_IterationAllowedThreads;
+ dCallReleaseeID m_LCP_fcStartReleasee;
+ volatile atomicord32 m_ji_4a;
+ volatile atomicord32 m_mi_iMJ;
+ volatile atomicord32 m_mi_fc;
+ volatile atomicord32 m_LCP_fcPrepareThreadsRemaining;
+ unsigned int m_LCP_fcCompleteThreadsTotal;
+ volatile atomicord32 m_mi_Ad;
+ unsigned int m_LCP_iteration;
+ unsigned int m_LCP_iterationThreadsTotal;
+ volatile atomicord32 m_LCP_iterationThreadsRemaining;
+ dCallReleaseeID m_LCP_iterationNextReleasee;
+ volatile atomicord32 m_SOR_reorderHeadTaken;
+ volatile atomicord32 m_SOR_reorderTailTaken;
+ volatile atomicord32 m_SOR_bi_zeroHeadTaken;
+ volatile atomicord32 m_SOR_bi_zeroTailTaken;
+ volatile atomicord32 m_SOR_mi_zeroHeadTaken;
+ volatile atomicord32 m_SOR_mi_zeroTailTaken;
+ volatile atomicord32 m_SOR_reorderThreadsRemaining;
+ volatile atomicord32 m_cf_4b;
+ volatile atomicord32 m_ji_4b;
+};
+
+
+static int dxQuickStepIsland_Stage4a_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_iMJ_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_iMJSync_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_fcStart_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_fc_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+#ifdef WARM_STARTING
+static int dxQuickStepIsland_Stage4LCP_fcWarmComplete_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+#endif
+static int dxQuickStepIsland_Stage4LCP_Ad_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_ReorderPrep_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_IterationStart_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_ConstraintsReordering_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_ConstraintsReorderingSync_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_Iteration_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4LCP_IterationSync_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage4b_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage5_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+
+static void dxQuickStepIsland_Stage4a(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_iMJComputation(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_MTfcComputation(dxQuickStepperStage4CallContext *stage4CallContext, dCallReleaseeID callThisReleasee);
+#ifdef WARM_STARTING
+static void dxQuickStepIsland_Stage4LCP_MTfcComputation_warm(dxQuickStepperStage4CallContext *stage4CallContext, dCallReleaseeID callThisReleasee);
+static void dxQuickStepIsland_Stage4LCP_MTfcComputation_warmZeroArrays(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_MTfcComputation_warmPrepare(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_MTfcComputation_warmComplete(dxQuickStepperStage4CallContext *stage4CallContext);
+#endif
+static void dxQuickStepIsland_Stage4LCP_MTfcComputation_cold(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_STfcComputation(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_AdComputation(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_ReorderPrep(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_ConstraintsReordering(dxQuickStepperStage4CallContext *stage4CallContext);
+static bool dxQuickStepIsland_Stage4LCP_ConstraintsShuffling(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int iteration);
+static void dxQuickStepIsland_Stage4LCP_LinksArraysZeroing(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_DependencyMapForNewOrderRebuilding(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_DependencyMapFromSavedLevelsReconstruction(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_MTIteration(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int initiallyKnownToBeCompletedLevel);
+static void dxQuickStepIsland_Stage4LCP_STIteration(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage4LCP_IterationStep(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int i);
+static void dxQuickStepIsland_Stage4b(dxQuickStepperStage4CallContext *stage4CallContext);
+static void dxQuickStepIsland_Stage5(dxQuickStepperStage5CallContext *stage5CallContext);
+
+
+struct dxQuickStepperStage6CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, const dxQuickStepperLocalContext *localContext)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ m_bi_6a = 0;
+ m_bi_6b = 0;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ const dxQuickStepperLocalContext *m_localContext;
+ volatile atomicord32 m_bi_6a;
+ volatile atomicord32 m_bi_6b;
+};
+
+static int dxQuickStepIsland_Stage6a_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage6aSync_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxQuickStepIsland_Stage6b_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+
+static void dxQuickStepIsland_Stage6a(dxQuickStepperStage6CallContext *stage6CallContext);
+static void dxQuickStepIsland_Stage6_VelocityCheck(dxQuickStepperStage6CallContext *stage6CallContext);
+static void dxQuickStepIsland_Stage6b(dxQuickStepperStage6CallContext *stage6CallContext);
+
+//***************************************************************************
+// various common computations involving the matrix J
+
+// compute iMJ = inv(M)*J'
+
+template<unsigned int step_size>
+void compute_invM_JT (volatile atomicord32 *mi_storage, dReal *iMJ,
+ unsigned int m, const dReal *J, const dxJBodiesItem *jb,
+ dxBody * const *body, const dReal *invI)
+{
+ unsigned int m_steps = (m + (step_size - 1)) / step_size;
+
+ unsigned mi_step;
+ while ((mi_step = ThrsafeIncrementIntUpToLimit(mi_storage, m_steps)) != m_steps) {
+ unsigned int mi = mi_step * step_size;
+ const unsigned int miend = mi + dMIN(step_size, m - mi);
+
+ dReal *iMJ_ptr = iMJ + (sizeint)mi * IMJ__MAX;
+ const dReal *J_ptr = J + (sizeint)mi * JME__MAX;
+ while (true) {
+ int b1 = jb[mi].first;
+ int b2 = jb[mi].second;
+
+ dReal k1 = body[(unsigned)b1]->invMass;
+ for (unsigned int j = 0; j != JVE__L_COUNT; j++) iMJ_ptr[IMJ__1L_MIN + j] = k1 * J_ptr[JME__J1L_MIN + j];
+ const dReal *invIrow1 = invI + (sizeint)(unsigned)b1 * IIE__MAX + IIE__MATRIX_MIN;
+ dMultiply0_331 (iMJ_ptr + IMJ__1A_MIN, invIrow1, J_ptr + JME__J1A_MIN);
+
+ if (b2 != -1) {
+ dReal k2 = body[(unsigned)b2]->invMass;
+ for (unsigned int j = 0; j != JVE__L_COUNT; ++j) iMJ_ptr[IMJ__2L_MIN + j] = k2 * J_ptr[JME__J2L_MIN + j];
+ const dReal *invIrow2 = invI + (sizeint)(unsigned)b2 * IIE__MAX + IIE__MATRIX_MIN;
+ dMultiply0_331 (iMJ_ptr + IMJ__2A_MIN, invIrow2, J_ptr + JME__J2A_MIN);
+ }
+
+ if (++mi == miend) {
+ break;
+ }
+ iMJ_ptr += IMJ__MAX;
+ J_ptr += JME__MAX;
+ }
+ }
+}
+
+#ifdef WARM_STARTING
+
+static
+void multiply_invM_JT_init_array(unsigned int nb, atomicord32 *bi_links/*=[nb]*/)
+{
+ // const unsigned businessIndex_none = dxENCODE_INDEX(-1);
+ // for (unsigned int bi = 0; bi != nb; ++bi) {
+ // bi_links[bi] = businessIndex_none;
+ // }
+ memset(bi_links, 0, nb * sizeof(bi_links[0]));
+}
+
+// compute out = inv(M)*J'*in.
+template<unsigned int step_size>
+void multiply_invM_JT_prepare(volatile atomicord32 *mi_storage,
+ unsigned int m, const dxJBodiesItem *jb, atomicord32 *bi_links/*=[nb]*/, atomicord32 *mi_links/*=[2*m]*/)
+{
+ unsigned int m_steps = (m + (step_size - 1)) / step_size;
+
+ unsigned mi_step;
+ while ((mi_step = ThrsafeIncrementIntUpToLimit(mi_storage, m_steps)) != m_steps) {
+ unsigned int mi = mi_step * step_size;
+ const unsigned int miend = mi + dMIN(step_size, m - mi);
+
+ while (true) {
+ int b1 = jb[mi].first;
+ int b2 = jb[mi].second;
+
+ const unsigned encoded_mi = dxENCODE_INDEX(mi);
+ unsigned oldIndex_b1 = ThrsafeExchange(&bi_links[b1], encoded_mi);
+ mi_links[(sizeint)mi * 2] = oldIndex_b1;
+
+ if (b2 != -1) {
+ unsigned oldIndex_b2 = ThrsafeExchange(&bi_links[b2], encoded_mi);
+ mi_links[(sizeint)mi * 2 + 1] = oldIndex_b2;
+ }
+
+ if (++mi == miend) {
+ break;
+ }
+ }
+ }
+}
+
+template<unsigned int step_size, unsigned int out_offset, unsigned int out_stride>
+void multiply_invM_JT_complete(volatile atomicord32 *bi_storage, dReal *out,
+ unsigned int nb, const dReal *iMJ, const dxJBodiesItem *jb, const dReal *in,
+ atomicord32 *bi_links/*=[nb]*/, atomicord32 *mi_links/*=[2*m]*/)
+{
+ const unsigned businessIndex_none = dxENCODE_INDEX(-1);
+
+ unsigned int nb_steps = (nb + (step_size - 1)) / step_size;
+
+ unsigned bi_step;
+ while ((bi_step = ThrsafeIncrementIntUpToLimit(bi_storage, nb_steps)) != nb_steps) {
+ unsigned int bi = bi_step * step_size;
+ const unsigned int biend = bi + dMIN(step_size, nb - bi);
+
+ dReal *out_ptr = out + (sizeint)bi * out_stride + out_offset;
+ while (true) {
+ dReal psum0 = REAL(0.0), psum1 = REAL(0.0), psum2 = REAL(0.0), psum3 = REAL(0.0), psum4 = REAL(0.0), psum5 = REAL(0.0);
+
+ unsigned businessIndex = bi_links[bi];
+ while (businessIndex != businessIndex_none) {
+ unsigned int mi = dxDECODE_INDEX(businessIndex);
+ const dReal *iMJ_ptr;
+
+ if (bi == jb[mi].first) {
+ iMJ_ptr = iMJ + (sizeint)mi * IMJ__MAX + IMJ__1_MIN;
+ businessIndex = mi_links[(sizeint)mi * 2];
+ }
+ else {
+ dIASSERT(bi == jb[mi].second);
+
+ iMJ_ptr = iMJ + (sizeint)mi * IMJ__MAX + IMJ__2_MIN;
+ businessIndex = mi_links[(sizeint)mi * 2 + 1];
+ }
+
+ const dReal in_i = in[mi];
+ psum0 += in_i * iMJ_ptr[JVE_LX]; psum1 += in_i * iMJ_ptr[JVE_LY]; psum2 += in_i * iMJ_ptr[JVE_LZ];
+ psum3 += in_i * iMJ_ptr[JVE_AX]; psum4 += in_i * iMJ_ptr[JVE_AY]; psum5 += in_i * iMJ_ptr[JVE_AZ];
+ }
+
+ out_ptr[dDA_LX] = psum0; out_ptr[dDA_LY] = psum1; out_ptr[dDA_LZ] = psum2;
+ out_ptr[dDA_AX] = psum3; out_ptr[dDA_AY] = psum4; out_ptr[dDA_AZ] = psum5;
+
+ if (++bi == biend) {
+ break;
+ }
+ out_ptr += out_stride;
+ }
+ }
+}
+
+template<unsigned int out_offset, unsigned int out_stride>
+void _multiply_invM_JT (dReal *out,
+ unsigned int m, unsigned int nb, dReal *iMJ, const dxJBodiesItem *jb, const dReal *in)
+{
+ dSetZero (out, (sizeint)nb * out_stride);
+ const dReal *iMJ_ptr = iMJ;
+ for (unsigned int i=0; i<m; i++) {
+ int b1 = jb[i].first;
+ int b2 = jb[i].second;
+ const dReal in_i = in[i];
+
+ dReal *out_ptr = out + (sizeint)(unsigned)b1 * out_stride + out_offset;
+ for (unsigned int j = JVE__MIN; j != JVE__MAX; j++) out_ptr[j - JVE__MIN] += iMJ_ptr[IMJ__1_MIN + j] * in_i;
+ dSASSERT(out_stride - out_offset >= JVE__MAX);
+ dSASSERT(JVE__MAX == (int)dDA__MAX);
+
+ if (b2 != -1) {
+ out_ptr = out + (sizeint)(unsigned)b2 * out_stride + out_offset;
+ for (unsigned int j = JVE__MIN; j != JVE__MAX; j++) out_ptr[j - JVE__MIN] += iMJ_ptr[IMJ__2_MIN + j] * in_i;
+ dSASSERT(out_stride - out_offset >= JVE__MAX);
+ dSASSERT(JVE__MAX == (int)dDA__MAX);
+ }
+
+ iMJ_ptr += IMJ__MAX;
+ }
+}
+#endif
+
+// compute out = J*in.
+template<unsigned int step_size, unsigned int in_offset, unsigned int in_stride>
+void multiplyAdd_J (volatile atomicord32 *mi_storage,
+ unsigned int m, dReal *J, const dxJBodiesItem *jb, const dReal *in)
+{
+ unsigned int m_steps = (m + (step_size - 1)) / step_size;
+
+ unsigned mi_step;
+ while ((mi_step = ThrsafeIncrementIntUpToLimit(mi_storage, m_steps)) != m_steps) {
+ unsigned int mi = mi_step * step_size;
+ const unsigned int miend = mi + dMIN(step_size, m - mi);
+
+ dReal *J_ptr = J + (sizeint)mi * JME__MAX;
+ while (true) {
+ int b1 = jb[mi].first;
+ int b2 = jb[mi].second;
+ dReal sum = REAL(0.0);
+ const dReal *in_ptr = in + (sizeint)(unsigned)b1 * in_stride + in_offset;
+ for (unsigned int j = 0; j != JME__J1_COUNT; ++j) sum += J_ptr[j + JME__J1_MIN] * in_ptr[j];
+ dSASSERT(in_offset + JME__J1_COUNT <= in_stride);
+
+ if (b2 != -1) {
+ in_ptr = in + (sizeint)(unsigned)b2 * in_stride + in_offset;
+ for (unsigned int j = 0; j != JME__J2_COUNT; ++j) sum += J_ptr[j + JME__J2_MIN] * in_ptr[j];
+ dSASSERT(in_offset + JME__J2_COUNT <= in_stride);
+ }
+ J_ptr[JME_RHS] += sum;
+
+ if (++mi == miend) {
+ break;
+ }
+ J_ptr += JME__MAX;
+ }
+ }
+}
+
+
+struct IndexError {
+#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
+ dReal error; // error to sort on
+#endif
+ int index; // row index
+};
+
+
+#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
+
+static int compare_index_error (const void *a, const void *b)
+{
+ const IndexError *i1 = (IndexError*) a;
+ const IndexError *i2 = (IndexError*) b;
+ if (i1->error < i2->error) return -1;
+ if (i1->error > i2->error) return 1;
+ return 0;
+}
+
+#endif // #if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
+
+static inline
+bool IsSORConstraintsReorderRequiredForIteration(unsigned iteration)
+{
+ bool result = false;
+
+#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
+
+ result = true;
+
+
+#elif CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__RANDOMLY
+
+ // This logic is intended to skip randomization on the very first iteration
+ if (!dIN_RANGE(iteration, 0, RANDOM_CONSTRAINTS_REORDERING_FREQUENCY)
+ ? dIN_RANGE(iteration % RANDOM_CONSTRAINTS_REORDERING_FREQUENCY, RRS__MIN, RRS__MAX)
+ : iteration == 0) {
+ result = true;
+ }
+
+
+#else // #if CONSTRAINTS_REORDERING_METHOD != REORDERING_METHOD__BY_ERROR && CONSTRAINTS_REORDERING_METHOD != REORDERING_METHOD__RANDOMLY
+
+ if (iteration == 0) {
+ result = true;
+ }
+
+
+#endif
+
+ return result;
+}
+
+/*extern */
+void dxQuickStepIsland(const dxStepperProcessingCallContext *callContext)
+{
+ dxWorldProcessMemArena *memarena = callContext->m_stepperArena;
+ unsigned int nb = callContext->m_islandBodiesCount;
+ unsigned int _nj = callContext->m_islandJointsCount;
+
+ dReal *invI = memarena->AllocateOveralignedArray<dReal>((sizeint)nb * IIE__MAX, INVI_ALIGNMENT);
+ dJointWithInfo1 *const jointinfos = memarena->AllocateArray<dJointWithInfo1>(_nj);
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ dIASSERT(allowedThreads != 0);
+
+ void *stagesMemArenaState = memarena->SaveState();
+
+ dxQuickStepperStage1CallContext *stage1CallContext = (dxQuickStepperStage1CallContext *)memarena->AllocateBlock(sizeof(dxQuickStepperStage1CallContext));
+ stage1CallContext->Initialize(callContext, stagesMemArenaState, invI, jointinfos);
+
+ dxQuickStepperStage0BodiesCallContext *stage0BodiesCallContext = (dxQuickStepperStage0BodiesCallContext *)memarena->AllocateBlock(sizeof(dxQuickStepperStage0BodiesCallContext));
+ stage0BodiesCallContext->Initialize(callContext, invI);
+
+ dxQuickStepperStage0JointsCallContext *stage0JointsCallContext = (dxQuickStepperStage0JointsCallContext *)memarena->AllocateBlock(sizeof(dxQuickStepperStage0JointsCallContext));
+ stage0JointsCallContext->Initialize(callContext, jointinfos, &stage1CallContext->m_stage0Outputs);
+
+ if (allowedThreads == 1)
+ {
+ IFTIMING(dTimerStart("preprocessing"));
+ dxQuickStepIsland_Stage0_Bodies(stage0BodiesCallContext);
+ dxQuickStepIsland_Stage0_Joints(stage0JointsCallContext);
+ dxQuickStepIsland_Stage1(stage1CallContext);
+ }
+ else
+ {
+ unsigned bodyThreads = CalculateOptimalThreadsCount<1U>(nb, allowedThreads);
+ unsigned jointThreads = 1;
+
+ dxWorld *world = callContext->m_world;
+
+ dCallReleaseeID stage1CallReleasee;
+ world->PostThreadedCallForUnawareReleasee(NULL, &stage1CallReleasee, bodyThreads + jointThreads, callContext->m_finalReleasee,
+ NULL, &dxQuickStepIsland_Stage1_Callback, stage1CallContext, 0, "QuickStepIsland Stage1");
+
+ // It is preferable to post single threaded task first to be started sooner
+ world->PostThreadedCall(NULL, NULL, 0, stage1CallReleasee, NULL, &dxQuickStepIsland_Stage0_Joints_Callback, stage0JointsCallContext, 0, "QuickStepIsland Stage0-Joints");
+ dIASSERT(jointThreads == 1);
+
+ if (bodyThreads > 1) {
+ world->PostThreadedCallsGroup(NULL, bodyThreads - 1, stage1CallReleasee, &dxQuickStepIsland_Stage0_Bodies_Callback, stage0BodiesCallContext, "QuickStepIsland Stage0-Bodies");
+ }
+ dxQuickStepIsland_Stage0_Bodies(stage0BodiesCallContext);
+ world->AlterThreadedCallDependenciesCount(stage1CallReleasee, -1);
+ }
+}
+
+static
+int dxQuickStepIsland_Stage0_Bodies_Callback(void *_callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage0BodiesCallContext *callContext = (dxQuickStepperStage0BodiesCallContext *)_callContext;
+ dxQuickStepIsland_Stage0_Bodies(callContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage0_Bodies(dxQuickStepperStage0BodiesCallContext *callContext)
+{
+ dxBody * const *body = callContext->m_stepperCallContext->m_islandBodiesStart;
+ unsigned int nb = callContext->m_stepperCallContext->m_islandBodiesCount;
+
+ if (ThrsafeExchange(&callContext->m_tagsTaken, 1) == 0)
+ {
+ // number all bodies in the body list - set their tag values
+ for (unsigned int i=0; i<nb; i++) body[i]->tag = i;
+ }
+
+ if (ThrsafeExchange(&callContext->m_gravityTaken, 1) == 0)
+ {
+ dxWorld *world = callContext->m_stepperCallContext->m_world;
+
+ // add the gravity force to all bodies
+ // since gravity does normally have only one component it's more efficient
+ // to run three loops for each individual component
+ dxBody *const *const bodyend = body + nb;
+ dReal gravity_x = world->gravity[0];
+ if (gravity_x) {
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; bodycurr++) {
+ dxBody *b = *bodycurr;
+ if ((b->flags & dxBodyNoGravity) == 0) {
+ b->facc[0] += b->mass.mass * gravity_x;
+ }
+ }
+ }
+ dReal gravity_y = world->gravity[1];
+ if (gravity_y) {
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; bodycurr++) {
+ dxBody *b = *bodycurr;
+ if ((b->flags & dxBodyNoGravity) == 0) {
+ b->facc[1] += b->mass.mass * gravity_y;
+ }
+ }
+ }
+ dReal gravity_z = world->gravity[2];
+ if (gravity_z) {
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; bodycurr++) {
+ dxBody *b = *bodycurr;
+ if ((b->flags & dxBodyNoGravity) == 0) {
+ b->facc[2] += b->mass.mass * gravity_z;
+ }
+ }
+ }
+ }
+
+ // for all bodies, compute the inertia tensor and its inverse in the global
+ // frame, and compute the rotational force and add it to the torque
+ // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
+ {
+ dReal *invI = callContext->m_invI;
+ unsigned int bodyIndex;
+ while ((bodyIndex = ThrsafeIncrementIntUpToLimit(&callContext->m_inertiaBodyIndex, nb)) != nb) {
+ dReal *invIrow = invI + (sizeint)bodyIndex * IIE__MAX;
+ dxBody *b = body[bodyIndex];
+
+ dMatrix3 tmp;
+ // compute inverse inertia tensor in global frame
+ dMultiply2_333 (tmp, b->invI, b->posr.R);
+ dMultiply0_333 (invIrow + IIE__MATRIX_MIN, b->posr.R, tmp);
+
+ // Don't apply gyroscopic torques to bodies
+ // if not flagged or the body is kinematic
+ if ((b->flags & dxBodyGyroscopic) && (b->invMass > 0)) {
+ dMatrix3 I;
+ // compute inertia tensor in global frame
+ dMultiply2_333 (tmp, b->mass.I, b->posr.R);
+ dMultiply0_333 (I, b->posr.R, tmp);
+ // compute rotational force
+#if 0
+ // Explicit computation
+ dMultiply0_331 (tmp, I, b->avel);
+ dSubtractVectorCross3(b->tacc, b->avel, tmp);
+#else
+ // Do the implicit computation based on
+ //"Stabilizing Gyroscopic Forces in Rigid Multibody Simulations"
+ // (Lacoursière 2006)
+ dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
+ dVector3 L; // Compute angular momentum
+ dMultiply0_331(L, I, b->avel);
+
+ // Compute a new effective 'inertia tensor'
+ // for the implicit step: the cross-product
+ // matrix of the angular momentum plus the
+ // old tensor scaled by the timestep.
+ // Itild may not be symmetric pos-definite,
+ // but we can still use it to compute implicit
+ // gyroscopic torques.
+ dMatrix3 Itild = { 0 };
+ dSetCrossMatrixMinus(Itild, L, 4);
+ for (int ii = dM3E__MIN; ii < dM3E__MAX; ++ii) {
+ Itild[ii] = Itild[ii] * h + I[ii];
+ }
+
+ // Scale momentum by inverse time to get
+ // a sort of "torque"
+ dScaleVector3(L, dRecip(h));
+ // Invert the pseudo-tensor
+ dMatrix3 itInv;
+ // This is a closed-form inversion.
+ // It's probably not numerically stable
+ // when dealing with small masses with
+ // a large asymmetry.
+ // An LU decomposition might be better.
+ if (dInvertMatrix3(itInv, Itild) != 0) {
+ // "Divide" the original tensor
+ // by the pseudo-tensor (on the right)
+ dMultiply0_333(Itild, I, itInv);
+ // Subtract an identity matrix
+ Itild[dM3E_XX] -= 1; Itild[dM3E_YY] -= 1; Itild[dM3E_ZZ] -= 1;
+
+ // This new inertia matrix rotates the
+ // momentum to get a new set of torques
+ // that will work correctly when applied
+ // to the old inertia matrix as explicit
+ // torques with a semi-implicit update
+ // step.
+ dVector3 tau0;
+ dMultiply0_331(tau0, Itild, L);
+
+ // Add the gyro torques to the torque
+ // accumulator
+ dAddVectors3(b->tacc, b->tacc, tau0);
+ }
+#endif
+ }
+ }
+ }
+}
+
+static
+int dxQuickStepIsland_Stage0_Joints_Callback(void *_callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage0JointsCallContext *callContext = (dxQuickStepperStage0JointsCallContext *)_callContext;
+ dxQuickStepIsland_Stage0_Joints(callContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage0_Joints(dxQuickStepperStage0JointsCallContext *callContext)
+{
+ dxJoint * const *_joint = callContext->m_stepperCallContext->m_islandJointsStart;
+ unsigned int _nj = callContext->m_stepperCallContext->m_islandJointsCount;
+
+ // get joint information (m = total constraint dimension, nub = number of unbounded variables).
+ // joints with m=0 are inactive and are removed from the joints array
+ // entirely, so that the code that follows does not consider them.
+ {
+ unsigned int mcurr = 0, mfbcurr = 0;
+ dJointWithInfo1 *jicurr = callContext->m_jointinfos;
+ dxJoint *const *const _jend = _joint + _nj;
+ for (dxJoint *const *_jcurr = _joint; _jcurr != _jend; _jcurr++) { // jicurr=dest, _jcurr=src
+ dxJoint *j = *_jcurr;
+ j->getInfo1 (&jicurr->info);
+ dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 6 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
+
+ unsigned int jm = jicurr->info.m;
+ if (jm != 0) {
+ mcurr += jm;
+ if (j->feedback != NULL) {
+ mfbcurr += jm;
+ }
+ jicurr->joint = j;
+ jicurr++;
+ }
+ }
+ callContext->m_stage0Outputs->m = mcurr;
+ callContext->m_stage0Outputs->mfb = mfbcurr;
+ callContext->m_stage0Outputs->nj = (unsigned int)(jicurr - callContext->m_jointinfos);
+ dIASSERT((sizeint)(jicurr - callContext->m_jointinfos) < UINT_MAX || (sizeint)(jicurr - callContext->m_jointinfos) == UINT_MAX); // to avoid "...always evaluates to true" warnings
+ }
+}
+
+static
+int dxQuickStepIsland_Stage1_Callback(void *_stage1CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage1CallContext *stage1CallContext = (dxQuickStepperStage1CallContext *)_stage1CallContext;
+ dxQuickStepIsland_Stage1(stage1CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage1(dxQuickStepperStage1CallContext *stage1CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage1CallContext->m_stepperCallContext;
+ dReal *invI = stage1CallContext->m_invI;
+ dJointWithInfo1 *jointinfos = stage1CallContext->m_jointinfos;
+ unsigned int nj = stage1CallContext->m_stage0Outputs.nj;
+ unsigned int m = stage1CallContext->m_stage0Outputs.m;
+ unsigned int mfb = stage1CallContext->m_stage0Outputs.mfb;
+
+ dxWorldProcessMemArena *memarena = callContext->m_stepperArena;
+ memarena->RestoreState(stage1CallContext->m_stageMemArenaState);
+ stage1CallContext = NULL; // WARNING! _stage1CallContext is not valid after this point!
+ dIVERIFY(stage1CallContext == NULL); // To suppress unused variable assignment warnings
+
+ {
+ unsigned int _nj = callContext->m_islandJointsCount;
+ memarena->ShrinkArray<dJointWithInfo1>(jointinfos, _nj, nj);
+ }
+
+ dxMIndexItem *mindex = NULL;
+ dxJBodiesItem *jb = NULL;
+ int *findex = NULL;
+ dReal *J = NULL, *Jcopy = NULL;
+
+ // if there are constraints, compute the constraint force
+ if (m > 0) {
+ mindex = memarena->AllocateArray<dxMIndexItem>(nj + 1);
+ {
+ dxMIndexItem *mcurr = mindex;
+ unsigned int moffs = 0, mfboffs = 0;
+ mcurr->mIndex = moffs;
+ mcurr->fbIndex = mfboffs;
+ ++mcurr;
+
+ const dJointWithInfo1 *const jiend = jointinfos + nj;
+ for (const dJointWithInfo1 *jicurr = jointinfos; jicurr != jiend; ++jicurr) {
+ dxJoint *joint = jicurr->joint;
+ moffs += jicurr->info.m;
+ if (joint->feedback) { mfboffs += jicurr->info.m; }
+ mcurr->mIndex = moffs;
+ mcurr->fbIndex = mfboffs;
+ ++mcurr;
+ }
+ }
+
+ jb = memarena->AllocateArray<dxJBodiesItem>(m);
+ findex = memarena->AllocateArray<int>(m);
+ J = memarena->AllocateOveralignedArray<dReal>((sizeint)m * JME__MAX, JACOBIAN_ALIGNMENT);
+ Jcopy = memarena->AllocateOveralignedArray<dReal>((sizeint)mfb * JCE__MAX, JCOPY_ALIGNMENT);
+ }
+
+ dxQuickStepperLocalContext *localContext = (dxQuickStepperLocalContext *)memarena->AllocateBlock(sizeof(dxQuickStepperLocalContext));
+ localContext->Initialize(invI, jointinfos, nj, m, mfb, mindex, jb, findex, J, Jcopy);
+
+ void *stage1MemarenaState = memarena->SaveState();
+ dxQuickStepperStage3CallContext *stage3CallContext = (dxQuickStepperStage3CallContext*)memarena->AllocateBlock(sizeof(dxQuickStepperStage3CallContext));
+ stage3CallContext->Initialize(callContext, localContext, stage1MemarenaState);
+
+ if (m > 0) {
+ unsigned int nb = callContext->m_islandBodiesCount;
+ // create a constraint equation right hand side vector `rhs', a constraint
+ // force mixing vector `cfm', and LCP low and high bound vectors, and an
+ // 'findex' vector.
+ dReal *rhs_tmp = memarena->AllocateArray<dReal>((sizeint)nb * RHS__MAX);
+
+ dxQuickStepperStage2CallContext *stage2CallContext = (dxQuickStepperStage2CallContext*)memarena->AllocateBlock(sizeof(dxQuickStepperStage2CallContext));
+ stage2CallContext->Initialize(callContext, localContext, rhs_tmp);
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ dIASSERT(allowedThreads != 0);
+
+ if (allowedThreads == 1)
+ {
+ IFTIMING (dTimerNow ("create J"));
+ dxQuickStepIsland_Stage2a(stage2CallContext);
+ IFTIMING (dTimerNow ("compute rhs_tmp"));
+ dxQuickStepIsland_Stage2b(stage2CallContext);
+ dxQuickStepIsland_Stage2c(stage2CallContext);
+ dxQuickStepIsland_Stage3(stage3CallContext);
+ }
+ else
+ {
+ dxWorld *world = callContext->m_world;
+
+ dCallReleaseeID stage3CallReleasee;
+ world->PostThreadedCallForUnawareReleasee(NULL, &stage3CallReleasee, 1, callContext->m_finalReleasee,
+ NULL, &dxQuickStepIsland_Stage3_Callback, stage3CallContext, 0, "QuickStepIsland Stage3");
+
+ dCallReleaseeID stage2bSyncReleasee;
+ world->PostThreadedCall(NULL, &stage2bSyncReleasee, 1, stage3CallReleasee,
+ NULL, &dxQuickStepIsland_Stage2bSync_Callback, stage2CallContext, 0, "QuickStepIsland Stage2b Sync");
+
+ unsigned stage2a_allowedThreads = CalculateOptimalThreadsCount<1U>(nj, allowedThreads);
+
+ dCallReleaseeID stage2aSyncReleasee;
+ world->PostThreadedCall(NULL, &stage2aSyncReleasee, stage2a_allowedThreads, stage2bSyncReleasee,
+ NULL, &dxQuickStepIsland_Stage2aSync_Callback, stage2CallContext, 0, "QuickStepIsland Stage2a Sync");
+
+ if (stage2a_allowedThreads > 1) {
+ world->PostThreadedCallsGroup(NULL, stage2a_allowedThreads - 1, stage2aSyncReleasee, &dxQuickStepIsland_Stage2a_Callback, stage2CallContext, "QuickStepIsland Stage2a");
+ }
+ dxQuickStepIsland_Stage2a(stage2CallContext);
+ world->AlterThreadedCallDependenciesCount(stage2aSyncReleasee, -1);
+ }
+ }
+ else {
+ dxQuickStepIsland_Stage3(stage3CallContext);
+ }
+}
+
+
+static
+int dxQuickStepIsland_Stage2a_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage2CallContext *stage2CallContext = (dxQuickStepperStage2CallContext *)_stage2CallContext;
+ dxQuickStepIsland_Stage2a(stage2CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage2a(dxQuickStepperStage2CallContext *stage2CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ dxQuickStepperLocalContext *localContext = stage2CallContext->m_localContext;
+ dJointWithInfo1 *jointinfos = localContext->m_jointinfos;
+ unsigned int nj = localContext->m_nj;
+ const dxMIndexItem *mindex = localContext->m_mindex;
+
+ const dReal stepsizeRecip = dRecip(callContext->m_stepSize);
+ {
+ int *findex = localContext->m_findex;
+ dReal *J = localContext->m_J;
+ dReal *JCopy = localContext->m_Jcopy;
+
+ // get jacobian data from constraints. an m*16 matrix will be created
+ // to store the two jacobian blocks from each constraint. it has this
+ // format:
+ //
+ // l1 l1 l1 a1 a1 a1 rhs cfm l2 l2 l2 a2 a2 a2 lo hi \ .
+ // l1 l1 l1 a1 a1 a1 rhs cfm l2 l2 l2 a2 a2 a2 lo hi }-- jacobian for joint 0, body 1 and body 2 (3 rows)
+ // l1 l1 l1 a1 a1 a1 rhs cfm l2 l2 l2 a2 a2 a2 lo hi /
+ // l1 l1 l1 a1 a1 a1 rhs cfm l2 l2 l2 a2 a2 a2 lo hi }--- jacobian for joint 1, body 1 and body 2 (3 rows)
+ // etc...
+ //
+ // (lll) = linear jacobian data
+ // (aaa) = angular jacobian data
+ //
+ dxWorld *world = callContext->m_world;
+ const dReal worldERP = world->global_erp;
+ const dReal worldCFM = world->global_cfm;
+
+ unsigned validFIndices = 0;
+
+ unsigned ji;
+ while ((ji = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_ji_J, nj)) != nj) {
+ const unsigned ofsi = mindex[ji].mIndex;
+ const unsigned int infom = mindex[ji + 1].mIndex - ofsi;
+
+ dReal *const JRow = J + (sizeint)ofsi * JME__MAX;
+ {
+ dReal *const JEnd = JRow + infom * JME__MAX;
+ for (dReal *JCurr = JRow; JCurr != JEnd; JCurr += JME__MAX) {
+ dSetZero(JCurr + JME__J1_MIN, JME__J1_COUNT);
+ JCurr[JME_RHS] = REAL(0.0);
+ JCurr[JME_CFM] = worldCFM;
+ dSetZero(JCurr + JME__J2_MIN, JME__J2_COUNT);
+ JCurr[JME_LO] = -dInfinity;
+ JCurr[JME_HI] = dInfinity;
+ dSASSERT(JME__J1_COUNT + 2 + JME__J2_COUNT + 2 == JME__MAX);
+ }
+ }
+ int *findexRow = findex + ofsi;
+ dSetValue(findexRow, infom, -1);
+
+ dxJoint *joint = jointinfos[ji].joint;
+ joint->getInfo2(stepsizeRecip, worldERP, JME__MAX, JRow + JME__J1_MIN, JRow + JME__J2_MIN, JME__MAX, JRow + JME__RHS_CFM_MIN, JRow + JME__LO_HI_MIN, findexRow);
+
+ // findex iteration is compact and is not going to pollute caches - do it first
+ {
+ // adjust returned findex values for global index numbering
+ int *const findicesEnd = findexRow + infom;
+ for (int *findexCurr = findexRow; findexCurr != findicesEnd; ++findexCurr) {
+ int fival = *findexCurr;
+ if (fival != -1) {
+ *findexCurr = fival + ofsi;
+ ++validFIndices;
+ }
+ }
+ }
+ {
+ dReal *const JEnd = JRow + infom * JME__MAX;
+ for (dReal *JCurr = JRow; JCurr != JEnd; JCurr += JME__MAX) {
+ JCurr[JME_RHS] *= stepsizeRecip;
+ JCurr[JME_CFM] *= stepsizeRecip;
+ }
+ }
+ {
+ // we need a copy of Jacobian for joint feedbacks
+ // because it gets destroyed by SOR solver
+ // instead of saving all Jacobian, we can save just rows
+ // for joints, that requested feedback (which is normally much less)
+ unsigned mfbIndex = mindex[ji].fbIndex;
+ if (mfbIndex != mindex[ji + 1].fbIndex) {
+ dReal *const JEnd = JRow + infom * JME__MAX;
+ dReal *JCopyRow = JCopy + mfbIndex * JCE__MAX; // Random access by mfbIndex here! Do not optimize!
+ for (const dReal *JCurr = JRow; ; ) {
+ for (unsigned i = 0; i != JME__J1_COUNT; ++i) { JCopyRow[i + JCE__J1_MIN] = JCurr[i + JME__J1_MIN]; }
+ for (unsigned j = 0; j != JME__J2_COUNT; ++j) { JCopyRow[j + JCE__J2_MIN] = JCurr[j + JME__J2_MIN]; }
+ JCopyRow += JCE__MAX;
+ dSASSERT((unsigned)JCE__J1_COUNT == JME__J1_COUNT);
+ dSASSERT((unsigned)JCE__J2_COUNT == JME__J2_COUNT);
+ dSASSERT(JCE__J1_COUNT + JCE__J2_COUNT == JCE__MAX);
+
+ if ((JCurr += JME__MAX) == JEnd) {
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ if (validFIndices != 0) {
+ ThrsafeAdd(&localContext->m_valid_findices, validFIndices);
+ }
+ }
+
+ {
+ dxJBodiesItem *jb = localContext->m_jb;
+
+ // create an array of body numbers for each joint row
+ unsigned ji;
+ while ((ji = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_ji_jb, nj)) != nj) {
+ dxJoint *joint = jointinfos[ji].joint;
+ int b1 = (joint->node[0].body) ? (joint->node[0].body->tag) : -1;
+ int b2 = (joint->node[1].body) ? (joint->node[1].body->tag) : -1;
+
+ dxJBodiesItem *const jb_end = jb + mindex[ji + 1].mIndex;
+ dxJBodiesItem *jb_ptr = jb + mindex[ji].mIndex;
+ for (; jb_ptr != jb_end; ++jb_ptr) {
+ jb_ptr->first = b1;
+ jb_ptr->second = b2;
+ }
+ }
+ }
+}
+
+static
+int dxQuickStepIsland_Stage2aSync_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ dxQuickStepperStage2CallContext *stage2CallContext = (dxQuickStepperStage2CallContext *)_stage2CallContext;
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const unsigned int nb = callContext->m_islandBodiesCount;
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ unsigned int stage2b_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE2B_STEP>(nb, allowedThreads);
+
+ if (stage2b_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage2b_allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, stage2b_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage2b_Callback, stage2CallContext, "QuickStepIsland Stage2b");
+ }
+ dxQuickStepIsland_Stage2b(stage2CallContext);
+
+ return 1;
+}
+
+static
+int dxQuickStepIsland_Stage2b_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage2CallContext *stage2CallContext = (dxQuickStepperStage2CallContext *)_stage2CallContext;
+ dxQuickStepIsland_Stage2b(stage2CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage2b(dxQuickStepperStage2CallContext *stage2CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage2CallContext->m_localContext;
+
+ const dReal stepsizeRecip = dRecip(callContext->m_stepSize);
+ {
+ // Warning!!!
+ // This code reads facc/tacc fields of body objects which (the fields)
+ // may be modified by dxJoint::getInfo2(). Therefore the code must be
+ // in different sub-stage from Jacobian construction in Stage2a
+ // to ensure proper synchronization and avoid accessing numbers being modified.
+ // Warning!!!
+ dxBody * const *const body = callContext->m_islandBodiesStart;
+ const unsigned int nb = callContext->m_islandBodiesCount;
+ const dReal *invI = localContext->m_invI;
+ dReal *rhs_tmp = stage2CallContext->m_rhs_tmp;
+
+ // compute the right hand side `rhs'
+
+ const unsigned int step_size = dxQUICKSTEPISLAND_STAGE2B_STEP;
+ unsigned int nb_steps = (nb + (step_size - 1)) / step_size;
+
+ // put -(v/h + invM*fe) into rhs_tmp
+ unsigned bi_step;
+ while ((bi_step = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_bi, nb_steps)) != nb_steps) {
+ unsigned int bi = bi_step * step_size;
+ const unsigned int biend = bi + dMIN(step_size, nb - bi);
+
+ dReal *rhscurr = rhs_tmp + (sizeint)bi * RHS__MAX;
+ const dReal *invIrow = invI + (sizeint)bi * IIE__MAX;
+ while (true) {
+ dxBody *b = body[bi];
+ dReal body_invMass = b->invMass;
+ for (unsigned int j = dSA__MIN; j != dSA__MAX; ++j) rhscurr[RHS__L_MIN + j] = -(b->facc[dV3E__AXES_MIN + j] * body_invMass + b->lvel[dV3E__AXES_MIN + j] * stepsizeRecip);
+ dMultiply0_331 (rhscurr + RHS__A_MIN, invIrow + IIE__MATRIX_MIN, b->tacc);
+ for (unsigned int k = dSA__MIN; k != dSA__MAX; ++k) rhscurr[RHS__A_MIN + k] = -(b->avel[dV3E__AXES_MIN + k] * stepsizeRecip) - rhscurr[RHS__A_MIN + k];
+
+ if (++bi == biend) {
+ break;
+ }
+ rhscurr += RHS__MAX;
+ invIrow += IIE__MAX;
+ }
+ }
+ }
+}
+
+static
+int dxQuickStepIsland_Stage2bSync_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ dxQuickStepperStage2CallContext *stage2CallContext = (dxQuickStepperStage2CallContext *)_stage2CallContext;
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+
+ const dxQuickStepperLocalContext *localContext = stage2CallContext->m_localContext;
+ unsigned int m = localContext->m_m;
+
+ unsigned int stage2c_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE2C_STEP>(m, allowedThreads);
+
+ if (stage2c_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage2c_allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, stage2c_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage2c_Callback, stage2CallContext, "QuickStepIsland Stage2c");
+ }
+ dxQuickStepIsland_Stage2c(stage2CallContext);
+
+ return 1;
+}
+
+
+static
+int dxQuickStepIsland_Stage2c_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage2CallContext *stage2CallContext = (dxQuickStepperStage2CallContext *)_stage2CallContext;
+ dxQuickStepIsland_Stage2c(stage2CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage2c(dxQuickStepperStage2CallContext *stage2CallContext)
+{
+ //const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage2CallContext->m_localContext;
+
+ //const dReal stepsizeRecip = dRecip(callContext->m_stepSize);
+ {
+ // Warning!!!
+ // This code depends on rhs_tmp and therefore must be in different sub-stage
+ // from rhs_tmp calculation in Stage2b to ensure proper synchronization
+ // and avoid accessing numbers being modified.
+ // Warning!!!
+ dReal *J = localContext->m_J;
+ const dxJBodiesItem *jb = localContext->m_jb;
+ const dReal *rhs_tmp = stage2CallContext->m_rhs_tmp;
+ const unsigned int m = localContext->m_m;
+
+ // add J*rhs_tmp to rhs
+ multiplyAdd_J<dxQUICKSTEPISLAND_STAGE2C_STEP, RHS__DYNAMICS_MIN, RHS__MAX>(&stage2CallContext->m_Jrhsi, m, J, jb, rhs_tmp);
+ }
+}
+
+
+static
+int dxQuickStepIsland_Stage3_Callback(void *_stage3CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage3CallContext *stage3CallContext = (dxQuickStepperStage3CallContext *)_stage3CallContext;
+ dxQuickStepIsland_Stage3(stage3CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage3(dxQuickStepperStage3CallContext *stage3CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage3CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage3CallContext->m_localContext;
+
+ dxWorldProcessMemArena *memarena = callContext->m_stepperArena;
+ memarena->RestoreState(stage3CallContext->m_stage1MemArenaState);
+ stage3CallContext = NULL; // WARNING! stage3CallContext is not valid after this point!
+ dIVERIFY(stage3CallContext == NULL); // To suppress unused variable assignment warnings
+
+ void *stage3MemarenaState = memarena->SaveState();
+ dxQuickStepperStage5CallContext *stage5CallContext = (dxQuickStepperStage5CallContext *)memarena->AllocateBlock(sizeof(dxQuickStepperStage5CallContext));
+ stage5CallContext->Initialize(callContext, localContext, stage3MemarenaState);
+
+ unsigned int m = localContext->m_m;
+
+ if (m > 0) {
+ // load lambda from the value saved on the previous iteration
+ dReal *lambda = memarena->AllocateArray<dReal>(m);
+
+ unsigned int nb = callContext->m_islandBodiesCount;
+ dReal *cforce = memarena->AllocateArray<dReal>((sizeint)nb * CFE__MAX);
+ dReal *iMJ = memarena->AllocateOveralignedArray<dReal>((sizeint)m * IMJ__MAX, INVMJ_ALIGNMENT);
+ // order to solve constraint rows in
+ IndexError *order = memarena->AllocateArray<IndexError>(m);
+ dReal *last_lambda = NULL;
+#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
+ // the lambda computed at the previous iteration.
+ // this is used to measure error for when we are reordering the indexes.
+ last_lambda = memarena->AllocateArray<dReal>(m);
+#endif
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ bool singleThreadedExecution = allowedThreads == 1;
+ dIASSERT(allowedThreads >= 1);
+
+ atomicord32 *bi_links_or_mi_levels = NULL;
+ atomicord32 *mi_links = NULL;
+#if !dTHREADING_INTF_DISABLED
+ bi_links_or_mi_levels = memarena->AllocateArray<atomicord32>(dMAX(nb, m));
+ mi_links = memarena->AllocateArray<atomicord32>(2 * ((sizeint)m + 1));
+#else
+ dIASSERT(singleThreadedExecution);
+#endif
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)memarena->AllocateBlock(sizeof(dxQuickStepperStage4CallContext));
+ stage4CallContext->Initialize(callContext, localContext, lambda, cforce, iMJ, order, last_lambda, bi_links_or_mi_levels, mi_links);
+
+ if (singleThreadedExecution)
+ {
+ dxQuickStepIsland_Stage4a(stage4CallContext);
+
+ IFTIMING (dTimerNow ("solving LCP problem"));
+ dxQuickStepIsland_Stage4LCP_iMJComputation(stage4CallContext);
+ dxQuickStepIsland_Stage4LCP_STfcComputation(stage4CallContext);
+ dxQuickStepIsland_Stage4LCP_AdComputation(stage4CallContext);
+ dxQuickStepIsland_Stage4LCP_ReorderPrep(stage4CallContext);
+
+ dxWorld *world = callContext->m_world;
+ const unsigned int num_iterations = world->qs.num_iterations;
+ for (unsigned int iteration=0; iteration < num_iterations; iteration++) {
+ if (IsSORConstraintsReorderRequiredForIteration(iteration)) {
+ stage4CallContext->ResetSOR_ConstraintsReorderVariables(0);
+ dxQuickStepIsland_Stage4LCP_ConstraintsShuffling(stage4CallContext, iteration);
+ }
+ dxQuickStepIsland_Stage4LCP_STIteration(stage4CallContext);
+ }
+
+ dxQuickStepIsland_Stage4b(stage4CallContext);
+ dxQuickStepIsland_Stage5(stage5CallContext);
+ }
+ else
+ {
+ dxWorld *world = callContext->m_world;
+
+ dCallReleaseeID stage5CallReleasee;
+ world->PostThreadedCallForUnawareReleasee(NULL, &stage5CallReleasee, 1, callContext->m_finalReleasee,
+ NULL, &dxQuickStepIsland_Stage5_Callback, stage5CallContext, 0, "QuickStepIsland Stage5");
+
+ dCallReleaseeID stage4LCP_IterationSyncReleasee;
+ world->PostThreadedCall(NULL, &stage4LCP_IterationSyncReleasee, 1, stage5CallReleasee,
+ NULL, &dxQuickStepIsland_Stage4LCP_IterationSync_Callback, stage4CallContext, 0, "QuickStepIsland Stage4LCP_Iteration Sync");
+
+ unsigned int stage4LCP_Iteration_allowedThreads = CalculateOptimalThreadsCount<1U>(m, allowedThreads);
+ stage4CallContext->AssignLCP_IterationData(stage4LCP_IterationSyncReleasee, stage4LCP_Iteration_allowedThreads);
+
+ dCallReleaseeID stage4LCP_IterationStartReleasee;
+ world->PostThreadedCall(NULL, &stage4LCP_IterationStartReleasee, 3, stage4LCP_IterationSyncReleasee,
+ NULL, &dxQuickStepIsland_Stage4LCP_IterationStart_Callback, stage4CallContext, 0, "QuickStepIsland Stage4LCP_Iteration Start");
+
+ unsigned int nj = localContext->m_nj;
+ unsigned int stage4a_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE4A_STEP>(nj, allowedThreads);
+
+ dCallReleaseeID stage4LCP_fcStartReleasee;
+ // Note: It is unnecessary to make fc dependent on 4a if there is no WARM_STARTING
+ // However I'm doing so to minimize use of preprocessor conditions in sources
+ unsigned stage4LCP_fcDependenciesCountToUse = stage4a_allowedThreads;
+#ifdef WARM_STARTING
+ // Posted with extra dependency to be removed from dxQuickStepIsland_Stage4LCP_iMJSync_Callback
+ stage4LCP_fcDependenciesCountToUse += 1;
+#endif
+ world->PostThreadedCall(NULL, &stage4LCP_fcStartReleasee, stage4LCP_fcDependenciesCountToUse, stage4LCP_IterationStartReleasee,
+ NULL, &dxQuickStepIsland_Stage4LCP_fcStart_Callback, stage4CallContext, 0, "QuickStepIsland Stage4LCP_fc Start");
+#ifdef WARM_STARTING
+ stage4CallContext->AssignLCP_fcStartReleasee(stage4LCP_fcStartReleasee);
+#endif
+
+ unsigned stage4LCP_iMJ_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE4LCP_IMJ_STEP>(m, allowedThreads);
+
+ dCallReleaseeID stage4LCP_iMJSyncReleasee;
+ world->PostThreadedCall(NULL, &stage4LCP_iMJSyncReleasee, stage4LCP_iMJ_allowedThreads, stage4LCP_IterationStartReleasee,
+ NULL, &dxQuickStepIsland_Stage4LCP_iMJSync_Callback, stage4CallContext, 0, "QuickStepIsland Stage4LCP_iMJ Sync");
+
+ world->PostThreadedCall(NULL, NULL, 0, stage4LCP_IterationStartReleasee, NULL, &dxQuickStepIsland_Stage4LCP_ReorderPrep_Callback, stage4CallContext, 0, "QuickStepIsland Stage4LCP_ReorderPrep");
+ world->PostThreadedCallsGroup(NULL, stage4a_allowedThreads, stage4LCP_fcStartReleasee, &dxQuickStepIsland_Stage4a_Callback, stage4CallContext, "QuickStepIsland Stage4a");
+
+ if (stage4LCP_iMJ_allowedThreads > 1) {
+ world->PostThreadedCallsGroup(NULL, stage4LCP_iMJ_allowedThreads - 1, stage4LCP_iMJSyncReleasee, &dxQuickStepIsland_Stage4LCP_iMJ_Callback, stage4CallContext, "QuickStepIsland Stage4LCP_iMJ");
+ }
+ dxQuickStepIsland_Stage4LCP_iMJComputation(stage4CallContext);
+ world->AlterThreadedCallDependenciesCount(stage4LCP_iMJSyncReleasee, -1);
+ }
+ }
+ else {
+ dxQuickStepIsland_Stage5(stage5CallContext);
+ }
+}
+
+static
+int dxQuickStepIsland_Stage4a_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ dxQuickStepIsland_Stage4a(stage4CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4a(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ dReal *lambda = stage4CallContext->m_lambda;
+ const dxMIndexItem *mindex = localContext->m_mindex;
+#ifdef WARM_STARTING
+ dJointWithInfo1 *jointinfos = localContext->m_jointinfos;
+#endif
+ unsigned int nj = localContext->m_nj;
+ const unsigned int step_size = dxQUICKSTEPISLAND_STAGE4A_STEP;
+ unsigned int nj_steps = (nj + (step_size - 1)) / step_size;
+
+ unsigned ji_step;
+ while ((ji_step = ThrsafeIncrementIntUpToLimit(&stage4CallContext->m_ji_4a, nj_steps)) != nj_steps) {
+ unsigned int ji = ji_step * step_size;
+ dReal *lambdacurr = lambda + mindex[ji].mIndex;
+#ifdef WARM_STARTING
+ const dJointWithInfo1 *jicurr = jointinfos + ji;
+ const dJointWithInfo1 *const jiend = jicurr + dMIN(step_size, nj - ji);
+
+ do {
+ const dReal *joint_lambdas = jicurr->joint->lambda;
+ dReal *const lambdsnext = lambdacurr + jicurr->info.m;
+
+ while (true) {
+ // for warm starting, multiplication by 0.9 seems to be necessary to prevent
+ // jerkiness in motor-driven joints. I have no idea why this works.
+ *lambdacurr = *joint_lambdas * 0.9;
+
+ if (++lambdacurr == lambdsnext) {
+ break;
+ }
+
+ ++joint_lambdas;
+ }
+ }
+ while (++jicurr != jiend);
+#else
+ dReal *lambdsnext = lambda + mindex[ji + dMIN(step_size, nj - ji)].mIndex;
+ dSetZero(lambdacurr, lambdsnext - lambdacurr);
+#endif
+ }
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_iMJ_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ dxQuickStepIsland_Stage4LCP_iMJComputation(stage4CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_iMJComputation(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ dReal *iMJ = stage4CallContext->m_iMJ;
+ unsigned int m = localContext->m_m;
+ dReal *J = localContext->m_J;
+ const dxJBodiesItem *jb = localContext->m_jb;
+ dxBody * const *body = callContext->m_islandBodiesStart;
+ dReal *invI = localContext->m_invI;
+
+ // precompute iMJ = inv(M)*J'
+ compute_invM_JT<dxQUICKSTEPISLAND_STAGE4LCP_IMJ_STEP>(&stage4CallContext->m_mi_iMJ, iMJ, m, J, jb, body, invI);
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_iMJSync_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ unsigned int m = localContext->m_m;
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+
+ unsigned int stage4LCP_Ad_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE4LCP_AD_STEP>(m, allowedThreads);
+
+#ifdef WARM_STARTING
+ {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(stage4CallContext->m_LCP_fcStartReleasee, -1);
+ }
+#endif
+
+ if (stage4LCP_Ad_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage4LCP_Ad_allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, stage4LCP_Ad_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage4LCP_Ad_Callback, stage4CallContext, "QuickStepIsland Stage4LCP_Ad");
+ }
+ dxQuickStepIsland_Stage4LCP_AdComputation(stage4CallContext);
+
+ return 1;
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_fcStart_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ unsigned int fcPrepareComplexity, fcCompleteComplexity;
+#ifdef WARM_STARTING
+ fcPrepareComplexity = localContext->m_m / dxQUICKSTEPISLAND_STAGE4LCP_FC_COMPLETE_TO_PREPARE_COMPLEXITY_DIVISOR;
+ fcCompleteComplexity = callContext->m_islandBodiesCount;
+#else
+ fcPrepareComplexity = localContext->m_m;
+ fcCompleteComplexity = 0;
+#endif
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ unsigned int stage4LCP_fcPrepare_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP>(fcPrepareComplexity, allowedThreads);
+ unsigned int stage4LCP_fcComplete_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP>(fcCompleteComplexity, allowedThreads);
+ stage4CallContext->AssignLCP_fcAllowedThreads(stage4LCP_fcPrepare_allowedThreads, stage4LCP_fcComplete_allowedThreads);
+
+#ifdef WARM_STARTING
+ dxQuickStepIsland_Stage4LCP_MTfcComputation_warmZeroArrays(stage4CallContext);
+#endif
+
+ if (stage4LCP_fcPrepare_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage4LCP_fcPrepare_allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, stage4LCP_fcPrepare_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage4LCP_fc_Callback, stage4CallContext, "QuickStepIsland Stage4LCP_fc");
+ }
+ dxQuickStepIsland_Stage4LCP_MTfcComputation(stage4CallContext, callThisReleasee);
+
+ return 1;
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_fc_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ dxQuickStepIsland_Stage4LCP_MTfcComputation(stage4CallContext, callThisReleasee);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_MTfcComputation(dxQuickStepperStage4CallContext *stage4CallContext, dCallReleaseeID callThisReleasee)
+{
+#ifdef WARM_STARTING
+ dxQuickStepIsland_Stage4LCP_MTfcComputation_warm(stage4CallContext, callThisReleasee);
+#else
+ (void)callThisReleasee; // unused
+ dxQuickStepIsland_Stage4LCP_MTfcComputation_cold(stage4CallContext);
+#endif
+}
+
+#ifdef WARM_STARTING
+
+static
+void dxQuickStepIsland_Stage4LCP_MTfcComputation_warm(dxQuickStepperStage4CallContext *stage4CallContext, dCallReleaseeID callThisReleasee)
+{
+ dxQuickStepIsland_Stage4LCP_MTfcComputation_warmPrepare(stage4CallContext);
+
+ if (ThrsafeExchangeAdd(&stage4CallContext->m_LCP_fcPrepareThreadsRemaining, (atomicord32)(-1)) == 1) {
+ stage4CallContext->ResetLCP_fcComputationIndex();
+
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ unsigned int stage4LCP_fcComplete_allowedThreads = stage4CallContext->m_LCP_fcCompleteThreadsTotal;
+
+ if (stage4LCP_fcComplete_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage4LCP_fcComplete_allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, stage4LCP_fcComplete_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage4LCP_fcWarmComplete_Callback, stage4CallContext, "QuickStepIsland Stage4LCP_fcWarmComplete");
+ }
+ dxQuickStepIsland_Stage4LCP_MTfcComputation_warmComplete(stage4CallContext);
+ }
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_MTfcComputation_warmZeroArrays(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+
+ unsigned int nb = callContext->m_islandBodiesCount;
+ atomicord32 *bi_links = stage4CallContext->m_bi_links_or_mi_levels;
+
+ multiply_invM_JT_init_array(nb, bi_links);
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_MTfcComputation_warmPrepare(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ unsigned int m = localContext->m_m;
+ const dxJBodiesItem *jb = localContext->m_jb;
+
+ // Prepare to compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
+ // as we change lambda.
+ multiply_invM_JT_prepare<dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP_PREPARE>(&stage4CallContext->m_mi_fc, m, jb, stage4CallContext->m_bi_links_or_mi_levels, stage4CallContext->m_mi_links);
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_fcWarmComplete_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+
+ dxQuickStepIsland_Stage4LCP_MTfcComputation_warmComplete(stage4CallContext);
+
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_MTfcComputation_warmComplete(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ dReal *fc = stage4CallContext->m_cforce;
+ unsigned int nb = callContext->m_islandBodiesCount;
+ dReal *iMJ = stage4CallContext->m_iMJ;
+ const dxJBodiesItem *jb = localContext->m_jb;
+ dReal *lambda = stage4CallContext->m_lambda;
+
+ // Complete computation of fc=(inv(M)*J')*lambda. we will incrementally maintain fc
+ // as we change lambda.
+ multiply_invM_JT_complete<dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP_COMPLETE, CFE__DYNAMICS_MIN, CFE__MAX>(&stage4CallContext->m_mi_fc, fc, nb, iMJ, jb, lambda, stage4CallContext->m_bi_links_or_mi_levels, stage4CallContext->m_mi_links);
+}
+
+#else // #ifndef WARM_STARTING
+
+static
+void dxQuickStepIsland_Stage4LCP_MTfcComputation_cold(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+
+ dReal *fc = stage4CallContext->m_cforce;
+ unsigned int nb = callContext->m_islandBodiesCount;
+ const unsigned int step_size = dxQUICKSTEPISLAND_STAGE4LCP_FC_STEP;
+ unsigned int nb_steps = (nb + (step_size - 1)) / step_size;
+
+ unsigned bi_step;
+ while ((bi_step = ThrsafeIncrementIntUpToLimit(&stage4CallContext->m_mi_fc, nb_steps)) != nb_steps) {
+ unsigned int bi = bi_step * step_size;
+ unsigned int bicnt = dMIN(step_size, nb - bi);
+ dSetZero(fc + (sizeint)bi * CFE__MAX, (sizeint)bicnt * CFE__MAX);
+ }
+}
+
+#endif // #ifndef WARM_STARTING
+
+
+static
+void dxQuickStepIsland_Stage4LCP_STfcComputation(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+#ifdef WARM_STARTING
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ dReal *fc = stage4CallContext->m_cforce;
+ unsigned int m = localContext->m_m;
+ unsigned int nb = callContext->m_islandBodiesCount;
+ dReal *iMJ = stage4CallContext->m_iMJ;
+ const dxJBodiesItem *jb = localContext->m_jb;
+ dReal *lambda = stage4CallContext->m_lambda;
+
+ // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
+ // as we change lambda.
+ _multiply_invM_JT<CFE__DYNAMICS_MIN, CFE__MAX>(fc, m, nb, iMJ, jb, lambda);
+#else
+ dReal *fc = stage4CallContext->m_cforce;
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ unsigned int nb = callContext->m_islandBodiesCount;
+
+ dSetZero(fc, (sizeint)nb * CFE__MAX);
+#endif
+
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_Ad_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ dxQuickStepIsland_Stage4LCP_AdComputation(stage4CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_AdComputation(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ const dxJBodiesItem *jb = localContext->m_jb;
+ dReal *J = localContext->m_J;
+ unsigned int m = localContext->m_m;
+
+ dxWorld *world = callContext->m_world;
+ dxQuickStepParameters *qs = &world->qs;
+ const dReal sor_w = qs->w; // SOR over-relaxation parameter
+
+ dReal *iMJ = stage4CallContext->m_iMJ;
+
+ const unsigned int step_size = dxQUICKSTEPISLAND_STAGE4LCP_AD_STEP;
+ unsigned int m_steps = (m + (step_size - 1)) / step_size;
+
+ unsigned mi_step;
+ while ((mi_step = ThrsafeIncrementIntUpToLimit(&stage4CallContext->m_mi_Ad, m_steps)) != m_steps) {
+ unsigned int mi = mi_step * step_size;
+ const unsigned int miend = mi + dMIN(step_size, m - mi);
+
+ const dReal *iMJ_ptr = iMJ + (sizeint)mi * IMJ__MAX;
+ dReal *J_ptr = J + (sizeint)mi * JME__MAX;
+ while (true) {
+ dReal sum = REAL(0.0);
+ {
+ for (unsigned int j = JVE__MIN; j != JVE__MAX; ++j) sum += iMJ_ptr[IMJ__1_MIN + j] * J_ptr[JME__J1_MIN + j];
+ dSASSERT(JME__J1_COUNT == (int)JVE__MAX);
+ }
+
+ int b2 = jb[mi].second;
+ if (b2 != -1) {
+ for (unsigned int k = JVE__MIN; k != JVE__MAX; ++k) sum += iMJ_ptr[IMJ__2_MIN + k] * J_ptr[JME__J2_MIN + k];
+ dSASSERT(JME__J2_COUNT == (int)JVE__MAX);
+ }
+
+ dReal cfm_i = J_ptr[JME_CFM];
+ dReal Ad_i = sor_w / (sum + cfm_i);
+
+ // NOTE: This may seem unnecessary but it's indeed an optimization
+ // to move multiplication by Ad[i] and cfm[i] out of iteration loop.
+
+ // scale cfm, J and b by Ad
+ J_ptr[JME_CFM] = cfm_i * Ad_i;
+ J_ptr[JME_RHS] *= Ad_i;
+
+ {
+ for (unsigned int j = JVE__MIN; j != JVE__MAX; ++j) J_ptr[JME__J1_MIN + j] *= Ad_i;
+ dSASSERT(JME__J1_COUNT == (int)JVE__MAX);
+ }
+
+ if (b2 != -1) {
+ for (unsigned int k = JVE__MIN; k != JVE__MAX; ++k) J_ptr[JME__J2_MIN + k] *= Ad_i;
+ dSASSERT(JME__J2_COUNT == (int)JVE__MAX);
+ }
+
+ if (++mi == miend) {
+ break;
+ }
+ iMJ_ptr += IMJ__MAX;
+ J_ptr += JME__MAX;
+ }
+ }
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_ReorderPrep_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ dxQuickStepIsland_Stage4LCP_ReorderPrep(stage4CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_ReorderPrep(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+ unsigned int m = localContext->m_m;
+ unsigned int valid_findices = localContext->m_valid_findices;
+
+ IndexError *order = stage4CallContext->m_order;
+
+ {
+ // make sure constraints with findex < 0 come first.
+ IndexError *orderhead = order, *ordertail = order + (m - valid_findices);
+ const int *findex = localContext->m_findex;
+
+ // Fill the array from both ends
+ for (unsigned int i = 0; i != m; ++i) {
+ if (findex[i] == -1) {
+ orderhead->index = i; // Place them at the front
+ ++orderhead;
+ } else {
+ ordertail->index = i; // Place them at the end
+ ++ordertail;
+ }
+ }
+ dIASSERT(orderhead == order + (m - valid_findices));
+ dIASSERT(ordertail == order + m);
+ }
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_IterationStart_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+
+ dxWorld *world = callContext->m_world;
+ dxQuickStepParameters *qs = &world->qs;
+
+ const unsigned int num_iterations = qs->num_iterations;
+ unsigned iteration = stage4CallContext->m_LCP_iteration;
+
+ if (iteration < num_iterations)
+ {
+ dCallReleaseeID nextReleasee;
+ dCallReleaseeID stage4LCP_IterationSyncReleasee = stage4CallContext->m_LCP_IterationSyncReleasee;
+ unsigned int stage4LCP_Iteration_allowedThreads = stage4CallContext->m_LCP_IterationAllowedThreads;
+
+ bool reorderRequired = false;
+
+ if (IsSORConstraintsReorderRequiredForIteration(iteration))
+ {
+ reorderRequired = true;
+ }
+
+ unsigned syncCallDependencies = reorderRequired ? 1 : stage4LCP_Iteration_allowedThreads;
+
+ // Increment iterations counter in advance as anyway it needs to be incremented
+ // before independent tasks (the reordering or the iteration) are posted
+ // (otherwise next iteration may complete before the increment
+ // and the same iteration index may be used again).
+ stage4CallContext->m_LCP_iteration = iteration + 1;
+
+ if (iteration + 1 != num_iterations) {
+ dCallReleaseeID stage4LCP_IterationStartReleasee;
+ world->PostThreadedCallForUnawareReleasee(NULL, &stage4LCP_IterationStartReleasee, syncCallDependencies, stage4LCP_IterationSyncReleasee,
+ NULL, &dxQuickStepIsland_Stage4LCP_IterationStart_Callback, stage4CallContext, 0, "QuickStepIsland Stage4LCP_Iteration Start");
+ nextReleasee = stage4LCP_IterationStartReleasee;
+ }
+ else {
+ world->AlterThreadedCallDependenciesCount(stage4LCP_IterationSyncReleasee, syncCallDependencies);
+ nextReleasee = stage4LCP_IterationSyncReleasee;
+ }
+
+ if (reorderRequired) {
+ const unsigned int reorderThreads = 2;
+ dIASSERT(callContext->m_stepperAllowedThreads >= 2); // Otherwise the single-threaded execution path would be taken
+
+ stage4CallContext->ResetSOR_ConstraintsReorderVariables(reorderThreads);
+
+ dCallReleaseeID stage4LCP_ConstraintsReorderingSyncReleasee;
+ world->PostThreadedCall(NULL, &stage4LCP_ConstraintsReorderingSyncReleasee, reorderThreads, nextReleasee,
+ NULL, &dxQuickStepIsland_Stage4LCP_ConstraintsReorderingSync_Callback, stage4CallContext, 0, "QuickStepIsland Stage4LCP_ConstraintsReordering Sync");
+
+ if (reorderThreads > 1) {
+ world->PostThreadedCallsGroup(NULL, reorderThreads - 1, stage4LCP_ConstraintsReorderingSyncReleasee, &dxQuickStepIsland_Stage4LCP_ConstraintsReordering_Callback, stage4CallContext, "QuickStepIsland Stage4LCP_ConstraintsReordering");
+ }
+ dxQuickStepIsland_Stage4LCP_ConstraintsReordering(stage4CallContext);
+ world->AlterThreadedCallDependenciesCount(stage4LCP_ConstraintsReorderingSyncReleasee, -1);
+ }
+ else {
+ dIASSERT(iteration != 0); {
+ dxQuickStepIsland_Stage4LCP_DependencyMapFromSavedLevelsReconstruction(stage4CallContext);
+ }
+
+ stage4CallContext->RecordLCP_IterationStart(stage4LCP_Iteration_allowedThreads, nextReleasee);
+
+ unsigned knownToBeCompletedLevel = dxHEAD_INDEX;
+ if (stage4LCP_Iteration_allowedThreads > 1) {
+ world->PostThreadedCallsIndexOverridenGroup(NULL, stage4LCP_Iteration_allowedThreads - 1, nextReleasee, &dxQuickStepIsland_Stage4LCP_Iteration_Callback, stage4CallContext, knownToBeCompletedLevel, "QuickStepIsland Stage4LCP_Iteration");
+ }
+ dxQuickStepIsland_Stage4LCP_MTIteration(stage4CallContext, knownToBeCompletedLevel);
+ world->AlterThreadedCallDependenciesCount(nextReleasee, -1);
+ }
+ }
+
+ return 1;
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_ConstraintsReordering_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ dxQuickStepIsland_Stage4LCP_ConstraintsReordering(stage4CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_ConstraintsReordering(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ unsigned int iteration = stage4CallContext->m_LCP_iteration - 1; // Iteration is pre-incremented before scheduled tasks are released for execution
+ if (dxQuickStepIsland_Stage4LCP_ConstraintsShuffling(stage4CallContext, iteration)) {
+
+ dxQuickStepIsland_Stage4LCP_LinksArraysZeroing(stage4CallContext);
+ if (ThrsafeExchangeAdd(&stage4CallContext->m_SOR_reorderThreadsRemaining, (atomicord32)(-1)) == 1) { // If last thread has exited the reordering routine...
+ // Rebuild the object dependency map
+ dxQuickStepIsland_Stage4LCP_DependencyMapForNewOrderRebuilding(stage4CallContext);
+ }
+ }
+ else {
+ // NOTE: So far, this branch is only called in CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR case
+ if (ThrsafeExchangeAdd(&stage4CallContext->m_SOR_reorderThreadsRemaining, (atomicord32)(-1)) == 1) { // If last thread has exited the reordering routine...
+ dIASSERT(iteration != 0);
+ dxQuickStepIsland_Stage4LCP_DependencyMapFromSavedLevelsReconstruction(stage4CallContext);
+ }
+ }
+}
+
+static
+bool dxQuickStepIsland_Stage4LCP_ConstraintsShuffling(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int iteration)
+{
+ bool result = false;
+
+#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
+
+ struct ConstraintsReorderingHelper
+ {
+ void operator ()(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int startIndex, unsigned int endIndex)
+ {
+ const dReal *lambda = stage4CallContext->m_lambda;
+ dReal *last_lambda = stage4CallContext->m_last_lambda;
+ IndexError *order = stage4CallContext->m_order;
+
+ for (unsigned int index = startIndex; index != endIndex; ++index) {
+ unsigned int i = order[index].index;
+ dReal lambda_i = lambda[i];
+ if (lambda_i != REAL(0.0)) {
+ //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
+ order[index].error = dFabs(lambda_i - last_lambda[i]);
+ }
+ else if (last_lambda[i] != REAL(0.0)) {
+ //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
+ order[index].error = dFabs(/*lambda_i - */last_lambda[i]); // lambda_i == 0
+ }
+ else {
+ order[index].error = dInfinity;
+ }
+ // Finally copy the lambda for the next iteration
+ last_lambda[i] = lambda_i;
+ }
+ qsort (order + startIndex, endIndex - startIndex, sizeof(IndexError), &compare_index_error);
+ }
+ };
+
+ if (iteration > 1) { // Only reorder starting from iteration #2
+ // sort the constraints so that the ones converging slowest
+ // get solved last. use the absolute (not relative) error.
+ /*
+ * Full reorder needs to be done.
+ * Even though this contradicts the initial idea of moving dependent constraints
+ * to the order end the algorithm does not work the other way well.
+ * It looks like the iterative method needs a shake after it already found
+ * some initial approximations and those incurred errors help it to converge even better.
+ */
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_reorderHeadTaken, 1) == 0) {
+ // Process the head
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+ ConstraintsReorderingHelper()(stage4CallContext, 0, localContext->m_m);
+ }
+
+ result = true;
+ }
+ else if (iteration == 1) {
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_reorderHeadTaken, 1) == 0) {
+ // Process the first half
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+ unsigned int startIndex = 0;
+ unsigned int indicesCount = localContext->m_m / 2;
+ // Just copy the lambdas for the next iteration
+ memcpy(stage4CallContext->m_last_lambda + startIndex, stage4CallContext->m_lambda + startIndex, indicesCount * sizeof(dReal));
+ }
+
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_reorderTailTaken, 1) == 0) {
+ // Process the second half
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+ unsigned int startIndex = localContext->m_m / 2;
+ unsigned int indicesCount = localContext->m_m - startIndex;
+ // Just copy the lambdas for the next iteration
+ memcpy(stage4CallContext->m_last_lambda + startIndex, stage4CallContext->m_lambda + startIndex, indicesCount * sizeof(dReal));
+ }
+
+ // result = false; -- already 'false'
+ }
+ else /*if (iteration < 1) */{
+ result = true; // return true on 0th iteration to build dependency map for the initial order
+ }
+
+
+#elif CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__RANDOMLY
+
+ if (iteration != 0) {
+ dIASSERT(!dIN_RANGE(iteration, 0, RANDOM_CONSTRAINTS_REORDERING_FREQUENCY));
+
+ dIASSERT(iteration % RANDOM_CONSTRAINTS_REORDERING_FREQUENCY == RRS_REORDERING); {
+ struct ConstraintsReorderingHelper
+ {
+ void operator ()(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int startIndex, unsigned int indicesCount)
+ {
+ IndexError *order = stage4CallContext->m_order + startIndex;
+
+ for (unsigned int index = 1; index < indicesCount; ++index) {
+ int swapIndex = dRandInt(index + 1);
+ IndexError tmp = order[index];
+ order[index] = order[swapIndex];
+ order[swapIndex] = tmp;
+ }
+ }
+ };
+
+ /*
+ * Full reorder needs to be done.
+ * Even though this contradicts the initial idea of moving dependent constraints
+ * to the order end the algorithm does not work the other way well.
+ * It looks like the iterative method needs a shake after it already found
+ * some initial approximations and those incurred errors help it to converge even better.
+ */
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_reorderHeadTaken, 1) == 0) {
+ // Process the head
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+ ConstraintsReorderingHelper()(stage4CallContext, 0, localContext->m_m);
+ }
+ }
+ dIASSERT((RRS__MAX, true)); // A reference to RRS__MAX to be located by Find in Files
+ }
+ else {
+ // Just return true and skip the randomization for the very first iteration
+ }
+
+ result = true;
+
+#else // #if CONSTRAINTS_REORDERING_METHOD != REORDERING_METHOD__BY_ERROR && CONSTRAINTS_REORDERING_METHOD != REORDERING_METHOD__RANDOMLY
+
+ dIASSERT(iteration == 0); // The reordering request is only returned for the first iteration
+ result = true;
+
+
+#endif
+
+ return result;
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_LinksArraysZeroing(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_bi_zeroHeadTaken, 1) == 0) {
+ atomicord32 *bi_links = stage4CallContext->m_bi_links_or_mi_levels;/*=[nb]*/
+ unsigned int nb = callContext->m_islandBodiesCount;
+ memset(bi_links, 0, sizeof(bi_links[0]) * (nb / 2));
+ }
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_bi_zeroTailTaken, 1) == 0) {
+ atomicord32 *bi_links = stage4CallContext->m_bi_links_or_mi_levels;/*=[nb]*/
+ unsigned int nb = callContext->m_islandBodiesCount;
+ memset(bi_links + nb / 2, 0, sizeof(bi_links[0]) * (nb - nb / 2));
+ }
+
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_mi_zeroHeadTaken, 1) == 0) {
+ atomicord32 *mi_links = stage4CallContext->m_mi_links;/*=[2*(m + 1)]*/
+ unsigned int m = localContext->m_m;
+ memset(mi_links, 0, sizeof(mi_links[0]) * (m + 1));
+ }
+ if (ThrsafeExchange(&stage4CallContext->m_SOR_mi_zeroTailTaken, 1) == 0) {
+ atomicord32 *mi_links = stage4CallContext->m_mi_links;/*=[2*(m + 1)]*/
+ unsigned int m = localContext->m_m;
+ memset(mi_links + (m + 1), 0, sizeof(mi_links[0]) * (m + 1));
+ }
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_DependencyMapForNewOrderRebuilding(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ atomicord32 *bi_links = stage4CallContext->m_bi_links_or_mi_levels;/*=[nb]*/
+ atomicord32 *mi_links = stage4CallContext->m_mi_links;/*=[2*(m + 1)]*/
+
+ IndexError *order = stage4CallContext->m_order;
+ const dxJBodiesItem *jb = localContext->m_jb;
+
+ unsigned int m = localContext->m_m;
+ for (unsigned int i = 0; i != m; ++i) {
+ unsigned int index = order[i].index;
+
+ int b1 = jb[index].first;
+ int b2 = jb[index].second;
+
+ unsigned int encioded_i = dxENCODE_INDEX(i);
+
+ unsigned int encoded_depi = bi_links[(unsigned int)b1];
+ bi_links[(unsigned int)b1] = encioded_i;
+
+ if (b2 != -1 && b2 != b1) {
+ if (encoded_depi < (unsigned int)bi_links[(unsigned int)b2]) {
+ encoded_depi = bi_links[(unsigned int)b2];
+ }
+ bi_links[(unsigned int)b2] = encioded_i;
+ }
+
+ // OD: There is also a dependency on findex[index],
+ // however the findex can only refer to the rows of the same joint
+ // and hence that index is going to have the same bodies. Since the
+ // indices are sorted in a way that the meaningful findex values
+ // always come last, the dependency of findex[index] is going to
+ // be implicitly satisfied via matching bodies at smaller "i"s.
+
+ // Check that the dependency targets an earlier "i"
+ dIASSERT(encoded_depi < encioded_i);
+
+ unsigned encoded_downi = mi_links[(sizeint)encoded_depi * 2 + 1];
+ mi_links[(sizeint)encoded_depi * 2 + 1] = encioded_i; // Link i as down-dependency for depi
+ mi_links[(sizeint)encioded_i * 2 + 0] = encoded_downi; // Link previous down-chain as the level-dependency with i
+ }
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_DependencyMapFromSavedLevelsReconstruction(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ atomicord32 *mi_levels = stage4CallContext->m_bi_links_or_mi_levels;/*=[m]*/
+ atomicord32 *mi_links = stage4CallContext->m_mi_links;/*=[2*(m + 1)]*/
+
+ // NOTE!
+ // OD: The mi_links array is not zero-filled before the reconstruction.
+ // Iteration ends with all the down links zeroed. And since down links
+ // are moved to the next level links when parent-child relations are established,
+ // the horizontal levels are properly terminated.
+ // The leaf nodes had their links zero-initialized initially
+ // and those zeros remain intact during the solving. This way the down links
+ // are properly terminated as well.
+ // This is very obscure and error prone and would need an assertion check at least
+ // but the simplest assertion approach I can imagine would be
+ // zero filling and building another tree with the memory buffer comparison afterwards.
+ // That would be stupid, obviously.
+ //
+ // NOTE!
+ // OD: This routine can be threaded. However having two threads messing
+ // in one integer array with random access and kicking each other memory lines
+ // out of cache would probably work worse than letting a single thread do the whole job.
+ unsigned int m = localContext->m_m;
+ for (unsigned int i = 0; i != m; ++i) {
+ unsigned int currentLevelRoot = mi_levels[i];
+ unsigned int currentLevelFirstLink = mi_links[2 * (sizeint)currentLevelRoot + 1];
+ unsigned int encoded_i = dxENCODE_INDEX(i);
+ mi_links[2 * (sizeint)currentLevelRoot + 1] = encoded_i;
+ mi_links[2 * (sizeint)encoded_i + 0] = currentLevelFirstLink;
+ }
+
+ // Additionally reset available level root's list head
+ mi_links[2 * dxHEAD_INDEX + 0] = dxHEAD_INDEX;
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_ConstraintsReorderingSync_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ unsigned int stage4LCP_Iteration_allowedThreads = stage4CallContext->m_LCP_IterationAllowedThreads;
+
+ stage4CallContext->RecordLCP_IterationStart(stage4LCP_Iteration_allowedThreads, callThisReleasee);
+
+ unsigned knownToBeCompletedLevel = dxHEAD_INDEX;
+ if (stage4LCP_Iteration_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage4LCP_Iteration_allowedThreads - 1);
+ world->PostThreadedCallsIndexOverridenGroup(NULL, stage4LCP_Iteration_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage4LCP_Iteration_Callback, stage4CallContext, knownToBeCompletedLevel, "QuickStepIsland Stage4LCP_Iteration");
+ }
+ dxQuickStepIsland_Stage4LCP_MTIteration(stage4CallContext, knownToBeCompletedLevel);
+
+ return 1;
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_Iteration_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ unsigned int initiallyKnownToBeCompletedLevel = (unsigned int)callInstanceIndex;
+ dIASSERT(initiallyKnownToBeCompletedLevel == callInstanceIndex); // A truncation check...
+
+ dxQuickStepIsland_Stage4LCP_MTIteration(stage4CallContext, initiallyKnownToBeCompletedLevel);
+ return 1;
+}
+
+/*
+ * +0 +0
+ * Root───┬─────────────────┬──...
+ * +1│ +1│
+ * ┌┴â”+0 ┌─â”+0 .
+ * │A├─────┤B├─...
+ * └┬┘ └┬┘
+ * +1│ +1│
+ * ┌┴â”+0 .
+ * │C├─...
+ * └┬┘
+ * +1│
+ * .
+ *
+ * Lower tree levels depend on their parents. Same level nodes are independent with respect to each other.
+ *
+ * 1. B is linked in place of A
+ * 2. A is processed
+ * 3. C is inserted at the Root level
+ *
+ * The tree starts with a single child subtree at the root level ("down" link of slot #0 is used for that).
+ * Then, additional "C" nodes are added to the root level by building horizontal link via slots of
+ * their former parent "A"s that had become free.
+ * The "level" link of slot #0 is used to find the root level head.
+ *
+ * Since the tree is altered during iteration, mi_levels record each node parents so that the tree could be reconstructed.
+ */
+static
+void dxQuickStepIsland_Stage4LCP_MTIteration(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int initiallyKnownToBeCompletedLevel)
+{
+ atomicord32 *mi_levels = stage4CallContext->m_bi_links_or_mi_levels;
+ atomicord32 *mi_links = stage4CallContext->m_mi_links;
+
+ unsigned int knownToBeCompletedLevel = initiallyKnownToBeCompletedLevel;
+
+ while (true) {
+ unsigned int initialLevelRoot = mi_links[2 * dxHEAD_INDEX + 0];
+ if (initialLevelRoot != dxHEAD_INDEX && initialLevelRoot == knownToBeCompletedLevel) {
+ // No work is (currently) available
+ break;
+ }
+
+ for (unsigned int currentLevelRoot = initialLevelRoot; ; currentLevelRoot = mi_links[2 * (sizeint)currentLevelRoot + 0]) {
+ while (true) {
+ const unsigned invalid_link = dxENCODE_INDEX(-1);
+
+ unsigned currentLevelFirstLink = mi_links[2 * (sizeint)currentLevelRoot + 1];
+ if (currentLevelFirstLink == invalid_link) {
+ break;
+ }
+
+ // Try to extract first record from linked list
+ unsigned currentLevelNextLink = mi_links[2 * (sizeint)currentLevelFirstLink + 0];
+ if (ThrsafeCompareExchange(&mi_links[2 * (sizeint)currentLevelRoot + 1], currentLevelFirstLink, currentLevelNextLink)) {
+ // if succeeded, execute selected iteration step...
+ dxQuickStepIsland_Stage4LCP_IterationStep(stage4CallContext, dxDECODE_INDEX(currentLevelFirstLink));
+
+ // Check if there are any dependencies
+ unsigned level0DownLink = mi_links[2 * (sizeint)currentLevelFirstLink + 1];
+ if (level0DownLink != invalid_link) {
+ // ...and if yes, insert the record into the list of available level roots
+ unsigned int levelRootsFirst;
+ do {
+ levelRootsFirst = mi_links[2 * dxHEAD_INDEX + 0];
+ mi_links[2 * (sizeint)currentLevelFirstLink + 0] = levelRootsFirst;
+ }
+ while (!ThrsafeCompareExchange(&mi_links[2 * dxHEAD_INDEX + 0], levelRootsFirst, currentLevelFirstLink));
+
+ // If another level was added and some threads have already exited...
+ unsigned int threadsTotal = stage4CallContext->m_LCP_iterationThreadsTotal;
+ unsigned int threadsRemaining = ThrsafeIncrementIntUpToLimit(&stage4CallContext->m_LCP_iterationThreadsRemaining, threadsTotal);
+ if (threadsRemaining != threadsTotal) {
+ // ...go on an schedule one more...
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ dxWorld *world = callContext->m_world;
+ // ...passing knownToBeCompletedLevel as the initial one for the spawned call
+ world->PostThreadedCallForUnawareReleasee(NULL, NULL, 0, stage4CallContext->m_LCP_iterationNextReleasee, NULL, &dxQuickStepIsland_Stage4LCP_Iteration_Callback, stage4CallContext, knownToBeCompletedLevel, "QuickStepIsland Stage4LCP_Iteration");
+ // NOTE: it's hard to predict whether it is reasonable to re-post a call
+ // each time a new level is added (provided some calls have already exited, of course).
+ // The efficiency very much depends on dependencies patterns between levels
+ // (i.e. it depends on the amount of available work added with each level).
+ // The strategy of re-posting exited calls as frequently as possible
+ // leads to potential wasting execution cycles in some cores for the aid
+ // of keeping other cores busy as much as possible and not letting all the
+ // work be executed by just a partial cores subset. With emergency of large
+ // available work amounts (the work that is not dependent on anything and
+ // ready to be executed immediately) this strategy is going to transit into
+ // full cores set being busy executing useful work. If amounts of work
+ // emerging from added levels are small, the strategy should lead to
+ // approximately the same efficiency as if the work was done by only a cores subset
+ // with the remaining cores wasting (some) cycles for re-scheduling calls
+ // to those busy cores rather than being idle or handling other islands.
+ }
+ }
+
+ // Finally record the root index of current record's level
+ mi_levels[dxDECODE_INDEX(currentLevelFirstLink)] = currentLevelRoot;
+ }
+ }
+
+ if (currentLevelRoot == knownToBeCompletedLevel) {
+ break;
+ }
+ dIASSERT(currentLevelRoot != dxHEAD_INDEX); // Zero level is expected to be the deepest one in the list and execution must not loop past it.
+ }
+ // Save the level root we started from as known to be completed
+ knownToBeCompletedLevel = initialLevelRoot;
+ }
+
+ // Decrement running threads count on exit
+ ThrsafeAdd(&stage4CallContext->m_LCP_iterationThreadsRemaining, (atomicord32)(-1));
+}
+
+static
+void dxQuickStepIsland_Stage4LCP_STIteration(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ unsigned int m = localContext->m_m;
+ for (unsigned int i = 0; i != m; ++i) {
+ dxQuickStepIsland_Stage4LCP_IterationStep(stage4CallContext, i);
+ }
+}
+
+//***************************************************************************
+// SOR-LCP method
+
+// nb is the number of bodies in the body array.
+// J is an m*16 matrix of constraint rows with rhs, cfm, lo and hi in padding
+// jb is an array of first and second body numbers for each constraint row
+// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
+//
+// this returns lambda and fc (the constraint force).
+// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
+//
+// b, lo and hi are modified on exit
+
+static
+void dxQuickStepIsland_Stage4LCP_IterationStep(dxQuickStepperStage4CallContext *stage4CallContext, unsigned int i)
+{
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ IndexError *order = stage4CallContext->m_order;
+ unsigned int index = order[i].index;
+
+ dReal *fc_ptr1;
+ dReal *fc_ptr2 = NULL;
+ dReal delta;
+
+ dReal *lambda = stage4CallContext->m_lambda;
+ dReal old_lambda = lambda[index];
+
+ dReal *J = localContext->m_J;
+ const dReal *J_ptr = J + (sizeint)index * JME__MAX;
+
+ {
+ delta = J_ptr[JME_RHS] - old_lambda * J_ptr[JME_CFM];
+
+ dReal *fc = stage4CallContext->m_cforce;
+
+ const dxJBodiesItem *jb = localContext->m_jb;
+ int b2 = jb[index].second;
+ int b1 = jb[index].first;
+
+ // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
+ fc_ptr1 = fc + (sizeint)(unsigned)b1 * CFE__MAX;
+ delta -= fc_ptr1[CFE_LX] * J_ptr[JME_J1LX] + fc_ptr1[CFE_LY] * J_ptr[JME_J1LY] +
+ fc_ptr1[CFE_LZ] * J_ptr[JME_J1LZ] + fc_ptr1[CFE_AX] * J_ptr[JME_J1AX] +
+ fc_ptr1[CFE_AY] * J_ptr[JME_J1AY] + fc_ptr1[CFE_AZ] * J_ptr[JME_J1AZ];
+ // @@@ potential optimization: handle 1-body constraints in a separate
+ // loop to avoid the cost of test & jump?
+ if (b2 != -1) {
+ fc_ptr2 = fc + (sizeint)(unsigned)b2 * CFE__MAX;
+ delta -= fc_ptr2[CFE_LX] * J_ptr[JME_J2LX] + fc_ptr2[CFE_LY] * J_ptr[JME_J2LY] +
+ fc_ptr2[CFE_LZ] * J_ptr[JME_J2LZ] + fc_ptr2[CFE_AX] * J_ptr[JME_J2AX] +
+ fc_ptr2[CFE_AY] * J_ptr[JME_J2AY] + fc_ptr2[CFE_AZ] * J_ptr[JME_J2AZ];
+ }
+ }
+
+ {
+ dReal hi_act, lo_act;
+
+ // set the limits for this constraint.
+ // this is the place where the QuickStep method differs from the
+ // direct LCP solving method, since that method only performs this
+ // limit adjustment once per time step, whereas this method performs
+ // once per iteration per constraint row.
+ // the constraints are ordered so that all lambda[] values needed have
+ // already been computed.
+ const int *findex = localContext->m_findex;
+ if (findex[index] != -1) {
+ hi_act = dFabs (J_ptr[JME_HI] * lambda[(unsigned)findex[index]]);
+ lo_act = -hi_act;
+ } else {
+ hi_act = J_ptr[JME_HI];
+ lo_act = J_ptr[JME_LO];
+ }
+
+ // compute lambda and clamp it to [lo,hi].
+ // @@@ potential optimization: does SSE have clamping instructions
+ // to save test+jump penalties here?
+ dReal new_lambda = old_lambda + delta;
+ if (new_lambda < lo_act) {
+ delta = lo_act - old_lambda;
+ lambda[index] = lo_act;
+ }
+ else if (new_lambda > hi_act) {
+ delta = hi_act - old_lambda;
+ lambda[index] = hi_act;
+ }
+ else {
+ lambda[index] = new_lambda;
+ }
+ }
+
+ //@@@ a trick that may or may not help
+ //dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations));
+ //delta *= ramp;
+
+ {
+ dReal *iMJ = stage4CallContext->m_iMJ;
+ const dReal *iMJ_ptr = iMJ + (sizeint)index * IMJ__MAX;
+ // update fc.
+ // @@@ potential optimization: SIMD for this and the b2 >= 0 case
+ fc_ptr1[CFE_LX] += delta * iMJ_ptr[IMJ_1LX];
+ fc_ptr1[CFE_LY] += delta * iMJ_ptr[IMJ_1LY];
+ fc_ptr1[CFE_LZ] += delta * iMJ_ptr[IMJ_1LZ];
+ fc_ptr1[CFE_AX] += delta * iMJ_ptr[IMJ_1AX];
+ fc_ptr1[CFE_AY] += delta * iMJ_ptr[IMJ_1AY];
+ fc_ptr1[CFE_AZ] += delta * iMJ_ptr[IMJ_1AZ];
+ // @@@ potential optimization: handle 1-body constraints in a separate
+ // loop to avoid the cost of test & jump?
+ if (fc_ptr2) {
+ fc_ptr2[CFE_LX] += delta * iMJ_ptr[IMJ_2LX];
+ fc_ptr2[CFE_LY] += delta * iMJ_ptr[IMJ_2LY];
+ fc_ptr2[CFE_LZ] += delta * iMJ_ptr[IMJ_2LZ];
+ fc_ptr2[CFE_AX] += delta * iMJ_ptr[IMJ_2AX];
+ fc_ptr2[CFE_AY] += delta * iMJ_ptr[IMJ_2AY];
+ fc_ptr2[CFE_AZ] += delta * iMJ_ptr[IMJ_2AZ];
+ }
+ }
+}
+
+static inline
+bool IsStage4bJointInfosIterationRequired(const dxQuickStepperLocalContext *localContext)
+{
+ return
+#ifdef WARM_STARTING
+ true ||
+#endif
+ localContext->m_mfb > 0;
+}
+
+static
+int dxQuickStepIsland_Stage4LCP_IterationSync_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ unsigned int stage4b_allowedThreads = 1;
+ if (IsStage4bJointInfosIterationRequired(localContext)) {
+ unsigned int allowedThreads = callContext->m_stepperAllowedThreads;
+ dIASSERT(allowedThreads >= stage4b_allowedThreads);
+ stage4b_allowedThreads += CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE4B_STEP>(localContext->m_nj, allowedThreads - stage4b_allowedThreads);
+ }
+
+ if (stage4b_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage4b_allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, stage4b_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage4b_Callback, stage4CallContext, "QuickStepIsland Stage4b");
+ }
+ dxQuickStepIsland_Stage4b(stage4CallContext);
+
+ return 1;
+}
+
+static
+int dxQuickStepIsland_Stage4b_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)_stage4CallContext;
+ dxQuickStepIsland_Stage4b(stage4CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage4b(dxQuickStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ if (ThrsafeExchange(&stage4CallContext->m_cf_4b, 1) == 0) {
+ dxBody * const *body = callContext->m_islandBodiesStart;
+ unsigned int nb = callContext->m_islandBodiesCount;
+ const dReal *cforce = stage4CallContext->m_cforce;
+ dReal stepsize = callContext->m_stepSize;
+ // add stepsize * cforce to the body velocity
+ const dReal *cforcecurr = cforce;
+ dxBody *const *const bodyend = body + nb;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; cforcecurr += CFE__MAX, bodycurr++) {
+ dxBody *b = *bodycurr;
+ for (unsigned int j = dSA__MIN; j != dSA__MAX; j++) {
+ b->lvel[dV3E__AXES_MIN + j] += stepsize * cforcecurr[CFE__L_MIN + j];
+ b->avel[dV3E__AXES_MIN + j] += stepsize * cforcecurr[CFE__A_MIN + j];
+ }
+ }
+ }
+
+
+ // note that the SOR method overwrites rhs and J at this point, so
+ // they should not be used again.
+
+ if (IsStage4bJointInfosIterationRequired(localContext)) {
+ dReal data[JVE__MAX];
+ const dReal *Jcopy = localContext->m_Jcopy;
+ const dReal *lambda = stage4CallContext->m_lambda;
+ const dxMIndexItem *mindex = localContext->m_mindex;
+ dJointWithInfo1 *jointinfos = localContext->m_jointinfos;
+
+ unsigned int nj = localContext->m_nj;
+ const unsigned int step_size = dxQUICKSTEPISLAND_STAGE4B_STEP;
+ unsigned int nj_steps = (nj + (step_size - 1)) / step_size;
+
+ unsigned ji_step;
+ while ((ji_step = ThrsafeIncrementIntUpToLimit(&stage4CallContext->m_ji_4b, nj_steps)) != nj_steps) {
+ unsigned int ji = ji_step * step_size;
+ const unsigned int jiend = ji + dMIN(step_size, nj - ji);
+
+ const dReal *Jcopycurr = Jcopy + (sizeint)mindex[ji].fbIndex * JCE__MAX;
+
+ while (true) {
+ // straightforward computation of joint constraint forces:
+ // multiply related lambdas with respective J' block for joints
+ // where feedback was requested
+ const unsigned int fb_infom = mindex[ji + 1].fbIndex - mindex[ji].fbIndex;
+ if (fb_infom != 0) {
+ dIASSERT(fb_infom == mindex[ji + 1].mIndex - mindex[ji].mIndex);
+
+ const dReal *lambdacurr = lambda + mindex[ji].mIndex;
+ dxJoint *joint = jointinfos[ji].joint;
+
+#ifdef WARM_STARTING
+ memcpy(joint->lambda, lambdacurr, fb_infom * sizeof(dReal));
+#endif
+
+ dJointFeedback *fb = joint->feedback;
+
+ if (joint->node[1].body) {
+ Multiply1_12q1 (data, Jcopycurr + JCE__J2_MIN, lambdacurr, fb_infom);
+ dSASSERT(JCE__MAX == 12);
+
+ fb->f2[dSA_X] = data[JVE_LX];
+ fb->f2[dSA_Y] = data[JVE_LY];
+ fb->f2[dSA_Z] = data[JVE_LZ];
+ fb->t2[dSA_X] = data[JVE_AX];
+ fb->t2[dSA_Y] = data[JVE_AY];
+ fb->t2[dSA_Z] = data[JVE_AZ];
+ }
+
+ Multiply1_12q1 (data, Jcopycurr + JCE__J1_MIN, lambdacurr, fb_infom);
+ dSASSERT(JCE__MAX == 12);
+
+ fb->f1[dSA_X] = data[JVE_LX];
+ fb->f1[dSA_Y] = data[JVE_LY];
+ fb->f1[dSA_Z] = data[JVE_LZ];
+ fb->t1[dSA_X] = data[JVE_AX];
+ fb->t1[dSA_Y] = data[JVE_AY];
+ fb->t1[dSA_Z] = data[JVE_AZ];
+
+ Jcopycurr += fb_infom * JCE__MAX;
+ }
+ else {
+#ifdef WARM_STARTING
+ const dReal *lambdacurr = lambda + mindex[ji].mIndex;
+ const unsigned int infom = mindex[ji + 1].mIndex - mindex[ji].mIndex;
+ dxJoint *joint = jointinfos[ji].joint;
+ memcpy(joint->lambda, lambdacurr, infom * sizeof(dReal));
+#endif
+ }
+
+ if (++ji == jiend) {
+ break;
+ }
+ }
+ }
+ }
+}
+
+static
+int dxQuickStepIsland_Stage5_Callback(void *_stage5CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage5CallContext *stage5CallContext = (dxQuickStepperStage5CallContext *)_stage5CallContext;
+ dxQuickStepIsland_Stage5(stage5CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage5(dxQuickStepperStage5CallContext *stage5CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage5CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage5CallContext->m_localContext;
+
+ dxWorldProcessMemArena *memarena = callContext->m_stepperArena;
+ memarena->RestoreState(stage5CallContext->m_stage3MemArenaState);
+ stage5CallContext = NULL; // WARNING! stage3CallContext is not valid after this point!
+ dIVERIFY(stage5CallContext == NULL); // To suppress unused variable assignment warnings
+
+ dxQuickStepperStage6CallContext *stage6CallContext = (dxQuickStepperStage6CallContext *)memarena->AllocateBlock(sizeof(dxQuickStepperStage6CallContext));
+ stage6CallContext->Initialize(callContext, localContext);
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ dIASSERT(allowedThreads >= 1);
+
+ if (allowedThreads == 1) {
+ IFTIMING (dTimerNow ("compute velocity update"));
+ dxQuickStepIsland_Stage6a(stage6CallContext);
+ dxQuickStepIsland_Stage6_VelocityCheck(stage6CallContext);
+ IFTIMING (dTimerNow ("update position and tidy up"));
+ dxQuickStepIsland_Stage6b(stage6CallContext);
+ IFTIMING (dTimerEnd());
+ IFTIMING (if (m > 0) dTimerReport (stdout,1));
+ }
+ else {
+ unsigned int nb = callContext->m_islandBodiesCount;
+ unsigned int stage6a_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE6A_STEP>(nb, allowedThreads);
+
+ dxWorld *world = callContext->m_world;
+
+ dCallReleaseeID stage6aSyncReleasee;
+ world->PostThreadedCallForUnawareReleasee(NULL, &stage6aSyncReleasee, stage6a_allowedThreads, callContext->m_finalReleasee,
+ NULL, &dxQuickStepIsland_Stage6aSync_Callback, stage6CallContext, 0, "QuickStepIsland Stage6a Sync");
+
+ if (stage6a_allowedThreads > 1) {
+ world->PostThreadedCallsGroup(NULL, stage6a_allowedThreads - 1, stage6aSyncReleasee, &dxQuickStepIsland_Stage6a_Callback, stage6CallContext, "QuickStepIsland Stage6a");
+ }
+ dxQuickStepIsland_Stage6a(stage6CallContext);
+ world->AlterThreadedCallDependenciesCount(stage6aSyncReleasee, -1);
+ }
+}
+
+
+static
+int dxQuickStepIsland_Stage6a_Callback(void *_stage6CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage6CallContext *stage6CallContext = (dxQuickStepperStage6CallContext *)_stage6CallContext;
+ dxQuickStepIsland_Stage6a(stage6CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage6a(dxQuickStepperStage6CallContext *stage6CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage6CallContext->m_stepperCallContext;
+ const dxQuickStepperLocalContext *localContext = stage6CallContext->m_localContext;
+
+ dReal stepsize = callContext->m_stepSize;
+ dReal *invI = localContext->m_invI;
+ dxBody * const *body = callContext->m_islandBodiesStart;
+
+ unsigned int nb = callContext->m_islandBodiesCount;
+ const unsigned int step_size = dxQUICKSTEPISLAND_STAGE6A_STEP;
+ unsigned int nb_steps = (nb + (step_size - 1)) / step_size;
+
+ unsigned bi_step;
+ while ((bi_step = ThrsafeIncrementIntUpToLimit(&stage6CallContext->m_bi_6a, nb_steps)) != nb_steps) {
+ unsigned int bi = bi_step * step_size;
+ unsigned int bicnt = dMIN(step_size, nb - bi);
+
+ const dReal *invIrow = invI + (sizeint)bi * IIE__MAX;
+ dxBody *const *bodycurr = body + bi;
+ dxBody *const *bodyend = bodycurr + bicnt;
+ while (true) {
+ // compute the velocity update:
+ // add stepsize * invM * fe to the body velocity
+ dxBody *b = *bodycurr;
+ dReal body_invMass_mul_stepsize = stepsize * b->invMass;
+ for (unsigned int j = dSA__MIN; j != dSA__MAX; ++j) {
+ b->lvel[dV3E__AXES_MIN + j] += body_invMass_mul_stepsize * b->facc[dV3E__AXES_MIN + j];
+ b->tacc[dV3E__AXES_MIN + j] *= stepsize;
+ }
+ dMultiplyAdd0_331 (b->avel, invIrow + IIE__MATRIX_MIN, b->tacc);
+
+ if (++bodycurr == bodyend) {
+ break;
+ }
+ invIrow += IIE__MAX;
+ }
+ }
+}
+
+
+static
+int dxQuickStepIsland_Stage6aSync_Callback(void *_stage6CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage6CallContext *stage6CallContext = (dxQuickStepperStage6CallContext *)_stage6CallContext;
+ dxQuickStepIsland_Stage6_VelocityCheck(stage6CallContext);
+
+ const dxStepperProcessingCallContext *callContext = stage6CallContext->m_stepperCallContext;
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ unsigned int nb = callContext->m_islandBodiesCount;
+ unsigned int stage6b_allowedThreads = CalculateOptimalThreadsCount<dxQUICKSTEPISLAND_STAGE6B_STEP>(nb, allowedThreads);
+
+ if (stage6b_allowedThreads > 1) {
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, stage6b_allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, stage6b_allowedThreads - 1, callThisReleasee, &dxQuickStepIsland_Stage6b_Callback, stage6CallContext, "QuickStepIsland Stage6b");
+ }
+ dxQuickStepIsland_Stage6b(stage6CallContext);
+
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage6_VelocityCheck(dxQuickStepperStage6CallContext *stage6CallContext)
+{
+ (void)stage6CallContext; // can be unused
+#ifdef CHECK_VELOCITY_OBEYS_CONSTRAINT
+ const dxQuickStepperLocalContext *localContext = stage6CallContext->m_localContext;
+
+ unsigned int m = localContext->m_m;
+ if (m > 0) {
+ const dxStepperProcessingCallContext *callContext = stage6CallContext->m_stepperCallContext;
+ dxBody * const *body = callContext->m_islandBodiesStart;
+ dReal *J = localContext->m_J;
+ const dxJBodiesItem *jb = localContext->m_jb;
+
+ dReal error = 0;
+ const dReal* J_ptr = J;
+ for (unsigned int i = 0; i < m; ++i) {
+ int b1 = jb[i].first;
+ int b2 = jb[i].second;
+ dReal sum = 0;
+ dxBody *bodycurr = body[(unsigned)b1];
+ for (unsigned int j = dSA__MIN; j != dSA__MAX; ++j) sum += J_ptr[JME__J1L_MIN + j] * bodycurr->lvel[dV3E__AXES_MIN + j] + J_ptr[JME__J1A_MIN + j] * bodycurr->avel[dV3E__AXES_MIN + j];
+ if (b2 != -1) {
+ dxBody *bodycurr = body[(unsigned)b2];
+ for (unsigned int k = dSA__MIN; k != dSA__MAX; ++k) sum += J_ptr[JME__J2L_MIN + k] * bodycurr->lvel[dV3E__AXES_MIN + k] + J_ptr[JME__J2A_MIN + k] * bodycurr->avel[dV3E__AXES_MIN + k];
+ }
+ J_ptr += JME__MAX;
+ error += dFabs(sum);
+ }
+ printf ("velocity error = %10.6e\n", error);
+ }
+#endif
+}
+
+static
+int dxQuickStepIsland_Stage6b_Callback(void *_stage6CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxQuickStepperStage6CallContext *stage6CallContext = (dxQuickStepperStage6CallContext *)_stage6CallContext;
+ dxQuickStepIsland_Stage6b(stage6CallContext);
+ return 1;
+}
+
+static
+void dxQuickStepIsland_Stage6b(dxQuickStepperStage6CallContext *stage6CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage6CallContext->m_stepperCallContext;
+
+ dReal stepsize = callContext->m_stepSize;
+ dxBody * const *body = callContext->m_islandBodiesStart;
+
+ // update the position and orientation from the new linear/angular velocity
+ // (over the given timestep)
+ unsigned int nb = callContext->m_islandBodiesCount;
+ const unsigned int step_size = dxQUICKSTEPISLAND_STAGE6B_STEP;
+ unsigned int nb_steps = (nb + (step_size - 1)) / step_size;
+
+ unsigned bi_step;
+ while ((bi_step = ThrsafeIncrementIntUpToLimit(&stage6CallContext->m_bi_6b, nb_steps)) != nb_steps) {
+ unsigned int bi = bi_step * step_size;
+ unsigned int bicnt = dMIN(step_size, nb - bi);
+
+ dxBody *const *bodycurr = body + bi;
+ dxBody *const *bodyend = bodycurr + bicnt;
+ while (true) {
+ dxBody *b = *bodycurr;
+ dxStepBody (b, stepsize);
+ dZeroVector3 (b->facc);
+ dZeroVector3 (b->tacc);
+ if (++bodycurr == bodyend) {
+ break;
+ }
+ }
+ }
+}
+
+
+
+/*extern */
+sizeint dxEstimateQuickStepMemoryRequirements (dxBody * const *body,
+ unsigned int nb,
+ dxJoint * const *_joint,
+ unsigned int _nj)
+{
+ (void)body; // unused
+ unsigned int nj, m, mfb;
+
+ {
+ unsigned int njcurr = 0, mcurr = 0, mfbcurr = 0;
+ dxJoint::SureMaxInfo info;
+ dxJoint *const *const _jend = _joint + _nj;
+ for (dxJoint *const *_jcurr = _joint; _jcurr != _jend; _jcurr++) {
+ dxJoint *j = *_jcurr;
+ j->getSureMaxInfo (&info);
+
+ unsigned int jm = info.max_m;
+ if (jm > 0) {
+ njcurr++;
+
+ mcurr += jm;
+ if (j->feedback)
+ mfbcurr += jm;
+ }
+ }
+ nj = njcurr; m = mcurr; mfb = mfbcurr;
+ }
+
+ sizeint res = 0;
+
+ res += dOVERALIGNED_SIZE(sizeof(dReal) * IIE__MAX * nb, INVI_ALIGNMENT); // for invI
+
+ {
+ sizeint sub1_res1 = dEFFICIENT_SIZE(sizeof(dJointWithInfo1) * _nj); // for initial jointinfos
+
+ sizeint sub1_res2 = dEFFICIENT_SIZE(sizeof(dJointWithInfo1) * nj); // for shrunk jointinfos
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dxQuickStepperLocalContext)); // for dxQuickStepLocalContext
+ if (m > 0) {
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dxMIndexItem) * (nj + 1)); // for mindex
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dxJBodiesItem) * m); // for jb
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(int) * m); // for findex
+ sub1_res2 += dOVERALIGNED_SIZE(sizeof(dReal) * JME__MAX * m, JACOBIAN_ALIGNMENT); // for J
+ sub1_res2 += dOVERALIGNED_SIZE(sizeof(dReal) * JCE__MAX * mfb, JCOPY_ALIGNMENT); // for Jcopy
+ {
+ sizeint sub2_res1 = dEFFICIENT_SIZE(sizeof(dxQuickStepperStage3CallContext)); // for dxQuickStepperStage3CallContext
+ sub2_res1 += dEFFICIENT_SIZE(sizeof(dReal) * RHS__MAX * nb); // for rhs_tmp
+ sub2_res1 += dEFFICIENT_SIZE(sizeof(dxQuickStepperStage2CallContext)); // for dxQuickStepperStage2CallContext
+
+ sizeint sub2_res2 = 0;
+ {
+ sizeint sub3_res1 = dEFFICIENT_SIZE(sizeof(dxQuickStepperStage5CallContext)); // for dxQuickStepperStage5CallContext;
+ sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for lambda
+ sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * CFE__MAX * nb); // for cforce
+ sub3_res1 += dOVERALIGNED_SIZE(sizeof(dReal) * IMJ__MAX * m, INVMJ_ALIGNMENT); // for iMJ
+ sub3_res1 += dEFFICIENT_SIZE(sizeof(IndexError) * m); // for order
+#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
+ sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for last_lambda
+#endif
+#if !dTHREADING_INTF_DISABLED
+ sub3_res1 += dEFFICIENT_SIZE(sizeof(atomicord32) * dMAX(nb, m)); // for bi_links_or_mi_levels
+ sub3_res1 += dEFFICIENT_SIZE(sizeof(atomicord32) * 2 * ((sizeint)m + 1)); // for mi_links
+#endif
+ sub3_res1 += dEFFICIENT_SIZE(sizeof(dxQuickStepperStage4CallContext)); // for dxQuickStepperStage4CallContext;
+
+ sizeint sub3_res2 = dEFFICIENT_SIZE(sizeof(dxQuickStepperStage6CallContext)); // for dxQuickStepperStage6CallContext;
+
+ sub2_res2 += dMAX(sub3_res1, sub3_res2);
+ }
+
+ sub1_res2 += dMAX(sub2_res1, sub2_res2);
+ }
+ }
+ else {
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dxQuickStepperStage3CallContext)); // for dxQuickStepperStage3CallContext
+ }
+
+ sizeint sub1_res12_max = dMAX(sub1_res1, sub1_res2);
+ sizeint stage01_contexts = dEFFICIENT_SIZE(sizeof(dxQuickStepperStage0BodiesCallContext))
+ + dEFFICIENT_SIZE(sizeof(dxQuickStepperStage0JointsCallContext))
+ + dEFFICIENT_SIZE(sizeof(dxQuickStepperStage1CallContext));
+ res += dMAX(sub1_res12_max, stage01_contexts);
+ }
+
+ return res;
+}
+
+/*extern */
+unsigned dxEstimateQuickStepMaxCallCount(unsigned activeThreadCount, unsigned allowedThreadCount)
+{
+ (void)activeThreadCount; // unused
+ unsigned result = 1 // dxQuickStepIsland itself
+ + 5 + (2 * allowedThreadCount + 1) // for Stage4 related schedules
+ + 1 // dxStepIsland_Stage5
+ + allowedThreadCount; // Reserve
+ return result;
+}
+
diff --git a/libs/ode-0.16.1/ode/src/quickstep.h b/libs/ode-0.16.1/ode/src/quickstep.h new file mode 100644 index 0000000..4433e9c --- /dev/null +++ b/libs/ode-0.16.1/ode/src/quickstep.h @@ -0,0 +1,39 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_QUICK_STEP_H_ +#define _ODE_QUICK_STEP_H_ + +#include <ode/common.h> + +struct dxStepperProcessingCallContext; + + +sizeint dxEstimateQuickStepMemoryRequirements( + dxBody * const *body, unsigned int nb, dxJoint * const *_joint, unsigned int _nj); +unsigned dxEstimateQuickStepMaxCallCount( + unsigned activeThreadCount, unsigned allowedThreadCount); + +void dxQuickStepIsland(const dxStepperProcessingCallContext *callContext); + + +#endif diff --git a/libs/ode-0.16.1/ode/src/ray.cpp b/libs/ode-0.16.1/ode/src/ray.cpp new file mode 100644 index 0000000..7709e8b --- /dev/null +++ b/libs/ode-0.16.1/ode/src/ray.cpp @@ -0,0 +1,735 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// ray public API + +dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1) +{ + type = dRayClass; + length = _length; +} + + +void dxRay::computeAABB() +{ + dVector3 e; + e[0] = final_posr->pos[0] + final_posr->R[0*4+2]*length; + e[1] = final_posr->pos[1] + final_posr->R[1*4+2]*length; + e[2] = final_posr->pos[2] + final_posr->R[2*4+2]*length; + + if (final_posr->pos[0] < e[0]){ + aabb[0] = final_posr->pos[0]; + aabb[1] = e[0]; + } + else{ + aabb[0] = e[0]; + aabb[1] = final_posr->pos[0]; + } + + if (final_posr->pos[1] < e[1]){ + aabb[2] = final_posr->pos[1]; + aabb[3] = e[1]; + } + else{ + aabb[2] = e[1]; + aabb[3] = final_posr->pos[1]; + } + + if (final_posr->pos[2] < e[2]){ + aabb[4] = final_posr->pos[2]; + aabb[5] = e[2]; + } + else{ + aabb[4] = e[2]; + aabb[5] = final_posr->pos[2]; + } +} + + +dGeomID dCreateRay (dSpaceID space, dReal length) +{ + return new dxRay (space,length); +} + + +void dGeomRaySetLength (dGeomID g, dReal length) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + r->length = length; + dGeomMoved (g); +} + + +dReal dGeomRayGetLength (dGeomID g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + return r->length; +} + + +void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, + dReal dx, dReal dy, dReal dz) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + g->recomputePosr(); + dReal* rot = g->final_posr->R; + dReal* pos = g->final_posr->pos; + dVector3 n; + pos[0] = px; + pos[1] = py; + pos[2] = pz; + + n[0] = dx; + n[1] = dy; + n[2] = dz; + dNormalize3(n); + rot[0*4+2] = n[0]; + rot[1*4+2] = n[1]; + rot[2*4+2] = n[2]; + dGeomMoved (g); +} + + +void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + g->recomputePosr(); + start[0] = g->final_posr->pos[0]; + start[1] = g->final_posr->pos[1]; + start[2] = g->final_posr->pos[2]; + dir[0] = g->final_posr->R[0*4+2]; + dir[1] = g->final_posr->R[1*4+2]; + dir[2] = g->final_posr->R[2*4+2]; +} + + +void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + dGeomRaySetFirstContact(g, FirstContact); + dGeomRaySetBackfaceCull(g, BackfaceCull); +} + + +void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + (*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0); + (*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0); +} + + +// set/get backface culling flag +void dGeomRaySetBackfaceCull (dxGeom *g, int backfaceCull) +{ + + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + if (backfaceCull) { + g->gflags |= RAY_BACKFACECULL; + } else { + g->gflags &= ~RAY_BACKFACECULL; + } +} + + +int dGeomRayGetBackfaceCull (dxGeom *g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + return ((g->gflags & RAY_BACKFACECULL) != 0); +} + + +// set/get first contact flag +void dGeomRaySetFirstContact (dxGeom *g, int firstContact) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + if (firstContact) { + g->gflags |= RAY_FIRSTCONTACT; + } else { + g->gflags &= ~RAY_FIRSTCONTACT; + } +} + + +int dGeomRayGetFirstContact (dxGeom *g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + return ((g->gflags & RAY_FIRSTCONTACT) != 0); +} + + +void dGeomRaySetClosestHit (dxGeom *g, int closestHit) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + if (closestHit){ + g->gflags |= RAY_CLOSEST_HIT; + } + else g->gflags &= ~RAY_CLOSEST_HIT; +} + + +int dGeomRayGetClosestHit (dxGeom *g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + return ((g->gflags & RAY_CLOSEST_HIT) != 0); +} + + + +// if mode==1 then use the sphere exit contact, not the entry contact + +static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, + dContactGeom *contact, int mode) +{ + dVector3 q; + q[0] = ray->final_posr->pos[0] - sphere_pos[0]; + q[1] = ray->final_posr->pos[1] - sphere_pos[1]; + q[2] = ray->final_posr->pos[2] - sphere_pos[2]; + dReal B = dCalcVectorDot3_14(q,ray->final_posr->R+2); + dReal C = dCalcVectorDot3(q,q) - radius*radius; + // note: if C <= 0 then the start of the ray is inside the sphere + dReal k = B*B - C; + if (k < 0) return 0; + k = dSqrt(k); + dReal alpha; + if (mode && C >= 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + else { + alpha = -B - k; + if (alpha < 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + } + if (alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0); + contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]); + contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]); + contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; +} + + +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxSphere *sphere = (dxSphere*) o2; + contact->g1 = ray; + contact->g2 = sphere; + contact->side1 = -1; + contact->side2 = -1; + return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0); +} + + +int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = ray; + contact->g2 = box; + contact->side1 = -1; + contact->side2 = -1; + + int i; + + // compute the start and delta of the ray relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,v; + tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0]; + tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1]; + tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2]; + dMultiply1_331 (s,box->final_posr->R,tmp); + tmp[0] = ray->final_posr->R[0*4+2]; + tmp[1] = ray->final_posr->R[1*4+2]; + tmp[2] = ray->final_posr->R[2*4+2]; + dMultiply1_331 (v,box->final_posr->R,tmp); + + // mirror the line so that v has all components >= 0 + dVector3 sign; + for (i=0; i<3; i++) { + if (v[i] < 0) { + s[i] = -s[i]; + v[i] = -v[i]; + sign[i] = 1; + } + else sign[i] = -1; + } + + // compute the half-sides of the box + dReal h[3]; + h[0] = REAL(0.5) * box->side[0]; + h[1] = REAL(0.5) * box->side[1]; + h[2] = REAL(0.5) * box->side[2]; + + // do a few early exit tests + if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] || + (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] || + (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] || + (v[0] == 0 && v[1] == 0 && v[2] == 0)) { + return 0; + } + + // compute the t=[lo..hi] range for where s+v*t intersects the box + dReal lo = -dInfinity; + dReal hi = dInfinity; + int nlo = 0, nhi = 0; + for (i=0; i<3; i++) { + if (v[i] != 0) { + dReal k = (-h[i] - s[i])/v[i]; + if (k > lo) { + lo = k; + nlo = i; + } + k = (h[i] - s[i])/v[i]; + if (k < hi) { + hi = k; + nhi = i; + } + } + } + + // check if the ray intersects + if (lo > hi) return 0; + dReal alpha; + int n; + if (lo >= 0) { + alpha = lo; + n = nlo; + } + else { + alpha = hi; + n = nhi; + } + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + contact->normal[0] = box->final_posr->R[0*4+n] * sign[n]; + contact->normal[1] = box->final_posr->R[1*4+n] * sign[n]; + contact->normal[2] = box->final_posr->R[2*4+n] * sign[n]; + contact->depth = alpha; + return 1; +} + + +int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxCapsule *ccyl = (dxCapsule*) o2; + + contact->g1 = ray; + contact->g2 = ccyl; + contact->side1 = -1; + contact->side2 = -1; + + dReal lz2 = ccyl->lz * REAL(0.5); + + // compute some useful info + dVector3 cs,q,r; + dReal C,k; + cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0]; + cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1]; + cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2]; + k = dCalcVectorDot3_41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis + q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0]; + q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1]; + q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2]; + C = dCalcVectorDot3(q,q) - ccyl->radius*ccyl->radius; + // if C < 0 then ray start position within infinite extension of cylinder + + // see if ray start position is inside the capped cylinder + int inside_ccyl = 0; + if (C < 0) { + if (k < -lz2) k = -lz2; + else if (k > lz2) k = lz2; + r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]; + r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]; + r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]; + if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) + + (ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) + + (ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) { + inside_ccyl = 1; + } + } + + // compute ray collision with infinite cylinder, except for the case where + // the ray is outside the capped cylinder but within the infinite cylinder + // (it that case the ray can only hit endcaps) + if (!inside_ccyl && C < 0) { + // set k to cap position to check + if (k < 0) k = -lz2; else k = lz2; + } + else { + dReal uv = dCalcVectorDot3_44(ccyl->final_posr->R+2,ray->final_posr->R+2); + r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2]; + r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2]; + r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2]; + dReal A = dCalcVectorDot3(r,r); + // A == 0 means that the ray and ccylinder axes are parallel + if (A == 0) { // There is a division by A below... + // set k to cap position to check + if (uv < 0) k = -lz2; else k = lz2; + } + else { + dReal B = 2*dCalcVectorDot3(q,r); + k = B*B-4*A*C; + if (k < 0) { + // the ray does not intersect the infinite cylinder, but if the ray is + // inside and parallel to the cylinder axis it may intersect the end + // caps. set k to cap position to check. + if (!inside_ccyl) return 0; + if (uv < 0) k = -lz2; else k = lz2; + } + else { + k = dSqrt(k); + A = dRecip (2*A); + dReal alpha = (-B-k)*A; + if (alpha < 0) { + alpha = (-B+k)*A; + if (alpha < 0) return 0; + } + if (alpha > ray->length) return 0; + + // the ray intersects the infinite cylinder. check to see if the + // intersection point is between the caps + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + q[0] = contact->pos[0] - ccyl->final_posr->pos[0]; + q[1] = contact->pos[1] - ccyl->final_posr->pos[1]; + q[2] = contact->pos[2] - ccyl->final_posr->pos[2]; + k = dCalcVectorDot3_14(q,ccyl->final_posr->R+2); + dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0); + if (k >= -lz2 && k <= lz2) { + contact->normal[0] = nsign * (contact->pos[0] - + (ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2])); + contact->normal[1] = nsign * (contact->pos[1] - + (ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2])); + contact->normal[2] = nsign * (contact->pos[2] - + (ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2])); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; + } + + // the infinite cylinder intersection point is not between the caps. + // set k to cap position to check. + if (k < 0) k = -lz2; else k = lz2; + } + } + } + + // check for ray intersection with the caps. k must indicate the cap + // position to check + q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]; + q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]; + q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]; + return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl); +} + + +int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxPlane *plane = (dxPlane*) o2; + + dReal alpha = plane->p[3] - dCalcVectorDot3 (plane->p,ray->final_posr->pos); + // note: if alpha > 0 the starting point is below the plane + dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0); + dReal k = dCalcVectorDot3_14(plane->p,ray->final_posr->R+2); + if (k==0) return 0; // ray parallel to plane + alpha /= k; + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + contact->normal[0] = nsign*plane->p[0]; + contact->normal[1] = nsign*plane->p[1]; + contact->normal[2] = nsign*plane->p[2]; + contact->depth = alpha; + contact->g1 = ray; + contact->g2 = plane; + contact->side1 = -1; + contact->side2 = -1; + return 1; +} + +// Ray-Cylinder collider by Joseph Cooper (2011) +int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dRayClass ); + dIASSERT( o2->type == dCylinderClass ); + dIASSERT( (flags & NUMC_MASK) >= 1 ); + + dxRay* ray = (dxRay*)( o1 ); + dxCylinder* cyl = (dxCylinder*)( o2 ); + + // Fill in contact information. + contact->g1 = ray; + contact->g2 = cyl; + contact->side1 = -1; + contact->side2 = -1; + + const dReal half_length = cyl->lz * REAL( 0.5 ); + + + /* Possible collision cases: + * Ray origin between/outside caps + * Ray origin within/outside radius + * Ray direction left/right/perpendicular + * Ray direction parallel/perpendicular/other + * + * Ray origin cases (ignoring origin on surface) + * + * A B + * /-\-----------\ + * C ( ) D ) + * \_/___________/ + * + * Cases A and D can collide with caps or cylinder + * Case C can only collide with the caps + * Case B can only collide with the cylinder + * Case D will produce inverted normals + * If the ray is perpendicular, only check the cylinder + * If the ray is parallel to cylinder axis, + * we can only check caps + * If the ray points right, + * Case A,C Check left cap + * Case D Check right cap + * If the ray points left + * Case A,C Check right cap + * Case D Check left cap + * Case B, check only first possible cylinder collision + * Case D, check only second possible cylinder collision + */ + // Find the ray in the cylinder coordinate frame: + dVector3 tmp; + dVector3 pos; // Ray origin in cylinder frame + dVector3 dir; // Ray direction in cylinder frame + // Translate ray start by inverse cyl + dSubtractVectors3(tmp,ray->final_posr->pos,cyl->final_posr->pos); + // Rotate ray start by inverse cyl + dMultiply1_331(pos,cyl->final_posr->R,tmp); + + // Get the ray's direction + tmp[0] = ray->final_posr->R[2]; + tmp[1] = ray->final_posr->R[6]; + tmp[2] = ray->final_posr->R[10]; + // Rotate the ray direction by inverse cyl + dMultiply1_331(dir,cyl->final_posr->R,tmp); + + // Is the ray origin inside of the (extended) cylinder? + dReal r2 = cyl->radius*cyl->radius; + dReal C = pos[0]*pos[0] + pos[1]*pos[1] - r2; + + // Find the different cases + // Is ray parallel to the cylinder length? + int parallel = (dir[0]==0 && dir[1]==0); + // Is ray perpendicular to the cylinder length? + int perpendicular = (dir[2]==0); + // Is ray origin within the radius of the caps? + int inRadius = (C<=0); + // Is ray origin between the top and bottom caps? + int inCaps = (dFabs(pos[2])<=half_length); + + int checkCaps = (!perpendicular && (!inCaps || inRadius)); + int checkCyl = (!parallel && (!inRadius || inCaps)); + int flipNormals = (inCaps&&inRadius); + + dReal tt=-dInfinity; // Depth to intersection + dVector3 tmpNorm = {dNaN, dNaN, dNaN}; // ensure we don't leak garbage + + if (checkCaps) { + // Make it so we only need to check one cap + int flipDir = 0; + // Wish c had logical xor... + if ((dir[2]<0 && flipNormals) || (dir[2]>0 && !flipNormals)) { + flipDir = 1; + dir[2]=-dir[2]; + pos[2]=-pos[2]; + } + // The cap is half the cylinder's length + // from the cylinder's origin + // We only checkCaps if dir[2]!=0 + tt = (half_length-pos[2])/dir[2]; + if (tt>=0 && tt<=ray->length) { + tmp[0] = pos[0] + tt*dir[0]; + tmp[1] = pos[1] + tt*dir[1]; + // Ensure collision point is within cap circle + if (tmp[0]*tmp[0] + tmp[1]*tmp[1] <= r2) { + // Successful collision + tmp[2] = (flipDir)?-half_length:half_length; + tmpNorm[0]=0; + tmpNorm[1]=0; + tmpNorm[2]=(flipDir!=flipNormals)?-REAL(1.0):REAL(1.0); + checkCyl = 0; // Short circuit cylinder check + } else { + // Ray hits cap plane outside of cap circle + tt=-dInfinity; // No collision yet + } + } else { + // The cap plane is beyond (or behind) the ray length + tt=-dInfinity; // No collision yet + } + if (flipDir) { + // Flip back + dir[2]=-dir[2]; + pos[2]=-pos[2]; + } + } + if (checkCyl) { + // Compute quadratic formula for parametric ray equation + dReal A = dir[0]*dir[0] + dir[1]*dir[1]; + dReal B = 2*(pos[0]*dir[0] + pos[1]*dir[1]); + // Already computed C + + dReal k = B*B - 4*A*C; + // Check collision with infinite cylinder + // k<0 means the ray passes outside the cylinder + // k==0 means ray is tangent to cylinder (or parallel) + // + // Our quadratic formula: tt = (-B +- sqrt(k))/(2*A) + // + // A must be positive (otherwise we wouldn't be checking + // cylinder because ray is parallel) + // if (k<0) ray doesn't collide with sphere + // if (B > sqrt(k)) then both times are negative + // -- don't calculate + // if (B<-sqrt(k)) then both times are positive (Case A or B) + // -- only calculate first, if first isn't valid + // -- second can't be without first going through a cap + // otherwise (fabs(B)<=sqrt(k)) then C<=0 (ray-origin inside/on cylinder) + // -- only calculate second collision + if (k>=0 && (B<0 || B*B<=k)) { + k = dSqrt(k); + A = dRecip(2*A); + if (dFabs(B)<=k) { + tt = (-B + k)*A; // Second solution + // If ray origin is on surface and pointed out, we + // can get a tt=0 solution... + } else { + tt = (-B - k)*A; // First solution + } + if (tt<=ray->length) { + tmp[2] = pos[2] + tt*dir[2]; + if (dFabs(tmp[2])<=half_length) { + // Valid solution + tmp[0] = pos[0] + tt*dir[0]; + tmp[1] = pos[1] + tt*dir[1]; + tmpNorm[0] = tmp[0]/cyl->radius; + tmpNorm[1] = tmp[1]/cyl->radius; + tmpNorm[2] = 0; + if (flipNormals) { + // Ray origin was inside cylinder + tmpNorm[0] = -tmpNorm[0]; + tmpNorm[1] = -tmpNorm[1]; + } + } else { + // Ray hits cylinder outside of caps + tt=-dInfinity; + } + } else { + // Ray doesn't reach the cylinder + tt=-dInfinity; + } + } + } + + if (tt>0) { + contact->depth = tt; + // Transform the point back to world coordinates + tmpNorm[3]=0; + tmp[3] = 0; + dMultiply0_331(contact->normal,cyl->final_posr->R,tmpNorm); + dMultiply0_331(contact->pos,cyl->final_posr->R,tmp); + contact->pos[0]+=cyl->final_posr->pos[0]; + contact->pos[1]+=cyl->final_posr->pos[1]; + contact->pos[2]+=cyl->final_posr->pos[2]; + + return 1; + } + // No contact with anything. + return 0; +} diff --git a/libs/ode-0.16.1/ode/src/resource_control.cpp b/libs/ode-0.16.1/ode/src/resource_control.cpp new file mode 100644 index 0000000..29a3d83 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/resource_control.cpp @@ -0,0 +1,259 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Resource accounting/preallocation class implementations + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#include <ode/common.h> +#include <ode/cooperative.h> +#include "config.h" +#include "resource_control.h" +#include "simple_cooperative.h" + + +////////////////////////////////////////////////////////////////////////// +// dxResourceRequirementDescriptor(); + +dxResourceRequirementDescriptor::~dxResourceRequirementDescriptor() +{ + // Do nothing +} + + +////////////////////////////////////////////////////////////////////////// +// dxRequiredResourceContainer + +dxRequiredResourceContainer::~dxRequiredResourceContainer() +{ + freeResources(); +} + + +bool dxRequiredResourceContainer::allocateResources(const dxResourceRequirementDescriptor &requirementDescriptor) +{ + bool result = false; + + bool bufferAllocated = false; + + do + { + sizeint memorySizeRequirement = requirementDescriptor.getMemorySizeRequirement(); + + if (memorySizeRequirement != 0) + { + unsigned memoryAlignmentRequirement = requirementDescriptor.getMemoryAlignmentRequirement(); + void *bufferAllocated = m_memoryAllocation.allocAligned(memorySizeRequirement, memoryAlignmentRequirement); + if (bufferAllocated == NULL) + { + break; + } + } + bufferAllocated = true; + + dxThreadingBase *relatedThreading = requirementDescriptor.getrelatedThreading(); + dIASSERT(relatedThreading != NULL); + + unsigned simultaneousCallRequirement = requirementDescriptor.getSimultaneousCallRequirement(); + if (simultaneousCallRequirement != 0) + { + if (!relatedThreading->PreallocateResourcesForThreadedCalls(simultaneousCallRequirement)) + { + break; + } + } + + dCallWaitID stockCallWait = NULL; + + if (requirementDescriptor.getIsStockCallWaitRequired()) + { + stockCallWait = relatedThreading->AllocateOrRetrieveStockCallWaitID(); + if (stockCallWait == NULL) + { + break; + } + } + + m_relatedThreading = relatedThreading; + m_stockCallWait = stockCallWait; + + result = true; + } + while (false); + + if (!result) + { + if (bufferAllocated) + { + m_memoryAllocation.freeAllocation(); + } + } + + return result; + +} + +void dxRequiredResourceContainer::freeResources() +{ + if (m_relatedThreading != NULL) + { + m_relatedThreading = NULL; + m_stockCallWait = NULL; + m_memoryAllocation.freeAllocation(); + } + else + { + dIASSERT(m_stockCallWait == NULL); + dIASSERT(m_memoryAllocation.getUserAreaPointer() == NULL); + } +} + + +////////////////////////////////////////////////////////////////////////// +// Public interface functions + +static inline +dResourceRequirementsID encodeResourceRequirementsID(dxResourceRequirementDescriptor *requirementsDescriptor) +{ + return (dResourceRequirementsID)requirementsDescriptor; +} + + +/*extern ODE_API */ +dResourceRequirementsID dResourceRequirementsCreate(dCooperativeID cooperative) +{ + dAASSERT(cooperative != NULL); + + dxSimpleCooperative *cooperativeInstance = decodeCooperativeID(cooperative); + dxThreadingBase *threading = cooperativeInstance->getRelatedThreading(); + + dxResourceRequirementDescriptor *requirementsDescriptor = new dxResourceRequirementDescriptor(threading); + + dResourceRequirementsID result = encodeResourceRequirementsID(requirementsDescriptor); + return result; +} + +/*extern ODE_API */ +void dResourceRequirementsDestroy(dResourceRequirementsID requirements) +{ + dxResourceRequirementDescriptor *requirementsDescriptor = decodeResourceRequirementsID(requirements); + + if (requirementsDescriptor != NULL) + { + delete requirementsDescriptor; + } +} + + +/*extern ODE_API */ +dResourceRequirementsID dResourceRequirementsClone(/*const */dResourceRequirementsID requirements) +{ + dAASSERT(requirements != NULL); + + dxResourceRequirementDescriptor *requirementsDescriptor = decodeResourceRequirementsID(requirements); + + dxResourceRequirementDescriptor *descriptorClone = new dxResourceRequirementDescriptor(*requirementsDescriptor); + + dResourceRequirementsID result = encodeResourceRequirementsID(descriptorClone); + return result; +} + +/*extern ODE_API */ +void dResourceRequirementsMergeIn(dResourceRequirementsID summaryRequirements, /*const */dResourceRequirementsID extraRequirements) +{ + dAASSERT(summaryRequirements != NULL); + dAASSERT(extraRequirements != NULL); + + dxResourceRequirementDescriptor *summaryDescriptor = decodeResourceRequirementsID(summaryRequirements); + dxResourceRequirementDescriptor *extraDescriptor = decodeResourceRequirementsID(extraRequirements); + + summaryDescriptor->mergeAnotherDescriptorIn(*extraDescriptor); +} + + +////////////////////////////////////////////////////////////////////////// + +static inline +dResourceContainerID encodeResourceContainerID(dxRequiredResourceContainer *containerInstance) +{ + return (dResourceContainerID)containerInstance; +} + + +/*extern ODE_API */ +dResourceContainerID dResourceContainerAcquire(/*const */dResourceRequirementsID requirements) +{ + dAASSERT(requirements != NULL); + + dResourceContainerID result = NULL; + bool allocationSucceeded = false; + + dxRequiredResourceContainer *containerInstance; + bool containerAllocated = false; + + dxResourceRequirementDescriptor *requirementsInstance = decodeResourceRequirementsID(requirements); + + do + { + containerInstance = new dxRequiredResourceContainer(); + + if (containerInstance == NULL) + { + break; + } + + containerAllocated = true; + + if (!containerInstance->allocateResources(*requirementsInstance)) + { + break; + } + + result = encodeResourceContainerID(containerInstance); + allocationSucceeded = true; + } + while (false); + + if (!allocationSucceeded) + { + if (containerAllocated) + { + delete containerInstance; + } + } + + return result; +} + +/*extern ODE_API */ +void dResourceContainerDestroy(dResourceContainerID resources) +{ + dxRequiredResourceContainer *containerInstance = decodeResourceContainerID(resources); + + if (containerInstance != NULL) + { + delete containerInstance; + } +} + diff --git a/libs/ode-0.16.1/ode/src/resource_control.h b/libs/ode-0.16.1/ode/src/resource_control.h new file mode 100644 index 0000000..cadae0e --- /dev/null +++ b/libs/ode-0.16.1/ode/src/resource_control.h @@ -0,0 +1,151 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Resource accounting/preallocation class declarations + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + +#ifndef _ODE__PRIVATE_RESOURCE_CONTRIOL_H_ +#define _ODE__PRIVATE_RESOURCE_CONTRIOL_H_ + + +#include "objects.h" +#include "threading_base.h" +#include "odeou.h" +#include "common.h" +#include "error.h" + + +using _OU_NAMESPACE::CSimpleFlags; + + +class dxResourceRequirementDescriptor: + public dBase +{ +public: + explicit dxResourceRequirementDescriptor(dxThreadingBase *relatedThreading): + dBase(), + m_relatedThreading(relatedThreading), + m_memorySizeRequirement(0), + m_memoryAlignmentRequirement(0), + m_simultaneousCallRequirement(0), + m_featureRequirements() + { + } + + ~dxResourceRequirementDescriptor(); + + enum + { + STOCK_CALLWAIT_REQUIRED = 0x00000001, + }; + + void mergeAnotherDescriptorIn(const dxResourceRequirementDescriptor &anotherDescriptor) + { + dIASSERT(getrelatedThreading() == anotherDescriptor.getrelatedThreading()); // m_simultaneousCallRequirement typically depends on threading used + + CSimpleFlags::value_type allOtherFeatureFlags = anotherDescriptor.queryAllFeatureFlags(); + mergeAnotherDescriptorIn(anotherDescriptor.m_memorySizeRequirement, anotherDescriptor.m_memoryAlignmentRequirement, anotherDescriptor.m_simultaneousCallRequirement, allOtherFeatureFlags); + } + + void mergeAnotherDescriptorIn(sizeint memorySizeRequirement/*=0*/, unsigned memoryAlignmentRequirement, + unsigned simultaneousCallRequirement/*=0*/, unsigned featureRequirement/*=0*/) + { + m_memorySizeRequirement = dMACRO_MAX(m_memorySizeRequirement, memorySizeRequirement); + m_memoryAlignmentRequirement = dMACRO_MAX(m_memoryAlignmentRequirement, memoryAlignmentRequirement); + m_simultaneousCallRequirement = dMACRO_MAX(m_simultaneousCallRequirement, simultaneousCallRequirement); + mergeFeatureFlags(featureRequirement); + } + +public: + dxThreadingBase *getrelatedThreading() const { return m_relatedThreading; } + sizeint getMemorySizeRequirement() const { return m_memorySizeRequirement; } + unsigned getMemoryAlignmentRequirement() const { return m_memoryAlignmentRequirement; } + + unsigned getSimultaneousCallRequirement() const { return m_simultaneousCallRequirement; } + + bool getIsStockCallWaitRequired() const { return getStockCallWaitRequiredFlag(); } + +private: + enum + { + FL_STOCK_CALLWAIT_REQUIRED = STOCK_CALLWAIT_REQUIRED, + }; + + bool getStockCallWaitRequiredFlag() const { return m_featureRequirements.GetFlagsMaskValue(FL_STOCK_CALLWAIT_REQUIRED); } + + CSimpleFlags::value_type queryAllFeatureFlags() const { return m_featureRequirements.QueryFlagsAllValues(); } + void mergeFeatureFlags(CSimpleFlags::value_type flagValues) { m_featureRequirements.SignalFlagsMaskValue(flagValues); } + +private: + dxThreadingBase *m_relatedThreading; + sizeint m_memorySizeRequirement; + unsigned m_memoryAlignmentRequirement; + unsigned m_simultaneousCallRequirement; + CSimpleFlags m_featureRequirements; +}; + +static inline +dxResourceRequirementDescriptor *decodeResourceRequirementsID(dResourceRequirementsID requirements) +{ + return (dxResourceRequirementDescriptor *)requirements; +} + + +class dxRequiredResourceContainer: + public dBase +{ +public: + dxRequiredResourceContainer(): + dBase(), + m_relatedThreading(NULL), + m_stockCallWait(NULL), + m_memoryAllocation() + { + } + + ~dxRequiredResourceContainer(); + + bool allocateResources(const dxResourceRequirementDescriptor &requirementDescriptor); + void freeResources(); + +public: + dxThreadingBase *getThreadingInstance() const { return m_relatedThreading; } + dCallWaitID getStockCallWait() const { return m_stockCallWait; } + void *getMemoryBufferPointer() const { return m_memoryAllocation.getUserAreaPointer(); } + sizeint getMemoryBufferSize() const { return m_memoryAllocation.getUserAreaSize(); } + +private: + dxThreadingBase *m_relatedThreading; + dCallWaitID m_stockCallWait; + dxAlignedAllocation m_memoryAllocation; +}; + +static inline +dxRequiredResourceContainer *decodeResourceContainerID(dResourceContainerID resources) +{ + return (dxRequiredResourceContainer *)resources; +} + + +#endif // #ifndef _ODE__PRIVATE_RESOURCE_CONTRIOL_H_ diff --git a/libs/ode-0.16.1/ode/src/rotation.cpp b/libs/ode-0.16.1/ode/src/rotation.cpp new file mode 100644 index 0000000..e813ba3 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/rotation.cpp @@ -0,0 +1,317 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the +"rotation axis" and s is the "rotation angle". + +*/ + +#include <ode/rotation.h> +#include "config.h" +#include "odemath.h" + + +#define _R(i,j) R[(i)*4+(j)] + +#define SET_3x3_IDENTITY \ + _R(0,0) = REAL(1.0); \ + _R(0,1) = REAL(0.0); \ + _R(0,2) = REAL(0.0); \ + _R(0,3) = REAL(0.0); \ + _R(1,0) = REAL(0.0); \ + _R(1,1) = REAL(1.0); \ + _R(1,2) = REAL(0.0); \ + _R(1,3) = REAL(0.0); \ + _R(2,0) = REAL(0.0); \ + _R(2,1) = REAL(0.0); \ + _R(2,2) = REAL(1.0); \ + _R(2,3) = REAL(0.0); + + +void dRSetIdentity (dMatrix3 R) +{ + dAASSERT (R); + SET_3x3_IDENTITY; +} + + +void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal angle) +{ + dAASSERT (R); + dQuaternion q; + dQFromAxisAndAngle (q,ax,ay,az,angle); + dQtoR (q,R); +} + + +void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi) +{ + dReal sphi,cphi,stheta,ctheta,spsi,cpsi; + dAASSERT (R); + sphi = dSin(phi); + cphi = dCos(phi); + stheta = dSin(theta); + ctheta = dCos(theta); + spsi = dSin(psi); + cpsi = dCos(psi); + _R(0,0) = cpsi*ctheta; + _R(0,1) = spsi*ctheta; + _R(0,2) =-stheta; + _R(0,3) = REAL(0.0); + _R(1,0) = cpsi*stheta*sphi - spsi*cphi; + _R(1,1) = spsi*stheta*sphi + cpsi*cphi; + _R(1,2) = ctheta*sphi; + _R(1,3) = REAL(0.0); + _R(2,0) = cpsi*stheta*cphi + spsi*sphi; + _R(2,1) = spsi*stheta*cphi - cpsi*sphi; + _R(2,2) = ctheta*cphi; + _R(2,3) = REAL(0.0); +} + + +void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal bx, dReal by, dReal bz) +{ + dReal l,k; + dAASSERT (R); + l = dSqrt (ax*ax + ay*ay + az*az); + if (l <= REAL(0.0)) { + dDEBUGMSG ("zero length vector"); + return; + } + l = dRecip(l); + ax *= l; + ay *= l; + az *= l; + k = ax*bx + ay*by + az*bz; + bx -= k*ax; + by -= k*ay; + bz -= k*az; + l = dSqrt (bx*bx + by*by + bz*bz); + if (l <= REAL(0.0)) { + dDEBUGMSG ("zero length vector"); + return; + } + l = dRecip(l); + bx *= l; + by *= l; + bz *= l; + _R(0,0) = ax; + _R(1,0) = ay; + _R(2,0) = az; + _R(0,1) = bx; + _R(1,1) = by; + _R(2,1) = bz; + _R(0,2) = - by*az + ay*bz; + _R(1,2) = - bz*ax + az*bx; + _R(2,2) = - bx*ay + ax*by; + _R(0,3) = REAL(0.0); + _R(1,3) = REAL(0.0); + _R(2,3) = REAL(0.0); +} + + +void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az) +{ + dVector3 n,p,q; + n[0] = ax; + n[1] = ay; + n[2] = az; + dNormalize3 (n); + dPlaneSpace (n,p,q); + _R(0,0) = p[0]; + _R(1,0) = p[1]; + _R(2,0) = p[2]; + _R(0,1) = q[0]; + _R(1,1) = q[1]; + _R(2,1) = q[2]; + _R(0,2) = n[0]; + _R(1,2) = n[1]; + _R(2,2) = n[2]; + _R(0,3) = REAL(0.0); + _R(1,3) = REAL(0.0); + _R(2,3) = REAL(0.0); +} + + +void dQSetIdentity (dQuaternion q) +{ + dAASSERT (q); + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; +} + + +void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, + dReal angle) +{ + dAASSERT (q); + dReal l = ax*ax + ay*ay + az*az; + if (l > REAL(0.0)) { + angle *= REAL(0.5); + q[0] = dCos (angle); + l = dSin(angle) * dRecipSqrt(l); + q[1] = ax*l; + q[2] = ay*l; + q[3] = az*l; + } + else { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } +} + + +void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; + qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; + qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; + qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; +} + + +void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; + qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; + qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; + qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; +} + + +void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; + qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; + qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; + qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; +} + + +void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; + qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; + qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; + qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; +} + + +// dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction +// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained +// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon +// University, 1997. + +void dRfromQ (dMatrix3 R, const dQuaternion q) +{ + dAASSERT (q && R); + // q = (s,vx,vy,vz) + dReal qq1 = 2*q[1]*q[1]; + dReal qq2 = 2*q[2]*q[2]; + dReal qq3 = 2*q[3]*q[3]; + _R(0,0) = 1 - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(0,3) = REAL(0.0); + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1 - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(1,3) = REAL(0.0); + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1 - qq1 - qq2; + _R(2,3) = REAL(0.0); +} + + +void dQfromR (dQuaternion q, const dMatrix3 R) +{ + dAASSERT (q && R); + dReal tr,s; + tr = _R(0,0) + _R(1,1) + _R(2,2); + if (tr >= 0) { + s = dSqrt (tr + 1); + q[0] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[1] = (_R(2,1) - _R(1,2)) * s; + q[2] = (_R(0,2) - _R(2,0)) * s; + q[3] = (_R(1,0) - _R(0,1)) * s; + } + else { + // find the largest diagonal element and jump to the appropriate case + if (_R(1,1) > _R(0,0)) { + if (_R(2,2) > _R(1,1)) goto case_2; + goto case_1; + } + if (_R(2,2) > _R(0,0)) goto case_2; + goto case_0; + +case_0: + s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1); + q[1] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[2] = (_R(0,1) + _R(1,0)) * s; + q[3] = (_R(2,0) + _R(0,2)) * s; + q[0] = (_R(2,1) - _R(1,2)) * s; + return; + +case_1: + s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1); + q[2] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[3] = (_R(1,2) + _R(2,1)) * s; + q[1] = (_R(0,1) + _R(1,0)) * s; + q[0] = (_R(0,2) - _R(2,0)) * s; + return; + +case_2: + s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1); + q[3] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[1] = (_R(2,0) + _R(0,2)) * s; + q[2] = (_R(1,2) + _R(2,1)) * s; + q[0] = (_R(1,0) - _R(0,1)) * s; + return; + } +} + + +void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q) +{ + dAASSERT (w && q && dq); + dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]); + dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]); + dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]); + dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]); +} diff --git a/libs/ode-0.16.1/ode/src/simple_cooperative.cpp b/libs/ode-0.16.1/ode/src/simple_cooperative.cpp new file mode 100644 index 0000000..f8f6f7d --- /dev/null +++ b/libs/ode-0.16.1/ode/src/simple_cooperative.cpp @@ -0,0 +1,84 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading base wrapper class header file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * The simple cooperative class implementation + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#include <ode/common.h> +#include <ode/cooperative.h> +#include "config.h" +#include "simple_cooperative.h" +#include "default_threading.h" + + +/*virtual */ +dxSimpleCooperative::~dxSimpleCooperative() +{ + // The virtual destructor +} + + +/*virtual */ +const dxThreadingFunctionsInfo *dxSimpleCooperative::retrieveThreadingDefaultImpl(dThreadingImplementationID &out_defaultImpl) +{ + out_defaultImpl = DefaultThreadingHolder::getDefaultThreadingImpl(); + return DefaultThreadingHolder::getDefaultThreadingFunctions(); +} + + +////////////////////////////////////////////////////////////////////////// +// Public interface functions + +static inline +dCooperativeID encodeCooperativeID(dxSimpleCooperative *cooperativeInstance) +{ + return (dCooperativeID)cooperativeInstance; +} + + +/*extern ODE_API */ +dCooperativeID dCooperativeCreate(const dThreadingFunctionsInfo *functionInfo/*=NULL*/, dThreadingImplementationID threadingImpl/*=NULL*/) +{ + dxSimpleCooperative *cooperativeInstance = new dxSimpleCooperative(functionInfo, threadingImpl); + + dCooperativeID result = encodeCooperativeID(cooperativeInstance); + return result; +} + +/*extern ODE_API */ +void dCooperativeDestroy(dCooperativeID cooperative) +{ + dxSimpleCooperative *cooperativeInstance = decodeCooperativeID(cooperative); + + if (cooperativeInstance != NULL) + { + delete cooperativeInstance; + } +} + diff --git a/libs/ode-0.16.1/ode/src/simple_cooperative.h b/libs/ode-0.16.1/ode/src/simple_cooperative.h new file mode 100644 index 0000000..8fcbc99 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/simple_cooperative.h @@ -0,0 +1,73 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading base wrapper class header file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * A simple cooperative class definition + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + +#ifndef _ODE__PRIVATE_SIMPLE_COOPERATIVE_H_ +#define _ODE__PRIVATE_SIMPLE_COOPERATIVE_H_ + + +#include "objects.h" +#include "threading_base.h" + + +typedef dxThreadingBase dxSimpleCooperative_ThreadingParent; +class dxSimpleCooperative: + public dBase, + public dxSimpleCooperative_ThreadingParent, + private dxIThreadingDefaultImplProvider +{ +public: + dxSimpleCooperative(const dxThreadingFunctionsInfo *functionInfo, dThreadingImplementationID threadingImpl): + dBase(), + dxSimpleCooperative_ThreadingParent() + { + dxSimpleCooperative_ThreadingParent::setThreadingDefaultImplProvider(this); + dxSimpleCooperative_ThreadingParent::assignThreadingImpl(functionInfo, threadingImpl); + } + + virtual ~dxSimpleCooperative(); + +public: + dxThreadingBase *getRelatedThreading() const { return const_cast<dxSimpleCooperative *>(this); } + +private: // dxIThreadingDefaultImplProvider + virtual const dxThreadingFunctionsInfo *retrieveThreadingDefaultImpl(dThreadingImplementationID &out_defaultImpl); +}; + + +static inline +dxSimpleCooperative *decodeCooperativeID(dCooperativeID cooperative) +{ + return (dxSimpleCooperative *)cooperative; +} + + +#endif // #ifndef _ODE__PRIVATE_SIMPLE_COOPERATIVE_H_ diff --git a/libs/ode-0.16.1/ode/src/sphere.cpp b/libs/ode-0.16.1/ode/src/sphere.cpp new file mode 100644 index 0000000..e894bac --- /dev/null +++ b/libs/ode-0.16.1/ode/src/sphere.cpp @@ -0,0 +1,251 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include <ode/common.h> +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + + +//**************************************************************************** +// sphere public API + +dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1) +{ + dAASSERT (_radius >= 0); + type = dSphereClass; + radius = _radius; + updateZeroSizedFlag(!_radius); +} + + +void dxSphere::computeAABB() +{ + aabb[0] = final_posr->pos[0] - radius; + aabb[1] = final_posr->pos[0] + radius; + aabb[2] = final_posr->pos[1] - radius; + aabb[3] = final_posr->pos[1] + radius; + aabb[4] = final_posr->pos[2] - radius; + aabb[5] = final_posr->pos[2] + radius; +} + + +dGeomID dCreateSphere (dSpaceID space, dReal radius) +{ + return new dxSphere (space,radius); +} + + +void dGeomSphereSetRadius (dGeomID g, dReal radius) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + dAASSERT (radius >= 0); + dxSphere *s = (dxSphere*) g; + s->radius = radius; + s->updateZeroSizedFlag(!radius); + dGeomMoved (g); +} + + +dReal dGeomSphereGetRadius (dGeomID g) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + dxSphere *s = (dxSphere*) g; + return s->radius; +} + + +dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + g->recomputePosr(); + + dxSphere *s = (dxSphere*) g; + dReal * pos = s->final_posr->pos; + return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) + + (y-pos[1])*(y-pos[1]) + + (z-pos[2])*(z-pos[2])); +} + +//**************************************************************************** +// pairwise collision functions for standard geom types + +int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxSphere *sphere1 = (dxSphere*) o1; + dxSphere *sphere2 = (dxSphere*) o2; + + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + + return dCollideSpheres (o1->final_posr->pos,sphere1->radius, + o2->final_posr->pos,sphere2->radius,contact); +} + + +int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // this is easy. get the sphere center `p' relative to the box, and then clip + // that to the boundary of the box (call that point `q'). if q is on the + // boundary of the box and |p-q| is <= sphere radius, they touch. + // if q is inside the box, the sphere is inside the box, so set a contact + // normal to push the sphere to the closest box face. + + dVector3 l,t,p,q,r; + dReal depth; + int onborder = 0; + + dxSphere *sphere = (dxSphere*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + + p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0]; + p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1]; + p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2]; + + l[0] = box->side[0]*REAL(0.5); + t[0] = dCalcVectorDot3_14(p,o2->final_posr->R); + if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; } + if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; } + + l[1] = box->side[1]*REAL(0.5); + t[1] = dCalcVectorDot3_14(p,o2->final_posr->R+1); + if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; } + if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; } + + t[2] = dCalcVectorDot3_14(p,o2->final_posr->R+2); + l[2] = box->side[2]*REAL(0.5); + if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; } + if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; } + + if (!onborder) { + // sphere center inside box. find closest face to `t' + dReal min_distance = l[0] - dFabs(t[0]); + int mini = 0; + for (int i=1; i<3; i++) { + dReal face_distance = l[i] - dFabs(t[i]); + if (face_distance < min_distance) { + min_distance = face_distance; + mini = i; + } + } + // contact position = sphere center + contact->pos[0] = o1->final_posr->pos[0]; + contact->pos[1] = o1->final_posr->pos[1]; + contact->pos[2] = o1->final_posr->pos[2]; + // contact normal points to closest face + dVector3 tmp; + tmp[0] = 0; + tmp[1] = 0; + tmp[2] = 0; + tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0); + dMultiply0_331 (contact->normal,o2->final_posr->R,tmp); + // contact depth = distance to wall along normal plus radius + contact->depth = min_distance + sphere->radius; + return 1; + } + + t[3] = 0; //@@@ hmmm + dMultiply0_331 (q,o2->final_posr->R,t); + r[0] = p[0] - q[0]; + r[1] = p[1] - q[1]; + r[2] = p[2] - q[2]; + depth = sphere->radius - dSqrt(dCalcVectorDot3(r,r)); + if (depth < 0) return 0; + contact->pos[0] = q[0] + o2->final_posr->pos[0]; + contact->pos[1] = q[1] + o2->final_posr->pos[1]; + contact->pos[2] = q[2] + o2->final_posr->pos[2]; + contact->normal[0] = r[0]; + contact->normal[1] = r[1]; + contact->normal[2] = r[2]; + dNormalize3 (contact->normal); + contact->depth = depth; + return 1; +} + + +int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxSphere *sphere = (dxSphere*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + + dReal k = dCalcVectorDot3 (o1->final_posr->pos,plane->p); + dReal depth = plane->p[3] - k + sphere->radius; + if (depth >= 0) { + contact->normal[0] = plane->p[0]; + contact->normal[1] = plane->p[1]; + contact->normal[2] = plane->p[2]; + contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius; + contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius; + contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius; + contact->depth = depth; + return 1; + } + else return 0; +} diff --git a/libs/ode-0.16.1/ode/src/step.cpp b/libs/ode-0.16.1/ode/src/step.cpp new file mode 100644 index 0000000..033e879 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/step.cpp @@ -0,0 +1,1672 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#include <ode/odeconfig.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include "config.h"
+#include "odemath.h"
+#include "matrix.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "lcp.h"
+#include "util.h"
+#include "threadingutils.h"
+
+#include <new>
+
+
+#define dMIN(A,B) ((A)>(B) ? (B) : (A))
+#define dMAX(A,B) ((B)>(A) ? (B) : (A))
+
+//****************************************************************************
+// misc defines
+
+//#define TIMING
+
+
+#ifdef TIMING
+#define IFTIMING(x) x
+#else
+#define IFTIMING(x) ((void)0)
+#endif
+
+
+struct dJointWithInfo1
+{
+ dxJoint *joint;
+ dxJoint::Info1 info;
+};
+
+enum dxRHSCFMElement
+{
+ RCE_RHS = dxJoint::GI2_RHS,
+ RCE_CFM = dxJoint::GI2_CFM,
+
+ // Elements for array reuse
+ RLE_RHS = RCE_RHS,
+ RLE_LAMBDA = RCE_CFM,
+
+ RCE__RHS_CFM_MAX = dxJoint::GI2__RHS_CFM_MAX,
+ RLE__RHS_LAMBDA_MAX = RCE__RHS_CFM_MAX,
+};
+
+enum dxLoHiElement
+{
+ LHE_LO = dxJoint::GI2_LO,
+ LHE_HI = dxJoint::GI2_HI,
+
+ LHE__LO_HI_MAX = dxJoint::GI2__LO_HI_MAX,
+};
+
+enum dxJacobiVectorElement
+{
+ JVE__MIN,
+
+ JVE__L_MIN = JVE__MIN + dDA__L_MIN,
+
+ JVE_LX = JVE__L_MIN + dSA_X,
+ JVE_LY = JVE__L_MIN + dSA_Y,
+ JVE_LZ = JVE__L_MIN + dSA_Z,
+
+ JVE__L_MAX = JVE__L_MIN + dSA__MAX,
+
+ JVE__A_MIN = JVE__MIN + dDA__A_MIN,
+
+ JVE_AX = JVE__A_MIN + dSA_X,
+ JVE_AY = JVE__A_MIN + dSA_Y,
+ JVE_AZ = JVE__A_MIN + dSA_Z,
+
+ JVE__A_MAX = JVE__A_MIN + dSA__MAX,
+
+ JVE__MAX = JVE__MIN + dDA__MAX,
+
+ JVE__L_COUNT = JVE__L_MAX - JVE__L_MIN,
+ JVE__A_COUNT = JVE__A_MAX - JVE__A_MIN,
+};
+
+
+enum dxJacobiMatrixElement
+{
+ JME__MIN,
+
+ JME__J_MIN = JME__MIN,
+ JME__JL_MIN = JME__J_MIN + JVE__L_MIN,
+
+ JME_JLX = JME__J_MIN + JVE_LX,
+ JME_JLY = JME__J_MIN + JVE_LY,
+ JME_JLZ = JME__J_MIN + JVE_LZ,
+
+ JME__JL_MAX = JME__J_MIN + JVE__L_MAX,
+
+ JME__JA_MIN = JME__J_MIN + JVE__A_MIN,
+
+ JME_JAX = JME__J_MIN + JVE_AX,
+ JME_JAY = JME__J_MIN + JVE_AY,
+ JME_JAZ = JME__J_MIN + JVE_AZ,
+
+ JME__JA_MAX = JME__J_MIN + JVE__A_MAX,
+ JME__J_MAX = JME__J_MIN + JVE__MAX,
+
+ JME__MAX = JME__J_MAX,
+
+ JME__J_COUNT = JME__J_MAX - JME__J_MIN,
+};
+
+enum dxJInvMElement
+{
+ JIM__MIN,
+
+ JIM__L_MIN = JIM__MIN + dMD_LINEAR * dV3E__MAX,
+
+ JIM__L_AXES_MIN = JIM__L_MIN + dV3E__AXES_MIN,
+
+ JIM_LX = JIM__L_MIN + dV3E_X,
+ JIM_LY = JIM__L_MIN + dV3E_Y,
+ JIM_LZ = JIM__L_MIN + dV3E_Z,
+
+ JIM__L_AXES_MAX = JIM__L_MIN + dV3E__AXES_MAX,
+
+ JIM_LPAD = JIM__L_MIN + dV3E_PAD,
+
+ JIM__L_MAX = JIM__L_MIN + dV3E__MAX,
+
+ JIM__A_MIN = JIM__MIN + dMD_ANGULAR * dV3E__MAX,
+
+ JIM__A_AXES_MIN = JIM__A_MIN + dV3E__AXES_MIN,
+
+ JIM_AX = JIM__A_MIN + dV3E_X,
+ JIM_AY = JIM__A_MIN + dV3E_Y,
+ JIM_AZ = JIM__A_MIN + dV3E_Z,
+
+ JIM__A_AXES_MAX = JIM__A_MIN + dV3E__AXES_MAX,
+
+ JIM_APAD = JIM__A_MIN + dV3E_PAD,
+
+ JIM__A_MAX = JIM__A_MIN + dV3E__MAX,
+
+ JIM__MAX = JIM__MIN + dMD__MAX * dV3E__MAX,
+};
+
+enum dxContactForceElement
+{
+ CFE__MIN,
+
+ CFE__DYNAMICS_MIN = CFE__MIN,
+
+ CFE__L_MIN = CFE__DYNAMICS_MIN + dDA__L_MIN,
+
+ CFE_LX = CFE__DYNAMICS_MIN + dDA_LX,
+ CFE_LY = CFE__DYNAMICS_MIN + dDA_LY,
+ CFE_LZ = CFE__DYNAMICS_MIN + dDA_LZ,
+
+ CFE__L_MAX = CFE__DYNAMICS_MIN + dDA__L_MAX,
+
+ CFE__A_MIN = CFE__DYNAMICS_MIN + dDA__A_MIN,
+
+ CFE_AX = CFE__DYNAMICS_MIN + dDA_AX,
+ CFE_AY = CFE__DYNAMICS_MIN + dDA_AY,
+ CFE_AZ = CFE__DYNAMICS_MIN + dDA_AZ,
+
+ CFE__A_MAX = CFE__DYNAMICS_MIN + dDA__A_MAX,
+
+ CFE__DYNAMICS_MAX = CFE__DYNAMICS_MIN + dDA__MAX,
+
+ CFE__MAX = CFE__DYNAMICS_MAX,
+};
+
+
+#define AMATRIX_ALIGNMENT dMAX(64, EFFICIENT_ALIGNMENT)
+#define INVI_ALIGNMENT dMAX(32, EFFICIENT_ALIGNMENT)
+#define JINVM_ALIGNMENT dMAX(64, EFFICIENT_ALIGNMENT)
+
+struct dxStepperStage0Outputs
+{
+ sizeint ji_start;
+ sizeint ji_end;
+ unsigned int m;
+ unsigned int nub;
+};
+
+struct dxStepperStage1CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *stepperCallContext, void *stageMemArenaState, dReal *invI, dJointWithInfo1 *jointinfos)
+ {
+ m_stepperCallContext = stepperCallContext;
+ m_stageMemArenaState = stageMemArenaState;
+ m_invI = invI;
+ m_jointinfos = jointinfos;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ void *m_stageMemArenaState;
+ dReal *m_invI;
+ dJointWithInfo1 *m_jointinfos;
+ dxStepperStage0Outputs m_stage0Outputs;
+};
+
+struct dxStepperStage0BodiesCallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *stepperCallContext, dReal *invI)
+ {
+ m_stepperCallContext = stepperCallContext;
+ m_invI = invI;
+ m_tagsTaken = 0;
+ m_gravityTaken = 0;
+ m_inertiaBodyIndex = 0;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ dReal *m_invI;
+ atomicord32 m_tagsTaken;
+ atomicord32 m_gravityTaken;
+ volatile atomicord32 m_inertiaBodyIndex;
+};
+
+struct dxStepperStage0JointsCallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *stepperCallContext, dJointWithInfo1 *jointinfos, dxStepperStage0Outputs *stage0Outputs)
+ {
+ m_stepperCallContext = stepperCallContext;
+ m_jointinfos = jointinfos;
+ m_stage0Outputs = stage0Outputs;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ dJointWithInfo1 *m_jointinfos;
+ dxStepperStage0Outputs *m_stage0Outputs;
+};
+
+static int dxStepIsland_Stage0_Bodies_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+// static int dxStepIsland_Stage0_Joints_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxStepIsland_Stage1_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+
+static void dxStepIsland_Stage0_Bodies(dxStepperStage0BodiesCallContext *callContext);
+static void dxStepIsland_Stage0_Joints(dxStepperStage0JointsCallContext *callContext);
+static void dxStepIsland_Stage1(dxStepperStage1CallContext *callContext);
+
+
+struct dxStepperLocalContext
+{
+ void Initialize(dReal *invI, dJointWithInfo1 *jointinfos, unsigned int nj,
+ unsigned int m, unsigned int nub, const unsigned int *mindex, int *findex,
+ dReal *J, dReal *A, dReal *pairsRhsCfm, dReal *pairsLoHi,
+ atomicord32 *bodyStartJoints, atomicord32 *bodyJointLinks)
+ {
+ m_invI = invI;
+ m_jointinfos = jointinfos;
+ m_nj = nj;
+ m_m = m;
+ m_nub = nub;
+ m_mindex = mindex;
+ m_findex = findex;
+ m_J = J;
+ m_A = A;
+ m_pairsRhsCfm = pairsRhsCfm;
+ m_pairsLoHi = pairsLoHi;
+ m_bodyStartJoints = bodyStartJoints;
+ m_bodyJointLinks = bodyJointLinks;
+ }
+
+ dReal *m_invI;
+ dJointWithInfo1 *m_jointinfos;
+ unsigned int m_nj;
+ unsigned int m_m;
+ unsigned int m_nub;
+ const unsigned int *m_mindex;
+ int *m_findex;
+ dReal *m_J;
+ dReal *m_A;
+ dReal *m_pairsRhsCfm;
+ dReal *m_pairsLoHi;
+ atomicord32 *m_bodyStartJoints;
+ atomicord32 *m_bodyJointLinks;
+};
+
+struct dxStepperStage2CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, const dxStepperLocalContext *localContext,
+ dReal *JinvM, dReal *rhs_tmp)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ m_JinvM = JinvM;
+ m_rhs_tmp = rhs_tmp;
+ m_ji_J = 0;
+ m_ji_Ainit = 0;
+ m_ji_JinvM = 0;
+ m_ji_Aaddjb = 0;
+ m_bi_rhs_tmp = 0;
+ m_ji_rhs = 0;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ const dxStepperLocalContext *m_localContext;
+ dReal *m_JinvM;
+ dReal *m_rhs_tmp;
+ volatile atomicord32 m_ji_J;
+ volatile atomicord32 m_ji_Ainit;
+ volatile atomicord32 m_ji_JinvM;
+ volatile atomicord32 m_ji_Aaddjb;
+ volatile atomicord32 m_bi_rhs_tmp;
+ volatile atomicord32 m_ji_rhs;
+};
+
+struct dxStepperStage3CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, const dxStepperLocalContext *localContext,
+ void *stage1MemArenaState)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ m_stage1MemArenaState = stage1MemArenaState;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ const dxStepperLocalContext *m_localContext;
+ void *m_stage1MemArenaState;
+};
+
+struct dxStepperStage4CallContext
+{
+ void Initialize(const dxStepperProcessingCallContext *callContext, const dxStepperLocalContext *localContext/*,
+ void *stage3MemarenaState*/)
+ {
+ m_stepperCallContext = callContext;
+ m_localContext = localContext;
+ // m_stage3MemarenaState = stage3MemarenaState;
+ m_bi_constrForce = 0;
+ }
+
+ const dxStepperProcessingCallContext *m_stepperCallContext;
+ const dxStepperLocalContext *m_localContext;
+ // void *m_stage3MemarenaState;
+ volatile atomicord32 m_bi_constrForce;
+};
+
+static int dxStepIsland_Stage2a_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxStepIsland_Stage2aSync_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxStepIsland_Stage2b_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxStepIsland_Stage2bSync_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxStepIsland_Stage2c_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static int dxStepIsland_Stage3_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+
+static void dxStepIsland_Stage2a(dxStepperStage2CallContext *callContext);
+static void dxStepIsland_Stage2b(dxStepperStage2CallContext *callContext);
+static void dxStepIsland_Stage2c(dxStepperStage2CallContext *callContext);
+static void dxStepIsland_Stage3(dxStepperStage3CallContext *callContext);
+
+static int dxStepIsland_Stage4_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee);
+static void dxStepIsland_Stage4(dxStepperStage4CallContext *stage4CallContext);
+
+
+//****************************************************************************
+// special matrix multipliers
+
+
+// this assumes the 4th and 8th rows of B and C are zero.
+
+static inline
+void MultiplyAddJinvMxJToA (dReal *Arow, const dReal *JinvMRow, const dReal *JRow,
+ unsigned int infomJinvM, unsigned int infomJ, unsigned int mskip)
+{
+ dIASSERT (infomJinvM > 0 && infomJ > 0 && Arow && JinvMRow && JRow);
+ const unsigned int mskip_munus_infomJ_plus_1 = mskip - infomJ + 1;
+ dIASSERT(mskip >= infomJ);
+ dReal *currA = Arow;
+ const dReal *currJinvM = JinvMRow;
+ for (unsigned int i = infomJinvM; ; ) {
+ dReal JiM0 = currJinvM[JIM_LX];
+ dReal JiM1 = currJinvM[JIM_LY];
+ dReal JiM2 = currJinvM[JIM_LZ];
+ dReal JiM4 = currJinvM[JIM_AX];
+ dReal JiM5 = currJinvM[JIM_AY];
+ dReal JiM6 = currJinvM[JIM_AZ];
+ const dReal *currJ = JRow;
+ for (unsigned int j = infomJ; ; ) {
+ dReal sum;
+ sum = JiM0 * currJ[JME_JLX];
+ sum += JiM1 * currJ[JME_JLY];
+ sum += JiM2 * currJ[JME_JLZ];
+ sum += JiM4 * currJ[JME_JAX];
+ sum += JiM5 * currJ[JME_JAY];
+ sum += JiM6 * currJ[JME_JAZ];
+ *currA += sum;
+ if (--j == 0) {
+ break;
+ }
+ ++currA;
+ currJ += JME__MAX;
+ }
+ if (--i == 0) {
+ break;
+ }
+ currJinvM += JIM__MAX;
+ currA += mskip_munus_infomJ_plus_1;
+ }
+}
+
+
+// this assumes the 4th and 8th rows of B are zero.
+
+static inline
+void MultiplySubJxRhsTmpFromRHS (dReal *rowRhsCfm, const dReal *JRow, const dReal *rowRhsTmp, unsigned int infom)
+{
+ dIASSERT (infom > 0 && rowRhsCfm && JRow && rowRhsTmp);
+ dReal *currRhs = rowRhsCfm + RCE_RHS;
+ const dReal *currJ = JRow;
+ const dReal RT_LX = rowRhsTmp[dDA_LX], RT_LY = rowRhsTmp[dDA_LY], RT_LZ = rowRhsTmp[dDA_LZ];
+ const dReal RT_AX = rowRhsTmp[dDA_AX], RT_AY = rowRhsTmp[dDA_AY], RT_AZ = rowRhsTmp[dDA_AZ];
+ for (unsigned int i = infom; ; ) {
+ dReal sum;
+ sum = currJ[JME_JLX] * RT_LX;
+ sum += currJ[JME_JLY] * RT_LY;
+ sum += currJ[JME_JLZ] * RT_LZ;
+ sum += currJ[JME_JAX] * RT_AX;
+ sum += currJ[JME_JAY] * RT_AY;
+ sum += currJ[JME_JAZ] * RT_AZ;
+ *currRhs -= sum;
+ if (--i == 0) {
+ break;
+ }
+ currRhs += RCE__RHS_CFM_MAX;
+ currJ += JME__MAX;
+ }
+}
+
+
+static inline
+void MultiplyAddJxLambdaToCForce(dReal cforce[CFE__MAX],
+ const dReal *JRow, const dReal *rowRhsLambda, unsigned int infom,
+ dJointFeedback *fb/*=NULL*/, unsigned jointBodyIndex)
+{
+ dIASSERT (infom > 0 && cforce && JRow && rowRhsLambda);
+ dReal sumLX = 0, sumLY = 0, sumLZ = 0, sumAX=0, sumAY = 0, sumAZ = 0;
+ const dReal *currJ = JRow, *currLambda = rowRhsLambda + RLE_LAMBDA;
+ for (unsigned int k = infom; ; ) {
+ const dReal lambda = *currLambda;
+ sumLX += currJ[JME_JLX] * lambda;
+ sumLY += currJ[JME_JLY] * lambda;
+ sumLZ += currJ[JME_JLZ] * lambda;
+ sumAX += currJ[JME_JAX] * lambda;
+ sumAY += currJ[JME_JAY] * lambda;
+ sumAZ += currJ[JME_JAZ] * lambda;
+ if (--k == 0) {
+ break;
+ }
+ currJ += JME__MAX;
+ currLambda += RLE__RHS_LAMBDA_MAX;
+ }
+ if (fb != NULL) {
+ if (jointBodyIndex == dJCB__MIN) {
+ fb->f1[dV3E_X] = sumLX;
+ fb->f1[dV3E_Y] = sumLY;
+ fb->f1[dV3E_Z] = sumLZ;
+ fb->t1[dV3E_X] = sumAX;
+ fb->t1[dV3E_Y] = sumAY;
+ fb->t1[dV3E_Z] = sumAZ;
+ }
+ else {
+ dIASSERT(jointBodyIndex == dJCB__MIN + 1);
+ dSASSERT(dJCB__MAX == 2);
+
+ fb->f2[dV3E_X] = sumLX;
+ fb->f2[dV3E_Y] = sumLY;
+ fb->f2[dV3E_Z] = sumLZ;
+ fb->t2[dV3E_X] = sumAX;
+ fb->t2[dV3E_Y] = sumAY;
+ fb->t2[dV3E_Z] = sumAZ;
+ }
+ }
+ cforce[CFE_LX] += sumLX;
+ cforce[CFE_LY] += sumLY;
+ cforce[CFE_LZ] += sumLZ;
+ cforce[CFE_AX] += sumAX;
+ cforce[CFE_AY] += sumAY;
+ cforce[CFE_AZ] += sumAZ;
+}
+
+
+//****************************************************************************
+
+/*extern */
+void dxStepIsland(const dxStepperProcessingCallContext *callContext)
+{
+ IFTIMING(dTimerStart("preprocessing"));
+
+ dxWorldProcessMemArena *memarena = callContext->m_stepperArena;
+ dxWorld *world = callContext->m_world;
+ unsigned int nb = callContext->m_islandBodiesCount;
+ unsigned int _nj = callContext->m_islandJointsCount;
+
+ dReal *invI = memarena->AllocateOveralignedArray<dReal>(dM3E__MAX * (sizeint)nb, INVI_ALIGNMENT);
+ // Reserve twice as much memory and start from the middle so that regardless of
+ // what direction the array grows to there would be sufficient room available.
+ const sizeint ji_reserve_count = 2 * (sizeint)_nj;
+ dJointWithInfo1 *const jointinfos = memarena->AllocateArray<dJointWithInfo1>(ji_reserve_count);
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ dIASSERT(allowedThreads != 0);
+
+ void *stagesMemArenaState = memarena->SaveState();
+
+ dxStepperStage1CallContext *stage1CallContext = (dxStepperStage1CallContext *)memarena->AllocateBlock(sizeof(dxStepperStage1CallContext));
+ stage1CallContext->Initialize(callContext, stagesMemArenaState, invI, jointinfos);
+
+ dxStepperStage0BodiesCallContext *stage0BodiesCallContext = (dxStepperStage0BodiesCallContext *)memarena->AllocateBlock(sizeof(dxStepperStage0BodiesCallContext));
+ stage0BodiesCallContext->Initialize(callContext, invI);
+
+ dxStepperStage0JointsCallContext *stage0JointsCallContext = (dxStepperStage0JointsCallContext *)memarena->AllocateBlock(sizeof(dxStepperStage0JointsCallContext));
+ stage0JointsCallContext->Initialize(callContext, jointinfos, &stage1CallContext->m_stage0Outputs);
+
+ if (allowedThreads == 1)
+ {
+ dxStepIsland_Stage0_Bodies(stage0BodiesCallContext);
+ dxStepIsland_Stage0_Joints(stage0JointsCallContext);
+ dxStepIsland_Stage1(stage1CallContext);
+ }
+ else
+ {
+ unsigned bodyThreads = allowedThreads;
+ unsigned jointThreads = 1;
+
+ dCallReleaseeID stage1CallReleasee;
+ world->PostThreadedCallForUnawareReleasee(NULL, &stage1CallReleasee, bodyThreads + jointThreads, callContext->m_finalReleasee,
+ NULL, &dxStepIsland_Stage1_Callback, stage1CallContext, 0, "StepIsland Stage1");
+
+ world->PostThreadedCallsGroup(NULL, bodyThreads, stage1CallReleasee, &dxStepIsland_Stage0_Bodies_Callback, stage0BodiesCallContext, "StepIsland Stage0-Bodies");
+
+ dxStepIsland_Stage0_Joints(stage0JointsCallContext);
+ world->AlterThreadedCallDependenciesCount(stage1CallReleasee, -1);
+ dIASSERT(jointThreads == 1);
+ }
+}
+
+static
+int dxStepIsland_Stage0_Bodies_Callback(void *_callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxStepperStage0BodiesCallContext *callContext = (dxStepperStage0BodiesCallContext *)_callContext;
+ dxStepIsland_Stage0_Bodies(callContext);
+ return 1;
+}
+
+static
+void dxStepIsland_Stage0_Bodies(dxStepperStage0BodiesCallContext *callContext)
+{
+ dxBody * const *body = callContext->m_stepperCallContext->m_islandBodiesStart;
+ unsigned int nb = callContext->m_stepperCallContext->m_islandBodiesCount;
+
+ if (ThrsafeExchange(&callContext->m_tagsTaken, 1) == 0)
+ {
+ // number all bodies in the body list - set their tag values
+ for (unsigned int i=0; i<nb; i++) body[i]->tag = i;
+ }
+
+ if (ThrsafeExchange(&callContext->m_gravityTaken, 1) == 0)
+ {
+ dxWorld *world = callContext->m_stepperCallContext->m_world;
+
+ // add the gravity force to all bodies
+ // since gravity does normally have only one component it's more efficient
+ // to run three loops for each individual component
+ dxBody *const *const bodyend = body + nb;
+ dReal gravity_x = world->gravity[0];
+ if (gravity_x) {
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
+ dxBody *b = *bodycurr;
+ if ((b->flags & dxBodyNoGravity) == 0) {
+ b->facc[dV3E_X] += b->mass.mass * gravity_x;
+ }
+ }
+ }
+ dReal gravity_y = world->gravity[1];
+ if (gravity_y) {
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
+ dxBody *b = *bodycurr;
+ if ((b->flags & dxBodyNoGravity) == 0) {
+ b->facc[dV3E_Y] += b->mass.mass * gravity_y;
+ }
+ }
+ }
+ dReal gravity_z = world->gravity[2];
+ if (gravity_z) {
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; ++bodycurr) {
+ dxBody *b = *bodycurr;
+ if ((b->flags & dxBodyNoGravity) == 0) {
+ b->facc[dV3E_Z] += b->mass.mass * gravity_z;
+ }
+ }
+ }
+ }
+
+ // for all bodies, compute the inertia tensor and its inverse in the global
+ // frame, and compute the rotational force and add it to the torque
+ // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
+ {
+ dReal *invIrow = callContext->m_invI;
+ unsigned int bodyIndex = ThrsafeIncrementIntUpToLimit(&callContext->m_inertiaBodyIndex, nb);
+
+ for (unsigned int i = 0; i != nb; invIrow += dM3E__MAX, ++i) {
+ if (i == bodyIndex) {
+ dMatrix3 tmp;
+ dxBody *b = body[i];
+
+ // compute inverse inertia tensor in global frame
+ dMultiply2_333 (tmp, b->invI, b->posr.R);
+ dMultiply0_333 (invIrow, b->posr.R, tmp);
+
+ // Don't apply gyroscopic torques to bodies
+ // if not flagged or the body is kinematic
+ if ((b->flags & dxBodyGyroscopic) && (b->invMass > 0)) {
+ dMatrix3 I;
+ // compute inertia tensor in global frame
+ dMultiply2_333 (tmp,b->mass.I,b->posr.R);
+ dMultiply0_333 (I,b->posr.R,tmp);
+ // compute rotational force
+#if 0
+ // Explicit computation
+ dMultiply0_331 (tmp,I,b->avel);
+ dSubtractVectorCross3(b->tacc,b->avel,tmp);
+#else
+ // Do the implicit computation based on
+ //"Stabilizing Gyroscopic Forces in Rigid Multibody Simulations"
+ // (Lacoursière 2006)
+ dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
+ dVector3 L; // Compute angular momentum
+ dMultiply0_331(L, I, b->avel);
+
+ // Compute a new effective 'inertia tensor'
+ // for the implicit step: the cross-product
+ // matrix of the angular momentum plus the
+ // old tensor scaled by the timestep.
+ // Itild may not be symmetric pos-definite,
+ // but we can still use it to compute implicit
+ // gyroscopic torques.
+ dMatrix3 Itild = { 0 };
+ dSetCrossMatrixMinus(Itild, L, dV3E__MAX);
+ for (int ii = dM3E__MIN; ii != dM3E__MAX; ++ii) {
+ Itild[ii] = Itild[ii] * h + I[ii];
+ }
+
+ // Scale momentum by inverse time to get
+ // a sort of "torque"
+ dScaleVector3(L, dRecip(h));
+ // Invert the pseudo-tensor
+ dMatrix3 itInv;
+ // This is a closed-form inversion.
+ // It's probably not numerically stable
+ // when dealing with small masses with
+ // a large asymmetry.
+ // An LU decomposition might be better.
+ if (dInvertMatrix3(itInv, Itild) != 0) {
+ // "Divide" the original tensor
+ // by the pseudo-tensor (on the right)
+ dMultiply0_333(Itild, I, itInv);
+ // Subtract an identity matrix
+ Itild[dM3E_XX] -= 1; Itild[dM3E_YY] -= 1; Itild[dM3E_ZZ] -= 1;
+
+ // This new inertia matrix rotates the
+ // momentum to get a new set of torques
+ // that will work correctly when applied
+ // to the old inertia matrix as explicit
+ // torques with a semi-implicit update
+ // step.
+ dVector3 tau0;
+ dMultiply0_331(tau0,Itild,L);
+
+ // Add the gyro torques to the torque
+ // accumulator
+ for (int ii = dSA__MIN; ii != dSA__MAX; ++ii) {
+ b->tacc[dV3E__AXES_MIN + ii] += tau0[dV3E__AXES_MIN + ii];
+ }
+ }
+#endif
+ }
+
+ bodyIndex = ThrsafeIncrementIntUpToLimit(&callContext->m_inertiaBodyIndex, nb);
+ }
+ }
+ }
+}
+
+// static
+// int dxStepIsland_Stage0_Joints_Callback(void *_callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+// {
+// (void)callInstanceIndex; // unused
+// (void)callThisReleasee; // unused
+// dxStepperStage0JointsCallContext *callContext = (dxStepperStage0JointsCallContext *)_callContext;
+// dxStepIsland_Stage0_Joints(callContext);
+// return 1;
+// }
+
+static
+void dxStepIsland_Stage0_Joints(dxStepperStage0JointsCallContext *callContext)
+{
+ dxJoint * const *_joint = callContext->m_stepperCallContext->m_islandJointsStart;
+ dJointWithInfo1 *jointinfos = callContext->m_jointinfos;
+ unsigned int _nj = callContext->m_stepperCallContext->m_islandJointsCount;
+
+ // get m = total constraint dimension, nub = number of unbounded variables.
+ // create constraint offset array and number-of-rows array for all joints.
+ // the constraints are re-ordered as follows: the purely unbounded
+ // constraints, the mixed unbounded + LCP constraints, and last the purely
+ // LCP constraints. this assists the LCP solver to put all unbounded
+ // variables at the start for a quick factorization.
+ //
+ // joints with m=0 are inactive and are removed from the joints array
+ // entirely, so that the code that follows does not consider them.
+ // also number all active joints in the joint list (set their tag values).
+ // inactive joints receive a tag value of -1.
+
+ sizeint ji_start, ji_end;
+ {
+ unsigned int mcurr = 0;
+ sizeint unb_start, mix_start, mix_end, lcp_end;
+ unb_start = mix_start = mix_end = lcp_end = _nj;
+
+ dJointWithInfo1 *jicurr = jointinfos + lcp_end;
+ dxJoint *const *const _jend = _joint + _nj;
+ dxJoint *const *_jcurr = _joint;
+ while (true) {
+ // -------------------------------------------------------------------------
+ // Switch to growing array forward
+ {
+ bool fwd_end_reached = false;
+ dJointWithInfo1 *jimixend = jointinfos + mix_end;
+ while (true) { // jicurr=dest, _jcurr=src
+ if (_jcurr == _jend) {
+ lcp_end = jicurr - jointinfos;
+ fwd_end_reached = true;
+ break;
+ }
+ dxJoint *j = *_jcurr++;
+ j->getInfo1 (&jicurr->info);
+ dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 6 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
+ if (jicurr->info.m != 0) {
+ mcurr += jicurr->info.m;
+ if (jicurr->info.nub == 0) { // A lcp info - a correct guess!!!
+ jicurr->joint = j;
+ ++jicurr;
+ } else if (jicurr->info.nub < jicurr->info.m) { // A mixed case
+ if (unb_start == mix_start) { // no unbounded infos yet - just move to opposite side of mixed-s
+ unb_start = mix_start = mix_start - 1;
+ dJointWithInfo1 *jimixstart = jointinfos + mix_start;
+ jimixstart->info = jicurr->info;
+ jimixstart->joint = j;
+ } else if (jimixend != jicurr) { // have to swap to the tail of mixed-s
+ dxJoint::Info1 tmp_info = jicurr->info;
+ *jicurr = *jimixend;
+ jimixend->info = tmp_info;
+ jimixend->joint = j;
+ ++jimixend; ++jicurr;
+ } else { // no need to swap as there are no LCP info-s yet
+ jicurr->joint = j;
+ jimixend = jicurr = jicurr + 1;
+ }
+ } else { // A purely unbounded case -- break out and proceed growing in opposite direction
+ unb_start = unb_start - 1;
+ dJointWithInfo1 *jiunbstart = jointinfos + unb_start;
+ jiunbstart->info = jicurr->info;
+ jiunbstart->joint = j;
+ lcp_end = jicurr - jointinfos;
+ mix_end = jimixend - jointinfos;
+ jicurr = jiunbstart - 1;
+ break;
+ }
+ } else {
+ j->tag = -1;
+ }
+ }
+ if (fwd_end_reached) {
+ break;
+ }
+ }
+ // -------------------------------------------------------------------------
+ // Switch to growing array backward
+ {
+ bool bkw_end_reached = false;
+ dJointWithInfo1 *jimixstart = jointinfos + mix_start - 1;
+ while (true) { // jicurr=dest, _jcurr=src
+ if (_jcurr == _jend) {
+ unb_start = (jicurr + 1) - jointinfos;
+ mix_start = (jimixstart + 1) - jointinfos;
+ bkw_end_reached = true;
+ break;
+ }
+ dxJoint *j = *_jcurr++;
+ j->getInfo1 (&jicurr->info);
+ dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 6 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
+ if (jicurr->info.m != 0) {
+ mcurr += jicurr->info.m;
+ if (jicurr->info.nub == jicurr->info.m) { // An unbounded info - a correct guess!!!
+ jicurr->joint = j;
+ --jicurr;
+ } else if (jicurr->info.nub != 0) { // A mixed case
+ if (mix_end == lcp_end) { // no lcp infos yet - just move to opposite side of mixed-s
+ dJointWithInfo1 *jimixend = jointinfos + mix_end;
+ lcp_end = mix_end = mix_end + 1;
+ jimixend->info = jicurr->info;
+ jimixend->joint = j;
+ } else if (jimixstart != jicurr) { // have to swap to the head of mixed-s
+ dxJoint::Info1 tmp_info = jicurr->info;
+ *jicurr = *jimixstart;
+ jimixstart->info = tmp_info;
+ jimixstart->joint = j;
+ --jimixstart; --jicurr;
+ } else { // no need to swap as there are no unbounded info-s yet
+ jicurr->joint = j;
+ jimixstart = jicurr = jicurr - 1;
+ }
+ } else { // A purely lcp case -- break out and proceed growing in opposite direction
+ dJointWithInfo1 *jilcpend = jointinfos + lcp_end;
+ lcp_end = lcp_end + 1;
+ jilcpend->info = jicurr->info;
+ jilcpend->joint = j;
+ unb_start = (jicurr + 1) - jointinfos;
+ mix_start = (jimixstart + 1) - jointinfos;
+ jicurr = jilcpend + 1;
+ break;
+ }
+ } else {
+ j->tag = -1;
+ }
+ }
+ if (bkw_end_reached) {
+ break;
+ }
+ }
+ }
+
+ callContext->m_stage0Outputs->m = mcurr;
+ callContext->m_stage0Outputs->nub = (unsigned)(mix_start - unb_start);
+ dIASSERT((sizeint)(mix_start - unb_start) <= (sizeint)UINT_MAX);
+ ji_start = unb_start;
+ ji_end = lcp_end;
+ }
+
+ {
+ const dJointWithInfo1 *jicurr = jointinfos + ji_start;
+ const dJointWithInfo1 *const jiend = jointinfos + ji_end;
+ for (unsigned int i = 0; jicurr != jiend; i++, ++jicurr) {
+ jicurr->joint->tag = i;
+ }
+ }
+
+ callContext->m_stage0Outputs->ji_start = ji_start;
+ callContext->m_stage0Outputs->ji_end = ji_end;
+}
+
+static
+int dxStepIsland_Stage1_Callback(void *_stage1CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxStepperStage1CallContext *stage1CallContext = (dxStepperStage1CallContext *)_stage1CallContext;
+ dxStepIsland_Stage1(stage1CallContext);
+ return 1;
+}
+
+static
+void dxStepIsland_Stage1(dxStepperStage1CallContext *stage1CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage1CallContext->m_stepperCallContext;
+ dJointWithInfo1 *_jointinfos = stage1CallContext->m_jointinfos;
+ dReal *invI = stage1CallContext->m_invI;
+ sizeint ji_start = stage1CallContext->m_stage0Outputs.ji_start;
+ sizeint ji_end = stage1CallContext->m_stage0Outputs.ji_end;
+ unsigned int m = stage1CallContext->m_stage0Outputs.m;
+ unsigned int nub = stage1CallContext->m_stage0Outputs.nub;
+
+ dxWorldProcessMemArena *memarena = callContext->m_stepperArena;
+ {
+ memarena->RestoreState(stage1CallContext->m_stageMemArenaState);
+ stage1CallContext = NULL; // WARNING! _stage1CallContext is not valid after this point!
+ dIVERIFY(stage1CallContext == NULL); // To suppress compiler warnings about unused variable assignment
+
+ unsigned int _nj = callContext->m_islandJointsCount;
+ const sizeint ji_reserve_count = 2 * (sizeint)_nj;
+ memarena->ShrinkArray<dJointWithInfo1>(_jointinfos, ji_reserve_count, ji_end);
+ }
+
+ dJointWithInfo1 *jointinfos = _jointinfos + ji_start;
+ unsigned int nj = (unsigned int)(ji_end - ji_start);
+ dIASSERT((sizeint)(ji_end - ji_start) <= (sizeint)UINT_MAX);
+
+ unsigned int *mindex = NULL;
+ dReal *J = NULL, *A = NULL, *pairsRhsCfm = NULL, *pairsLoHi = NULL;
+ int *findex = NULL;
+ atomicord32 *bodyStartJoints = NULL, *bodyJointLinks = NULL;
+
+ // if there are constraints, compute constrForce
+ if (m > 0) {
+ mindex = memarena->AllocateArray<unsigned int>((sizeint)(nj + 1));
+ {
+ unsigned int *mcurr = mindex;
+ unsigned int moffs = 0;
+ mcurr[0] = moffs;
+ mcurr += 1;
+
+ const dJointWithInfo1 *const jiend = jointinfos + nj;
+ for (const dJointWithInfo1 *jicurr = jointinfos; jicurr != jiend; ++jicurr) {
+ //dxJoint *joint = jicurr->joint;
+ moffs += jicurr->info.m;
+ mcurr[0] = moffs;
+ mcurr += 1;
+ }
+ }
+
+ // create a constraint equation right hand side vector `c', a constraint
+ // force mixing vector `cfm', and LCP low and high bound vectors, and an
+ // 'findex' vector.
+ findex = memarena->AllocateArray<int>(m);
+ J = memarena->AllocateArray<dReal>((sizeint)m * (2 * JME__MAX));
+ A = memarena->AllocateOveralignedArray<dReal>((sizeint)m * dPAD(m), AMATRIX_ALIGNMENT);
+ pairsRhsCfm = memarena->AllocateArray<dReal>((sizeint)m * RCE__RHS_CFM_MAX);
+ pairsLoHi = memarena->AllocateArray<dReal>((sizeint)m * LHE__LO_HI_MAX);
+ const unsigned int nb = callContext->m_islandBodiesCount;
+ bodyStartJoints = memarena->AllocateArray<atomicord32>(nb);
+ bodyJointLinks = memarena->AllocateArray<atomicord32>((sizeint)nj * dJCB__MAX);
+ dICHECK(nj < ~((atomicord32)0) / dJCB__MAX); // If larger joint counts are to be used, pointers (or sizeint) need to be stored rather than atomicord32 indices
+ }
+
+ dxStepperLocalContext *localContext = (dxStepperLocalContext *)memarena->AllocateBlock(sizeof(dxStepperLocalContext));
+ localContext->Initialize(invI, jointinfos, nj, m, nub, mindex, findex, J, A, pairsRhsCfm, pairsLoHi, bodyStartJoints, bodyJointLinks);
+
+ void *stage1MemarenaState = memarena->SaveState();
+ dxStepperStage3CallContext *stage3CallContext = (dxStepperStage3CallContext*)memarena->AllocateBlock(sizeof(dxStepperStage3CallContext));
+ stage3CallContext->Initialize(callContext, localContext, stage1MemarenaState);
+
+ if (m > 0) {
+ dReal *JinvM = memarena->AllocateOveralignedArray<dReal>((sizeint)m * (2 * JIM__MAX), JINVM_ALIGNMENT);
+ const unsigned int nb = callContext->m_islandBodiesCount;
+ dReal *rhs_tmp = memarena->AllocateArray<dReal>((sizeint)nb * dDA__MAX);
+
+ dxStepperStage2CallContext *stage2CallContext = (dxStepperStage2CallContext *)memarena->AllocateBlock(sizeof(dxStepperStage2CallContext));
+ stage2CallContext->Initialize(callContext, localContext, JinvM, rhs_tmp);
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ dIASSERT(allowedThreads != 0);
+
+ if (allowedThreads == 1) {
+ IFTIMING(dTimerNow("create J"));
+ dxStepIsland_Stage2a(stage2CallContext);
+ IFTIMING(dTimerNow("compute Adiag, JinvM and rhs_tmp"));
+ dxStepIsland_Stage2b(stage2CallContext);
+ IFTIMING(dTimerNow("compute A and rhs"));
+ dxStepIsland_Stage2c(stage2CallContext);
+ dxStepIsland_Stage3(stage3CallContext);
+ }
+ else {
+ dxWorld *world = callContext->m_world;
+ dCallReleaseeID stage3CallReleasee;
+ world->PostThreadedCallForUnawareReleasee(NULL, &stage3CallReleasee, 1, callContext->m_finalReleasee,
+ NULL, &dxStepIsland_Stage3_Callback, stage3CallContext, 0, "StepIsland Stage3");
+
+ dCallReleaseeID stage2bSyncReleasee;
+ world->PostThreadedCall(NULL, &stage2bSyncReleasee, 1, stage3CallReleasee,
+ NULL, &dxStepIsland_Stage2bSync_Callback, stage2CallContext, 0, "StepIsland Stage2b Sync");
+
+ dCallReleaseeID stage2aSyncReleasee;
+ world->PostThreadedCall(NULL, &stage2aSyncReleasee, allowedThreads, stage2bSyncReleasee,
+ NULL, &dxStepIsland_Stage2aSync_Callback, stage2CallContext, 0, "StepIsland Stage2a Sync");
+
+ dIASSERT(allowedThreads > 1); /*if (allowedThreads > 1) */{
+ world->PostThreadedCallsGroup(NULL, allowedThreads - 1, stage2aSyncReleasee, &dxStepIsland_Stage2a_Callback, stage2CallContext, "StepIsland Stage2a");
+ }
+ dxStepIsland_Stage2a(stage2CallContext);
+ world->AlterThreadedCallDependenciesCount(stage2aSyncReleasee, -1);
+ }
+ }
+ else {
+ dxStepIsland_Stage3(stage3CallContext);
+ }
+}
+
+
+static
+int dxStepIsland_Stage2a_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxStepperStage2CallContext *stage2CallContext = (dxStepperStage2CallContext *)_stage2CallContext;
+ dxStepIsland_Stage2a(stage2CallContext);
+ return 1;
+}
+
+static
+void dxStepIsland_Stage2a(dxStepperStage2CallContext *stage2CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const dxStepperLocalContext *localContext = stage2CallContext->m_localContext;
+ dJointWithInfo1 *jointinfos = localContext->m_jointinfos;
+ unsigned int nj = localContext->m_nj;
+ const unsigned int *mindex = localContext->m_mindex;
+
+ const dReal stepsizeRecip = dRecip(callContext->m_stepSize);
+ dxWorld *world = callContext->m_world;
+
+ {
+ int *findex = localContext->m_findex;
+ dReal *J = localContext->m_J;
+ dReal *pairsRhsCfm = localContext->m_pairsRhsCfm;
+ dReal *pairsLoHi = localContext->m_pairsLoHi;
+
+ // get jacobian data from constraints. a (2*m)x8 matrix will be created
+ // to store the two jacobian blocks from each constraint. it has this
+ // format:
+ //
+ // l l l 0 a a a 0 \ .
+ // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
+ // l l l 0 a a a 0 /
+ // l l l 0 a a a 0 \ .
+ // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
+ // l l l 0 a a a 0 /
+ // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
+ // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
+ // etc...
+ //
+ // (lll) = linear jacobian data
+ // (aaa) = angular jacobian data
+ //
+
+ const dReal worldERP = world->global_erp;
+ const dReal worldCFM = world->global_cfm;
+
+ unsigned ji;
+ while ((ji = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_ji_J, nj)) != nj) {
+ const unsigned ofsi = mindex[ji];
+ const unsigned int infom = mindex[ji + 1] - ofsi;
+
+ dReal *const JRow = J + (sizeint)ofsi * (2 * JME__MAX);
+ dReal *rowRhsCfm = pairsRhsCfm + (sizeint)ofsi * RCE__RHS_CFM_MAX;
+ dReal *rowLoHi = pairsLoHi + (sizeint)ofsi * LHE__LO_HI_MAX;
+ {
+ dSetZero (JRow, infom * (2 * JME__MAX));
+
+ dReal *const endRhsCfm = rowRhsCfm + infom * RCE__RHS_CFM_MAX;
+ for (dReal *currRhsCfm = rowRhsCfm; currRhsCfm != endRhsCfm; currRhsCfm += RCE__RHS_CFM_MAX) {
+ currRhsCfm[RCE_RHS] = REAL(0.0);
+ currRhsCfm[RCE_CFM] = worldCFM;
+ }
+
+ dReal *const endLoHi = rowLoHi + infom * LHE__LO_HI_MAX;
+ for (dReal *currLoHi = rowLoHi; currLoHi != endLoHi; currLoHi += LHE__LO_HI_MAX) {
+ currLoHi[LHE_LO] = -dInfinity;
+ currLoHi[LHE_HI] = dInfinity;
+ }
+ }
+ int *findexRow = findex + ofsi;
+ dSetValue(findexRow, infom, -1);
+
+ dxJoint *joint = jointinfos[ji].joint;
+ joint->getInfo2(stepsizeRecip, worldERP, JME__MAX, JRow + JME__J_MIN, JRow + infom * JME__MAX + JME__J_MIN, RCE__RHS_CFM_MAX, rowRhsCfm, rowLoHi, findexRow);
+ dSASSERT((int)LHE__LO_HI_MAX == RCE__RHS_CFM_MAX); // To make sure same step fits for both pairs in the call to dxJoint::getInfo2() above
+
+ // findex iteration is compact and is not going to pollute caches - do it first
+ {
+ // adjust returned findex values for global index numbering
+ int *const findicesEnd = findexRow + infom;
+ for (int *findexCurr = findexRow; findexCurr != findicesEnd; ++findexCurr) {
+ int fival = *findexCurr;
+ if (fival != -1) {
+ *findexCurr = fival + ofsi;
+ }
+ }
+ }
+ {
+ dReal *const endRhsCfm = rowRhsCfm + infom * RCE__RHS_CFM_MAX;
+ for (dReal *currRhsCfm = rowRhsCfm; currRhsCfm != endRhsCfm; currRhsCfm += RCE__RHS_CFM_MAX) {
+ currRhsCfm[RCE_RHS] *= stepsizeRecip;
+ currRhsCfm[RCE_CFM] *= stepsizeRecip;
+ }
+ }
+ }
+ }
+}
+
+static
+int dxStepIsland_Stage2aSync_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ dxStepperStage2CallContext *stage2CallContext = (dxStepperStage2CallContext *)_stage2CallContext;
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+
+ dIASSERT(allowedThreads > 1); /*if (allowedThreads > 1) */{ // The allowed thread count is greater than one as otherwise current function would not be scheduled for execution from the previous stage
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, allowedThreads - 1, callThisReleasee, &dxStepIsland_Stage2b_Callback, stage2CallContext, "StepIsland Stage2b");
+ }
+ dxStepIsland_Stage2b(stage2CallContext);
+
+ return 1;
+}
+
+static
+int dxStepIsland_Stage2b_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxStepperStage2CallContext *stage2CallContext = (dxStepperStage2CallContext *)_stage2CallContext;
+ dxStepIsland_Stage2b(stage2CallContext);
+ return 1;
+}
+
+static
+void dxStepIsland_Stage2b(dxStepperStage2CallContext *stage2CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const dxStepperLocalContext *localContext = stage2CallContext->m_localContext;
+ dJointWithInfo1 *jointinfos = localContext->m_jointinfos;
+ unsigned int nj = localContext->m_nj;
+ const unsigned int *mindex = localContext->m_mindex;
+
+ {
+ // Warning!!!
+ // This code depends on cfm elements and therefore must be in different sub-stage
+ // from Jacobian construction in Stage2a to ensure proper synchronization
+ // and avoid accessing numbers being modified.
+ // Warning!!!
+ dReal *A = localContext->m_A;
+ const dReal *pairsRhsCfm = localContext->m_pairsRhsCfm;
+ const unsigned m = localContext->m_m;
+
+ const unsigned int mskip = dPAD(m);
+
+ unsigned ji;
+ while ((ji = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_ji_Ainit, nj)) != nj) {
+ const unsigned ofsi = mindex[ji];
+ const unsigned int infom = mindex[ji + 1] - ofsi;
+
+ dReal *Arow = A + (sizeint)mskip * ofsi;
+ dSetZero(Arow, (sizeint)mskip * infom);
+ dReal *Adiag = Arow + ofsi;
+ const dReal *rowRfsCrm = pairsRhsCfm + (sizeint)ofsi * RCE__RHS_CFM_MAX;
+ for (unsigned int i = 0; i != infom; Adiag += mskip, ++i) {
+ Adiag[i] = (rowRfsCrm + i * RCE__RHS_CFM_MAX)[RCE_CFM];
+ }
+ }
+ }
+
+ {
+ // Warning!!!
+ // This code depends on J elements and therefore must be in different sub-stage
+ // from Jacobian construction in Stage2a to ensure proper synchronization
+ // and avoid accessing numbers being modified.
+ // Warning!!!
+ const dReal *invI = localContext->m_invI;
+ const dReal *J = localContext->m_J;
+ dReal *JinvM = stage2CallContext->m_JinvM;
+
+ // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
+ // format as J so we just go through the constraints in J multiplying by
+ // the appropriate scalars and matrices.
+ unsigned ji;
+ while ((ji = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_ji_JinvM, nj)) != nj) {
+ const unsigned ofsi = mindex[ji];
+ const unsigned int infom = mindex[ji + 1] - ofsi;
+
+ dReal *Jdst = JinvM + (sizeint)ofsi * (2 * JIM__MAX);
+ dSetZero(Jdst, infom * (2 * JIM__MAX));
+
+ const dReal *Jsrc = J + (sizeint)ofsi * (2 * JME__MAX);
+ dxJoint *joint = jointinfos[ji].joint;
+
+ dxBody *jb0 = joint->node[0].body;
+ if (true || jb0 != NULL) { // -- always true
+ dReal body_invMass0 = jb0->invMass;
+ const dReal *body_invI0 = invI + (sizeint)(unsigned int)jb0->tag * dM3E__MAX;
+ for (unsigned int j = infom; j != 0; --j) {
+ for (unsigned int k = dSA__MIN; k != dSA__MAX; ++k) Jdst[JIM__L_AXES_MIN + k] = Jsrc[JME__JL_MIN + k] * body_invMass0;
+ dMultiply0_133(Jdst + JIM__A_AXES_MIN, Jsrc + JME__JA_MIN, body_invI0);
+ Jsrc += JME__MAX;
+ Jdst += JIM__MAX;
+ }
+ }
+
+ dxBody *jb1 = joint->node[1].body;
+ if (jb1 != NULL) {
+ dReal body_invMass1 = jb1->invMass;
+ const dReal *body_invI1 = invI + (sizeint)(unsigned int)jb1->tag * dM3E__MAX;
+ for (unsigned int j = infom; j != 0; --j) {
+ for (unsigned int k = dSA__MIN; k != dSA__MAX; ++k) Jdst[JIM__L_AXES_MIN + k] = Jsrc[JME__JL_MIN + k] * body_invMass1;
+ dMultiply0_133 (Jdst + JIM__A_AXES_MIN, Jsrc + JME__JA_MIN, body_invI1);
+ Jsrc += JME__MAX;
+ Jdst += JIM__MAX;
+ }
+ }
+ }
+ }
+
+ {
+ // Warning!!!
+ // This code reads facc/tacc fields of body objects which (the fields)
+ // may be modified by dxJoint::getInfo2(). Therefore the code must be
+ // in different sub-stage from Jacobian construction in Stage2a
+ // to ensure proper synchronization and avoid accessing numbers being modified.
+ // Warning!!!
+ dxBody * const *const body = callContext->m_islandBodiesStart;
+ const unsigned int nb = callContext->m_islandBodiesCount;
+ const dReal *invI = localContext->m_invI;
+ atomicord32 *bodyStartJoints = localContext->m_bodyStartJoints;
+ dReal *rhs_tmp = stage2CallContext->m_rhs_tmp;
+
+ // compute the right hand side `rhs'
+ const dReal stepsizeRecip = dRecip(callContext->m_stepSize);
+
+ // put v/h + invM*fe into rhs_tmp
+ unsigned bi;
+ while ((bi = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_bi_rhs_tmp, nb)) != nb) {
+ dReal *tmp1curr = rhs_tmp + (sizeint)bi * dDA__MAX;
+ const dReal *invIrow = invI + (sizeint)bi * dM3E__MAX;
+ dxBody *b = body[bi];
+ // dSetZero(tmp1curr, 8); -- not needed
+ for (unsigned int j = dSA__MIN; j != dSA__MAX; ++j) tmp1curr[dDA__L_MIN + j] = b->facc[dV3E__AXES_MIN + j] * b->invMass + b->lvel[dV3E__AXES_MIN + j] * stepsizeRecip;
+ dMultiply0_331 (tmp1curr + dDA__A_MIN, invIrow, b->tacc);
+ for (unsigned int k = dSA__MIN; k != dSA__MAX; ++k) tmp1curr[dDA__A_MIN + k] += b->avel[dV3E__AXES_MIN + k] * stepsizeRecip;
+ // Initialize body start joint indices -- this will be needed later for building body related joint list in dxStepIsland_Stage2c
+ bodyStartJoints[bi] = 0;
+ }
+ }
+}
+
+static
+int dxStepIsland_Stage2bSync_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ dxStepperStage2CallContext *stage2CallContext = (dxStepperStage2CallContext *)_stage2CallContext;
+ const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+
+ dIASSERT(allowedThreads > 1); /*if (allowedThreads > 1) */{ // The allowed thread count is greater than one as otherwise current function would not be scheduled for execution from the previous stage
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(callThisReleasee, allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, allowedThreads - 1, callThisReleasee, &dxStepIsland_Stage2c_Callback, stage2CallContext, "StepIsland Stage2c");
+ }
+ dxStepIsland_Stage2c(stage2CallContext);
+
+ return 1;
+}
+
+
+static
+int dxStepIsland_Stage2c_Callback(void *_stage2CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxStepperStage2CallContext *stage2CallContext = (dxStepperStage2CallContext *)_stage2CallContext;
+ dxStepIsland_Stage2c(stage2CallContext);
+ return 1;
+}
+
+static
+void dxStepIsland_Stage2c(dxStepperStage2CallContext *stage2CallContext)
+{
+ //const dxStepperProcessingCallContext *callContext = stage2CallContext->m_stepperCallContext;
+ const dxStepperLocalContext *localContext = stage2CallContext->m_localContext;
+ dJointWithInfo1 *jointinfos = localContext->m_jointinfos;
+ unsigned int nj = localContext->m_nj;
+ const unsigned int *mindex = localContext->m_mindex;
+
+ {
+ // Warning!!!
+ // This code depends on A elements and JinvM elements and therefore
+ // must be in different sub-stage from A initialization and JinvM calculation in Stage2b
+ // to ensure proper synchronization and avoid accessing numbers being modified.
+ // Warning!!!
+ dReal *A = localContext->m_A;
+ const dReal *JinvM = stage2CallContext->m_JinvM;
+ const dReal *J = localContext->m_J;
+ const unsigned m = localContext->m_m;
+
+ // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
+ // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
+ // if joints i and j have at least one body in common.
+ const unsigned int mskip = dPAD(m);
+
+ unsigned ji;
+ while ((ji = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_ji_Aaddjb, nj)) != nj) {
+ const unsigned ofsi = mindex[ji];
+ const unsigned int infom = mindex[ji + 1] - ofsi;
+
+ dReal *Arow = A + (sizeint)mskip * ofsi;
+ const dReal *JinvMRow = JinvM + (sizeint)ofsi * (2 * JIM__MAX);
+ dxJoint *joint = jointinfos[ji].joint;
+
+ dxBody *jb0 = joint->node[0].body;
+ if (true || jb0 != NULL) { // -- always true
+ // compute diagonal block of A
+ const dReal *JRow = J + (sizeint)ofsi * (2 * JME__MAX);
+ MultiplyAddJinvMxJToA (Arow + ofsi, JinvMRow, JRow, infom, infom, mskip);
+
+ for (dxJointNode *n0 = (ji != 0 ? jb0->firstjoint : NULL); n0; n0 = n0->next) {
+ // if joint was tagged as -1 then it is an inactive (m=0 or disabled)
+ // joint that should not be considered
+ int j0 = n0->joint->tag;
+ if (j0 != -1 && (unsigned)j0 < ji) {
+ const unsigned int jiother_ofsi = mindex[j0];
+ const unsigned int jiother_infom = mindex[j0 + 1] - jiother_ofsi;
+ const dJointWithInfo1 *jiother = jointinfos + j0;
+ unsigned int smart_infom = (jiother->joint->node[1].body == jb0) ? jiother_infom : 0;
+ // set block of A
+ const dReal *JOther = J + ((sizeint)jiother_ofsi * 2 + smart_infom) * JME__MAX;
+ MultiplyAddJinvMxJToA (Arow + jiother_ofsi, JinvMRow, JOther, infom, jiother_infom, mskip);
+ }
+ }
+ }
+
+ dxBody *jb1 = joint->node[1].body;
+ dIASSERT(jb1 != jb0);
+ if (jb1 != NULL) {
+ const dReal *JinvMOther = JinvMRow + infom * JIM__MAX;
+ // compute diagonal block of A
+ const dReal *JRow = J + ((sizeint)ofsi * 2 + infom) * JME__MAX;
+ MultiplyAddJinvMxJToA (Arow + ofsi, JinvMOther, JRow, infom, infom, mskip);
+
+ for (dxJointNode *n1 = (ji != 0 ? jb1->firstjoint : NULL); n1; n1 = n1->next) {
+ // if joint was tagged as -1 then it is an inactive (m=0 or disabled)
+ // joint that should not be considered
+ int j1 = n1->joint->tag;
+ if (j1 != -1 && (unsigned)j1 < ji) {
+ const unsigned int jiother_ofsi = mindex[j1];
+ const unsigned int jiother_infom = mindex[j1 + 1] - jiother_ofsi;
+ const dJointWithInfo1 *jiother = jointinfos + j1;
+ unsigned int smart_infom = (jiother->joint->node[1].body == jb1) ? jiother_infom : 0;
+ // set block of A
+ const dReal *JOther = J + ((sizeint)jiother_ofsi * 2 + smart_infom) * JME__MAX;
+ MultiplyAddJinvMxJToA (Arow + jiother_ofsi, JinvMOther, JOther, infom, jiother_infom, mskip);
+ }
+ }
+ }
+ }
+ }
+
+ {
+ // Warning!!!
+ // This code depends on rhs_tmp elements and therefore must be in
+ // different sub-stage from rhs_tmp calculation in Stage2b to ensure
+ // proper synchronization and avoid accessing numbers being modified.
+ // Warning!!!
+ const dReal *J = localContext->m_J;
+ const dReal *rhs_tmp = stage2CallContext->m_rhs_tmp;
+ dReal *pairsRhsCfm = localContext->m_pairsRhsCfm;
+ atomicord32 *bodyStartJoints = localContext->m_bodyStartJoints;
+ atomicord32 *bodyJointLinks = localContext->m_bodyJointLinks;
+
+ // compute the right hand side `rhs'
+ // put J*rhs_tmp into rhs
+ unsigned ji;
+ while ((ji = ThrsafeIncrementIntUpToLimit(&stage2CallContext->m_ji_rhs, nj)) != nj) {
+ const unsigned ofsi = mindex[ji];
+ const unsigned int infom = mindex[ji + 1] - ofsi;
+
+ dReal *currRhsCfm = pairsRhsCfm + (sizeint)ofsi * RCE__RHS_CFM_MAX;
+ const dReal *JRow = J + (sizeint)ofsi * (2 * JME__MAX);
+
+ dxJoint *joint = jointinfos[ji].joint;
+
+ dxBody *jb0 = joint->node[0].body;
+ if (true || jb0 != NULL) { // -- always true
+ unsigned bodyIndex = (unsigned)jb0->tag;
+ MultiplySubJxRhsTmpFromRHS (currRhsCfm, JRow, rhs_tmp + (sizeint)bodyIndex * dDA__MAX, infom);
+
+ // Link joints connected to each body into a list to be used on results incorporation. The bodyStartJoints have been initialized in dxStepIsland_Stage2b.
+ const atomicord32 linkIndex = (atomicord32)((sizeint)ji * dJCB__MAX + dJCB_FIRST_BODY); // It is asserted at links buffer allocation that the indices can't overflow atomicord32
+ for (atomicord32 oldStartIndex = bodyStartJoints[bodyIndex]; ; oldStartIndex = bodyStartJoints[bodyIndex]) {
+ bodyJointLinks[linkIndex] = oldStartIndex;
+ if (ThrsafeCompareExchange(&bodyStartJoints[bodyIndex], oldStartIndex, linkIndex + 1)) { // The link index is stored incremented to allow 0 as end indicator
+ break;
+ }
+ }
+ }
+
+ dxBody *jb1 = joint->node[1].body;
+ if (jb1 != NULL) {
+ unsigned bodyIndex = (unsigned)jb1->tag;
+ MultiplySubJxRhsTmpFromRHS (currRhsCfm, JRow + infom * JME__MAX, rhs_tmp + (sizeint)bodyIndex * dDA__MAX, infom);
+
+ // Link joints connected to each body into a list to be used on results incorporation. The bodyStartJoints have been initialized in dxStepIsland_Stage2b
+ const atomicord32 linkIndex = (atomicord32)((sizeint)ji * dJCB__MAX + dJCB_SECOND_BODY); // It is asserted at links buffer allocation that the indices can't overflow atomicord32
+ for (atomicord32 oldStartIndex = bodyStartJoints[bodyIndex]; ; oldStartIndex = bodyStartJoints[bodyIndex]) {
+ bodyJointLinks[linkIndex] = oldStartIndex;
+ if (ThrsafeCompareExchange(&bodyStartJoints[bodyIndex], oldStartIndex, linkIndex + 1)) { // The link index is stored incremented to allow 0 as end indicator
+ break;
+ }
+ }
+ }
+ }
+ }
+}
+
+
+static
+int dxStepIsland_Stage3_Callback(void *_stage3CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxStepperStage3CallContext *stage3CallContext = (dxStepperStage3CallContext *)_stage3CallContext;
+ dxStepIsland_Stage3(stage3CallContext);
+ return 1;
+}
+
+static
+void dxStepIsland_Stage3(dxStepperStage3CallContext *stage3CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage3CallContext->m_stepperCallContext;
+ const dxStepperLocalContext *localContext = stage3CallContext->m_localContext;
+
+ dxWorldProcessMemArena *memarena = callContext->m_stepperArena;
+ memarena->RestoreState(stage3CallContext->m_stage1MemArenaState);
+ stage3CallContext = NULL; // WARNING! stage3CallContext is not valid after this point!
+ dIVERIFY(stage3CallContext == NULL); // To suppress unused variable assignment warnings
+
+ unsigned int m = localContext->m_m;
+ unsigned int nub = localContext->m_nub;
+ //const unsigned int *mindex = localContext->m_mindex;
+ int *findex = localContext->m_findex;
+ dReal *A = localContext->m_A;
+ dReal *pairsRhsLambda = localContext->m_pairsRhsCfm; // Reuse cfm buffer for lambdas as the former values are not needed any more
+ dReal *pairsLoHi = localContext->m_pairsLoHi;
+
+ if (m > 0) {
+ BEGIN_STATE_SAVE(memarena, lcpstate) {
+ IFTIMING(dTimerNow ("solve LCP problem"));
+
+ // solve the LCP problem and get lambda.
+ // this will destroy A but that's OK
+ dxSolveLCP (memarena, m, A, pairsRhsLambda, NULL, nub, pairsLoHi, findex);
+ dSASSERT((int)RLE__RHS_LAMBDA_MAX == PBX__MAX && (int)RLE_RHS == PBX_B && (int)RLE_LAMBDA == PBX_X);
+ dSASSERT((int)LHE__LO_HI_MAX == PLH__MAX && (int)LHE_LO == PLH_LO && (int)LHE_HI == PLH_HI);
+
+ } END_STATE_SAVE(memarena, lcpstate);
+ }
+
+ // void *stage3MemarenaState = memarena->SaveState();
+
+ dxStepperStage4CallContext *stage4CallContext = (dxStepperStage4CallContext *)memarena->AllocateBlock(sizeof(dxStepperStage4CallContext));
+ stage4CallContext->Initialize(callContext, localContext/*, stage3MemarenaState*/);
+
+ const unsigned allowedThreads = callContext->m_stepperAllowedThreads;
+ dIASSERT(allowedThreads != 0);
+
+ if (allowedThreads == 1) {
+ IFTIMING(dTimerNow ("compute and apply constraint force"));
+ dxStepIsland_Stage4(stage4CallContext);
+ IFTIMING(dTimerEnd());
+
+ if (m > 0) {
+ IFTIMING(dTimerReport(stdout,1));
+ }
+ }
+ else {
+ dCallReleaseeID finalReleasee = callContext->m_finalReleasee;
+ dxWorld *world = callContext->m_world;
+ world->AlterThreadedCallDependenciesCount(finalReleasee, allowedThreads - 1);
+ world->PostThreadedCallsGroup(NULL, allowedThreads - 1, finalReleasee, &dxStepIsland_Stage4_Callback, stage4CallContext, "StepIsland Stage4");
+ // Note: Adding another dependency for the finalReleasee is not necessary as it already depends on the current call
+ dxStepIsland_Stage4(stage4CallContext);
+ }
+}
+
+static
+int dxStepIsland_Stage4_Callback(void *_stage4CallContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee)
+{
+ (void)callInstanceIndex; // unused
+ (void)callThisReleasee; // unused
+ dxStepperStage4CallContext *stage4CallContext = (dxStepperStage4CallContext *)_stage4CallContext;
+ dxStepIsland_Stage4(stage4CallContext);
+ return 1;
+}
+
+static
+void dxStepIsland_Stage4(dxStepperStage4CallContext *stage4CallContext)
+{
+ const dxStepperProcessingCallContext *callContext = stage4CallContext->m_stepperCallContext;
+ const dxStepperLocalContext *localContext = stage4CallContext->m_localContext;
+
+ const dReal stepSize = callContext->m_stepSize;
+ dxBody *const *bodies = callContext->m_islandBodiesStart;
+ dReal *invI = localContext->m_invI;
+ dJointWithInfo1 *jointInfos = localContext->m_jointinfos;
+ dReal *J = localContext->m_J;
+ dReal *pairsRhsLambda = localContext->m_pairsRhsCfm;
+ const unsigned int *mIndex = localContext->m_mindex;
+ atomicord32 *bodyStartJoints = localContext->m_bodyStartJoints;
+ atomicord32 *bodyJointLinks = localContext->m_bodyJointLinks;
+ const unsigned int nb = callContext->m_islandBodiesCount;
+
+ unsigned bi;
+ while ((bi = ThrsafeIncrementIntUpToLimit(&stage4CallContext->m_bi_constrForce, nb)) != nb) {
+ dVector3 angularForceAccumulator;
+ dxBody *b = bodies[bi];
+ const dReal *invIrow = invI + (sizeint)bi * dM3E__MAX;
+ dReal body_invMass_mul_stepSize = stepSize * b->invMass;
+
+ dReal bodyConstrForce[CFE__MAX];
+ bool constrForceAvailable = false;
+
+ unsigned linkIndex = bodyStartJoints != NULL ? bodyStartJoints[bi] : 0;
+ if (linkIndex != 0) {
+ dSetZero(bodyConstrForce, dARRAY_SIZE(bodyConstrForce));
+ }
+
+ // compute the constraint force as constrForce = J'*lambda
+ for (; linkIndex != 0; constrForceAvailable = true, linkIndex = bodyJointLinks[linkIndex - 1]) {
+ unsigned jointIndex = (linkIndex - 1) / dJCB__MAX;
+ unsigned jointBodyIndex = (linkIndex - 1) % dJCB__MAX;
+
+ const dJointWithInfo1 *currJointInfo = jointInfos + jointIndex;
+ unsigned ofsi = mIndex[jointIndex];
+ dIASSERT(dIN_RANGE(jointIndex, 0, localContext->m_nj));
+
+ const dReal *JRow = J + (sizeint)ofsi * (2 * JME__MAX);
+ const dReal *rowRhsLambda = pairsRhsLambda + (sizeint)ofsi * RLE__RHS_LAMBDA_MAX;
+
+ dxJoint *joint = currJointInfo->joint;
+ const unsigned int infom = currJointInfo->info.m;
+
+ // unsigned jRowExtraOffset = jointBodyIndex * infom * JME__MAX;
+ unsigned jRowExtraOffset = jointBodyIndex != dJCB__MIN ? infom * JME__MAX : 0;
+ dSASSERT(dJCB__MAX == 2);
+
+ dJointFeedback *fb = joint->feedback;
+ MultiplyAddJxLambdaToCForce(bodyConstrForce, JRow + jRowExtraOffset, rowRhsLambda, infom, fb, jointBodyIndex);
+ }
+
+ // compute the velocity update
+ if (constrForceAvailable) {
+ // add fe to cforce and multiply cforce by stepSize
+ for (unsigned int j = dSA__MIN; j != dSA__MAX; ++j) {
+ b->lvel[dV3E__AXES_MIN + j] += (bodyConstrForce[CFE__L_MIN + j] + b->facc[dV3E__AXES_MIN + j]) * body_invMass_mul_stepSize;
+ }
+ for (unsigned int k = dSA__MIN; k != dSA__MAX; ++k) {
+ angularForceAccumulator[dV3E__AXES_MIN + k] = (bodyConstrForce[CFE__A_MIN + k] + b->tacc[dV3E__AXES_MIN + k]) * stepSize;
+ }
+ }
+ else {
+ // add fe to cforce and multiply cforce by stepSize
+ dAddVectorScaledVector3(b->lvel, b->lvel, b->facc, body_invMass_mul_stepSize);
+ dCopyScaledVector3(angularForceAccumulator, b->tacc, stepSize);
+ }
+
+ dMultiplyAdd0_331 (b->avel, invIrow, angularForceAccumulator + dV3E__AXES_MIN);
+
+ // update the position and orientation from the new linear/angular velocity
+ // (over the given time step)
+ dxStepBody (b, stepSize);
+
+ // zero all force accumulators
+ dZeroVector3(b->facc);
+ dZeroVector3(b->tacc);
+ }
+}
+
+
+//****************************************************************************
+
+/*extern */
+sizeint dxEstimateStepMemoryRequirements (dxBody * const *body, unsigned int nb, dxJoint * const *_joint, unsigned int _nj)
+{
+ (void)body; // unused
+ unsigned int nj, m;
+
+ {
+ unsigned int njcurr = 0, mcurr = 0;
+ dxJoint::SureMaxInfo info;
+ dxJoint *const *const _jend = _joint + _nj;
+ for (dxJoint *const *_jcurr = _joint; _jcurr != _jend; ++_jcurr) {
+ dxJoint *j = *_jcurr;
+ j->getSureMaxInfo (&info);
+
+ unsigned int jm = info.max_m;
+ if (jm > 0) {
+ njcurr++;
+
+ mcurr += jm;
+ }
+ }
+ nj = njcurr; m = mcurr;
+ }
+
+ sizeint res = 0;
+
+ res += dOVERALIGNED_SIZE(sizeof(dReal) * dM3E__MAX * nb, INVI_ALIGNMENT); // for invI
+
+ {
+ sizeint sub1_res1 = dEFFICIENT_SIZE(sizeof(dJointWithInfo1) * 2 * _nj); // for initial jointinfos
+
+ // The array can't grow right more than by nj
+ sizeint sub1_res2 = dEFFICIENT_SIZE(sizeof(dJointWithInfo1) * ((sizeint)_nj + (sizeint)nj)); // for shrunk jointinfos
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dxStepperLocalContext)); //for dxStepperLocalContext
+ if (m > 0) {
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(unsigned int) * (nj + 1)); // for mindex
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(int) * m); // for findex
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 2 * JME__MAX * m); // for J
+ unsigned int mskip = dPAD(m);
+ sub1_res2 += dOVERALIGNED_SIZE(sizeof(dReal) * mskip * m, AMATRIX_ALIGNMENT); // for A
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * RCE__RHS_CFM_MAX * m); // for pairsRhsCfm
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * LHE__LO_HI_MAX * m); // for pairsLoHi
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(atomicord32) * nb); // for bodyStartJoints
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(atomicord32)* dJCB__MAX * nj); // for bodyJointLinks
+ }
+
+ {
+ sizeint sub2_res1 = dEFFICIENT_SIZE(sizeof(dxStepperStage3CallContext)); // for dxStepperStage3CallContext
+
+ sizeint sub2_res2 = 0;
+
+ sizeint sub2_res3 = dEFFICIENT_SIZE(sizeof(dxStepperStage4CallContext)); // for dxStepperStage4CallContext
+
+ if (m > 0) {
+ sub2_res1 += dOVERALIGNED_SIZE(sizeof(dReal) * 2 * JIM__MAX * m, JINVM_ALIGNMENT); // for JinvM
+ sub2_res1 += dEFFICIENT_SIZE(sizeof(dReal) * dDA__MAX * nb); // for rhs_tmp
+ sub2_res1 += dEFFICIENT_SIZE(sizeof(dxStepperStage2CallContext)); // for dxStepperStage2CallContext
+
+ sub2_res2 += dxEstimateSolveLCPMemoryReq(m, false);
+ }
+
+ sub1_res2 += dMAX(sub2_res1, dMAX(sub2_res2, sub2_res3));
+ }
+
+ sizeint sub1_res12_max = dMAX(sub1_res1, sub1_res2);
+ sizeint stage01_contexts = dEFFICIENT_SIZE(sizeof(dxStepperStage0BodiesCallContext))
+ + dEFFICIENT_SIZE(sizeof(dxStepperStage0JointsCallContext))
+ + dEFFICIENT_SIZE(sizeof(dxStepperStage1CallContext));
+ res += dMAX(sub1_res12_max, stage01_contexts);
+ }
+
+ return res;
+}
+
+
+/*extern */
+unsigned dxEstimateStepMaxCallCount(
+ unsigned /*activeThreadCount*/, unsigned allowedThreadCount)
+{
+ unsigned result = 1 // dxStepIsland itself
+ + (2 * allowedThreadCount + 2) // (dxStepIsland_Stage2a + dxStepIsland_Stage2b) * allowedThreadCount + 2 * dxStepIsland_Stage2?_Sync
+ + 1; // dxStepIsland_Stage3
+ return result;
+}
diff --git a/libs/ode-0.16.1/ode/src/step.h b/libs/ode-0.16.1/ode/src/step.h new file mode 100644 index 0000000..dc8331a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/step.h @@ -0,0 +1,40 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_STEP_H_ +#define _ODE_STEP_H_ + +#include <ode/common.h> + +struct dxStepperProcessingCallContext; + + +sizeint dxEstimateStepMemoryRequirements( + dxBody * const *body, unsigned int nb, dxJoint * const *_joint, unsigned int _nj); +unsigned dxEstimateStepMaxCallCount( + unsigned activeThreadCount, unsigned allowedThreadCount); + +void dxStepIsland(const dxStepperProcessingCallContext *callContext); + + + +#endif diff --git a/libs/ode-0.16.1/ode/src/threaded_solver_ldlt.h b/libs/ode-0.16.1/ode/src/threaded_solver_ldlt.h new file mode 100644 index 0000000..c791508 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threaded_solver_ldlt.h @@ -0,0 +1,809 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Equation System Threaded Solver + * Copyright (c) 2017-2019 Oleh Derevenko, odar@eleks.com (change all "a" to "e") + */ + + + +#ifndef _ODE_THREADED_SOLVER_LDLT_H_ +#define _ODE_THREADED_SOLVER_LDLT_H_ + + +#include "coop_matrix_types.h" +#include <ode/threading.h> + + +class dxThreadingBase; +class dxResourceRequirementDescriptor; +class dxRequiredResourceContainer; + + +class ThreadedEquationSolverLDLT +{ +public: + static void estimateCooperativeFactoringLDLTResourceRequirements(dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount); + static void cooperativelyFactorLDLT(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip); + + static void estimateCooperativeSolvingL1StraightResourceRequirements(dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount); + static void cooperativelySolveL1Straight(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip); + + static void estimateCooperativeSolvingL1TransposedResourceRequirements(dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount); + static void cooperativelySolveL1Transposed(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip); + + static void estimateCooperativeScalingVectorResourceRequirements(dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned elementCount); + static void cooperativelyScaleVector(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *vectorData, const dReal *scaleData, unsigned elementCount); + + static void estimateCooperativeSolvingLDLTResourceRequirements(dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount); + static void cooperativelySolveLDLT(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip); + +public: + enum + { + ALLOCATION_DEFAULT_ALIGNMENT = COOP_THREAD_DATA_ALIGNMENT_SIZE, + }; + +private: + struct FactorizationSolveL1StripeCellContext; + struct FactorizationFactorizeL1StripeThreadContext; + + enum + { + FLDLT_D_STRIDE = 1, + FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM = 5, + + FSL1S_BLOCK_SIZE = 2, + + FSL1S_REGULAR_B_ROWS = FSL1S_BLOCK_SIZE, + FSL1S_FINAL_B_ROWS = 1, + + FFL1S_REGULAR_A_ROWS = FSL1S_BLOCK_SIZE, + FFL1S_FINAL_A_ROWS = 1, + FFL1S_REGULAR_BLOCK_SIZE = 16, // A suitable by magnitude number being a power of 2 and (naturally) not being divisible by 6 + FFL1S_FINAL_BLOCK_SIZE = 32, // A suitable by magnitude number being a power of 2 and (naturally) not being divisible by 6 + }; + + static unsigned restrictFactoringLDLTAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount); + static void doEstimateCooperativeFactoringLDLTResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount); + static void doCooperativelyFactorLDLTValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip); + + + static unsigned deriveSolvingL1StripeBlockCount(unsigned rowCount, unsigned blockStep) + { + return (rowCount + (blockStep - 1)) / blockStep; + } + + struct FactorizationSolvingL1StripeMemoryEstimates + { + void assignData(sizeint descriptorSizeRequired, sizeint contextSizeRequired) + { + m_descriptorSizeRequired = descriptorSizeRequired; + m_contextSizeRequired = contextSizeRequired; + } + + sizeint m_descriptorSizeRequired; + sizeint m_contextSizeRequired; + }; + + static unsigned deriveSolvingL1StripeThreadCount(unsigned blockCount, unsigned allowedThreadCount) + { + dIASSERT(allowedThreadCount >= 1); + + unsigned maximumCount = blockCount / 2; + return maximumCount >= allowedThreadCount ? allowedThreadCount : dMACRO_MAX(maximumCount, 1U); + } + + static sizeint estimateCooperativelySolvingL1Stripe_XMemoryRequirement(unsigned blockCount, + FactorizationSolvingL1StripeMemoryEstimates &ref_memoryEstimates) + { + sizeint descriptorSizeRequired = dOVERALIGNED_SIZE(sizeof(cellindexint) * blockCount, COOP_THREAD_DATA_ALIGNMENT_SIZE); + sizeint contextSizeRequired = dOVERALIGNED_SIZE(sizeof(FactorizationSolveL1StripeCellContext) * (CCI__MAX + 1) * blockCount, COOP_THREAD_DATA_ALIGNMENT_SIZE); + ref_memoryEstimates.assignData(descriptorSizeRequired, contextSizeRequired); + + sizeint totalSizeRequired = descriptorSizeRequired + contextSizeRequired; + return totalSizeRequired; + } + + static void *markCooperativelySolvingL1Stripe_XMemoryStructuresOut(void *buffer, + const FactorizationSolvingL1StripeMemoryEstimates &memoryEstimates, + cellindexint *&out_blockProgressDescriptors, FactorizationSolveL1StripeCellContext *&out_cellContexts) + { + void *currentLocation = buffer; + + out_blockProgressDescriptors = (cellindexint *)currentLocation; currentLocation = (uint8 *)currentLocation + memoryEstimates.m_descriptorSizeRequired; + out_cellContexts = (FactorizationSolveL1StripeCellContext *)currentLocation; currentLocation = (uint8 *)currentLocation + memoryEstimates.m_contextSizeRequired; + + return currentLocation; + } + + static void initializeCooperativelySolvingL1Stripe_XMemoryStructures(unsigned blockCount, + atomicord32 &out_blockCompletionProgress, cellindexint *blockProgressDescriptors, FactorizationSolveL1StripeCellContext *dUNUSED(cellContexts)) + { + out_blockCompletionProgress = 0; + memset(blockProgressDescriptors, 0, blockCount * sizeof(*blockProgressDescriptors)); + } + + template<unsigned int block_step, unsigned int b_rows> + static void participateSolvingL1Stripe_X(const dReal *L, dReal *B, unsigned blockCount, unsigned rowSkip, + volatile atomicord32 &refBlockCompletionProgress/*=0*/, volatile cellindexint *blockProgressDescriptors/*=[blockCount]*/, + FactorizationSolveL1StripeCellContext *cellContexts/*=[CCI__MAX x blockCount] + [blockCount]*/, unsigned ownThreadIndex); + + static unsigned deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(unsigned solvingBlockIndex, unsigned solvingBlockStep, unsigned blockARows) + { + unsigned factorizingBlockSize = deriveScalingAndFactorizingL1StripeBlockSize(blockARows); + return deriveScalingAndFactorizingL1StripeBlockCountFromFactorizationRow(solvingBlockIndex * solvingBlockStep, factorizingBlockSize); + } + + static unsigned deriveScalingAndFactorizingL1StripeBlockCountFromFactorizationRow(unsigned factorizationRowIndex, unsigned factorizationBlockSize) + { + return (factorizationRowIndex + (factorizationBlockSize - 1)) / factorizationBlockSize; + } + + static unsigned deriveScalingAndFactorizingL1StripeBlockSize(unsigned blockARows) + { + unsigned result = blockARows != 1 ? FFL1S_REGULAR_BLOCK_SIZE : FFL1S_FINAL_BLOCK_SIZE; + dIASSERT(blockARows >= 1 && blockARows <= 2); + + return result; + } + + + static unsigned deriveScalingAndFactorizingL1StripeThreadCount(unsigned blockCount, unsigned allowedThreadCount) + { + dIASSERT(blockCount != 0); + dIASSERT(allowedThreadCount >= 1); + + return dMACRO_MIN(blockCount, allowedThreadCount); + } + + struct FactorizationFactorizeL1StripeContext; + + struct FactorizationScalingAndFactorizingL1StripeMemoryEstimates + { + void assignData(sizeint contextSizeRequired) + { + m_contextSizeRequired = contextSizeRequired; + } + + sizeint m_contextSizeRequired; + }; + + static sizeint estimateCooperativelyScalingAndFactorizingL1Stripe_XMemoryRequirement(unsigned factorizingMaximumThreads, + FactorizationScalingAndFactorizingL1StripeMemoryEstimates &ref_memoryEstimates) + { + dIASSERT(factorizingMaximumThreads != 0); + + sizeint contextSizeRequired = dOVERALIGNED_SIZE(sizeof(FactorizationFactorizeL1StripeContext) + sizeof(FactorizationFactorizeL1StripeThreadContext) * (factorizingMaximumThreads - 1), COOP_THREAD_DATA_ALIGNMENT_SIZE); + ref_memoryEstimates.assignData(contextSizeRequired); + + sizeint totalSizeRequired = contextSizeRequired; + return totalSizeRequired; + } + + static void *markCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructuresOut(void *buffer, + const FactorizationScalingAndFactorizingL1StripeMemoryEstimates &memoryEstimates, FactorizationFactorizeL1StripeContext *&out_factorizationContext) + { + void *currentLocation = buffer; + + out_factorizationContext = (FactorizationFactorizeL1StripeContext *)currentLocation; currentLocation = (uint8 *)currentLocation + memoryEstimates.m_contextSizeRequired; + + return currentLocation; + } + + static void initializeCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructures( + FactorizationFactorizeL1StripeContext *factorizationContext, unsigned threadCount) + { + factorizationContext->initialize(threadCount); + } + + + template<unsigned int a_rows, unsigned int d_stride> + static void participateScalingAndFactorizingL1Stripe_X(dReal *ARow, dReal *d, unsigned factorizationRow, unsigned rowSkip, + FactorizationFactorizeL1StripeContext *factorizationContext, unsigned ownThreadIndex); + +private: + struct FactorLDLTWorkerContext + { + FactorLDLTWorkerContext(dxThreadingBase *threading, unsigned allowedThreadCount, + dReal *A, dReal *d, unsigned totalBlockCount, unsigned rowCount, unsigned rowSkip, + atomicord32 &ref_solvingBlockCompletionProgress, cellindexint *solvingBlockProgressDescriptors, + FactorizationSolveL1StripeCellContext *solvingCellContexts, + FactorizationFactorizeL1StripeContext *factorizingFactorizationContext, + dCallReleaseeID calculationFinishReleasee): + m_threading(threading), + m_allowedThreadCount(allowedThreadCount), + m_A(A), + m_ARow(A), + m_d(d), + m_solvingBlockIndex(0), + m_totalBlockCount(totalBlockCount), + m_rowCount(rowCount), + m_rowSkip(rowSkip), + m_refSolvingBlockCompletionProgress(ref_solvingBlockCompletionProgress), + m_solvingBlockProgressDescriptors(solvingBlockProgressDescriptors), + m_solvingCellContexts(solvingCellContexts), + m_factorizingFactorizationContext(factorizingFactorizationContext), + m_calculationFinishReleasee(calculationFinishReleasee) + { + } + + void incrementForNextBlock() + { + const unsigned blockStep = FSL1S_BLOCK_SIZE; + + m_ARow += blockStep * m_rowSkip; + m_solvingBlockIndex += 1; + } + + dxThreadingBase *m_threading; + unsigned m_allowedThreadCount; + dReal *m_A; + dReal *m_ARow; + dReal *m_d; + unsigned m_solvingBlockIndex; + unsigned m_totalBlockCount; + unsigned m_rowCount; + unsigned m_rowSkip; + atomicord32 &m_refSolvingBlockCompletionProgress; + cellindexint *m_solvingBlockProgressDescriptors; + FactorizationSolveL1StripeCellContext *m_solvingCellContexts; + FactorizationFactorizeL1StripeContext *m_factorizingFactorizationContext; + dCallReleaseeID m_calculationFinishReleasee; + }; + + static int factotLDLT_solvingComplete_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void factotLDLT_solvingComplete(FactorLDLTWorkerContext &ref_context, unsigned ownThreadIndex); + + static int factotLDLT_solvingCompleteSync_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void factotLDLT_solvingCompleteSync(FactorLDLTWorkerContext &ref_workerContext); + + static int factotLDLT_scalingAndFactorizingComplete_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void factotLDLT_scalingAndFactorizingComplete(FactorLDLTWorkerContext &ref_workerContext, unsigned ownThreadIndex); + + static int factotLDLT_scalingAndFactorizingCompleteSync_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void factotLDLT_scalingAndFactorizingCompleteSync(FactorLDLTWorkerContext &ref_workerContext); + + static int factotLDLT_solvingFinal_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void factotLDLT_solvingFinal(FactorLDLTWorkerContext &ref_context, unsigned ownThreadIndex); + + static int factotLDLT_solvingFinalSync_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void factotLDLT_solvingFinalSync(FactorLDLTWorkerContext &ref_workerContext); + + static int factotLDLT_scalingAndFactorizingFinal_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void factotLDLT_scalingAndFactorizingFinal(FactorLDLTWorkerContext &ref_workerContext, unsigned ownThreadIndex); + + static int factotLDLT_completion_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + +private: + struct FactorizationSolveL1StripeCellContext + { + template<unsigned int block_step, unsigned int b_rows> + static void initializePrecalculatedZs(dReal (&Z)[block_step][b_rows]) + { + Z[0][0] = 0; + if (b_rows >= 2) + { + Z[0][1] = 0; + } + Z[1][0] = 0; + if (b_rows >= 2) + { + Z[1][1] = 0; + } + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + } + + template<unsigned int block_step, unsigned int b_rows> + void loadPrecalculatedZs(dReal (&Z)[block_step][b_rows]) const + { + dSASSERT(block_step <= dARRAY_SIZE(m_c)); + dSASSERT(b_rows <= dARRAY_SIZE(m_c[0])); + + Z[0][0] = m_c[0][0]; + if (b_rows >= 2) + { + Z[0][1] = m_c[0][1]; + } + Z[1][0] = m_c[1][0]; + if (b_rows >= 2) + { + Z[1][1] = m_c[1][1]; + } + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + } + + template<unsigned int block_step, unsigned int b_rows> + void storePrecalculatedZs(const dReal (&Z)[block_step][b_rows]) + { + dSASSERT(block_step <= dARRAY_SIZE(m_c)); + dSASSERT(b_rows <= dARRAY_SIZE(m_c[0])); + + m_c[0][0] = Z[0][0]; + if (b_rows >= 2) + { + m_c[0][1] = Z[0][1]; + } + m_c[1][0] = Z[1][0]; + if (b_rows >= 2) + { + m_c[1][1] = Z[1][1]; + } + dSASSERT(block_step == 2); + dSASSERT(b_rows >= 1 && b_rows <= 2); + } + + dReal m_c[FSL1S_BLOCK_SIZE][FSL1S_REGULAR_B_ROWS]; + // dReal m_reserved[4]; + }; + + static FactorizationSolveL1StripeCellContext &buildBlockContextRef(FactorizationSolveL1StripeCellContext *cellContexts, unsigned blockIndex, CellContextInstance contextInstance) + { + return cellContexts[blockIndex * CCI__MAX + contextInstance]; + } + + static FactorizationSolveL1StripeCellContext &buildResultContextRef(FactorizationSolveL1StripeCellContext *cellContexts, unsigned blockIndex, unsigned blockCount) + { + return cellContexts[blockCount * CCI__MAX + blockIndex]; + } + +private: + struct FactorizationFactorizeL1StripeThreadContext + { + template<unsigned int a_rows> + void assignDataSum(const dReal (&sameZ)[a_rows], const dReal (&mixedZ)[dMACRO_MAX(a_rows - 1, 1)], + const FactorizationFactorizeL1StripeThreadContext &partialSumContext) + { + m_sameZ[0] = sameZ[0] + partialSumContext.m_sameZ[0]; + if (a_rows >= 2) + { + m_sameZ[1] = sameZ[1] + partialSumContext.m_sameZ[1]; + m_mixedZ[0] = mixedZ[0] + partialSumContext.m_mixedZ[0]; + } + } + + template<unsigned int a_rows> + void assignDataAlone(const dReal (&sameZ)[a_rows], const dReal (&mixedZ)[dMACRO_MAX(a_rows - 1, 1)]) + { + m_sameZ[0] = sameZ[0]; + if (a_rows >= 2) + { + m_sameZ[1] = sameZ[1]; + m_mixedZ[0] = mixedZ[0]; + } + } + + template<unsigned int a_rows> + void retrieveData(dReal (&out_sameZ)[a_rows], dReal (&out_mixedZ)[dMACRO_MAX(a_rows - 1, 1)]) const + { + out_sameZ[0] = m_sameZ[0]; + if (a_rows >= 2) + { + out_sameZ[1] = m_sameZ[1]; + out_mixedZ[0] = m_mixedZ[0]; + } + dAASSERT(a_rows >= 1 && a_rows <= 2); + } + + dReal m_sameZ[FFL1S_REGULAR_A_ROWS]; + dReal m_mixedZ[dMACRO_MAX(FFL1S_REGULAR_A_ROWS - 1, 1)]; + dReal m_reserved[1]; // [5]; // for alignment + }; + + struct FactorizationFactorizeL1StripeContext + { + void initialize(unsigned threadCount) + { + m_threadsRunning = threadCount; + m_nextColumnIndex = 0; + m_sumThreadIndex = 0; + } + + atomicord32 m_threadsRunning; + atomicord32 m_nextColumnIndex; + volatile atomicord32 m_sumThreadIndex; + atomicord32 m_reserved[1]; // [13]; // for alignment + FactorizationFactorizeL1StripeThreadContext m_threadContexts[1]; // =[threadCount] + }; + +private: + struct SolveL1StraightCellContext; + + enum + { + SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM = 8, + + SL1S_B_STRIDE = 1, + SL1S_BLOCK_SIZE = 4, + }; + + static unsigned restrictSolvingL1StraightAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount); + static void doEstimateCooperativeSolvingL1StraightResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount); + static void doCooperativelySolveL1StraightValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip); + + static unsigned deriveSolvingL1StraightBlockCount(unsigned rowCount, unsigned blockStep) + { + return (rowCount + (blockStep - 1)) / blockStep; + } + + struct SolvingL1StraightMemoryEstimates + { + void assignData(sizeint descriptorSizeRequired, sizeint contextSizeRequired) + { + m_descriptorSizeRequired = descriptorSizeRequired; + m_contextSizeRequired = contextSizeRequired; + } + + sizeint m_descriptorSizeRequired; + sizeint m_contextSizeRequired; + }; + + static unsigned deriveSolvingL1StraightThreadCount(unsigned blockCount, unsigned allowedThreadCount) + { + dIASSERT(allowedThreadCount >= 1); + + unsigned maximumCount = 1 + blockCount / SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM; + return maximumCount >= allowedThreadCount ? allowedThreadCount : dMACRO_MAX(maximumCount, 1U); + } + + template<unsigned int block_step> + static sizeint estimateCooperativelySolvingL1StraightMemoryRequirement(unsigned rowCount, SolvingL1StraightMemoryEstimates &ref_solvingMemoryEstimates); + + static void *markCooperativelySolvingL1StraightMemoryStructuresOut(void *buffer, + const SolvingL1StraightMemoryEstimates &solvingMemoryEstimates, + cellindexint *&out_blockProgressDescriptors, SolveL1StraightCellContext *&out_cellContexts) + { + void *currentLocation = buffer; + + out_blockProgressDescriptors = (cellindexint *)currentLocation; currentLocation = (uint8 *)currentLocation + solvingMemoryEstimates.m_descriptorSizeRequired; + out_cellContexts = (SolveL1StraightCellContext *)currentLocation; currentLocation = (uint8 *)currentLocation + solvingMemoryEstimates.m_contextSizeRequired; + return currentLocation; + } + + template<unsigned int block_step> + static void initializeCooperativelySolveL1StraightMemoryStructures(unsigned rowCount, + atomicord32 &out_blockCompletionProgress, cellindexint *blockProgressDescriptors, SolveL1StraightCellContext *cellContexts); + template<unsigned int block_step, unsigned int b_stride> + static void participateSolvingL1Straight(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip, + volatile atomicord32 &refBlockCompletionProgress/*=0*/, volatile cellindexint *blockProgressDescriptors/*=[blockCount]*/, + SolveL1StraightCellContext *cellContexts/*=[CCI__MAX x blockCount] + [blockCount]*/, unsigned ownThreadIndex); + +private: + struct SolveL1StraightWorkerContext + { + void init(const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip, + atomicord32 &ref_blockCompletionProgress, cellindexint *blockProgressDescriptors, SolveL1StraightCellContext *cellContexts) + { + m_L = L; + m_b = b; + m_rowCount = rowCount; + m_rowSkip = rowSkip; + m_ptrBlockCompletionProgress = &ref_blockCompletionProgress; + m_blockProgressDescriptors = blockProgressDescriptors; + m_cellContexts = cellContexts; + } + + const dReal *m_L; + dReal *m_b; + unsigned m_rowCount; + unsigned m_rowSkip; + atomicord32 *m_ptrBlockCompletionProgress; + cellindexint *m_blockProgressDescriptors; + SolveL1StraightCellContext *m_cellContexts; + }; + + static int solveL1Straight_worker_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void solveL1Straight_worker(SolveL1StraightWorkerContext &ref_context, unsigned ownThreadIndex); + + static int solveL1Straight_completion_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + +private: + struct SolveL1StraightCellContext + { + template<unsigned int block_step> + static void initializePrecalculatedZs(dReal (&Z)[block_step]) + { + std::fill(Z, Z + block_step, REAL(0.0)); + } + + template<unsigned int block_step> + void loadPrecalculatedZs(dReal (&Z)[block_step]) const + { + dSASSERT(block_step <= dARRAY_SIZE(m_c)); + + std::copy(m_c, m_c + block_step, Z); + } + + template<unsigned int block_step> + void storePrecalculatedZs(const dReal (&Z)[block_step]) + { + dSASSERT(block_step <= dARRAY_SIZE(m_c)); + + std::copy(Z, Z + block_step, m_c); + } + + dReal m_c[SL1S_BLOCK_SIZE]; + }; + + + static SolveL1StraightCellContext &buildBlockContextRef(SolveL1StraightCellContext *cellContexts, unsigned blockIndex, CellContextInstance contextInstance) + { + return cellContexts[blockIndex * CCI__MAX + contextInstance]; + } + + static SolveL1StraightCellContext &buildResultContextRef(SolveL1StraightCellContext *cellContexts, unsigned blockIndex, unsigned blockCount) + { + return cellContexts[blockCount * CCI__MAX + blockIndex]; + } + + +private: + struct SolveL1TransposedCellContext; + + enum + { + SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM = SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM, + + SL1T_B_STRIDE = SL1S_B_STRIDE, + SL1T_BLOCK_SIZE = 4, + }; + + static unsigned restrictSolvingL1TransposedAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount); + static void doEstimateCooperativeSolvingL1TransposedResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned rowCount); + static void doCooperativelySolveL1TransposedValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip); + + static unsigned deriveSolvingL1TransposedBlockCount(unsigned rowCount, unsigned blockStep) + { + return (rowCount + (blockStep - 1)) / blockStep; + } + + struct SolvingL1TransposedMemoryEstimates + { + void assignData(sizeint descriptorSizeRequired, sizeint contextSizeRequired) + { + m_descriptorSizeRequired = descriptorSizeRequired; + m_contextSizeRequired = contextSizeRequired; + } + + sizeint m_descriptorSizeRequired; + sizeint m_contextSizeRequired; + }; + + static unsigned deriveSolvingL1TransposedThreadCount(unsigned blockCount, unsigned allowedThreadCount) + { + dSASSERT(SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM + 0 == SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM); + + return deriveSolvingL1StraightThreadCount(blockCount, allowedThreadCount); + } + + template<unsigned int block_step> + static sizeint estimateCooperativelySolvingL1TransposedMemoryRequirement(unsigned rowCount, SolvingL1TransposedMemoryEstimates &ref_solvingMemoryEstimates); + + static void *markCooperativelySolvingL1TransposedMemoryStructuresOut(void *buffer, + const SolvingL1TransposedMemoryEstimates &solvingMemoryEstimates, + cellindexint *&out_blockProgressDescriptors, SolveL1TransposedCellContext *&out_cellContexts) + { + void *currentLocation = buffer; + + out_blockProgressDescriptors = (cellindexint *)currentLocation; currentLocation = (uint8 *)currentLocation + solvingMemoryEstimates.m_descriptorSizeRequired; + out_cellContexts = (SolveL1TransposedCellContext *)currentLocation; currentLocation = (uint8 *)currentLocation + solvingMemoryEstimates.m_contextSizeRequired; + return currentLocation; + } + + template<unsigned int block_step> + static void *allocateCooperativelySolveL1TransposedMemoryStructures(sizeint &out_sizeAllocated, unsigned rowCount, + cellindexint *&out_blockProgressDescriptors, SolveL1TransposedCellContext *&out_cellContexts); + template<unsigned int block_step> + static void initializeCooperativelySolveL1TransposedMemoryStructures(unsigned rowCount, + atomicord32 &out_blockCompletionProgress, cellindexint *blockProgressDescriptors, SolveL1TransposedCellContext *cellContexts); + template<unsigned int block_step, unsigned int b_stride> + static void participateSolvingL1Transposed(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip, + volatile atomicord32 &refBlockCompletionProgress/*=0*/, volatile cellindexint *blockProgressDescriptors/*=[blockCount]*/, + SolveL1TransposedCellContext *cellContexts/*=[CCI__MAX x blockCount] + [blockCount]*/, unsigned ownThreadIndex); + +private: + struct SolveL1TransposedWorkerContext + { + void init(const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip, + atomicord32 &ref_blockCompletionProgress, cellindexint *blockProgressDescriptors, SolveL1TransposedCellContext *cellContexts) + { + m_L = L; + m_b = b; + m_rowCount = rowCount; + m_rowSkip = rowSkip; + m_ptrBlockCompletionProgress = &ref_blockCompletionProgress; + m_blockProgressDescriptors = blockProgressDescriptors; + m_cellContexts = cellContexts; + } + + const dReal *m_L; + dReal *m_b; + unsigned m_rowCount; + unsigned m_rowSkip; + atomicord32 *m_ptrBlockCompletionProgress; + cellindexint *m_blockProgressDescriptors; + SolveL1TransposedCellContext *m_cellContexts; + }; + + static int solveL1Transposed_worker_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void solveL1Transposed_worker(SolveL1TransposedWorkerContext &ref_context, unsigned ownThreadIndex); + + static int solveL1Transposed_completion_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + +private: + struct SolveL1TransposedCellContext + { + template<unsigned int block_step> + static void initializePrecalculatedZs(dReal (&Z)[block_step]) + { + std::fill(Z, Z + block_step, REAL(0.0)); + } + + template<unsigned int block_step> + void loadPrecalculatedZs(dReal (&Z)[block_step]) const + { + dSASSERT(block_step <= dARRAY_SIZE(m_c)); + + std::copy(m_c, m_c + block_step, Z); + } + + template<unsigned int block_step> + void storePrecalculatedZs(const dReal (&Z)[block_step]) + { + dSASSERT(block_step <= dARRAY_SIZE(m_c)); + + std::copy(Z, Z + block_step, m_c); + } + + dReal m_c[SL1T_BLOCK_SIZE]; + }; + + static SolveL1TransposedCellContext &buildBlockContextRef(SolveL1TransposedCellContext *cellContexts, unsigned blockIndex, CellContextInstance contextInstance) + { + return cellContexts[blockIndex * CCI__MAX + contextInstance]; + } + + static SolveL1TransposedCellContext &buildResultContextRef(SolveL1TransposedCellContext *cellContexts, unsigned blockIndex, unsigned blockCount) + { + return cellContexts[blockCount * CCI__MAX + blockIndex]; + } + +private: + enum + { + SV_A_STRIDE = 1, + SV_D_STRIDE = 1, + + SV_BLOCK_SIZE = 128, + SV_COOPERATIVE_BLOCK_COUNT_MINIMUM = 3, + }; + + static unsigned restrictScalingVectorAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned elementCount); + static void doEstimateCooperativeScalingVectorResourceRequirementsValidated( + dxResourceRequirementDescriptor *summaryRequirementsDescriptor, + unsigned allowedThreadCount, unsigned elementCount); + static void doCooperativelyScaleVectorValidated(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, + dReal *vectorData, const dReal *scaleData, unsigned elementCount); + + static unsigned deriveScalingVectorBlockCount(unsigned elementCount, unsigned blockStep) + { + return (elementCount + (blockStep - 1)) / blockStep; + } + + static unsigned deriveScalingVectorThreadCount(unsigned lastBlockIndex, unsigned allowedThreadCount) + { + dIASSERT(allowedThreadCount >= 1); + + unsigned maximumCount = lastBlockIndex; + return maximumCount >= allowedThreadCount ? allowedThreadCount : dMACRO_MAX(maximumCount, 1U); + } + + static void initializeCooperativelyScaleVectorMemoryStructures(atomicord32 &out_blockCompletionProgress) + { + out_blockCompletionProgress = 0; + } + template<unsigned int block_step, unsigned int a_stride, unsigned int d_stride> + static void participateScalingVector(dReal *ptrAStart, const dReal *ptrDStart, const unsigned elementCount, + volatile atomicord32 &refBlockCompletionProgress/*=0*/); + +private: + struct ScaleVectorWorkerContext + { + void init(dReal *vectorData, const dReal *scaleData, unsigned elementCount, + atomicord32 &ref_blockCompletionProgress) + { + m_vectorData = vectorData; + m_scaleData = scaleData; + m_elementCount = elementCount; + m_ptrBlockCompletionProgress = &ref_blockCompletionProgress; + } + + dReal *m_vectorData; + const dReal *m_scaleData; + unsigned m_elementCount; + atomicord32 *m_ptrBlockCompletionProgress; + }; + + static int scaleVector_worker_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + static void scaleVector_worker(ScaleVectorWorkerContext &ref_context); + + static int scaleVector_completion_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + + +private: + enum SolvingLDLTStage + { + SLDLTS__MIN, + + SLDLTS_SOLVING_STRAIGHT = SLDLTS__MIN, + SLDLTS_SCALING_VECTOR, + SLDLTS_SOLVING_TRANSPOSED, + + SLDLTS__MAX, + }; + + enum + { + SLDLT_B_STRIDE = SL1S_B_STRIDE, + SLDLT_D_STRIDE = FLDLT_D_STRIDE, + }; + + static unsigned restrictSolvingLDLTAllowedThreadCount( + dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount, unsigned &out_stageBlockCountSifficiencyMask); + + static void doCooperativelySolveLDLTValidated( + dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, unsigned stageBlockCountSifficiencyMask, + const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip); +}; + + +#endif + diff --git a/libs/ode-0.16.1/ode/src/threading_atomics_provs.h b/libs/ode-0.16.1/ode/src/threading_atomics_provs.h new file mode 100644 index 0000000..3afc7b3 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_atomics_provs.h @@ -0,0 +1,194 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading atomics providers file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Fake atomics provider for built-in threading support provider. + * OU-based atomics provider for built-in threading support provider. + * + * The classes have been moved into a separate header as they are to be used + * in both WIN and POSIX implementations. + */ + + +#ifndef _ODE_THREADING_ATOMICS_PROVS_H_ +#define _ODE_THREADING_ATOMICS_PROVS_H_ + + +#include <ode/odeconfig.h> +#include <ode/error.h> + + +/************************************************************************/ +/* Fake atomics provider class implementation */ +/************************************************************************/ + +class dxFakeAtomicsProvider +{ +public: + typedef unsigned long atomicord_t; + typedef void *atomicptr_t; + +public: + static void IncrementTargetNoRet(volatile atomicord_t *value_accumulator_ptr) + { + ++(*value_accumulator_ptr); + } + + static void DecrementTargetNoRet(volatile atomicord_t *value_accumulator_ptr) + { + --(*value_accumulator_ptr); + } + + static atomicord_t QueryTargetValue(volatile atomicord_t *value_storage_ptr) + { + return *value_storage_ptr; + } + + template<unsigned type_size> + static sizeint AddValueToTarget(volatile void *value_accumulator_ptr, diffint value_addend); + + static bool CompareExchangeTargetPtr(volatile atomicptr_t *pointer_storage_ptr, + atomicptr_t comparand_value, atomicptr_t new_value) + { + bool exchange_result = false; + + atomicptr_t original_value = *pointer_storage_ptr; + + if (original_value == comparand_value) + { + *pointer_storage_ptr = new_value; + + exchange_result = true; + } + + return exchange_result; + } +}; + +template<> +inline sizeint dxFakeAtomicsProvider::AddValueToTarget<sizeof(dxFakeAtomicsProvider::atomicord_t)>(volatile void *value_accumulator_ptr, diffint value_addend) +{ + atomicord_t original_value = *(volatile atomicord_t *)value_accumulator_ptr; + + *(volatile atomicord_t *)value_accumulator_ptr = original_value + (atomicord_t)value_addend; + + return original_value; +} + +template<> +inline sizeint dxFakeAtomicsProvider::AddValueToTarget<2 * sizeof(dxFakeAtomicsProvider::atomicord_t)>(volatile void *value_accumulator_ptr, diffint value_addend) +{ + atomicptr_t original_value = *(volatile atomicptr_t *)value_accumulator_ptr; + + *(volatile atomicptr_t *)value_accumulator_ptr = (atomicptr_t)((sizeint)original_value + (sizeint)value_addend); + + return (sizeint)original_value; +} + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +/************************************************************************/ +/* dxOUAtomicsProvider class implementation */ +/************************************************************************/ + +#if !dOU_ENABLED +#error OU library must be enabled for this to compile +#elif !dATOMICS_ENABLED +#error OU Atomics must be enabled for this to compile +#endif +#include "odeou.h" + +class dxOUAtomicsProvider +{ +public: + typedef _OU_NAMESPACE::atomicord32 atomicord_t; + typedef _OU_NAMESPACE::atomicptr atomicptr_t; + +public: + static void IncrementTargetNoRet(volatile atomicord_t *value_accumulator_ptr) + { + _OU_NAMESPACE::AtomicIncrementNoResult(value_accumulator_ptr); + } + + static void DecrementTargetNoRet(volatile atomicord_t *value_accumulator_ptr) + { + _OU_NAMESPACE::AtomicDecrementNoResult(value_accumulator_ptr); + } + + static atomicord_t QueryTargetValue(volatile atomicord_t *value_storage_ptr) + { + // Query value with memory barrier before + atomicord_t result_value = *value_storage_ptr; + + if (!_OU_NAMESPACE::AtomicCompareExchange(value_storage_ptr, result_value, result_value)) + { + result_value = *value_storage_ptr; + } + + return result_value; + } + + template<unsigned type_size> + static sizeint AddValueToTarget(volatile void *value_accumulator_ptr, diffint value_addend); + + static bool CompareExchangeTargetPtr(volatile atomicptr_t *pointer_storage_ptr, + atomicptr_t comparand_value, atomicptr_t new_value) + { + return _OU_NAMESPACE::AtomicCompareExchangePointer(pointer_storage_ptr, comparand_value, new_value); + } +}; + +template<> +inline sizeint dxOUAtomicsProvider::AddValueToTarget<sizeof(dxOUAtomicsProvider::atomicord_t)>(volatile void *value_accumulator_ptr, diffint value_addend) +{ + return _OU_NAMESPACE::AtomicExchangeAdd((volatile atomicord_t *)value_accumulator_ptr, (atomicord_t)value_addend); +} + +template<> +inline sizeint dxOUAtomicsProvider::AddValueToTarget<2 * sizeof(dxOUAtomicsProvider::atomicord_t)>(volatile void *value_accumulator_ptr, diffint value_addend) +{ + atomicptr_t original_value; + + while (true) + { + original_value = *(volatile atomicptr_t *)value_accumulator_ptr; + + atomicptr_t new_value = (atomicptr_t)((sizeint)original_value + (sizeint)value_addend); + if (_OU_NAMESPACE::AtomicCompareExchangePointer((volatile atomicptr_t *)value_accumulator_ptr, original_value, new_value)) + { + break; + } + } + + return (sizeint)original_value; +} + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +#endif // #ifndef _ODE_THREADING_ATOMICS_PROVS_H_ diff --git a/libs/ode-0.16.1/ode/src/threading_base.cpp b/libs/ode-0.16.1/ode/src/threading_base.cpp new file mode 100644 index 0000000..9272eff --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_base.cpp @@ -0,0 +1,135 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading base wrapper class implementation file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Threading base class to be used for inheritance by dxWorld, dxSpace and others + * to take advantage of threaded execution. + */ + + +#include <ode/common.h> +#include "config.h" +#include "error.h" +#include "threading_base.h" + + +dxThreadingBase::~dxThreadingBase() +{ + DoFreeStockCallWait(); +} + + +void dxThreadingBase::PostThreadedCallsGroup( + int *out_summary_fault/*=NULL*/, + ddependencycount_t member_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, + const char *call_name/*=NULL*/) const +{ + dIASSERT(member_count != 0); + + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + + for (unsigned member_index = 0; member_index != member_count; ++member_index) { + // Post individual group member jobs + functions->post_call(impl, out_summary_fault, NULL, 0, dependent_releasee, NULL, call_func, call_context, member_index, call_name); + } +} + +void dxThreadingBase::PostThreadedCallsIndexOverridenGroup(int *out_summary_fault/*=NULL*/, + ddependencycount_t member_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, unsigned index_override, + const char *call_name/*=NULL*/) const +{ + dIASSERT(member_count != 0); + + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + + for (unsigned member_index = 0; member_index != member_count; ++member_index) { + // Post individual group member jobs + functions->post_call(impl, out_summary_fault, NULL, 0, dependent_releasee, NULL, call_func, call_context, index_override, call_name); + } +} + +void dxThreadingBase::PostThreadedCallForUnawareReleasee( + int *out_summary_fault/*=NULL*/, + dCallReleaseeID *out_post_releasee/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dCallWaitID call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index, + const char *call_name/*=NULL*/) const +{ + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + + functions->alter_call_dependencies_count(impl, dependent_releasee, 1); + functions->post_call(impl, out_summary_fault, out_post_releasee, dependencies_count, dependent_releasee, call_wait, call_func, call_context, instance_index, call_name); +} + + +const dxThreadingFunctionsInfo *dxThreadingBase::FindThreadingImpl(dThreadingImplementationID &out_impl_found) const +{ + const dxThreadingFunctionsInfo *functions_found = GetFunctionsInfo(); + + if (functions_found != NULL) + { + out_impl_found = GetThreadingImpl(); + } + else + { + functions_found = m_default_impl_provider->retrieveThreadingDefaultImpl(out_impl_found); + } + + return functions_found; +} + + +dCallWaitID dxThreadingBase::DoAllocateStockCallWait() +{ + dIASSERT(GetStockCallWait() == NULL); + + dCallWaitID stock_wait_id = AllocThreadedCallWait(); + + if (stock_wait_id != NULL) + { + SetStockCallWait(stock_wait_id); + } + + return stock_wait_id; +} + +void dxThreadingBase::DoFreeStockCallWait() +{ + dCallWaitID stock_wait_id = GetStockCallWait(); + + if (stock_wait_id != NULL) + { + FreeThreadedCallWait(stock_wait_id); + + SetStockCallWait(NULL); + } +} + diff --git a/libs/ode-0.16.1/ode/src/threading_base.h b/libs/ode-0.16.1/ode/src/threading_base.h new file mode 100644 index 0000000..cb38f7f --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_base.h @@ -0,0 +1,291 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading base wrapper class header file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Threading base class to be used for inheritance by dxWorld, dxSpace and others + * to take advantage of threaded execution. + */ + + +#ifndef _ODE_THREADING_BASE_H_ +#define _ODE_THREADING_BASE_H_ + + +#include "common.h" +#include <ode/threading.h> + + +struct dxIThreadingDefaultImplProvider +{ +public: + virtual const dxThreadingFunctionsInfo *retrieveThreadingDefaultImpl(dThreadingImplementationID &out_defaultImpl) = 0; +}; + + +class dxThreadingBase +{ +protected: + dxThreadingBase(): + m_default_impl_provider(NULL), + m_functions_info(NULL), + m_threading_impl(NULL), + m_stock_call_wait(NULL) + { + } + + // This ought to be done via constructor, but passing 'this' in base class initializer emits a warning in MSVC :( + void setThreadingDefaultImplProvider(dxIThreadingDefaultImplProvider *default_impl_provider) + { + m_default_impl_provider = default_impl_provider; + dIASSERT(GetStockCallWait() == NULL); + } + + ~dxThreadingBase(); + +public: + void assignThreadingImpl(const dxThreadingFunctionsInfo *functions_info, dThreadingImplementationID threading_impl) + { + dAASSERT((functions_info == NULL) == (threading_impl == NULL)); + + // Free the stock call wait first to have it executed before new pointer values are assigned + DoFreeStockCallWait(); + + m_functions_info = functions_info; + m_threading_impl = threading_impl; + } + +public: + unsigned calculateThreadingLimitedThreadCount(unsigned limitValue, bool countCallerAsExtraThread, unsigned *ptrOut_activeThreadCount=NULL) const + { + unsigned activeThreadCount = RetrieveThreadingThreadCount(); + + if (ptrOut_activeThreadCount != NULL) + { + *ptrOut_activeThreadCount = activeThreadCount; + } + + unsigned adjustedActiveThreads = countCallerAsExtraThread && activeThreadCount != UINT_MAX ? activeThreadCount + 1 : activeThreadCount; + return limitValue == dTHREADING_THREAD_COUNT_UNLIMITED + ? adjustedActiveThreads + : dMACRO_MIN(limitValue, adjustedActiveThreads); + } + +public: + dCallWaitID AllocateOrRetrieveStockCallWaitID() + { + dCallWaitID stock_wait_id = GetStockCallWait(); + return stock_wait_id != NULL ? (ResetThreadedCallWait(stock_wait_id), stock_wait_id) : DoAllocateStockCallWait(); + } + +public: + dMutexGroupID AllocMutexGroup(dmutexindex_t Mutex_count, const char *const *Mutex_names_ptr/*=NULL*/) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + return functions->alloc_mutex_group(impl, Mutex_count, Mutex_names_ptr); + } + + void FreeMutexGroup(dMutexGroupID mutex_group) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->free_mutex_group(impl, mutex_group); + } + + void LockMutexGroupMutex(dMutexGroupID mutex_group, dmutexindex_t mutex_index) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->lock_group_mutex(impl, mutex_group, mutex_index); + } + +// bool TryLockMutexGroupMutex(dMutexGroupID mutex_group, dmutexindex_t mutex_index) const +// { +// dThreadingImplementationID impl; +// const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); +// return functions->trylock_group_mutex(impl, mutex_group, mutex_index) != 0; +// } + + void UnlockMutexGroupMutex(dMutexGroupID mutex_group, dmutexindex_t mutex_index) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->unlock_group_mutex(impl, mutex_group, mutex_index); + } + + dCallWaitID AllocThreadedCallWait() const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + return functions->alloc_call_wait(impl); + } + + void ResetThreadedCallWait(dCallWaitID call_wait) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->reset_call_wait(impl, call_wait); + } + + void FreeThreadedCallWait(dCallWaitID call_wait) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->free_call_wait(impl, call_wait); + } + + void PostThreadedCall(int *out_summary_fault/*=NULL*/, + dCallReleaseeID *out_post_releasee/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dCallWaitID call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index, + const char *call_name/*=NULL*/) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->post_call(impl, out_summary_fault, out_post_releasee, dependencies_count, dependent_releasee, call_wait, call_func, call_context, instance_index, call_name); + } + + void AlterThreadedCallDependenciesCount(dCallReleaseeID target_releasee, + ddependencychange_t dependencies_count_change) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->alter_call_dependencies_count(impl, target_releasee, dependencies_count_change); + } + + void WaitThreadedCallExclusively(int *out_wait_status/*=NULL*/, + dCallWaitID call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/, + const char *wait_name/*=NULL*/) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->wait_call(impl, out_wait_status, call_wait, timeout_time_ptr, wait_name); + functions->reset_call_wait(impl, call_wait); + } + + void WaitThreadedCallCollectively(int *out_wait_status/*=NULL*/, + dCallWaitID call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/, + const char *wait_name/*=NULL*/) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + functions->wait_call(impl, out_wait_status, call_wait, timeout_time_ptr, wait_name); + } + + unsigned RetrieveThreadingThreadCount() const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + return functions->retrieve_thread_count(impl); + } + + bool PreallocateResourcesForThreadedCalls(unsigned max_simultaneous_calls_estimate) const + { + dThreadingImplementationID impl; + const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl); + return functions->preallocate_resources_for_calls(impl, max_simultaneous_calls_estimate) != 0; + } + +public: + void PostThreadedCallsGroup(int *out_summary_fault/*=NULL*/, + ddependencycount_t member_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, + const char *call_name/*=NULL*/) const; + void PostThreadedCallsIndexOverridenGroup(int *out_summary_fault/*=NULL*/, + ddependencycount_t member_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, unsigned index_override, + const char *call_name/*=NULL*/) const; + void PostThreadedCallForUnawareReleasee(int *out_summary_fault/*=NULL*/, + dCallReleaseeID *out_post_releasee/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dCallWaitID call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index, + const char *call_name/*=NULL*/) const; + +protected: + const dxThreadingFunctionsInfo *FindThreadingImpl(dThreadingImplementationID &out_impl_found) const; + +private: + dCallWaitID DoAllocateStockCallWait(); + void DoFreeStockCallWait(); + +private: + const dxThreadingFunctionsInfo *GetFunctionsInfo() const { return m_functions_info; } + dThreadingImplementationID GetThreadingImpl() const { return m_threading_impl; } + + void SetStockCallWait(dCallWaitID value) { m_stock_call_wait = value; } + dCallWaitID GetStockCallWait() const { return m_stock_call_wait; } + +private: + dxIThreadingDefaultImplProvider *m_default_impl_provider; + const dxThreadingFunctionsInfo *m_functions_info; + dThreadingImplementationID m_threading_impl; + dCallWaitID m_stock_call_wait; +}; + +class dxMutexGroupLockHelper +{ +public: + dxMutexGroupLockHelper(dxThreadingBase *threading_base, dMutexGroupID mutex_group, dmutexindex_t mutex_index): + m_threading_base(threading_base), + m_mutex_group(mutex_group), + m_mutex_index(mutex_index), + m_mutex_locked(true) + { + threading_base->LockMutexGroupMutex(mutex_group, mutex_index); + } + + ~dxMutexGroupLockHelper() + { + if (m_mutex_locked) + { + m_threading_base->UnlockMutexGroupMutex(m_mutex_group, m_mutex_index); + } + } + + void UnlockMutex() + { + dIASSERT(m_mutex_locked); + + m_threading_base->UnlockMutexGroupMutex(m_mutex_group, m_mutex_index); + m_mutex_locked = false; + } + + void RelockMutex() + { + dIASSERT(!m_mutex_locked); + + m_threading_base->LockMutexGroupMutex(m_mutex_group, m_mutex_index); + m_mutex_locked = true; + } + +private: + dxThreadingBase *m_threading_base; + dMutexGroupID m_mutex_group; + dmutexindex_t m_mutex_index; + bool m_mutex_locked; +}; + +#endif // #ifndef _ODE_THREADING_BASE_H_ diff --git a/libs/ode-0.16.1/ode/src/threading_fake_sync.h b/libs/ode-0.16.1/ode/src/threading_fake_sync.h new file mode 100644 index 0000000..d1c2524 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_fake_sync.h @@ -0,0 +1,128 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading fake synchronization objects file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Self-wakeup implementation for built-in threading support provider. + * Fake mutex implementation for built-in threading support provider. + * + * The classes have been moved into a separate header as they are to be used + * in both WIN and POSIX implementations. + */ + + +#ifndef _ODE_THREADING_FAKE_SYNC_H_ +#define _ODE_THREADING_FAKE_SYNC_H_ + + +#include <ode/odeconfig.h> +#include <ode/error.h> + + +/************************************************************************/ +/* dxSelfWakeup class definition */ +/************************************************************************/ + +class dxSelfWakeup +{ +public: + dxSelfWakeup(): + m_wakeup_state(false), + m_state_is_permanent(false) + { + } + + bool InitializeObject() { return true; } + +public: + void ResetWakeup() { m_wakeup_state = false; m_state_is_permanent = false; } + void WakeupAThread() { dIASSERT(!m_state_is_permanent); m_wakeup_state = true; } // Wakeup should not be used after permanent signal + void WakeupAllThreads() { m_wakeup_state = true; m_state_is_permanent = true; } + + bool WaitWakeup(const dThreadedWaitTime *timeout_time_ptr); + +private: + bool m_wakeup_state; + bool m_state_is_permanent; +}; + + +bool dxSelfWakeup::WaitWakeup(const dThreadedWaitTime *timeout_time_ptr) +{ + (void)timeout_time_ptr; // unused + bool wait_result = m_wakeup_state; + + if (m_wakeup_state) + { + m_wakeup_state = m_state_is_permanent; + } + else + { + dICHECK(false); // Self-wakeup should only be used in cases when waiting is called after object is signaled + } + + return wait_result; +} + + +/************************************************************************/ +/* Fake mutex class implementation */ +/************************************************************************/ + +class dxFakeMutex +{ +public: + dxFakeMutex() {} + + bool InitializeObject() { return true; } + +public: + void LockMutex() { /* Do nothing */ } + bool TryLockMutex() { /* Do nothing */ return true; } + void UnlockMutex() { /* Do nothing */ } +}; + + +/************************************************************************/ +/* Fake lull class implementation */ +/************************************************************************/ + +class dxFakeLull +{ +public: + dxFakeLull() {} + + bool InitializeObject() { return true; } + +public: + void RegisterToLull() { /* Do nothing */ } + void WaitForLullAlarm() { dICHECK(false); } // Fake lull can't be waited + void UnregisterFromLull() { /* Do nothing */ } + + void SignalLullAlarmIfAnyRegistrants() { /* Do nothing */ } +}; + + +#endif // #ifndef _ODE_THREADING_FAKE_SYNC_H_ diff --git a/libs/ode-0.16.1/ode/src/threading_impl.cpp b/libs/ode-0.16.1/ode/src/threading_impl.cpp new file mode 100644 index 0000000..aa30883 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_impl.cpp @@ -0,0 +1,282 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading subsystem implementation file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Subsystem APIs implementation for built-in threading support provider. + */ + + +#include <ode/common.h> +#include <ode/threading_impl.h> +#include "config.h" +#include "threading_impl_posix.h" +#include "threading_impl_win.h" +#include "threading_impl.h" + + +static dMutexGroupID AllocMutexGroup(dThreadingImplementationID impl, dmutexindex_t Mutex_count, const char *const *Mutex_names_ptr/*=NULL*/); +static void FreeMutexGroup(dThreadingImplementationID impl, dMutexGroupID mutex_group); +static void LockMutexGroupMutex(dThreadingImplementationID impl, dMutexGroupID mutex_group, dmutexindex_t mutex_index); +// static int TryLockMutexGroupMutex(dThreadingImplementationID impl, dMutexGroupID mutex_group, dmutexindex_t mutex_index); +static void UnlockMutexGroupMutex(dThreadingImplementationID impl, dMutexGroupID mutex_group, dmutexindex_t mutex_index); + +static dCallWaitID AllocThreadedCallWait(dThreadingImplementationID impl); +static void ResetThreadedCallWait(dThreadingImplementationID impl, dCallWaitID call_wait); +static void FreeThreadedCallWait(dThreadingImplementationID impl, dCallWaitID call_wait); + +static void PostThreadedCall( + dThreadingImplementationID impl, int *out_summary_fault/*=NULL*/, + dCallReleaseeID *out_post_releasee/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dCallWaitID call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index, + const char *call_name/*=NULL*/); +static void AlterThreadedCallDependenciesCount( + dThreadingImplementationID impl, dCallReleaseeID target_releasee, + ddependencychange_t dependencies_count_change); +static void WaitThreadedCall( + dThreadingImplementationID impl, int *out_wait_status/*=NULL*/, + dCallWaitID call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/, + const char *wait_name/*=NULL*/); + +static unsigned RetrieveThreadingThreadCount(dThreadingImplementationID impl); +static int PreallocateResourcesForThreadedCalls(dThreadingImplementationID impl, ddependencycount_t max_simultaneous_calls_estimate); + + +static const dxThreadingFunctionsInfo g_builtin_threading_functions = +{ + sizeof(dxThreadingFunctionsInfo), // unsigned struct_size; + + &AllocMutexGroup, // dMutexGroupAllocFunction *alloc_mutex_group; + &FreeMutexGroup, // dMutexGroupFreeFunction *free_mutex_group; + &LockMutexGroupMutex, // dMutexGroupMutexLockFunction *lock_group_mutex; + &UnlockMutexGroupMutex, // dMutexGroupMutexUnlockFunction *unlock_group_mutex; + + &AllocThreadedCallWait, // dThreadedCallWaitAllocFunction *alloc_call_wait; + &ResetThreadedCallWait, // dThreadedCallWaitResetFunction *reset_call_wait; + &FreeThreadedCallWait, // dThreadedCallWaitFreeFunction *free_call_wait; + + &PostThreadedCall, // dThreadedCallPostFunction *post_call; + &AlterThreadedCallDependenciesCount, // dThreadedCallDependenciesCountAlterFunction *alter_call_dependencies_count; + &WaitThreadedCall, // dThreadedCallWaitFunction *wait_call; + + &RetrieveThreadingThreadCount, // dThreadingImplThreadCountRetrieveFunction *retrieve_thread_count; + &PreallocateResourcesForThreadedCalls, // dThreadingImplResourcesForCallsPreallocateFunction *preallocate_resources_for_calls; + + // &TryLockMutexGroupMutex, // dMutexGroupMutexTryLockFunction *trylock_group_mutex; +}; + + +/*extern */dThreadingImplementationID dThreadingAllocateSelfThreadedImplementation() +{ + dxSelfThreadedThreading *threading = new dxSelfThreadedThreading(); + + if (threading != NULL && !threading->InitializeObject()) + { + delete threading; + threading = NULL; + } + + dxIThreadingImplementation *impl = threading; + return (dThreadingImplementationID)impl; +} + +/*extern */dThreadingImplementationID dThreadingAllocateMultiThreadedImplementation() +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dxMultiThreadedThreading *threading = new dxMultiThreadedThreading(); + + if (threading != NULL && !threading->InitializeObject()) + { + delete threading; + threading = NULL; + } +#else + dxIThreadingImplementation *threading = NULL; +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + dxIThreadingImplementation *impl = threading; + return (dThreadingImplementationID)impl; +} + +/*extern */const dThreadingFunctionsInfo *dThreadingImplementationGetFunctions(dThreadingImplementationID impl) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dAASSERT(impl != NULL); +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + const dThreadingFunctionsInfo *functions = NULL; + +#if !dBUILTIN_THREADING_IMPL_ENABLED + if (impl != NULL) +#endif // #if !dBUILTIN_THREADING_IMPL_ENABLED + { + functions = &g_builtin_threading_functions; + } + + return functions; +} + +/*extern */void dThreadingImplementationShutdownProcessing(dThreadingImplementationID impl) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dAASSERT(impl != NULL); +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + +#if !dBUILTIN_THREADING_IMPL_ENABLED + if (impl != NULL) +#endif // #if !dBUILTIN_THREADING_IMPL_ENABLED + { + ((dxIThreadingImplementation *)impl)->ShutdownProcessing(); + } +} + +/*extern */void dThreadingImplementationCleanupForRestart(dThreadingImplementationID impl) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dAASSERT(impl != NULL); +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + +#if !dBUILTIN_THREADING_IMPL_ENABLED + if (impl != NULL) +#endif // #if !dBUILTIN_THREADING_IMPL_ENABLED + { + ((dxIThreadingImplementation *)impl)->CleanupForRestart(); + } +} + +/*extern */void dThreadingFreeImplementation(dThreadingImplementationID impl) +{ + if (impl != NULL) + { + ((dxIThreadingImplementation *)impl)->FreeInstance(); + } +} + + +/*extern */void dExternalThreadingServeMultiThreadedImplementation(dThreadingImplementationID impl, + dThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dAASSERT(impl != NULL); +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + +#if !dBUILTIN_THREADING_IMPL_ENABLED + if (impl != NULL) +#endif // #if !dBUILTIN_THREADING_IMPL_ENABLED + { + ((dxIThreadingImplementation *)impl)->StickToJobsProcessing(readiness_callback, callback_context); + } +} + + +////////////////////////////////////////////////////////////////////////// + +static dMutexGroupID AllocMutexGroup(dThreadingImplementationID impl, dmutexindex_t Mutex_count, const char *const *Mutex_names_ptr/*=NULL*/) +{ + (void)Mutex_names_ptr; // unused + dIMutexGroup *mutex_group = ((dxIThreadingImplementation *)impl)->AllocMutexGroup(Mutex_count); + return (dMutexGroupID)mutex_group; +} + +static void FreeMutexGroup(dThreadingImplementationID impl, dMutexGroupID mutex_group) +{ + ((dxIThreadingImplementation *)impl)->FreeMutexGroup((dIMutexGroup *)mutex_group); +} + +static void LockMutexGroupMutex(dThreadingImplementationID impl, dMutexGroupID mutex_group, dmutexindex_t mutex_index) +{ + ((dxIThreadingImplementation *)impl)->LockMutexGroupMutex((dIMutexGroup *)mutex_group, mutex_index); +} + +// static int TryLockMutexGroupMutex(dThreadingImplementationID impl, dMutexGroupID mutex_group, dmutexindex_t mutex_index) +// { +// bool trylock_result = ((dxIThreadingImplementation *)impl)->TryLockMutexGroupMutex((dIMutexGroup *)mutex_group, mutex_index); +// return trylock_result; +// } + +static void UnlockMutexGroupMutex(dThreadingImplementationID impl, dMutexGroupID mutex_group, dmutexindex_t mutex_index) +{ + ((dxIThreadingImplementation *)impl)->UnlockMutexGroupMutex((dIMutexGroup *)mutex_group, mutex_index); +} + + +static dCallWaitID AllocThreadedCallWait(dThreadingImplementationID impl) +{ + dxICallWait *call_wait = ((dxIThreadingImplementation *)impl)->AllocACallWait(); + return (dCallWaitID)call_wait; +} + +static void ResetThreadedCallWait(dThreadingImplementationID impl, dCallWaitID call_wait) +{ + ((dxIThreadingImplementation *)impl)->ResetACallWait((dxICallWait *)call_wait); +} + +static void FreeThreadedCallWait(dThreadingImplementationID impl, dCallWaitID call_wait) +{ + ((dxIThreadingImplementation *)impl)->FreeACallWait((dxICallWait *)call_wait); +} + + +static void PostThreadedCall( + dThreadingImplementationID impl, int *out_summary_fault/*=NULL*/, + dCallReleaseeID *out_post_releasee/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dCallWaitID call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index, + const char *call_name/*=NULL*/) +{ + (void)call_name; // unused + ((dxIThreadingImplementation *)impl)->ScheduleNewJob(out_summary_fault, out_post_releasee, + dependencies_count, dependent_releasee, (dxICallWait *)call_wait, call_func, call_context, instance_index); +} + +static void AlterThreadedCallDependenciesCount( + dThreadingImplementationID impl, dCallReleaseeID target_releasee, + ddependencychange_t dependencies_count_change) +{ + ((dxIThreadingImplementation *)impl)->AlterJobDependenciesCount(target_releasee, dependencies_count_change); +} + +static void WaitThreadedCall( + dThreadingImplementationID impl, int *out_wait_status/*=NULL*/, + dCallWaitID call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/, + const char *wait_name/*=NULL*/) +{ + (void)wait_name; // unused + ((dxIThreadingImplementation *)impl)->WaitJobCompletion(out_wait_status, (dxICallWait *)call_wait, timeout_time_ptr); +} + + +static unsigned RetrieveThreadingThreadCount(dThreadingImplementationID impl) +{ + return ((dxIThreadingImplementation *)impl)->RetrieveActiveThreadsCount(); +} + +static int PreallocateResourcesForThreadedCalls(dThreadingImplementationID impl, ddependencycount_t max_simultaneous_calls_estimate) +{ + return ((dxIThreadingImplementation *)impl)->PreallocateJobInfos(max_simultaneous_calls_estimate); +} + + diff --git a/libs/ode-0.16.1/ode/src/threading_impl.h b/libs/ode-0.16.1/ode/src/threading_impl.h new file mode 100644 index 0000000..7fb5c60 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_impl.h @@ -0,0 +1,40 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading implementation private header file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Threading implementation header for library private functions. + */ + + +#ifndef _ODE__PRIVATE_THREADING_IMPL_H_ +#define _ODE__PRIVATE_THREADING_IMPL_H_ + + +#include <ode/threading_impl.h> + + + +#endif // #ifndef _ODE__PRIVATE_THREADING_IMPL_H_ diff --git a/libs/ode-0.16.1/ode/src/threading_impl_posix.h b/libs/ode-0.16.1/ode/src/threading_impl_posix.h new file mode 100644 index 0000000..0aaf4ae --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_impl_posix.h @@ -0,0 +1,638 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * Threading POSIX implementation file. *
+ * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. *
+ * e-mail: odar@eleks.com (change all "a" to "e") *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/*
+ * Threading POSIX implementation for built-in threading support provider.
+ */
+
+
+#ifndef _ODE_THREADING_IMPL_POSIX_H_
+#define _ODE_THREADING_IMPL_POSIX_H_
+
+
+#include <ode/common.h>
+
+
+#if !defined(_WIN32)
+
+
+#include "threading_impl_templates.h"
+#include "threading_fake_sync.h"
+#include "threading_atomics_provs.h"
+
+
+#if dBUILTIN_THREADING_IMPL_ENABLED
+
+#include <pthread.h>
+#include <time.h>
+#include <errno.h>
+
+#if !defined(EOK)
+#define EOK 0
+#endif
+
+
+#if defined(__APPLE__)
+
+#if HAVE_GETTIMEOFDAY
+
+#include <sys/time.h>
+
+#if !defined(CLOCK_MONOTONIC)
+#define CLOCK_MONOTONIC 2
+#endif
+
+static inline
+int _condvar_clock_gettime(int clock_type, timespec *ts)
+{
+ (void)clock_type; // Unused
+ timeval tv;
+ return gettimeofday(&tv, NULL) == 0 ? (ts->tv_sec = tv.tv_sec, ts->tv_nsec = tv.tv_usec * 1000, 0) : (-1);
+}
+
+
+#else // #if !HAVE_GETTIMEOFDAY
+
+#error It is necessary to check manuals for the correct way of getting condvar wait time for this Apple system
+
+
+#endif // #if !HAVE_GETTIMEOFDAY
+
+
+#else // #if !defined(__APPLE__)
+
+#if !HAVE_PTHREAD_CONDATTR_SETCLOCK && !HAVE_NO_PTHREAD_CONDATTR_SETCLOCK
+
+// The code must be compiled without autoconf run, having the project generated by other means.
+// Assume the pthread_condattr_setclock() is available as it is true in most cases and it is the best we can do in such cases.
+#define HAVE_PTHREAD_CONDATTR_SETCLOCK 1
+
+
+#endif // #if !HAVE_PTHREAD_CONDATTR_SETCLOCK && !HAVE_NO_PTHREAD_CONDATTR_SETCLOCK
+
+
+#if HAVE_PTHREAD_CONDATTR_SETCLOCK
+
+static inline
+int _condvar_clock_gettime(int clock_type, timespec *ts)
+{
+ return clock_gettime(clock_type, ts);
+}
+
+
+#else // #if !HAVE_PTHREAD_CONDATTR_SETCLOCK
+
+#error It is necessary to check manuals for the correct way of getting condvar wait time for this system
+
+
+#endif // #if !HAVE_PTHREAD_CONDATTR_SETCLOCK
+
+
+#endif // #if !defined(__APPLE__)
+
+
+/************************************************************************/
+/* dxCondvarWakeup class implementation */
+/************************************************************************/
+
+class dxCondvarWakeup
+{
+public:
+ dxCondvarWakeup(): m_waiters_list(NULL), m_signaled_state(false), m_state_is_permanent(false), m_object_initialized(false) {}
+ ~dxCondvarWakeup() { DoFinalizeObject(); }
+
+ bool InitializeObject() { return DoInitializeObject(); }
+
+private:
+ bool DoInitializeObject();
+ void DoFinalizeObject();
+
+public:
+ void ResetWakeup();
+ void WakeupAThread();
+ void WakeupAllThreads();
+
+ bool WaitWakeup(const dThreadedWaitTime *timeout_time_ptr);
+
+private:
+ bool BlockAsAWaiter(const dThreadedWaitTime *timeout_time_ptr);
+
+private:
+ struct dxWaiterInfo
+ {
+ dxWaiterInfo(): m_signal_state(false) {}
+
+ dxWaiterInfo **m_prev_info_ptr;
+ dxWaiterInfo *m_next_info;
+ bool m_signal_state;
+ };
+
+ void RegisterWaiterInList(dxWaiterInfo *waiter_info);
+ void UnregisterWaiterFromList(dxWaiterInfo *waiter_info);
+
+ bool MarkSignaledFirstWaiter();
+ static bool MarkSignaledFirstWaiterMeaningful(dxWaiterInfo *first_waiter);
+ bool MarkSignaledAllWaiters();
+ static bool MarkSignaledAllWaitersMeaningful(dxWaiterInfo *first_waiter);
+
+private:
+ dxWaiterInfo *m_waiters_list;
+ bool m_signaled_state;
+ bool m_state_is_permanent;
+ bool m_object_initialized;
+ pthread_mutex_t m_wakeup_mutex;
+ pthread_cond_t m_wakeup_cond;
+};
+
+
+bool dxCondvarWakeup::DoInitializeObject()
+{
+ dIASSERT(!m_object_initialized);
+
+ bool init_result = false;
+
+ pthread_condattr_t cond_condattr;
+ bool mutex_initialized = false, condattr_initialized = false;
+
+ do
+ {
+ int mutex_result = pthread_mutex_init(&m_wakeup_mutex, NULL);
+ if (mutex_result != EOK)
+ {
+ errno = mutex_result;
+ break;
+ }
+
+ mutex_initialized = true;
+
+ int condattr_init_result = pthread_condattr_init(&cond_condattr);
+ if (condattr_init_result != EOK)
+ {
+ errno = condattr_init_result;
+ break;
+ }
+
+ condattr_initialized = true;
+
+#if HAVE_PTHREAD_CONDATTR_SETCLOCK
+ int condattr_clock_result = pthread_condattr_setclock(&cond_condattr, CLOCK_MONOTONIC);
+ if (condattr_clock_result != EOK)
+ {
+ errno = condattr_clock_result;
+ break;
+ }
+#endif // #if HAVE_PTHREAD_CONDATTR_SETCLOCK
+
+ int cond_result = pthread_cond_init(&m_wakeup_cond, &cond_condattr);
+ if (cond_result != EOK)
+ {
+ errno = cond_result;
+ break;
+ }
+
+ pthread_condattr_destroy(&cond_condattr); // result can be ignored
+
+ m_object_initialized = true;
+ init_result = true;
+ }
+ while (false);
+
+ if (!init_result)
+ {
+ if (mutex_initialized)
+ {
+ if (condattr_initialized)
+ {
+ int condattr_destroy_result = pthread_condattr_destroy(&cond_condattr);
+ dICHECK(condattr_destroy_result == EOK || ((errno = condattr_destroy_result), false));
+ }
+
+ int mutex_destroy_result = pthread_mutex_destroy(&m_wakeup_mutex);
+ dICHECK(mutex_destroy_result == EOK || ((errno = mutex_destroy_result), false));
+ }
+ }
+
+ return init_result;
+
+}
+
+void dxCondvarWakeup::DoFinalizeObject()
+{
+ if (m_object_initialized)
+ {
+ int cond_result = pthread_cond_destroy(&m_wakeup_cond);
+ dICHECK(cond_result == EOK || ((errno = cond_result), false));
+
+ int mutex_result = pthread_mutex_destroy(&m_wakeup_mutex);
+ dICHECK(mutex_result == EOK || ((errno = mutex_result), false));
+
+ m_object_initialized = false;
+ }
+}
+
+
+void dxCondvarWakeup::ResetWakeup()
+{
+ int lock_result = pthread_mutex_lock(&m_wakeup_mutex);
+ dICHECK(lock_result == EOK || ((errno = lock_result), false));
+
+ m_signaled_state = false;
+ m_state_is_permanent = false;
+
+ int unlock_result = pthread_mutex_unlock(&m_wakeup_mutex);
+ dICHECK(unlock_result == EOK || ((errno = unlock_result), false));
+}
+
+void dxCondvarWakeup::WakeupAThread()
+{
+ int lock_result = pthread_mutex_lock(&m_wakeup_mutex);
+ dICHECK(lock_result == EOK || ((errno = lock_result), false));
+
+ dIASSERT(!m_state_is_permanent); // Wakeup should not be used after permanent signal
+
+ if (!m_signaled_state)
+ {
+ if (MarkSignaledFirstWaiter())
+ {
+ // All threads must be woken up regardless to the fact that only one waiter is marked.
+ // It is not possible to wake up a chosen thread personally
+ // and if a random thread is woken up it can't know if there was a condition signal for it
+ // or the sleep was interrupted by POSIX signal.
+ // On the other hand, without this it is not possible to guarantee that a thread
+ // will be woken up per each WakeupAThread() call if there is more than one waiter
+ // and wakeup requests will not accumulate if there are no waiters.
+ int broadcast_result = pthread_cond_broadcast(&m_wakeup_cond);
+ dICHECK(broadcast_result == EOK || ((errno = broadcast_result), false));
+ }
+ else
+ {
+ m_signaled_state = true;
+ }
+ }
+
+ int unlock_result = pthread_mutex_unlock(&m_wakeup_mutex);
+ dICHECK(unlock_result == EOK || ((errno = unlock_result), false));
+}
+
+void dxCondvarWakeup::WakeupAllThreads()
+{
+ int lock_result = pthread_mutex_lock(&m_wakeup_mutex);
+ dICHECK(lock_result == EOK || ((errno = lock_result), false));
+
+ m_state_is_permanent = true;
+
+ if (!m_signaled_state)
+ {
+ m_signaled_state = true;
+
+ if (MarkSignaledAllWaiters())
+ {
+ int broadcast_result = pthread_cond_broadcast(&m_wakeup_cond);
+ dICHECK(broadcast_result == EOK || ((errno = broadcast_result), false));
+ }
+ }
+
+ int unlock_result = pthread_mutex_unlock(&m_wakeup_mutex);
+ dICHECK(unlock_result == EOK || ((errno = unlock_result), false));
+}
+
+
+bool dxCondvarWakeup::WaitWakeup(const dThreadedWaitTime *timeout_time_ptr)
+{
+ bool wait_result;
+
+ int lock_result = pthread_mutex_lock(&m_wakeup_mutex);
+ dICHECK(lock_result == EOK || ((errno = lock_result), false));
+
+ if (!m_signaled_state)
+ {
+ if (!timeout_time_ptr || timeout_time_ptr->wait_nsec != 0 || timeout_time_ptr->wait_sec != 0)
+ {
+ wait_result = BlockAsAWaiter(timeout_time_ptr);
+ }
+ else
+ {
+ wait_result = false;
+ }
+ }
+ else
+ {
+ m_signaled_state = m_state_is_permanent;
+ wait_result = true;
+ }
+
+ int unlock_result = pthread_mutex_unlock(&m_wakeup_mutex);
+ dICHECK(unlock_result == EOK || ((errno = unlock_result), false));
+
+ return wait_result;
+}
+
+bool dxCondvarWakeup::BlockAsAWaiter(const dThreadedWaitTime *timeout_time_ptr)
+{
+ bool wait_result = false;
+
+ dxWaiterInfo waiter_info;
+ RegisterWaiterInList(&waiter_info);
+
+ timespec wakeup_time;
+
+ if (timeout_time_ptr != NULL)
+ {
+ timespec current_time;
+
+ int clock_result = _condvar_clock_gettime(CLOCK_MONOTONIC, ¤t_time);
+ dICHECK(clock_result != -1);
+
+ time_t wakeup_sec = current_time.tv_sec + timeout_time_ptr->wait_sec;
+ unsigned long wakeup_nsec = current_time.tv_nsec + timeout_time_ptr->wait_nsec;
+
+ if (wakeup_nsec >= 1000000000)
+ {
+ wakeup_nsec -= 1000000000;
+ wakeup_sec += 1;
+ }
+
+ wakeup_time.tv_sec = wakeup_sec;
+ wakeup_time.tv_nsec = wakeup_nsec;
+ }
+
+ while (true)
+ {
+ int cond_result = (timeout_time_ptr != NULL)
+ ? pthread_cond_timedwait(&m_wakeup_cond, &m_wakeup_mutex, &wakeup_time)
+ : pthread_cond_wait(&m_wakeup_cond, &m_wakeup_mutex);
+ dICHECK(cond_result == EOK || cond_result == ETIMEDOUT || ((errno = cond_result), false));
+
+ if (waiter_info.m_signal_state)
+ {
+ wait_result = true;
+ break;
+ }
+
+ if (cond_result == ETIMEDOUT)
+ {
+ dIASSERT(timeout_time_ptr != NULL);
+ break;
+ }
+ }
+
+ UnregisterWaiterFromList(&waiter_info);
+
+ return wait_result;
+}
+
+
+void dxCondvarWakeup::RegisterWaiterInList(dxWaiterInfo *waiter_info)
+{
+ dxWaiterInfo *const first_waiter = m_waiters_list;
+
+ if (first_waiter == NULL)
+ {
+ waiter_info->m_next_info = waiter_info;
+ waiter_info->m_prev_info_ptr = &waiter_info->m_next_info;
+ m_waiters_list = waiter_info;
+ }
+ else
+ {
+ waiter_info->m_next_info = first_waiter;
+ waiter_info->m_prev_info_ptr = first_waiter->m_prev_info_ptr;
+ *first_waiter->m_prev_info_ptr = waiter_info;
+ first_waiter->m_prev_info_ptr = &waiter_info->m_next_info;
+ }
+}
+
+void dxCondvarWakeup::UnregisterWaiterFromList(dxWaiterInfo *waiter_info)
+{
+ dxWaiterInfo *next_info = waiter_info->m_next_info;
+
+ if (next_info == waiter_info)
+ {
+ m_waiters_list = NULL;
+ }
+ else
+ {
+ next_info->m_prev_info_ptr = waiter_info->m_prev_info_ptr;
+ *waiter_info->m_prev_info_ptr = next_info;
+
+ if (waiter_info == m_waiters_list)
+ {
+ m_waiters_list = next_info;
+ }
+ }
+}
+
+
+bool dxCondvarWakeup::MarkSignaledFirstWaiter()
+{
+ bool waiter_found = false;
+
+ dxWaiterInfo *const first_waiter = m_waiters_list;
+
+ if (first_waiter)
+ {
+ waiter_found = MarkSignaledFirstWaiterMeaningful(first_waiter);
+ }
+
+ return waiter_found;
+}
+
+bool dxCondvarWakeup::MarkSignaledFirstWaiterMeaningful(dxWaiterInfo *first_waiter)
+{
+ bool waiter_found = false;
+
+ dxWaiterInfo *current_waiter = first_waiter;
+
+ while (true)
+ {
+ if (!current_waiter->m_signal_state)
+ {
+ current_waiter->m_signal_state = true;
+ waiter_found = true;
+ break;
+ }
+
+ current_waiter = current_waiter->m_next_info;
+ if (current_waiter == first_waiter)
+ {
+ break;
+ }
+ }
+
+ return waiter_found;
+}
+
+bool dxCondvarWakeup::MarkSignaledAllWaiters()
+{
+ bool waiter_found = false;
+
+ dxWaiterInfo *const first_waiter = m_waiters_list;
+
+ if (first_waiter)
+ {
+ waiter_found = MarkSignaledAllWaitersMeaningful(first_waiter);
+ }
+
+ return waiter_found;
+}
+
+bool dxCondvarWakeup::MarkSignaledAllWaitersMeaningful(dxWaiterInfo *first_waiter)
+{
+ bool waiter_found = false;
+
+ dxWaiterInfo *current_waiter = first_waiter;
+
+ while (true)
+ {
+ if (!current_waiter->m_signal_state)
+ {
+ current_waiter->m_signal_state = true;
+ waiter_found = true;
+ }
+
+ current_waiter = current_waiter->m_next_info;
+ if (current_waiter == first_waiter)
+ {
+ break;
+ }
+ }
+
+ return waiter_found;
+}
+
+
+/************************************************************************/
+/* dxMutexMutex class implementation */
+/************************************************************************/
+
+class dxMutexMutex
+{
+public:
+ dxMutexMutex(): m_mutex_allocated(false) {}
+ ~dxMutexMutex() { DoFinalizeObject(); }
+
+ bool InitializeObject() { return DoInitializeObject(); }
+
+private:
+ bool DoInitializeObject();
+ void DoFinalizeObject();
+
+public:
+ void LockMutex();
+ bool TryLockMutex();
+ void UnlockMutex();
+
+private:
+ pthread_mutex_t m_mutex_instance;
+ bool m_mutex_allocated;
+};
+
+
+bool dxMutexMutex::DoInitializeObject()
+{
+ dIASSERT(!m_mutex_allocated);
+
+ bool init_result = false;
+
+ do
+ {
+ int mutex_result = pthread_mutex_init(&m_mutex_instance, NULL);
+ if (mutex_result != EOK)
+ {
+ errno = mutex_result;
+ break;
+ }
+
+ m_mutex_allocated = true;
+ init_result = true;
+ }
+ while (false);
+
+ return init_result;
+}
+
+void dxMutexMutex::DoFinalizeObject()
+{
+ if (m_mutex_allocated)
+ {
+ int mutex_result = pthread_mutex_destroy(&m_mutex_instance);
+ dICHECK(mutex_result == EOK || ((errno = mutex_result), false));
+
+ m_mutex_allocated = false;
+ }
+}
+
+
+void dxMutexMutex::LockMutex()
+{
+ int lock_result = pthread_mutex_lock(&m_mutex_instance);
+ dICHECK(lock_result == EOK || ((errno = lock_result), false));
+}
+
+bool dxMutexMutex::TryLockMutex()
+{
+ int trylock_result = pthread_mutex_trylock(&m_mutex_instance);
+ dICHECK(trylock_result == EOK || trylock_result == EBUSY || ((errno = trylock_result), false));
+
+ return trylock_result == EOK;
+}
+
+void dxMutexMutex::UnlockMutex()
+{
+ int unlock_result = pthread_mutex_unlock(&m_mutex_instance);
+ dICHECK(unlock_result == EOK || ((errno = unlock_result), false));
+}
+
+
+#endif // #if dBUILTIN_THREADING_IMPL_ENABLED
+
+
+/************************************************************************/
+/* Self-threaded job list definition */
+/************************************************************************/
+
+typedef dxtemplateJobListContainer<dxFakeLull, dxFakeMutex, dxFakeAtomicsProvider> dxSelfThreadedJobListContainer;
+typedef dxtemplateJobListSelfHandler<dxSelfWakeup, dxSelfThreadedJobListContainer> dxSelfThreadedJobListHandler;
+typedef dxtemplateThreadingImplementation<dxSelfThreadedJobListContainer, dxSelfThreadedJobListHandler> dxSelfThreadedThreading;
+
+
+#if dBUILTIN_THREADING_IMPL_ENABLED
+
+/************************************************************************/
+/* Multi-threaded job list definition */
+/************************************************************************/
+
+typedef dxtemplateJobListContainer<dxtemplateThreadedLull<dxCondvarWakeup, dxOUAtomicsProvider, false>, dxMutexMutex, dxOUAtomicsProvider> dxMultiThreadedJobListContainer;
+typedef dxtemplateJobListThreadedHandler<dxCondvarWakeup, dxMultiThreadedJobListContainer> dxMultiThreadedJobListHandler;
+typedef dxtemplateThreadingImplementation<dxMultiThreadedJobListContainer, dxMultiThreadedJobListHandler> dxMultiThreadedThreading;
+
+
+#endif // #if dBUILTIN_THREADING_IMPL_ENABLED
+
+
+#endif // #if !defined(_WIN32)
+
+
+#endif // #ifndef _ODE_THREADING_IMPL_POSIX_H_
diff --git a/libs/ode-0.16.1/ode/src/threading_impl_templates.h b/libs/ode-0.16.1/ode/src/threading_impl_templates.h new file mode 100644 index 0000000..acecbc3 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_impl_templates.h @@ -0,0 +1,1265 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading implementation templates file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Job list and Mutex group implementation templates for built-in threading + * support provider. + */ + + +#ifndef _ODE_THREADING_IMPL_TEMPLATES_H_ +#define _ODE_THREADING_IMPL_TEMPLATES_H_ + + +#include <ode/common.h> +#include <ode/memory.h> + +#include <ode/threading.h> + +#include "objects.h" + +#include <new> + + +#define dMAKE_JOBINSTANCE_RELEASEE(job_instance) ((dCallReleaseeID)(job_instance)) +#define dMAKE_RELEASEE_JOBINSTANCE(releasee) ((dxThreadedJobInfo *)(releasee)) + + +template <class tThreadMutex> +class dxtemplateMutexGroup +{ +private: + dxtemplateMutexGroup() {} + ~dxtemplateMutexGroup() {} + +public: + static dxtemplateMutexGroup<tThreadMutex> *AllocateInstance(dmutexindex_t Mutex_count); + static void FreeInstance(dxtemplateMutexGroup<tThreadMutex> *mutex_group); + +private: + bool InitializeMutexArray(dmutexindex_t Mutex_count); + void FinalizeMutexArray(dmutexindex_t Mutex_count); + +public: + void LockMutex(dmutexindex_t mutex_index) { dIASSERT(mutex_index < m_un.m_mutex_count); m_Mutex_array[mutex_index].LockMutex(); } + bool TryLockMutex(dmutexindex_t mutex_index) { dIASSERT(mutex_index < m_un.m_mutex_count); return m_Mutex_array[mutex_index].TryLockMutex(); } + void UnlockMutex(dmutexindex_t mutex_index) { dIASSERT(mutex_index < m_un.m_mutex_count); m_Mutex_array[mutex_index].UnlockMutex(); } + +private: + union + { + dmutexindex_t m_mutex_count; + unsigned long m_reserved_for_allignment[2]; + + } m_un; + + tThreadMutex m_Mutex_array[1]; +}; + +template<class tThreadWakeup> +class dxtemplateCallWait: + public dBase +{ +public: + dxtemplateCallWait() {} + ~dxtemplateCallWait() { DoFinalizeObject(); } + + bool InitializeObject() { return DoInitializeObject(); } + +private: + bool DoInitializeObject() { return m_wait_wakeup.InitializeObject(); } + void DoFinalizeObject() { /* Do nothing */ } + +public: + typedef dxtemplateCallWait<tThreadWakeup> dxCallWait; + +public: + void ResetTheWait() { m_wait_wakeup.ResetWakeup(); } + void SignalTheWait() { m_wait_wakeup.WakeupAllThreads(); } + bool PerformWaiting(const dThreadedWaitTime *timeout_time_ptr/*=NULL*/) { return m_wait_wakeup.WaitWakeup(timeout_time_ptr); } + +public: + static void AbstractSignalTheWait(void *wait_wakeup_ptr) { ((dxCallWait *)wait_wakeup_ptr)->SignalTheWait(); } + +private: + tThreadWakeup m_wait_wakeup; +}; + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +template<class tThreadWakeup, class tAtomicsProvider, const bool tatomic_test_required> +class dxtemplateThreadedLull +{ +public: + dxtemplateThreadedLull(): m_registrant_count(0), m_alarm_wakeup() {} + ~dxtemplateThreadedLull() { dIASSERT(m_registrant_count == 0); DoFinalizeObject(); } + + bool InitializeObject() { return DoInitializeObject(); } + +private: + bool DoInitializeObject() { return m_alarm_wakeup.InitializeObject(); } + void DoFinalizeObject() { /* Do nothing */ } + +private: + typedef typename tAtomicsProvider::atomicord_t atomicord_t; + +public: + void RegisterToLull() { tAtomicsProvider::IncrementTargetNoRet(&m_registrant_count); } + void WaitForLullAlarm() { dIASSERT(m_registrant_count != 0); m_alarm_wakeup.WaitWakeup(NULL); } + void UnregisterFromLull() { tAtomicsProvider::DecrementTargetNoRet(&m_registrant_count); } + + void SignalLullAlarmIfAnyRegistrants() + { + if (tatomic_test_required ? (tAtomicsProvider::QueryTargetValue(&m_registrant_count) != 0) : (m_registrant_count != 0)) + { + m_alarm_wakeup.WakeupAThread(); + } + } + +private: + atomicord_t m_registrant_count; + tThreadWakeup m_alarm_wakeup; +}; + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +struct dxThreadedJobInfo: + public dBase +{ + dxThreadedJobInfo() {} + explicit dxThreadedJobInfo(void *): m_next_job(NULL) {} + + void AssignJobData(ddependencycount_t dependencies_count, dxThreadedJobInfo *dependent_job, void *call_wait, + int *fault_accumulator_ptr, dThreadedCallFunction *call_function, void *call_context, dcallindex_t call_index) + { + m_dependencies_count = dependencies_count; + m_dependent_job = dependent_job; + m_call_wait = call_wait; + m_fault_accumulator_ptr = fault_accumulator_ptr; + + m_call_fault = 0; + m_call_function = call_function; + m_call_context = call_context; + m_call_index = call_index; + } + + bool InvokeCallFunction() + { + int call_result = m_call_function(m_call_context, m_call_index, dMAKE_JOBINSTANCE_RELEASEE(this)); + return call_result != 0; + } + + dxThreadedJobInfo *m_next_job; + dxThreadedJobInfo **m_prev_job_next_ptr; + + ddependencycount_t m_dependencies_count; + dxThreadedJobInfo *m_dependent_job; + void *m_call_wait; + int *m_fault_accumulator_ptr; + + int m_call_fault; + dThreadedCallFunction *m_call_function; + void *m_call_context; + dcallindex_t m_call_index; +}; + + +template<class tThreadMutex> +class dxtemplateThreadingLockHelper +{ +public: + dxtemplateThreadingLockHelper(tThreadMutex &mutex_instance): m_mutex_instance(mutex_instance), m_lock_indicator_flag(false) { LockMutex(); } + ~dxtemplateThreadingLockHelper() { if (m_lock_indicator_flag) { UnlockMutex(); } } + + void LockMutex() { dIASSERT(!m_lock_indicator_flag); m_mutex_instance.LockMutex(); m_lock_indicator_flag = true; } + void UnlockMutex() { dIASSERT(m_lock_indicator_flag); m_mutex_instance.UnlockMutex(); m_lock_indicator_flag = false; } + +private: + tThreadMutex &m_mutex_instance; + bool m_lock_indicator_flag; +}; + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +class dxtemplateJobListContainer +{ +public: + dxtemplateJobListContainer(): + m_job_list(NULL), + m_info_pool((atomicptr_t)NULL), + m_pool_access_lock(), + m_list_access_lock(), + m_info_wait_lull(), + m_info_count_known_to_be_preallocated(0) + { + } + + ~dxtemplateJobListContainer() + { + dIASSERT(m_job_list == NULL); // Would not it be nice to wait for jobs to complete before deleting the list? + + FreeJobInfoPoolInfos(); + DoFinalizeObject(); + } + + bool InitializeObject() { return DoInitializeObject(); } + +private: + bool DoInitializeObject() { return m_pool_access_lock.InitializeObject() && m_list_access_lock.InitializeObject() && m_info_wait_lull.InitializeObject(); } + void DoFinalizeObject() { /* Do nothing */ } + +public: + typedef tAtomicsProvider dxAtomicsProvider; + typedef typename tAtomicsProvider::atomicord_t atomicord_t; + typedef typename tAtomicsProvider::atomicptr_t atomicptr_t; + typedef tThreadMutex dxThreadMutex; + typedef dxtemplateThreadingLockHelper<tThreadMutex> dxMutexLockHelper; + typedef void dWaitSignallingFunction(void *job_call_wait); + +public: + dxThreadedJobInfo *ReleaseAJobAndPickNextPendingOne( + dxThreadedJobInfo *job_to_release, bool job_result, dWaitSignallingFunction *wait_signal_proc_ptr, + bool &out_last_job_flag); + +private: + dxThreadedJobInfo *PickNextPendingJob(bool &out_last_job_flag); + void ReleaseAJob(dxThreadedJobInfo *job_instance, bool job_result, dWaitSignallingFunction *wait_signal_proc_ptr); + +public: + inline dxThreadedJobInfo *AllocateJobInfoFromPool(); + void QueueJobForProcessing(dxThreadedJobInfo *job_instance); + + void AlterJobProcessingDependencies(dxThreadedJobInfo *job_instance, ddependencychange_t dependencies_count_change, + bool &out_job_has_become_ready); + +private: + inline ddependencycount_t SmartAddJobDependenciesCount(dxThreadedJobInfo *job_instance, ddependencychange_t dependencies_count_change); + + inline void InsertJobInfoIntoListHead(dxThreadedJobInfo *job_instance); + inline void RemoveJobInfoFromList(dxThreadedJobInfo *job_instance); + + dxThreadedJobInfo *ExtractJobInfoFromPoolOrAllocate(); + inline void ReleaseJobInfoIntoPool(dxThreadedJobInfo *job_instance); + +private: + void FreeJobInfoPoolInfos(); + +public: + bool EnsureNumberOfJobInfosIsPreallocated(ddependencycount_t required_info_count); + +private: + bool DoPreallocateJobInfos(ddependencycount_t required_info_count); + +public: + bool IsJobListReadyForShutdown() const { return m_job_list == NULL; } + +private: + dxThreadedJobInfo *m_job_list; + volatile atomicptr_t m_info_pool; // dxThreadedJobInfo * + tThreadMutex m_pool_access_lock; + tThreadMutex m_list_access_lock; + tThreadLull m_info_wait_lull; + ddependencycount_t m_info_count_known_to_be_preallocated; +}; + + +typedef void (dxThreadReadyToServeCallback)(void *callback_context); + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +template<class tThreadWakeup, class tJobListContainer> +class dxtemplateJobListThreadedHandler +{ +public: + dxtemplateJobListThreadedHandler(tJobListContainer *list_container_ptr): + m_job_list_ptr(list_container_ptr), + m_processing_wakeup(), + m_active_thread_count(0), + m_shutdown_requested(0) + { + } + + ~dxtemplateJobListThreadedHandler() + { + dIASSERT(m_active_thread_count == 0); + + DoFinalizeObject(); + } + + bool InitializeObject() { return DoInitializeObject(); } + +private: + bool DoInitializeObject() { return m_processing_wakeup.InitializeObject(); } + void DoFinalizeObject() { /* Do nothing */ } + +public: + typedef dxtemplateCallWait<tThreadWakeup> dxCallWait; + +public: + inline void ProcessActiveJobAddition(); + inline void PrepareForWaitingAJobCompletion(); + +public: + inline unsigned RetrieveActiveThreadsCount(); + inline void StickToJobsProcessing(dxThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/); + +private: + void PerformJobProcessingUntilShutdown(); + void PerformJobProcessingSession(); + + void BlockAsIdleThread(); + void ActivateAnIdleThread(); + +public: + inline void ShutdownProcessing(); + inline void CleanupForRestart(); + +private: + bool IsShutdownRequested() const { return m_shutdown_requested != 0; } + +private: + typedef typename tJobListContainer::dxAtomicsProvider dxAtomicsProvider; + typedef typename tJobListContainer::atomicord_t atomicord_t; + + atomicord_t GetActiveThreadsCount() const { return m_active_thread_count; } + void RegisterAsActiveThread() { dxAtomicsProvider::template AddValueToTarget<sizeof(atomicord_t)>((volatile void *)&m_active_thread_count, 1); } + void UnregisterAsActiveThread() { dxAtomicsProvider::template AddValueToTarget<sizeof(atomicord_t)>((volatile void *)&m_active_thread_count, -1); } + +private: + tJobListContainer *m_job_list_ptr; + tThreadWakeup m_processing_wakeup; + volatile atomicord_t m_active_thread_count; + int m_shutdown_requested; +}; + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +template<class tThreadWakeup, class tJobListContainer> +class dxtemplateJobListSelfHandler +{ +public: + dxtemplateJobListSelfHandler(tJobListContainer *list_container_ptr): + m_job_list_ptr(list_container_ptr) + { + } + + ~dxtemplateJobListSelfHandler() + { + // Do nothing + } + + bool InitializeObject() { return true; } + +public: + typedef dxtemplateCallWait<tThreadWakeup> dxCallWait; + +public: + inline void ProcessActiveJobAddition(); + inline void PrepareForWaitingAJobCompletion(); + +public: + inline unsigned RetrieveActiveThreadsCount(); + inline void StickToJobsProcessing(dxThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/); + +private: + void PerformJobProcessingUntilExhaustion(); + void PerformJobProcessingSession(); + +public: + inline void ShutdownProcessing(); + inline void CleanupForRestart(); + +private: + tJobListContainer *m_job_list_ptr; +}; + + +struct dIMutexGroup; +struct dxICallWait; + +class dxIThreadingImplementation +{ +public: + virtual void FreeInstance() = 0; + +public: + virtual dIMutexGroup *AllocMutexGroup(dmutexindex_t Mutex_count) = 0; + virtual void FreeMutexGroup(dIMutexGroup *mutex_group) = 0; + virtual void LockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index) = 0; + // virtual bool TryLockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index) = 0; + virtual void UnlockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index) = 0; + +public: + virtual dxICallWait *AllocACallWait() = 0; + virtual void ResetACallWait(dxICallWait *call_wait) = 0; + virtual void FreeACallWait(dxICallWait *call_wait) = 0; + +public: + virtual bool PreallocateJobInfos(ddependencycount_t max_simultaneous_calls_estimate) = 0; + virtual void ScheduleNewJob(int *fault_accumulator_ptr/*=NULL*/, + dCallReleaseeID *out_post_releasee_ptr/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dxICallWait *call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index) = 0; + virtual void AlterJobDependenciesCount(dCallReleaseeID target_releasee, ddependencychange_t dependencies_count_change) = 0; + virtual void WaitJobCompletion(int *out_wait_status_ptr/*=NULL*/, + dxICallWait *call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/) = 0; + +public: + virtual unsigned RetrieveActiveThreadsCount() = 0; + virtual void StickToJobsProcessing(dxThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/) = 0; + virtual void ShutdownProcessing() = 0; + virtual void CleanupForRestart() = 0; +}; + + +template<class tJobListContainer, class tJobListHandler> +class dxtemplateThreadingImplementation: + public dBase, + public dxIThreadingImplementation +{ +public: + dxtemplateThreadingImplementation(): + dBase(), + m_list_container(), + m_list_handler(&m_list_container) + { + } + + virtual ~dxtemplateThreadingImplementation() + { + DoFinalizeObject(); + } + + bool InitializeObject() { return DoInitializeObject(); } + +private: + bool DoInitializeObject() { return m_list_container.InitializeObject() && m_list_handler.InitializeObject(); } + void DoFinalizeObject() { /* Do nothing */ } + +protected: + virtual void FreeInstance(); + +private: + typedef dxtemplateMutexGroup<typename tJobListContainer::dxThreadMutex> dxMutexGroup; + typedef typename tJobListHandler::dxCallWait dxCallWait; + +protected: + virtual dIMutexGroup *AllocMutexGroup(dmutexindex_t Mutex_count); + virtual void FreeMutexGroup(dIMutexGroup *mutex_group); + virtual void LockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index); + // virtual bool TryLockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index); + virtual void UnlockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index); + +protected: + virtual dxICallWait *AllocACallWait(); + virtual void ResetACallWait(dxICallWait *call_wait); + virtual void FreeACallWait(dxICallWait *call_wait); + +protected: + virtual bool PreallocateJobInfos(ddependencycount_t max_simultaneous_calls_estimate); + virtual void ScheduleNewJob(int *fault_accumulator_ptr/*=NULL*/, + dCallReleaseeID *out_post_releasee_ptr/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dxICallWait *call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index); + virtual void AlterJobDependenciesCount(dCallReleaseeID target_releasee, ddependencychange_t dependencies_count_change); + virtual void WaitJobCompletion(int *out_wait_status_ptr/*=NULL*/, + dxICallWait *call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/); + +protected: + virtual unsigned RetrieveActiveThreadsCount(); + virtual void StickToJobsProcessing(dxThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/); + virtual void ShutdownProcessing(); + virtual void CleanupForRestart(); + +private: + tJobListContainer m_list_container; + tJobListHandler m_list_handler; +}; + + +/************************************************************************/ +/* Implementation of dxtemplateMutexGroup */ +/************************************************************************/ + +template<class tThreadMutex> +/*static */dxtemplateMutexGroup<tThreadMutex> *dxtemplateMutexGroup<tThreadMutex>::AllocateInstance(dmutexindex_t Mutex_count) +{ + dAASSERT(Mutex_count != 0); + + const dxtemplateMutexGroup<tThreadMutex> *const dummy_group = (dxtemplateMutexGroup<tThreadMutex> *)(sizeint)8; + const sizeint size_requited = ((sizeint)(&dummy_group->m_Mutex_array) - (sizeint)dummy_group) + Mutex_count * sizeof(tThreadMutex); + dxtemplateMutexGroup<tThreadMutex> *mutex_group = (dxtemplateMutexGroup<tThreadMutex> *)dAlloc(size_requited); + + if (mutex_group != NULL) + { + mutex_group->m_un.m_mutex_count = Mutex_count; + + if (!mutex_group->InitializeMutexArray(Mutex_count)) + { + dFree((void *)mutex_group, size_requited); + mutex_group = NULL; + } + } + + return mutex_group; +} + +template<class tThreadMutex> +/*static */void dxtemplateMutexGroup<tThreadMutex>::FreeInstance(dxtemplateMutexGroup<tThreadMutex> *mutex_group) +{ + if (mutex_group != NULL) + { + dmutexindex_t Mutex_count = mutex_group->m_un.m_mutex_count; + mutex_group->FinalizeMutexArray(Mutex_count); + + const sizeint anyting_not_zero = 2 * sizeof(sizeint); + const dxtemplateMutexGroup<tThreadMutex> *const dummy_group = (dxtemplateMutexGroup<tThreadMutex> *)anyting_not_zero; + const sizeint size_requited = ((sizeint)(&dummy_group->m_Mutex_array) - (sizeint)dummy_group) + Mutex_count * sizeof(tThreadMutex); + dFree((void *)mutex_group, size_requited); + } +} + +template<class tThreadMutex> +bool dxtemplateMutexGroup<tThreadMutex>::InitializeMutexArray(dmutexindex_t Mutex_count) +{ + bool any_fault = false; + + dmutexindex_t mutex_index = 0; + for (; mutex_index != Mutex_count; ++mutex_index) + { + tThreadMutex *mutex_storage = m_Mutex_array + mutex_index; + + new(mutex_storage) tThreadMutex; + + if (!mutex_storage->InitializeObject()) + { + mutex_storage->tThreadMutex::~tThreadMutex(); + + any_fault = true; + break; + } + } + + if (any_fault) + { + FinalizeMutexArray(mutex_index); + } + + bool init_result = !any_fault; + return init_result; +} + +template<class tThreadMutex> +void dxtemplateMutexGroup<tThreadMutex>::FinalizeMutexArray(dmutexindex_t Mutex_count) +{ + for (dmutexindex_t mutex_index = 0; mutex_index != Mutex_count; ++mutex_index) + { + tThreadMutex *mutex_storage = m_Mutex_array + mutex_index; + + mutex_storage->tThreadMutex::~tThreadMutex(); + } +} + +/************************************************************************/ +/* Implementation of dxtemplateJobListContainer */ +/************************************************************************/ + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +dxThreadedJobInfo *dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::ReleaseAJobAndPickNextPendingOne( + dxThreadedJobInfo *job_to_release, bool job_result, dWaitSignallingFunction *wait_signal_proc_ptr, bool &out_last_job_flag) +{ + if (job_to_release != NULL) + { + ReleaseAJob(job_to_release, job_result, wait_signal_proc_ptr); + } + + dxMutexLockHelper list_access(m_list_access_lock); + + dxThreadedJobInfo *picked_job = PickNextPendingJob(out_last_job_flag); + return picked_job; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +dxThreadedJobInfo *dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::PickNextPendingJob( + bool &out_last_job_flag) +{ + dxThreadedJobInfo *current_job = m_job_list; + bool last_job_flag = false; + + while (current_job != NULL) + { + if (current_job->m_dependencies_count == 0) + { + // It is OK to assign in unsafe manner - dependencies count should not be changed + // after the job has become ready for execution + current_job->m_dependencies_count = 1; + last_job_flag = current_job->m_next_job == NULL; + + RemoveJobInfoFromList(current_job); + break; + } + + current_job = current_job->m_next_job; + } + + out_last_job_flag = last_job_flag; + return current_job; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +void dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::ReleaseAJob( + dxThreadedJobInfo *job_instance, bool job_result, dWaitSignallingFunction *wait_signal_proc_ptr) +{ + dxThreadedJobInfo *current_job = job_instance; + + if (!job_result) + { + // Accumulate call fault (be careful to not reset it!!!) + current_job->m_call_fault = 1; + } + + bool job_dequeued = true; + dIASSERT(current_job->m_prev_job_next_ptr == NULL); + + while (true) + { + dIASSERT(current_job->m_dependencies_count != 0); + + ddependencycount_t new_dependencies_count = SmartAddJobDependenciesCount(current_job, -1); + + if (new_dependencies_count != 0 || !job_dequeued) + { + break; + } + + void *job_call_wait = current_job->m_call_wait; + + if (job_call_wait != NULL) + { + wait_signal_proc_ptr(job_call_wait); + } + + int call_fault = current_job->m_call_fault; + + if (current_job->m_fault_accumulator_ptr) + { + *current_job->m_fault_accumulator_ptr = call_fault; + } + + dxThreadedJobInfo *dependent_job = current_job->m_dependent_job; + ReleaseJobInfoIntoPool(current_job); + + if (dependent_job == NULL) + { + break; + } + + if (call_fault) + { + // Accumulate call fault (be careful to not reset it!!!) + dependent_job->m_call_fault = 1; + } + + current_job = dependent_job; + job_dequeued = dependent_job->m_prev_job_next_ptr == NULL; + } +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +dxThreadedJobInfo *dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::AllocateJobInfoFromPool() +{ + // No locking is necessary + dxThreadedJobInfo *job_instance = ExtractJobInfoFromPoolOrAllocate(); + return job_instance; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +void dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::QueueJobForProcessing(dxThreadedJobInfo *job_instance) +{ + dxMutexLockHelper list_access(m_list_access_lock); + + InsertJobInfoIntoListHead(job_instance); +} + + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +void dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::AlterJobProcessingDependencies(dxThreadedJobInfo *job_instance, ddependencychange_t dependencies_count_change, + bool &out_job_has_become_ready) +{ + // Dependencies should not be changed when job has already become ready for execution + dIASSERT(job_instance->m_dependencies_count != 0); + // It's OK that access is not atomic - that is to be handled by external logic + dIASSERT(dependencies_count_change < 0 ? (job_instance->m_dependencies_count >= (ddependencycount_t)(-dependencies_count_change)) : ((ddependencycount_t)(-(ddependencychange_t)job_instance->m_dependencies_count) > (ddependencycount_t)dependencies_count_change)); + + ddependencycount_t new_dependencies_count = SmartAddJobDependenciesCount(job_instance, dependencies_count_change); + out_job_has_become_ready = new_dependencies_count == 0; +} + + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +ddependencycount_t dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::SmartAddJobDependenciesCount( + dxThreadedJobInfo *job_instance, ddependencychange_t dependencies_count_change) +{ + ddependencycount_t new_dependencies_count = tAtomicsProvider::template AddValueToTarget<sizeof(ddependencycount_t)>((volatile void *)&job_instance->m_dependencies_count, dependencies_count_change) + dependencies_count_change; + return new_dependencies_count; +} + + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +void dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::InsertJobInfoIntoListHead( + dxThreadedJobInfo *job_instance) +{ + dxThreadedJobInfo *job_list_head = m_job_list; + job_instance->m_next_job = job_list_head; + + if (job_list_head != NULL) + { + job_list_head->m_prev_job_next_ptr = &job_instance->m_next_job; + } + + job_instance->m_prev_job_next_ptr = &m_job_list; + m_job_list = job_instance; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +void dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::RemoveJobInfoFromList( + dxThreadedJobInfo *job_instance) +{ + if (job_instance->m_next_job) + { + job_instance->m_next_job->m_prev_job_next_ptr = job_instance->m_prev_job_next_ptr; + } + + *job_instance->m_prev_job_next_ptr = job_instance->m_next_job; + // Assign NULL to m_prev_job_next_ptr as an indicator that instance has been dequeued + job_instance->m_prev_job_next_ptr = NULL; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +dxThreadedJobInfo *dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::ExtractJobInfoFromPoolOrAllocate() +{ + dxThreadedJobInfo *result_info; + + bool waited_lull = false; + m_info_wait_lull.RegisterToLull(); + + while (true) + { + dxThreadedJobInfo *raw_head_info = (dxThreadedJobInfo *)m_info_pool; + + if (raw_head_info == NULL) + { + result_info = new dxThreadedJobInfo(); + + if (result_info != NULL) + { + break; + } + + m_info_wait_lull.WaitForLullAlarm(); + waited_lull = true; + } + + // Extraction must be locked so that other thread does not "steal" head info, + // use it and then reinsert back with a different "next" + dxMutexLockHelper pool_access(m_pool_access_lock); + + dxThreadedJobInfo *head_info = (dxThreadedJobInfo *)m_info_pool; // Head info must be re-read after mutex had been locked + + if (head_info != NULL) + { + dxThreadedJobInfo *next_info = head_info->m_next_job; + if (tAtomicsProvider::CompareExchangeTargetPtr(&m_info_pool, (atomicptr_t)head_info, (atomicptr_t)next_info)) + { + result_info = head_info; + break; + } + } + } + + m_info_wait_lull.UnregisterFromLull(); + + if (waited_lull) + { + // It is necessary to re-signal lull alarm if current thread was waiting as + // there might be other threads waiting which might have not received alarm signal. + m_info_wait_lull.SignalLullAlarmIfAnyRegistrants(); + } + + return result_info; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +void dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::ReleaseJobInfoIntoPool( + dxThreadedJobInfo *job_instance) +{ + while (true) + { + dxThreadedJobInfo *next_info = (dxThreadedJobInfo *)m_info_pool; + job_instance->m_next_job = next_info; + + if (tAtomicsProvider::CompareExchangeTargetPtr(&m_info_pool, (atomicptr_t)next_info, (atomicptr_t)job_instance)) + { + break; + } + } + + m_info_wait_lull.SignalLullAlarmIfAnyRegistrants(); +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +void dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::FreeJobInfoPoolInfos() +{ + dxThreadedJobInfo *current_info = (dxThreadedJobInfo *)m_info_pool; + + while (current_info != NULL) + { + dxThreadedJobInfo *info_save = current_info; + current_info = current_info->m_next_job; + + delete info_save; + } + + m_info_pool = (atomicptr_t)NULL; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +bool dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::EnsureNumberOfJobInfosIsPreallocated(ddependencycount_t required_info_count) +{ + bool result = required_info_count <= m_info_count_known_to_be_preallocated + || DoPreallocateJobInfos(required_info_count); + return result; +} + +template<class tThreadLull, class tThreadMutex, class tAtomicsProvider> +bool dxtemplateJobListContainer<tThreadLull, tThreadMutex, tAtomicsProvider>::DoPreallocateJobInfos(ddependencycount_t required_info_count) +{ + dIASSERT(required_info_count > m_info_count_known_to_be_preallocated); // Also ensures required_info_count > 0 + + bool allocation_failure = false; + + dxThreadedJobInfo *info_pool = (dxThreadedJobInfo *)m_info_pool; + + ddependencycount_t info_index = 0; + for (dxThreadedJobInfo **current_info_ptr = &info_pool; ; ) + { + dxThreadedJobInfo *current_info = *current_info_ptr; + + if (current_info == NULL) + { + current_info = new dxThreadedJobInfo(NULL); + + if (current_info == NULL) + { + allocation_failure = true; + break; + } + + *current_info_ptr = current_info; + } + + if (++info_index == required_info_count) + { + m_info_count_known_to_be_preallocated = info_index; + break; + } + + current_info_ptr = ¤t_info->m_next_job; + } + + // Make sure m_info_pool was not changed + dIASSERT(m_info_pool == NULL || m_info_pool == (atomicptr_t)info_pool); + + m_info_pool = (atomicptr_t)info_pool; + + bool result = !allocation_failure; + return result; +} + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +/************************************************************************/ +/* Implementation of dxtemplateJobListThreadedHandler */ +/************************************************************************/ + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::ProcessActiveJobAddition() +{ + ActivateAnIdleThread(); +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::PrepareForWaitingAJobCompletion() +{ + // Do nothing +} + +template<class tThreadWakeup, class tJobListContainer> +unsigned dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::RetrieveActiveThreadsCount() +{ + return GetActiveThreadsCount(); +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::StickToJobsProcessing(dxThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/) +{ + RegisterAsActiveThread(); + + if (readiness_callback != NULL) + { + (*readiness_callback)(callback_context); + } + + PerformJobProcessingUntilShutdown(); + + UnregisterAsActiveThread(); +} + + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::PerformJobProcessingUntilShutdown() +{ + while (true) + { + // It is expected that new jobs will not be queued any longer after shutdown had been requested + if (IsShutdownRequested() && m_job_list_ptr->IsJobListReadyForShutdown()) + { + break; + } + + PerformJobProcessingSession(); + + // It is expected that new jobs will not be queued any longer after shutdown had been requested + if (IsShutdownRequested() && m_job_list_ptr->IsJobListReadyForShutdown()) + { + break; + } + + BlockAsIdleThread(); + } +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::PerformJobProcessingSession() +{ + dxThreadedJobInfo *current_job = NULL; + bool job_result = false; + + while (true) + { + bool last_job_flag; + current_job = m_job_list_ptr->ReleaseAJobAndPickNextPendingOne(current_job, job_result, &dxCallWait::AbstractSignalTheWait, last_job_flag); + + if (!current_job) + { + break; + } + + if (!last_job_flag) + { + ActivateAnIdleThread(); + } + + job_result = current_job->InvokeCallFunction(); + } +} + + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::BlockAsIdleThread() +{ + m_processing_wakeup.WaitWakeup(NULL); +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::ActivateAnIdleThread() +{ + m_processing_wakeup.WakeupAThread(); +} + + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::ShutdownProcessing() +{ + m_shutdown_requested = true; + m_processing_wakeup.WakeupAllThreads(); +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListThreadedHandler<tThreadWakeup, tJobListContainer>::CleanupForRestart() +{ + m_shutdown_requested = false; + m_processing_wakeup.ResetWakeup(); +} + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +/************************************************************************/ +/* Implementation of dxtemplateJobListSelfHandler */ +/************************************************************************/ + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::ProcessActiveJobAddition() +{ + // Do nothing +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::PrepareForWaitingAJobCompletion() +{ + PerformJobProcessingUntilExhaustion(); +} + + +template<class tThreadWakeup, class tJobListContainer> +unsigned dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::RetrieveActiveThreadsCount() +{ + return 0U; // Return zero to indicate that there are no actual active threads provided. +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::StickToJobsProcessing(dxThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/) +{ + (void)readiness_callback; // unused + (void)callback_context; // unused + dIASSERT(false); // This method is not expected to be called for Self-Handler +} + + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::PerformJobProcessingUntilExhaustion() +{ + PerformJobProcessingSession(); +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::PerformJobProcessingSession() +{ + dxThreadedJobInfo *current_job = NULL; + bool job_result = false; + + while (true) + { + bool dummy_last_job_flag; + current_job = m_job_list_ptr->ReleaseAJobAndPickNextPendingOne(current_job, job_result, &dxCallWait::AbstractSignalTheWait, dummy_last_job_flag); + + if (!current_job) + { + break; + } + + job_result = current_job->InvokeCallFunction(); + } +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::ShutdownProcessing() +{ + // Do nothing +} + +template<class tThreadWakeup, class tJobListContainer> +void dxtemplateJobListSelfHandler<tThreadWakeup, tJobListContainer>::CleanupForRestart() +{ + // Do nothing +} + + +/************************************************************************/ +/* Implementation of dxtemplateThreadingImplementation */ +/************************************************************************/ + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::FreeInstance() +{ + delete this; +} + + +template<class tJobListContainer, class tJobListHandler> +dIMutexGroup *dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::AllocMutexGroup(dmutexindex_t Mutex_count) +{ + dxMutexGroup *mutex_group = dxMutexGroup::AllocateInstance(Mutex_count); + return (dIMutexGroup *)mutex_group; +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::FreeMutexGroup(dIMutexGroup *mutex_group) +{ + dxMutexGroup::FreeInstance((dxMutexGroup *)mutex_group); +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::LockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index) +{ + ((dxMutexGroup *)mutex_group)->LockMutex(mutex_index); +} + +// template<class tJobListContainer, class tJobListHandler> +// bool dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::TryLockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index) +// { +// return ((dxMutexGroup *)mutex_group)->TryLockMutex(mutex_index); +// } + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::UnlockMutexGroupMutex(dIMutexGroup *mutex_group, dmutexindex_t mutex_index) +{ + ((dxMutexGroup *)mutex_group)->UnlockMutex(mutex_index); +} + + +template<class tJobListContainer, class tJobListHandler> +dxICallWait *dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::AllocACallWait() +{ + dxCallWait *call_wait = new dxCallWait(); + + if (call_wait != NULL && !call_wait->InitializeObject()) + { + delete call_wait; + call_wait = NULL; + } + + return (dxICallWait *)call_wait; +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::ResetACallWait(dxICallWait *call_wait) +{ + ((dxCallWait *)call_wait)->ResetTheWait(); +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::FreeACallWait(dxICallWait *call_wait) +{ + delete ((dxCallWait *)call_wait); +} + + +template<class tJobListContainer, class tJobListHandler> +bool dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::PreallocateJobInfos(ddependencycount_t max_simultaneous_calls_estimate) +{ + // No multithreading protection here! + // Resources are to be preallocated before jobs start to be scheduled + // as otherwise there is no way to implement the preallocation. + bool result = m_list_container.EnsureNumberOfJobInfosIsPreallocated(max_simultaneous_calls_estimate); + return result; +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::ScheduleNewJob( + int *fault_accumulator_ptr/*=NULL*/, + dCallReleaseeID *out_post_releasee_ptr/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/, + dxICallWait *call_wait/*=NULL*/, + dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index) +{ + dxThreadedJobInfo *new_job = m_list_container.AllocateJobInfoFromPool(); + dIASSERT(new_job != NULL); + + new_job->AssignJobData(dependencies_count, dMAKE_RELEASEE_JOBINSTANCE(dependent_releasee), (dxCallWait *)call_wait, fault_accumulator_ptr, call_func, call_context, instance_index); + + if (out_post_releasee_ptr != NULL) + { + *out_post_releasee_ptr = dMAKE_JOBINSTANCE_RELEASEE(new_job); + } + + m_list_container.QueueJobForProcessing(new_job); + + if (dependencies_count == 0) + { + m_list_handler.ProcessActiveJobAddition(); + } +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::AlterJobDependenciesCount( + dCallReleaseeID target_releasee, ddependencychange_t dependencies_count_change) +{ + dIASSERT(dependencies_count_change != 0); + + dxThreadedJobInfo *job_instance = dMAKE_RELEASEE_JOBINSTANCE(target_releasee); + + bool job_has_become_ready; + m_list_container.AlterJobProcessingDependencies(job_instance, dependencies_count_change, job_has_become_ready); + + if (job_has_become_ready) + { + m_list_handler.ProcessActiveJobAddition(); + } +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::WaitJobCompletion( + int *out_wait_status_ptr/*=NULL*/, + dxICallWait *call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/) +{ + dIASSERT(call_wait != NULL); + + m_list_handler.PrepareForWaitingAJobCompletion(); + + bool wait_status = ((dxCallWait *)call_wait)->PerformWaiting(timeout_time_ptr); + dIASSERT(timeout_time_ptr != NULL || wait_status); + + if (out_wait_status_ptr) + { + *out_wait_status_ptr = wait_status; + } +} + + +template<class tJobListContainer, class tJobListHandler> +unsigned dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::RetrieveActiveThreadsCount() +{ + return m_list_handler.RetrieveActiveThreadsCount(); +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::StickToJobsProcessing(dxThreadReadyToServeCallback *readiness_callback/*=NULL*/, void *callback_context/*=NULL*/) +{ + m_list_handler.StickToJobsProcessing(readiness_callback, callback_context); +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::ShutdownProcessing() +{ + m_list_handler.ShutdownProcessing(); +} + +template<class tJobListContainer, class tJobListHandler> +void dxtemplateThreadingImplementation<tJobListContainer, tJobListHandler>::CleanupForRestart() +{ + m_list_handler.CleanupForRestart(); +} + + +#endif // #ifndef _ODE_THREADING_IMPL_TEMPLATES_H_ diff --git a/libs/ode-0.16.1/ode/src/threading_impl_win.h b/libs/ode-0.16.1/ode/src/threading_impl_win.h new file mode 100644 index 0000000..f3cb489 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_impl_win.h @@ -0,0 +1,273 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading Windows implementation file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Threading Windows implementation for built-in threading support provider. + */ + + +#ifndef _ODE_THREADING_IMPL_WIN_H_ +#define _ODE_THREADING_IMPL_WIN_H_ + + +#include <ode/common.h> + + +#if defined(_WIN32) + +#if dBUILTIN_THREADING_IMPL_ENABLED + +#if !defined(_WIN32_WINNT) +#define _WIN32_WINNT 0x0400 +#endif +#include <windows.h> + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +#include "threading_impl_templates.h" +#include "threading_fake_sync.h" +#include "threading_atomics_provs.h" + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +/************************************************************************/ +/* dxEventWakeup class implementation */ +/************************************************************************/ + +class dxEventWakeup +{ +public: + dxEventWakeup(): m_state_is_permanent(false), m_event_handle(NULL) {} + ~dxEventWakeup() { DoFinalizeObject(); } + + bool InitializeObject() { return DoInitializeObject(); } + +private: + bool DoInitializeObject(); + void DoFinalizeObject(); + +public: + void ResetWakeup(); + void WakeupAThread(); + void WakeupAllThreads(); + + bool WaitWakeup(const dThreadedWaitTime *timeout_time_ptr); + +private: + bool m_state_is_permanent; + HANDLE m_event_handle; +}; + + +bool dxEventWakeup::DoInitializeObject() +{ + dIASSERT(m_event_handle == NULL); + + bool init_result = false; + + do + { + HANDLE event_handle = CreateEvent(NULL, FALSE, FALSE, NULL); + if (event_handle == NULL) + { + break; + } + + m_event_handle = event_handle; + init_result = true; + } + while (false); + + return init_result; +} + +void dxEventWakeup::DoFinalizeObject() +{ + HANDLE event_handle = m_event_handle; + + if (event_handle != NULL) + { + BOOL close_result = CloseHandle(event_handle); + dICHECK(close_result != FALSE); + + m_event_handle = NULL; + } +} + + +void dxEventWakeup::ResetWakeup() +{ + // Order of assignment and resetting event is not important but it is preferable to be performed this way. + m_state_is_permanent = false; + + BOOL event_set_result = ResetEvent(m_event_handle); + dICHECK(event_set_result); +} + +void dxEventWakeup::WakeupAThread() +{ + dIASSERT(!m_state_is_permanent); // Wakeup should not be used after permanent signal + + BOOL event_reset_result = SetEvent(m_event_handle); + dICHECK(event_reset_result); +} + +void dxEventWakeup::WakeupAllThreads() +{ + // Order of assignment and setting event is important! + m_state_is_permanent = true; + + BOOL event_set_result = SetEvent(m_event_handle); + dICHECK(event_set_result); +} + + +bool dxEventWakeup::WaitWakeup(const dThreadedWaitTime *timeout_time_ptr) +{ + bool wait_result; + + if (timeout_time_ptr == NULL) + { + DWORD event_wait_result = WaitForSingleObject(m_event_handle, INFINITE); + dICHECK(event_wait_result == WAIT_OBJECT_0); + + wait_result = true; + } + else if (timeout_time_ptr->wait_sec == 0 && timeout_time_ptr->wait_nsec == 0) + { + DWORD event_wait_result = WaitForSingleObject(m_event_handle, 0); + + wait_result = event_wait_result == WAIT_OBJECT_0; + dICHECK(wait_result || event_wait_result == WAIT_TIMEOUT); + } + else + { + dIASSERT(timeout_time_ptr->wait_nsec < 1000000000UL); + + const DWORD max_wait_seconds_in_a_shot = ((INFINITE - 1) / 1000U) - 1; + + time_t timeout_seconds_remaining = timeout_time_ptr->wait_sec; + DWORD wait_timeout = timeout_time_ptr->wait_nsec != 0 ? ((timeout_time_ptr->wait_nsec + 999999UL) / 1000000UL) : 0; + + while (true) + { + if (timeout_seconds_remaining >= (time_t)max_wait_seconds_in_a_shot) + { + wait_timeout += max_wait_seconds_in_a_shot * 1000U; + timeout_seconds_remaining -= max_wait_seconds_in_a_shot; + } + else + { + wait_timeout += (DWORD)timeout_seconds_remaining * 1000U; + timeout_seconds_remaining = 0; + } + + DWORD event_wait_result = WaitForSingleObject(m_event_handle, wait_timeout); + + if (event_wait_result == WAIT_OBJECT_0) + { + wait_result = true; + break; + } + + dICHECK(event_wait_result == WAIT_TIMEOUT); + + if (timeout_seconds_remaining == 0) + { + wait_result = false; + break; + } + + wait_timeout = 0; + } + } + + if (wait_result && m_state_is_permanent) + { + // Since event is automatic it is necessary to set it back for the upcoming waiters + BOOL event_set_result = SetEvent(m_event_handle); + dICHECK(event_set_result); + } + + return wait_result; +} + + +/************************************************************************/ +/* dxCriticalSectionMutex class implementation */ +/************************************************************************/ + +class dxCriticalSectionMutex +{ +public: + dxCriticalSectionMutex() { InitializeCriticalSection(&m_critical_section); } + ~dxCriticalSectionMutex() { DeleteCriticalSection(&m_critical_section); } + + bool InitializeObject() { return true; } + +public: + void LockMutex() { EnterCriticalSection(&m_critical_section); } + bool TryLockMutex() { return TryEnterCriticalSection(&m_critical_section) != FALSE; } + void UnlockMutex() { LeaveCriticalSection(&m_critical_section); } + +private: + CRITICAL_SECTION m_critical_section; +}; + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +/************************************************************************/ +/* Self-threaded job list definition */ +/************************************************************************/ + +typedef dxtemplateJobListContainer<dxFakeLull, dxFakeMutex, dxFakeAtomicsProvider> dxSelfThreadedJobListContainer; +typedef dxtemplateJobListSelfHandler<dxSelfWakeup, dxSelfThreadedJobListContainer> dxSelfThreadedJobListHandler; +typedef dxtemplateThreadingImplementation<dxSelfThreadedJobListContainer, dxSelfThreadedJobListHandler> dxSelfThreadedThreading; + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +/************************************************************************/ +/* Multi-threaded job list definition */ +/************************************************************************/ + +typedef dxtemplateJobListContainer<dxtemplateThreadedLull<dxEventWakeup, dxOUAtomicsProvider, false>, dxCriticalSectionMutex, dxOUAtomicsProvider> dxMultiThreadedJobListContainer; +typedef dxtemplateJobListThreadedHandler<dxEventWakeup, dxMultiThreadedJobListContainer> dxMultiThreadedJobListHandler; +typedef dxtemplateThreadingImplementation<dxMultiThreadedJobListContainer, dxMultiThreadedJobListHandler> dxMultiThreadedThreading; + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +#endif // #if defined(_WIN32) + + +#endif // #ifndef _ODE_THREADING_IMPL_WIN_H_ diff --git a/libs/ode-0.16.1/ode/src/threading_pool_posix.cpp b/libs/ode-0.16.1/ode/src/threading_pool_posix.cpp new file mode 100644 index 0000000..39d0d56 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_pool_posix.cpp @@ -0,0 +1,823 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading POSIX thread pool implementation file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * POSIX thread pool implementation for built-in threading support provider. + */ + + +#if !defined(_WIN32) + +#include <ode/odeconfig.h> +#include <ode/error.h> +#include <ode/threading_impl.h> +#include <ode/odeinit.h> +#include "config.h" +#include "objects.h" +#include "threading_impl_templates.h" + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +#include <new> +#include <pthread.h> +#include <signal.h> +#include <errno.h> + +#if !defined(EOK) +#define EOK 0 +#endif + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +struct dxEventObject +{ +public: + dxEventObject(): m_event_allocated(false), m_event_manual(false), m_event_value(false) {} + ~dxEventObject() { FinalizeObject(); } + + bool InitializeObject(bool manual_reset, bool initial_state); + void FinalizeObject(); + + // WARNING! To make implementation simpler, event only releases a single thread even for manual mode. + bool WaitInfinitely(); + void SetEvent(); + void ResetEvent(); + +private: + bool m_event_allocated; + bool m_event_manual; + bool m_event_value; + pthread_mutex_t m_event_mutex; + pthread_cond_t m_event_cond; +}; + +bool dxEventObject::InitializeObject(bool manual_reset, bool initial_state) +{ + dIASSERT(!m_event_allocated); + + bool result = false; + + bool cond_allocated = false; + + do + { + int cond_result = pthread_cond_init(&m_event_cond, NULL); + if (cond_result != EOK) + { + errno = cond_result; + break; + } + + cond_allocated = true; + + int mutex_result = pthread_mutex_init(&m_event_mutex, NULL); + if (mutex_result != EOK) + { + errno = mutex_result; + break; + } + + m_event_manual = manual_reset; + m_event_value = initial_state; + m_event_allocated = true; + result = true; + } + while (false); + + if (!result) + { + if (cond_allocated) + { + int cond_destroy_result = pthread_cond_destroy(&m_event_cond); + dIVERIFY(cond_destroy_result == EOK); + } + } + + return result; +} + +void dxEventObject::FinalizeObject() +{ + if (m_event_allocated) + { + int mutex_destroy_result = pthread_mutex_destroy(&m_event_mutex); + dICHECK(mutex_destroy_result == EOK); // Why would mutex be unable to be destroyed? + + int cond_destroy_result = pthread_cond_destroy(&m_event_cond); + dICHECK(cond_destroy_result == EOK); // Why would condvar be unable to be destroyed? + + m_event_allocated = false; + } +} + +bool dxEventObject::WaitInfinitely() +{ + bool result = false; + + int lock_result = pthread_mutex_lock(&m_event_mutex); + dICHECK(lock_result == EOK); + + int wait_result = EOK; + if (!m_event_value) + { + wait_result = pthread_cond_wait(&m_event_cond, &m_event_mutex); + dICHECK(wait_result != EINTR); // Would caller be so kind to disable signal handling for thread for duration of the call to ODE at least? + } + + if (wait_result == EOK) + { + dIASSERT(m_event_value); + + if (!m_event_manual) + { + m_event_value = false; + } + + result = true; + } + + int unlock_result = pthread_mutex_unlock(&m_event_mutex); + dICHECK(unlock_result == EOK); + + return result; +} + +void dxEventObject::SetEvent() +{ + int lock_result = pthread_mutex_lock(&m_event_mutex); + dICHECK(lock_result == EOK); + + if (!m_event_value) + { + m_event_value = true; + + // NOTE! Event only releases a single thread even for manual mode to simplify implementation. + int signal_result = pthread_cond_signal(&m_event_cond); + dICHECK(signal_result == EOK); + } + + int unlock_result = pthread_mutex_unlock(&m_event_mutex); + dICHECK(unlock_result == EOK); +} + +void dxEventObject::ResetEvent() +{ + int lock_result = pthread_mutex_lock(&m_event_mutex); + dICHECK(lock_result == EOK); + + m_event_value = false; + + int unlock_result = pthread_mutex_unlock(&m_event_mutex); + dICHECK(unlock_result == EOK); +} + + +struct dxThreadPoolThreadInfo +{ +public: + dxThreadPoolThreadInfo(); + ~dxThreadPoolThreadInfo(); + + bool Initialize(sizeint stack_size, unsigned int ode_data_allocate_flags); + +private: + bool InitializeThreadAttributes(pthread_attr_t *thread_attr, sizeint stack_size); + void FinalizeThreadAttributes(pthread_attr_t *thread_attr); + bool WaitInitStatus(); + +private: + void Finalize(); + void WaitAndCloseThreadHandle(pthread_t thread_handle); + +public: + enum dxTHREADCOMMAND + { + dxTHREAD_COMMAND_EXIT, + dxTHREAD_COMMAND_NOOP, + dxTHREAD_COMMAND_SERVE_IMPLEMENTATION, + }; + + struct dxServeImplementationParams + { + dxServeImplementationParams(dThreadingImplementationID impl, dxEventObject *ready_wait_event): + m_impl(impl), m_ready_wait_event(ready_wait_event) + { + } + + dThreadingImplementationID m_impl; + dxEventObject *m_ready_wait_event; + }; + + void ExecuteThreadCommand(dxTHREADCOMMAND command, void *param, bool wait_response); + +private: + static void *ThreadProcedure_Callback(void *thread_param); + void ThreadProcedure(); + bool DisableSignalHandlers(); + void ReportInitStatus(bool init_result); + void RunCommandHandlingLoop(); + + void ThreadedServeImplementation(dThreadingImplementationID impl, dxEventObject *ready_wait_event); + static void ProcessThreadServeReadiness_Callback(void *context); + +private: + pthread_t m_thread_handle; + bool m_thread_allocated; + + unsigned int m_ode_data_allocate_flags; + dxTHREADCOMMAND m_command_code; + dxEventObject m_command_event; + dxEventObject m_acknowledgement_event; + void *m_command_param; +}; + + +dxThreadPoolThreadInfo::dxThreadPoolThreadInfo(): +m_thread_handle(), +m_thread_allocated(false), +m_ode_data_allocate_flags(0), +m_command_code(dxTHREAD_COMMAND_EXIT), +m_command_event(), +m_acknowledgement_event(), +m_command_param(NULL) +{ +} + +dxThreadPoolThreadInfo::~dxThreadPoolThreadInfo() +{ + Finalize(); +} + + +bool dxThreadPoolThreadInfo::Initialize(sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + bool result = false; + + bool command_event_allocated = false, acknowledgement_event_allocated = false; + + do + { + // -- There is no implicit limit on stack size in POSIX implementation + // if (stack_size > ...) + // { + // errno = EINVAL; + // break; + // } + + if (!m_command_event.InitializeObject(false, false)) + { + break; + } + + command_event_allocated = true; + + if (!m_acknowledgement_event.InitializeObject(true, false)) + { + break; + } + + acknowledgement_event_allocated = true; + + m_ode_data_allocate_flags = ode_data_allocate_flags; + + pthread_attr_t thread_attr; + if (!InitializeThreadAttributes(&thread_attr, stack_size)) + { + break; + } + + int thread_create_result = pthread_create(&m_thread_handle, &thread_attr, &ThreadProcedure_Callback, (void *)this); + + FinalizeThreadAttributes(&thread_attr); + + if (thread_create_result != EOK) + { + errno = thread_create_result; + break; + } + + bool thread_init_result = WaitInitStatus(); + if (!thread_init_result) + { + WaitAndCloseThreadHandle(m_thread_handle); + break; + } + + m_thread_allocated = true; + result = true; + } + while (false); + + if (!result) + { + if (command_event_allocated) + { + if (acknowledgement_event_allocated) + { + m_acknowledgement_event.FinalizeObject(); + } + + m_command_event.FinalizeObject(); + } + } + + return result; +} + +bool dxThreadPoolThreadInfo::InitializeThreadAttributes(pthread_attr_t *thread_attr, sizeint stack_size) +{ + bool result = false; + + bool attr_inited = false; + + do + { + int init_result = pthread_attr_init(thread_attr); + if (init_result != EOK) + { + errno = init_result; + break; + } + + attr_inited = true; + + int set_result; + if ((set_result = pthread_attr_setdetachstate(thread_attr, PTHREAD_CREATE_JOINABLE)) != EOK +#if (HAVE_PTHREAD_ATTR_SETINHERITSCHED) + || (set_result = pthread_attr_setinheritsched(thread_attr, PTHREAD_INHERIT_SCHED)) != EOK +#endif +#if (HAVE_PTHREAD_ATTR_SETSTACKLAZY) + || (set_result = pthread_attr_setstacklazy(thread_attr, PTHREAD_STACK_NOTLAZY)) != EOK +#endif + || (stack_size != 0 && (set_result = pthread_attr_setstacksize(thread_attr, stack_size)) != EOK)) + { + errno = set_result; + break; + } + + result = true; + } + while (false); + + if (!result) + { + if (attr_inited) + { + int destroy_result = pthread_attr_destroy(thread_attr); + dIVERIFY(destroy_result == EOK); + } + } + + return result; +} + +void dxThreadPoolThreadInfo::FinalizeThreadAttributes(pthread_attr_t *thread_attr) +{ + int destroy_result = pthread_attr_destroy(thread_attr); + dIVERIFY(destroy_result == EOK); +} + +bool dxThreadPoolThreadInfo::WaitInitStatus() +{ + bool acknowledgement_wait_result = m_acknowledgement_event.WaitInfinitely(); + dICHECK(acknowledgement_wait_result); + + int error_code = (int)(sizeint)m_command_param; + + bool init_status = error_code == EOK ? true : ((errno = error_code), false); + return init_status; +} + +void dxThreadPoolThreadInfo::Finalize() +{ + if (m_thread_allocated) + { + ExecuteThreadCommand(dxTHREAD_COMMAND_EXIT, NULL, false); + + WaitAndCloseThreadHandle(m_thread_handle); + m_thread_allocated = false; + + m_command_event.FinalizeObject(); + m_acknowledgement_event.FinalizeObject(); + } +} + +void dxThreadPoolThreadInfo::WaitAndCloseThreadHandle(pthread_t thread_handle) +{ + int join_result = pthread_join(thread_handle, NULL); + dICHECK(join_result == EOK); +} + +void dxThreadPoolThreadInfo::ExecuteThreadCommand(dxTHREADCOMMAND command, void *param, bool wait_response) +{ + bool acknowledgement_wait_result = m_acknowledgement_event.WaitInfinitely(); + dICHECK(acknowledgement_wait_result); + + m_acknowledgement_event.ResetEvent(); + + m_command_code = command; + m_command_param = param; + + m_command_event.SetEvent(); + + if (wait_response) + { + bool new_acknowledgement_wait_result = m_acknowledgement_event.WaitInfinitely(); + dICHECK(new_acknowledgement_wait_result); + } +} + +void *dxThreadPoolThreadInfo::ThreadProcedure_Callback(void *thread_param) +{ + dxThreadPoolThreadInfo *thread_info = (dxThreadPoolThreadInfo *)thread_param; + thread_info->ThreadProcedure(); + + return 0; +} + +void dxThreadPoolThreadInfo::ThreadProcedure() +{ + bool init_result = dAllocateODEDataForThread(m_ode_data_allocate_flags) != 0 + && DisableSignalHandlers(); + + ReportInitStatus(init_result); + + if (init_result) + { + RunCommandHandlingLoop(); + + // dCleanupODEAllDataForThread(); -- this function can only be called if ODE was initialized for manual cleanup. And that is unknown here... + } +} + +bool dxThreadPoolThreadInfo::DisableSignalHandlers() +{ + bool result = false; + + sigset_t set; + sigfillset( &set ); + + if (sigprocmask( SIG_BLOCK, &set, NULL ) != -1) + { + result = true; + } + + return result; +} + +void dxThreadPoolThreadInfo::ReportInitStatus(bool init_result) +{ + m_command_param = (void *)(sizeint)(init_result ? EOK : ((errno != EOK) ? errno : EFAULT)); + + m_acknowledgement_event.SetEvent(); +} + +void dxThreadPoolThreadInfo::RunCommandHandlingLoop() +{ + bool exit_requested = false; + + while (!exit_requested) + { + bool command_wait_result = m_command_event.WaitInfinitely(); + dICHECK(command_wait_result); + + const dxTHREADCOMMAND command_code = m_command_code; + switch (command_code) + { + case dxTHREAD_COMMAND_EXIT: + { + m_acknowledgement_event.SetEvent(); + + exit_requested = true; + break; + } + + default: + { + dIASSERT(false); + // break; -- proceed to case dxTHREAD_COMMAND_NOOP + } + + case dxTHREAD_COMMAND_NOOP: + { + m_acknowledgement_event.SetEvent(); + + // Do nothing + break; + } + + case dxTHREAD_COMMAND_SERVE_IMPLEMENTATION: + { + const dxServeImplementationParams *serve_params = (const dxServeImplementationParams *)m_command_param; + dThreadingImplementationID impl = serve_params->m_impl; + dxEventObject *ready_wait_event = serve_params->m_ready_wait_event; + + m_acknowledgement_event.SetEvent(); + + ThreadedServeImplementation(impl, ready_wait_event); + break; + } + } + } +} + +void dxThreadPoolThreadInfo::ThreadedServeImplementation(dThreadingImplementationID impl, dxEventObject *ready_wait_event) +{ + ((dxIThreadingImplementation *)impl)->StickToJobsProcessing(&ProcessThreadServeReadiness_Callback, (void *)ready_wait_event); +} + +void dxThreadPoolThreadInfo::ProcessThreadServeReadiness_Callback(void *context) +{ + dxEventObject *ready_wait_event = (dxEventObject *)context; + + ready_wait_event->SetEvent(); +} + + + +struct dxThreadingThreadPool: + public dBase +{ +public: + dxThreadingThreadPool(); + ~dxThreadingThreadPool(); + + bool InitializeThreads(sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags); + +private: + void FinalizeThreads(); + + bool InitializeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags); + void FinalizeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count); + + bool InitializeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info, sizeint stack_size, unsigned int ode_data_allocate_flags); + void FinalizeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info); + +public: + void ServeThreadingImplementation(dThreadingImplementationID impl); + void WaitIdleState(); + +private: + dxThreadPoolThreadInfo *m_thread_infos; + sizeint m_thread_count; + dxEventObject m_ready_wait_event; +}; + + +dxThreadingThreadPool::dxThreadingThreadPool(): +m_thread_infos(NULL), +m_thread_count(0), +m_ready_wait_event() +{ +} + +dxThreadingThreadPool::~dxThreadingThreadPool() +{ + FinalizeThreads(); +} + + +bool dxThreadingThreadPool::InitializeThreads(sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + dIASSERT(m_thread_infos == NULL); + + bool result = false; + + bool wait_event_allocated = false; + + dxThreadPoolThreadInfo *thread_infos = NULL; + bool thread_infos_allocated = false; + + do + { + if (!m_ready_wait_event.InitializeObject(false, false)) + { + break; + } + + wait_event_allocated = true; + + thread_infos = (dxThreadPoolThreadInfo *)dAlloc(thread_count * sizeof(dxThreadPoolThreadInfo)); + if (thread_infos == NULL) + { + break; + } + + thread_infos_allocated = true; + + if (!InitializeIndividualThreadInfos(thread_infos, thread_count, stack_size, ode_data_allocate_flags)) + { + break; + } + + m_thread_infos = thread_infos; + m_thread_count = thread_count; + result = true; + } + while (false); + + if (!result) + { + if (wait_event_allocated) + { + if (thread_infos_allocated) + { + dFree(thread_infos, thread_count * sizeof(dxThreadPoolThreadInfo)); + } + + m_ready_wait_event.FinalizeObject(); + } + } + + return result; +} + +void dxThreadingThreadPool::FinalizeThreads() +{ + dxThreadPoolThreadInfo *thread_infos = m_thread_infos; + if (thread_infos != NULL) + { + sizeint thread_count = m_thread_count; + + FinalizeIndividualThreadInfos(thread_infos, thread_count); + dFree(thread_infos, thread_count * sizeof(dxThreadPoolThreadInfo)); + + m_ready_wait_event.FinalizeObject(); + } +} + + +bool dxThreadingThreadPool::InitializeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + bool any_fault = false; + + dxThreadPoolThreadInfo *const infos_end = thread_infos + thread_count; + for (dxThreadPoolThreadInfo *current_info = thread_infos; current_info != infos_end; ++current_info) + { + if (!InitializeSingleThreadInfo(current_info, stack_size, ode_data_allocate_flags)) + { + FinalizeIndividualThreadInfos(thread_infos, current_info - thread_infos); + + any_fault = true; + break; + } + } + + bool result = !any_fault; + return result; +} + +void dxThreadingThreadPool::FinalizeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count) +{ + dxThreadPoolThreadInfo *const infos_end = thread_infos + thread_count; + for (dxThreadPoolThreadInfo *current_info = thread_infos; current_info != infos_end; ++current_info) + { + FinalizeSingleThreadInfo(current_info); + } +} + + +bool dxThreadingThreadPool::InitializeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info, sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + bool result = false; + + new(thread_info) dxThreadPoolThreadInfo(); + + if (thread_info->Initialize(stack_size, ode_data_allocate_flags)) + { + result = true; + } + else + { + thread_info->dxThreadPoolThreadInfo::~dxThreadPoolThreadInfo(); + } + + return result; +} + +void dxThreadingThreadPool::FinalizeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info) +{ + if (thread_info != NULL) + { + thread_info->dxThreadPoolThreadInfo::~dxThreadPoolThreadInfo(); + } +} + + +void dxThreadingThreadPool::ServeThreadingImplementation(dThreadingImplementationID impl) +{ + dxThreadPoolThreadInfo::dxServeImplementationParams params(impl, &m_ready_wait_event); + + dxThreadPoolThreadInfo *const infos_end = m_thread_infos + m_thread_count; + for (dxThreadPoolThreadInfo *current_info = m_thread_infos; current_info != infos_end; ++current_info) + { + current_info->ExecuteThreadCommand(dxThreadPoolThreadInfo::dxTHREAD_COMMAND_SERVE_IMPLEMENTATION, ¶ms, true); + + bool ready_wait_result = m_ready_wait_event.WaitInfinitely(); + dICHECK(ready_wait_result); + } +} + +void dxThreadingThreadPool::WaitIdleState() +{ + dxThreadPoolThreadInfo *const infos_end = m_thread_infos + m_thread_count; + for (dxThreadPoolThreadInfo *current_info = m_thread_infos; current_info != infos_end; ++current_info) + { + current_info->ExecuteThreadCommand(dxThreadPoolThreadInfo::dxTHREAD_COMMAND_NOOP, NULL, true); + } +} + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +/*extern */dThreadingThreadPoolID dThreadingAllocateThreadPool(unsigned thread_count, + sizeint stack_size, unsigned int ode_data_allocate_flags, void *reserved/*=NULL*/) +{ + dAASSERT(thread_count != 0); + +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = new dxThreadingThreadPool(); + if (thread_pool != NULL) + { + if (thread_pool->InitializeThreads(thread_count, stack_size, ode_data_allocate_flags)) + { + // do nothing + } + else + { + delete thread_pool; + thread_pool = NULL; + } + } +#else + dThreadingThreadPoolID thread_pool = NULL; + (void)stack_size; // unused + (void)ode_data_allocate_flags; // unused + (void)reserved; // unused +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + return (dThreadingThreadPoolID)thread_pool; +} + +/*extern */void dThreadingThreadPoolServeMultiThreadedImplementation(dThreadingThreadPoolID pool, dThreadingImplementationID impl) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = (dxThreadingThreadPool *)pool; + thread_pool->ServeThreadingImplementation(impl); +#else + (void)pool; // unused + (void)impl; // unused +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED +} + +/*extern */void dThreadingThreadPoolWaitIdleState(dThreadingThreadPoolID pool) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = (dxThreadingThreadPool *)pool; + thread_pool->WaitIdleState(); +#else + (void)pool; // unused +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED +} + +/*extern */void dThreadingFreeThreadPool(dThreadingThreadPoolID pool) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = (dxThreadingThreadPool *)pool; + delete thread_pool; +#else + (void)pool; // unused +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED +} + + +#endif // #if !defined(_WIN32) diff --git a/libs/ode-0.16.1/ode/src/threading_pool_win.cpp b/libs/ode-0.16.1/ode/src/threading_pool_win.cpp new file mode 100644 index 0000000..5c17f10 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threading_pool_win.cpp @@ -0,0 +1,670 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Threading Windows thread pool implementation file. * + * Copyright (C) 2011-2019 Oleh Derevenko. All rights reserved. * + * e-mail: odar@eleks.com (change all "a" to "e") * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + * Windows thread pool implementation for built-in threading support provider. + */ + + +#if defined(_WIN32) + +#include <ode/odeconfig.h> +#include <ode/error.h> +#include <ode/threading_impl.h> +#include <ode/odeinit.h> +#include "config.h" +#include "objects.h" +#include "threading_impl_templates.h" + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +#include <Windows.h> +#include <process.h> +#include <new> + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +#if dBUILTIN_THREADING_IMPL_ENABLED + +#define THREAD_STACK_MAX ((sizeint)(UINT_MAX - 1)) // The absolute maximum would be UINT_MAX but let it be a little bit less to avoid "Comparison is always false" warnings. ;) + + +struct dxEventObject +{ +public: + dxEventObject(): m_event_handle(NULL) {} + ~dxEventObject() { FinalizeObject(); } + + bool InitializeObject(bool manual_reset, bool initial_state); + void FinalizeObject(); + + bool WaitInfinitely() { return ::WaitForSingleObject(m_event_handle, INFINITE) == WAIT_OBJECT_0; } + void SetEvent(); + void ResetEvent(); + +private: + HANDLE m_event_handle; +}; + +bool dxEventObject::InitializeObject(bool manual_reset, bool initial_state) +{ + dIASSERT(m_event_handle == NULL); + + bool result = false; + + do + { + HANDLE event_handle = ::CreateEvent(NULL, manual_reset, initial_state, NULL); + if (event_handle == NULL) + { + break; + } + + m_event_handle = event_handle; + result = true; + } + while (false); + + return result; +} + +void dxEventObject::FinalizeObject() +{ + HANDLE event_handle = m_event_handle; + if (event_handle != NULL) + { + BOOL close_result = ::CloseHandle(event_handle); + dICHECK(close_result); // Object destruction should always succeed + + m_event_handle = NULL; + } +} + +void dxEventObject::SetEvent() +{ + BOOL set_result = ::SetEvent(m_event_handle); + dICHECK(set_result); +} + +void dxEventObject::ResetEvent() +{ + BOOL reset_result = ::ResetEvent(m_event_handle); + dICHECK(reset_result); +} + + + +struct dxThreadPoolThreadInfo +{ +public: + dxThreadPoolThreadInfo(); + ~dxThreadPoolThreadInfo(); + + bool Initialize(sizeint stack_size, unsigned int ode_data_allocate_flags); + +private: + bool WaitInitStatus(); + +private: + void Finalize(); + void WaitAndCloseThreadHandle(HANDLE thread_handle); + +public: + enum dxTHREADCOMMAND + { + dxTHREAD_COMMAND_EXIT, + dxTHREAD_COMMAND_NOOP, + dxTHREAD_COMMAND_SERVE_IMPLEMENTATION, + }; + + struct dxServeImplementationParams + { + dxServeImplementationParams(dThreadingImplementationID impl, dxEventObject *ready_wait_event): + m_impl(impl), m_ready_wait_event(ready_wait_event) + { + } + + dThreadingImplementationID m_impl; + dxEventObject *m_ready_wait_event; + }; + + void ExecuteThreadCommand(dxTHREADCOMMAND command, void *param, bool wait_response); + +private: + static unsigned CALLBACK ThreadProcedure_Callback(void *thread_param); + void ThreadProcedure(); + void ReportInitStatus(bool init_result); + void RunCommandHandlingLoop(); + + void ThreadedServeImplementation(dThreadingImplementationID impl, dxEventObject *ready_wait_event); + static void ProcessThreadServeReadiness_Callback(void *context); + +private: + HANDLE m_thread_handle; + + unsigned int m_ode_data_allocate_flags; + dxTHREADCOMMAND m_command_code; + dxEventObject m_command_event; + dxEventObject m_acknowledgement_event; + void *m_command_param; +}; + + +dxThreadPoolThreadInfo::dxThreadPoolThreadInfo(): +m_thread_handle(NULL), +m_ode_data_allocate_flags(0), +m_command_code(dxTHREAD_COMMAND_EXIT), +m_command_event(), +m_acknowledgement_event(), +m_command_param(NULL) +{ +} + +dxThreadPoolThreadInfo::~dxThreadPoolThreadInfo() +{ + Finalize(); +} + + +bool dxThreadPoolThreadInfo::Initialize(sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + bool result = false; + + bool command_event_allocated = false, acknowledgement_event_allocated = false; + + HANDLE thread_handle = NULL; + + do + { + if (stack_size > THREAD_STACK_MAX) + { + SetLastError(ERROR_INVALID_PARAMETER); + break; + } + + if (!m_command_event.InitializeObject(false, false)) + { + break; + } + + command_event_allocated = true; + + if (!m_acknowledgement_event.InitializeObject(true, false)) + { + break; + } + + acknowledgement_event_allocated = true; + + m_ode_data_allocate_flags = ode_data_allocate_flags; + + thread_handle = (HANDLE)_beginthreadex(NULL, (unsigned)stack_size, &ThreadProcedure_Callback, (void *)this, 0, NULL); + if (thread_handle == NULL) // Not a bug!!! _beginthreadex() returns NULL on failure + { + break; + } + + // It is OK to alter priority for thread without creating it in suspended state as + // it is anyway going to be waited for (waited for its init result) and + // will not be issues commands until after that. + int own_priority = GetThreadPriority(GetCurrentThread()); + if (own_priority != THREAD_PRIORITY_ERROR_RETURN) + { + if (!SetThreadPriority(thread_handle, own_priority)) + { + // own_priority = THREAD_PRIORITY_ERROR_RETURN; -- Well, if priority inheritance fails - just ignore it :-/ + } + } + + bool thread_init_result = WaitInitStatus(); + if (!thread_init_result) + { + DWORD error_save = GetLastError(); + WaitAndCloseThreadHandle(thread_handle); + SetLastError(error_save); + break; + } + + m_thread_handle = thread_handle; + result = true; + } + while (false); + + if (!result) + { + if (command_event_allocated) + { + if (acknowledgement_event_allocated) + { + m_acknowledgement_event.FinalizeObject(); + } + + m_command_event.FinalizeObject(); + } + } + + return result; +} + +bool dxThreadPoolThreadInfo::WaitInitStatus() +{ + bool acknowledgement_wait_result = m_acknowledgement_event.WaitInfinitely(); + dICHECK(acknowledgement_wait_result); + + DWORD error_code = (DWORD)(sizeint)m_command_param; + + bool init_status = error_code == ERROR_SUCCESS ? true : (SetLastError(error_code), false); + return init_status; +} + +void dxThreadPoolThreadInfo::Finalize() +{ + HANDLE thread_handle = m_thread_handle; + if (thread_handle != NULL) + { + ExecuteThreadCommand(dxTHREAD_COMMAND_EXIT, NULL, false); + + WaitAndCloseThreadHandle(thread_handle); + m_thread_handle = NULL; + + m_command_event.FinalizeObject(); + m_acknowledgement_event.FinalizeObject(); + } +} + +void dxThreadPoolThreadInfo::WaitAndCloseThreadHandle(HANDLE thread_handle) +{ + DWORD thread_wait_result = WaitForSingleObject(thread_handle, INFINITE); + dICHECK(thread_wait_result == WAIT_OBJECT_0); + + BOOL thread_close_result = CloseHandle(thread_handle); + dIVERIFY(thread_close_result); + +} + +void dxThreadPoolThreadInfo::ExecuteThreadCommand(dxTHREADCOMMAND command, void *param, bool wait_response) +{ + bool acknowledgement_wait_result = m_acknowledgement_event.WaitInfinitely(); + dICHECK(acknowledgement_wait_result); + + m_acknowledgement_event.ResetEvent(); + + m_command_code = command; + m_command_param = param; + + m_command_event.SetEvent(); + + if (wait_response) + { + bool new_acknowledgement_wait_result = m_acknowledgement_event.WaitInfinitely(); + dICHECK(new_acknowledgement_wait_result); + } +} + +unsigned CALLBACK dxThreadPoolThreadInfo::ThreadProcedure_Callback(void *thread_param) +{ + dxThreadPoolThreadInfo *thread_info = (dxThreadPoolThreadInfo *)thread_param; + thread_info->ThreadProcedure(); + + return 0; +} + +void dxThreadPoolThreadInfo::ThreadProcedure() +{ + bool init_result = dAllocateODEDataForThread(m_ode_data_allocate_flags) != 0; + + ReportInitStatus(init_result); + + if (init_result) + { + RunCommandHandlingLoop(); + + // dCleanupODEAllDataForThread(); -- this function can only be called if ODE was initialized for manual cleanup. And that is unknown here... + } +} + +void dxThreadPoolThreadInfo::ReportInitStatus(bool init_result) +{ + DWORD error_code; + m_command_param = (void *)(sizeint)(init_result ? ERROR_SUCCESS : ((error_code = GetLastError()) != ERROR_SUCCESS ? error_code : ERROR_INTERNAL_ERROR)); + + m_acknowledgement_event.SetEvent(); +} + +void dxThreadPoolThreadInfo::RunCommandHandlingLoop() +{ + bool exit_requested = false; + + while (!exit_requested) + { + bool command_wait_result = m_command_event.WaitInfinitely(); + dICHECK(command_wait_result); + + const dxTHREADCOMMAND command_code = m_command_code; + switch (command_code) + { + case dxTHREAD_COMMAND_EXIT: + { + m_acknowledgement_event.SetEvent(); + + exit_requested = true; + break; + } + + default: + { + dIASSERT(false); + // break; -- proceed to case dxTHREAD_COMMAND_NOOP + } + + case dxTHREAD_COMMAND_NOOP: + { + m_acknowledgement_event.SetEvent(); + + // Do nothing + break; + } + + case dxTHREAD_COMMAND_SERVE_IMPLEMENTATION: + { + const dxServeImplementationParams *serve_params = (const dxServeImplementationParams *)m_command_param; + dThreadingImplementationID impl = serve_params->m_impl; + dxEventObject *ready_wait_event = serve_params->m_ready_wait_event; + + m_acknowledgement_event.SetEvent(); + + ThreadedServeImplementation(impl, ready_wait_event); + break; + } + } + } +} + +void dxThreadPoolThreadInfo::ThreadedServeImplementation(dThreadingImplementationID impl, dxEventObject *ready_wait_event) +{ + ((dxIThreadingImplementation *)impl)->StickToJobsProcessing(&ProcessThreadServeReadiness_Callback, (void *)ready_wait_event); +} + +void dxThreadPoolThreadInfo::ProcessThreadServeReadiness_Callback(void *context) +{ + dxEventObject *ready_wait_event = (dxEventObject *)context; + + ready_wait_event->SetEvent(); +} + + + +struct dxThreadingThreadPool: + public dBase +{ +public: + dxThreadingThreadPool(); + ~dxThreadingThreadPool(); + + bool InitializeThreads(sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags); + +private: + void FinalizeThreads(); + + bool InitializeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags); + void FinalizeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count); + + bool InitializeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info, sizeint stack_size, unsigned int ode_data_allocate_flags); + void FinalizeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info); + +public: + void ServeThreadingImplementation(dThreadingImplementationID impl); + void WaitIdleState(); + +private: + dxThreadPoolThreadInfo *m_thread_infos; + sizeint m_thread_count; + dxEventObject m_ready_wait_event; +}; + + +dxThreadingThreadPool::dxThreadingThreadPool(): +m_thread_infos(NULL), +m_thread_count(0), +m_ready_wait_event() +{ +} + +dxThreadingThreadPool::~dxThreadingThreadPool() +{ + FinalizeThreads(); +} + + +bool dxThreadingThreadPool::InitializeThreads(sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + dIASSERT(m_thread_infos == NULL); + + bool result = false; + + bool wait_event_allocated = false; + + dxThreadPoolThreadInfo *thread_infos = NULL; + bool thread_infos_allocated = false; + + do + { + if (!m_ready_wait_event.InitializeObject(false, false)) + { + break; + } + + wait_event_allocated = true; + + thread_infos = (dxThreadPoolThreadInfo *)dAlloc(thread_count * sizeof(dxThreadPoolThreadInfo)); + if (thread_infos == NULL) + { + break; + } + + thread_infos_allocated = true; + + if (!InitializeIndividualThreadInfos(thread_infos, thread_count, stack_size, ode_data_allocate_flags)) + { + break; + } + + m_thread_infos = thread_infos; + m_thread_count = thread_count; + result = true; + } + while (false); + + if (!result) + { + if (wait_event_allocated) + { + if (thread_infos_allocated) + { + dFree(thread_infos, thread_count * sizeof(dxThreadPoolThreadInfo)); + } + + m_ready_wait_event.FinalizeObject(); + } + } + + return result; +} + +void dxThreadingThreadPool::FinalizeThreads() +{ + dxThreadPoolThreadInfo *thread_infos = m_thread_infos; + if (thread_infos != NULL) + { + sizeint thread_count = m_thread_count; + + FinalizeIndividualThreadInfos(thread_infos, thread_count); + dFree(thread_infos, thread_count * sizeof(dxThreadPoolThreadInfo)); + + m_ready_wait_event.FinalizeObject(); + } +} + + +bool dxThreadingThreadPool::InitializeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count, sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + bool any_fault = false; + + dxThreadPoolThreadInfo *const infos_end = thread_infos + thread_count; + for (dxThreadPoolThreadInfo *current_info = thread_infos; current_info != infos_end; ++current_info) + { + if (!InitializeSingleThreadInfo(current_info, stack_size, ode_data_allocate_flags)) + { + FinalizeIndividualThreadInfos(thread_infos, current_info - thread_infos); + + any_fault = true; + break; + } + } + + bool result = !any_fault; + return result; +} + +void dxThreadingThreadPool::FinalizeIndividualThreadInfos(dxThreadPoolThreadInfo *thread_infos, sizeint thread_count) +{ + dxThreadPoolThreadInfo *const infos_end = thread_infos + thread_count; + for (dxThreadPoolThreadInfo *current_info = thread_infos; current_info != infos_end; ++current_info) + { + FinalizeSingleThreadInfo(current_info); + } +} + + +bool dxThreadingThreadPool::InitializeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info, sizeint stack_size, unsigned int ode_data_allocate_flags) +{ + bool result = false; + + new(thread_info) dxThreadPoolThreadInfo(); + + if (thread_info->Initialize(stack_size, ode_data_allocate_flags)) + { + result = true; + } + else + { + thread_info->dxThreadPoolThreadInfo::~dxThreadPoolThreadInfo(); + } + + return result; +} + +void dxThreadingThreadPool::FinalizeSingleThreadInfo(dxThreadPoolThreadInfo *thread_info) +{ + if (thread_info != NULL) + { + thread_info->dxThreadPoolThreadInfo::~dxThreadPoolThreadInfo(); + } +} + + +void dxThreadingThreadPool::ServeThreadingImplementation(dThreadingImplementationID impl) +{ + dxThreadPoolThreadInfo::dxServeImplementationParams params(impl, &m_ready_wait_event); + + dxThreadPoolThreadInfo *const infos_end = m_thread_infos + m_thread_count; + for (dxThreadPoolThreadInfo *current_info = m_thread_infos; current_info != infos_end; ++current_info) + { + current_info->ExecuteThreadCommand(dxThreadPoolThreadInfo::dxTHREAD_COMMAND_SERVE_IMPLEMENTATION, ¶ms, true); + + bool ready_wait_result = m_ready_wait_event.WaitInfinitely(); + dICHECK(ready_wait_result); + } +} + +void dxThreadingThreadPool::WaitIdleState() +{ + dxThreadPoolThreadInfo *const infos_end = m_thread_infos + m_thread_count; + for (dxThreadPoolThreadInfo *current_info = m_thread_infos; current_info != infos_end; ++current_info) + { + current_info->ExecuteThreadCommand(dxThreadPoolThreadInfo::dxTHREAD_COMMAND_NOOP, NULL, true); + } +} + + +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + +/*extern */dThreadingThreadPoolID dThreadingAllocateThreadPool(unsigned thread_count, + sizeint stack_size, unsigned int ode_data_allocate_flags, void *reserved/*=NULL*/) +{ + dAASSERT(thread_count != 0); + +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = new dxThreadingThreadPool(); + if (thread_pool != NULL) + { + if (thread_pool->InitializeThreads(thread_count, stack_size, ode_data_allocate_flags)) + { + // do nothing + } + else + { + delete thread_pool; + thread_pool = NULL; + } + } +#else + dThreadingThreadPoolID thread_pool = NULL; +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED + + return (dThreadingThreadPoolID)thread_pool; +} + +/*extern */void dThreadingThreadPoolServeMultiThreadedImplementation(dThreadingThreadPoolID pool, dThreadingImplementationID impl) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = (dxThreadingThreadPool *)pool; + thread_pool->ServeThreadingImplementation(impl); +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED +} + +/*extern */void dThreadingThreadPoolWaitIdleState(dThreadingThreadPoolID pool) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = (dxThreadingThreadPool *)pool; + thread_pool->WaitIdleState(); +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED +} + +/*extern */void dThreadingFreeThreadPool(dThreadingThreadPoolID pool) +{ +#if dBUILTIN_THREADING_IMPL_ENABLED + dxThreadingThreadPool *thread_pool = (dxThreadingThreadPool *)pool; + delete thread_pool; +#endif // #if dBUILTIN_THREADING_IMPL_ENABLED +} + + +#endif // #if defined(_WIN32) diff --git a/libs/ode-0.16.1/ode/src/threadingutils.h b/libs/ode-0.16.1/ode/src/threadingutils.h new file mode 100644 index 0000000..fb67052 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/threadingutils.h @@ -0,0 +1,157 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_THREADINGUTILS_H_
+#define _ODE_THREADINGUTILS_H_
+
+
+#include "odeou.h"
+
+
+#if !dTHREADING_INTF_DISABLED
+
+static inline
+bool ThrsafeCompareExchange(volatile atomicord32 *paoDestination, atomicord32 aoComparand, atomicord32 aoExchange)
+{
+ return AtomicCompareExchange(paoDestination, aoComparand, aoExchange);
+}
+
+static inline
+atomicord32 ThrsafeExchange(volatile atomicord32 *paoDestination, atomicord32 aoExchange)
+{
+ return AtomicExchange(paoDestination, aoExchange);
+}
+
+static inline
+void ThrsafeAdd(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
+{
+ AtomicExchangeAddNoResult(paoDestination, aoAddend);
+}
+
+static inline
+atomicord32 ThrsafeExchangeAdd(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
+{
+ return AtomicExchangeAdd(paoDestination, aoAddend);
+}
+
+static inline
+bool ThrsafeCompareExchangePointer(volatile atomicptr *papDestination, atomicptr apComparand, atomicptr apExchange)
+{
+ return AtomicCompareExchangePointer(papDestination, apComparand, apExchange);
+}
+
+static inline
+atomicptr ThrsafeExchangePointer(volatile atomicptr *papDestination, atomicptr apExchange)
+{
+ return AtomicExchangePointer(papDestination, apExchange);
+}
+
+
+#else // #if dTHREADING_INTF_DISABLED
+
+static inline
+bool ThrsafeCompareExchange(volatile atomicord32 *paoDestination, atomicord32 aoComparand, atomicord32 aoExchange)
+{
+ return (*paoDestination == aoComparand) ? ((*paoDestination = aoExchange), true) : false;
+}
+
+static inline
+atomicord32 ThrsafeExchange(volatile atomicord32 *paoDestination, atomicord32 aoExchange)
+{
+ atomicord32 aoDestinationValue = *paoDestination;
+ *paoDestination = aoExchange;
+ return aoDestinationValue;
+}
+
+static inline
+void ThrsafeAdd(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
+{
+ *paoDestination += aoAddend;
+}
+
+static inline
+atomicord32 ThrsafeExchangeAdd(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
+{
+ atomicord32 aoDestinationValue = *paoDestination;
+ *paoDestination += aoAddend;
+ return aoDestinationValue;
+}
+
+static inline
+bool ThrsafeCompareExchangePointer(volatile atomicptr *papDestination, atomicptr apComparand, atomicptr apExchange)
+{
+ return (*papDestination == apComparand) ? ((*papDestination = apExchange), true) : false;
+}
+
+static inline
+atomicptr ThrsafeExchangePointer(volatile atomicptr *papDestination, atomicptr apExchange)
+{
+ atomicptr apDestinationValue = *papDestination;
+ *papDestination = apExchange;
+ return apDestinationValue;
+}
+
+
+#endif // #if dTHREADING_INTF_DISABLED
+
+
+static inline
+unsigned int ThrsafeIncrementIntUpToLimit(volatile atomicord32 *storagePointer, unsigned int limitValue)
+{
+ unsigned int resultValue;
+ while (true) {
+ resultValue = *storagePointer;
+ // The ">=" comparison is used here to allow continuing incrementing the destination
+ // without waiting for all the threads to pass the barrier of checking its value
+ if (resultValue >= limitValue) {
+ resultValue = limitValue;
+ break;
+ }
+ if (ThrsafeCompareExchange(storagePointer, (atomicord32)resultValue, (atomicord32)(resultValue + 1))) {
+ break;
+ }
+ }
+ return resultValue;
+}
+
+static inline
+sizeint ThrsafeIncrementSizeUpToLimit(volatile sizeint *storagePointer, sizeint limitValue)
+{
+ sizeint resultValue;
+ while (true) {
+ resultValue = *storagePointer;
+ // The ">=" comparison is not required here at present ("==" could be used).
+ // It is just used this way to match the other function above.
+ if (resultValue >= limitValue) {
+ resultValue = limitValue;
+ break;
+ }
+ if (ThrsafeCompareExchangePointer((volatile atomicptr *)storagePointer, (atomicptr)resultValue, (atomicptr)(resultValue + 1))) {
+ break;
+ }
+ }
+ return resultValue;
+}
+
+
+
+#endif // _ODE_THREADINGUTILS_H_
diff --git a/libs/ode-0.16.1/ode/src/timer.cpp b/libs/ode-0.16.1/ode/src/timer.cpp new file mode 100644 index 0000000..4f3434a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/timer.cpp @@ -0,0 +1,424 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +TODO +---- + +* gettimeofday() and the pentium time stamp counter return the real time, + not the process time. fix this somehow! + +*/ + +#include <ode/common.h> +#include <ode/timer.h> +#include "config.h" +#include "common.h" + + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// implementation for windows based on the multimedia performance counter. + +#ifdef WIN32 + +#include "windows.h" + +static inline void getClockCount (unsigned long cc[2]) +{ + LARGE_INTEGER a; + QueryPerformanceCounter (&a); + cc[0] = a.LowPart; + cc[1] = a.HighPart; +} + + +static inline void serialize() +{ +} + + +static inline double loadClockCount (unsigned long cc[2]) +{ + LARGE_INTEGER a; + a.LowPart = cc[0]; + a.HighPart = cc[1]; + return double(a.QuadPart); +} + + +double dTimerResolution() +{ + return 1.0/dTimerTicksPerSecond(); +} + + +double dTimerTicksPerSecond() +{ + static int query=0; + static double hz=0.0; + if (!query) { + LARGE_INTEGER a; + QueryPerformanceFrequency (&a); + hz = double(a.QuadPart); + query = 1; + } + return hz; +} + +#endif + +//**************************************************************************** +// implementation based on the pentium time stamp counter. the timer functions +// can be serializing or non-serializing. serializing will ensure that all +// instructions have executed and data has been written back before the cpu +// time stamp counter is read. the CPUID instruction is used to serialize. + +#if defined(PENTIUM) && !defined(WIN32) + +// we need to know the clock rate so that the timing function can report +// accurate times. this number only needs to be set accurately if we're +// doing performance tests and care about real-world time numbers - otherwise, +// just ignore this. i have not worked out how to determine this number +// automatically yet. + +#define PENTIUM_HZ (500e6) + +static inline void getClockCount (unsigned long cc[2]) +{ +#ifndef X86_64_SYSTEM + asm volatile ( + "rdtsc\n" + "movl %%eax,(%%esi)\n" + "movl %%edx,4(%%esi)\n" + : : "S" (cc) : "%eax","%edx","cc","memory"); +#else + asm volatile ( + "rdtsc\n" + "movl %%eax,(%%rsi)\n" + "movl %%edx,4(%%rsi)\n" + : : "S" (cc) : "%eax","%edx","cc","memory"); +#endif +} + + +static inline void serialize() +{ +#ifndef X86_64_SYSTEM + asm volatile ( + "mov $0,%%eax\n" + "push %%ebx\n" + "cpuid\n" + "pop %%ebx\n" + : : : "%eax","%ecx","%edx","cc","memory"); +#else + asm volatile ( + "mov $0,%%rax\n" + "push %%rbx\n" + "cpuid\n" + "pop %%rbx\n" + : : : "%rax","%rcx","%rdx","cc","memory"); +#endif +} + + +static inline double loadClockCount (unsigned long a[2]) +{ + double ret; +#ifndef X86_64_SYSTEM + asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : + "cc","memory"); +#else + asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : + "cc","memory"); +#endif + return ret; +} + + +double dTimerResolution() +{ + return 1.0/PENTIUM_HZ; +} + + +double dTimerTicksPerSecond() +{ + return PENTIUM_HZ; +} + +#endif + +//**************************************************************************** +// otherwise, do the implementation based on gettimeofday(). + +#if !defined(PENTIUM) && !defined(WIN32) + +#ifndef macintosh + +#include <sys/time.h> +#include <unistd.h> + + +static inline void getClockCount (unsigned long cc[2]) +{ + struct timeval tv; + gettimeofday (&tv,0); + cc[0] = tv.tv_usec; + cc[1] = tv.tv_sec; +} + +#else // macintosh + +#include <CoreServices/CoreServices.h> +#include <ode/Timer.h> + +static inline void getClockCount (unsigned long cc[2]) +{ + UnsignedWide ms; + Microseconds (&ms); + cc[1] = ms.lo / 1000000; + cc[0] = ms.lo - ( cc[1] * 1000000 ); +} + +#endif + + +static inline void serialize() +{ +} + + +static inline double loadClockCount (unsigned long a[2]) +{ + return a[1]*1.0e6 + a[0]; +} + + +double dTimerResolution() +{ + unsigned long cc1[2],cc2[2]; + getClockCount (cc1); + do { + getClockCount (cc2); + } + while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); + do { + getClockCount (cc1); + } + while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); + double t1 = loadClockCount (cc1); + double t2 = loadClockCount (cc2); + return (t1-t2) / dTimerTicksPerSecond(); +} + + +double dTimerTicksPerSecond() +{ + return 1000000; +} + +#endif + +//**************************************************************************** +// stop watches + +void dStopwatchReset (dStopwatch *s) +{ + s->time = 0; + s->cc[0] = 0; + s->cc[1] = 0; +} + + +void dStopwatchStart (dStopwatch *s) +{ + serialize(); + getClockCount (s->cc); +} + + +void dStopwatchStop (dStopwatch *s) +{ + unsigned long cc[2]; + serialize(); + getClockCount (cc); + double t1 = loadClockCount (s->cc); + double t2 = loadClockCount (cc); + s->time += t2-t1; +} + + +double dStopwatchTime (dStopwatch *s) +{ + return s->time / dTimerTicksPerSecond(); +} + +//**************************************************************************** +// code timers + +// maximum number of events to record +#define MAXNUM 100 + +static int num = 0; // number of entries used in event array +static struct { + unsigned long cc[2]; // clock counts + double total_t; // total clocks used in this slot. + double total_p; // total percentage points used in this slot. + int count; // number of times this slot has been updated. + const char *description; // pointer to static string +} event[MAXNUM]; + + +// make sure all slot totals and counts reset to 0 at start + +static void initSlots() +{ + static int initialized=0; + if (!initialized) { + for (int i=0; i<MAXNUM; i++) { + event[i].count = 0; + event[i].total_t = 0; + event[i].total_p = 0; + } + initialized = 1; + } +} + + +void dTimerStart (const char *description) +{ + initSlots(); + event[0].description = const_cast<char*> (description); + num = 1; + serialize(); + getClockCount (event[0].cc); +} + + +void dTimerNow (const char *description) +{ + if (num < MAXNUM) { + // do not serialize + getClockCount (event[num].cc); + event[num].description = const_cast<char*> (description); + num++; + } +} + + +void dTimerEnd() +{ + if (num < MAXNUM) { + serialize(); + getClockCount (event[num].cc); + event[num].description = "TOTAL"; + num++; + } +} + +//**************************************************************************** +// print report + +static void fprintDoubleWithPrefix (FILE *f, double a, const char *fmt) +{ + if (a >= 0.999999) { + fprintf (f,fmt,a); + return; + } + a *= 1000.0; + if (a >= 0.999999) { + fprintf (f,fmt,a); + fprintf (f,"m"); + return; + } + a *= 1000.0; + if (a >= 0.999999) { + fprintf (f,fmt,a); + fprintf (f,"u"); + return; + } + a *= 1000.0; + fprintf (f,fmt,a); + fprintf (f,"n"); +} + + +void dTimerReport (FILE *fout, int average) +{ + int i; + sizeint maxl; + double ccunit = 1.0/dTimerTicksPerSecond(); + fprintf (fout,"\nTimer Report ("); + fprintDoubleWithPrefix (fout,ccunit,"%.2f "); + fprintf (fout,"s resolution)\n------------\n"); + if (num < 1) return; + + // get maximum description length + maxl = 0; + for (i=0; i<num; i++) { + sizeint l = strlen (event[i].description); + if (l > maxl) maxl = l; + } + + // calculate total time + double t1 = loadClockCount (event[0].cc); + double t2 = loadClockCount (event[num-1].cc); + double total = t2 - t1; + if (total <= 0) total = 1; + + // compute time difference for all slots except the last one. update totals + double *times = (double*) ALLOCA (num * sizeof(double)); + for (i=0; i < (num-1); i++) { + double t1 = loadClockCount (event[i].cc); + double t2 = loadClockCount (event[i+1].cc); + times[i] = t2 - t1; + event[i].count++; + event[i].total_t += times[i]; + event[i].total_p += times[i]/total * 100.0; + } + + // print report (with optional averages) + for (i=0; i<num; i++) { + double t,p; + if (i < (num-1)) { + t = times[i]; + p = t/total * 100.0; + } + else { + t = total; + p = 100.0; + } + fprintf (fout,"%-*s %7.2fms %6.2f%%",(int)maxl,event[i].description, + t*ccunit * 1000.0, p); + if (average && i < (num-1)) { + fprintf (fout," (avg %7.2fms %6.2f%%)", + (event[i].total_t / event[i].count)*ccunit * 1000.0, + event[i].total_p / event[i].count); + } + fprintf (fout,"\n"); + } + fprintf (fout,"\n"); +} diff --git a/libs/ode-0.16.1/ode/src/typedefs.h b/libs/ode-0.16.1/ode/src/typedefs.h new file mode 100644 index 0000000..c8164c3 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/typedefs.h @@ -0,0 +1,74 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_TYPEDEFS_H_ +#define _ODE_TYPEDEFS_H_ + +#include <ode/odeconfig.h> + +#include "error.h" + + +/* + * Internal typedefs to map public types into more convenient private types + */ + + +typedef dint64 int64; +dSASSERT(sizeof(int64) == 8); + +typedef duint64 uint64; +dSASSERT(sizeof(uint64) == 8); + +typedef dint32 int32; +dSASSERT(sizeof(int32) == 4); + +typedef duint32 uint32; +dSASSERT(sizeof(uint32) == 4); + +typedef dint16 int16; +dSASSERT(sizeof(int16) == 2); + +typedef duint16 uint16; +dSASSERT(sizeof(uint16) == 2); + +typedef dint8 int8; +dSASSERT(sizeof(int8) == 1); + +typedef duint8 uint8; +dSASSERT(sizeof(uint8) == 1); + + +typedef dintptr intptr; +dSASSERT(sizeof(intptr) == sizeof(void *)); + +typedef duintptr uintptr; +dSASSERT(sizeof(uintptr) == sizeof(void *)); + +typedef ddiffint diffint; +dSASSERT(sizeof(diffint) == sizeof(void *)); // So far, we choose to not support systems that have accessible memory segment size smaller than the pointer size + +typedef dsizeint sizeint; +dSASSERT(sizeof(sizeint) == sizeof(void *)); // So far, we choose to not support systems that have accessible memory segment size smaller than the pointer size + + +#endif diff --git a/libs/ode-0.16.1/ode/src/util.cpp b/libs/ode-0.16.1/ode/src/util.cpp new file mode 100644 index 0000000..17b9e8a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/util.cpp @@ -0,0 +1,1231 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include "config.h" +#include "util.h" +#include "objects.h" +#include "joints/joint.h" +#include "threadingutils.h" + +#include <new> + + +#define dMIN(A,B) ((A)>(B) ? (B) : (A)) +#define dMAX(A,B) ((B)>(A) ? (B) : (A)) + + +//**************************************************************************** +// Malloc based world stepping memory manager + +/*extern */dxWorldProcessMemoryManager g_WorldProcessMallocMemoryManager(dAlloc, dRealloc, dFree); +/*extern */dxWorldProcessMemoryReserveInfo g_WorldProcessDefaultReserveInfo(dWORLDSTEP_RESERVEFACTOR_DEFAULT, dWORLDSTEP_RESERVESIZE_DEFAULT); + + +//**************************************************************************** +// dxWorldProcessContext + +const char *const dxWorldProcessContext::m_aszContextMutexNames[dxPCM__MAX] = +{ + "Stepper Arena Obtain Lock" , // dxPCM_STEPPER_ARENA_OBTAIN, + "Joint addLimot Serialize Lock" , // dxPCM_STEPPER_ADDLIMOT_SERIALIZE + "Stepper StepBody Serialize Lock" , // dxPCM_STEPPER_STEPBODY_SERIALIZE, +}; + +dxWorldProcessContext::dxWorldProcessContext(): + m_pmaIslandsArena(NULL), + m_pmaStepperArenas(NULL), + m_pswObjectsAllocWorld(NULL), + m_pmgStepperMutexGroup(NULL), + m_pcwIslandsSteppingWait(NULL) +{ + // Do nothing +} + +dxWorldProcessContext::~dxWorldProcessContext() +{ + dIASSERT((m_pswObjectsAllocWorld != NULL) == (m_pmgStepperMutexGroup != NULL)); + dIASSERT((m_pswObjectsAllocWorld != NULL) == (m_pcwIslandsSteppingWait != NULL)); + + if (m_pswObjectsAllocWorld != NULL) + { + m_pswObjectsAllocWorld->FreeMutexGroup(m_pmgStepperMutexGroup); + // m_pswObjectsAllocWorld->FreeThreadedCallWait(m_pcwIslandsSteppingWait); -- The stock call wait can not be freed + } + + dxWorldProcessMemArena *pmaStepperArenas = m_pmaStepperArenas; + if (pmaStepperArenas != NULL) + { + FreeArenasList(pmaStepperArenas); + } + + if (m_pmaIslandsArena != NULL) + { + dxWorldProcessMemArena::FreeMemArena(m_pmaIslandsArena); + } +} + +void dxWorldProcessContext::CleanupWorldReferences(dxWorld *pswWorldInstance) +{ + dIASSERT((m_pswObjectsAllocWorld != NULL) == (m_pmgStepperMutexGroup != NULL)); + dIASSERT((m_pswObjectsAllocWorld != NULL) == (m_pcwIslandsSteppingWait != NULL)); + + if (m_pswObjectsAllocWorld == pswWorldInstance) + { + m_pswObjectsAllocWorld->FreeMutexGroup(m_pmgStepperMutexGroup); + // m_pswObjectsAllocWorld->FreeThreadedCallWait(m_pcwIslandsSteppingWait); -- The stock call wait can not be freed + + m_pswObjectsAllocWorld = NULL; + m_pmgStepperMutexGroup = NULL; + m_pcwIslandsSteppingWait = NULL; + } +} + +bool dxWorldProcessContext::EnsureStepperSyncObjectsAreAllocated(dxWorld *pswWorldInstance) +{ + dIASSERT((m_pswObjectsAllocWorld != NULL) == (m_pmgStepperMutexGroup != NULL)); + dIASSERT((m_pswObjectsAllocWorld != NULL) == (m_pcwIslandsSteppingWait != NULL)); + + bool bResult = false; + + dMutexGroupID pmbStepperMutexGroup = NULL; + bool bStepperMutexGroupAllocated = false; + + do + { + if (m_pswObjectsAllocWorld == NULL) + { + pmbStepperMutexGroup = pswWorldInstance->AllocMutexGroup(dxPCM__MAX, m_aszContextMutexNames); + if (pmbStepperMutexGroup == NULL) + { + break; + } + + bStepperMutexGroupAllocated = true; + + dCallWaitID pcwIslandsSteppingWait = pswWorldInstance->AllocateOrRetrieveStockCallWaitID(); + if (pcwIslandsSteppingWait == NULL) + { + break; + } + + m_pswObjectsAllocWorld = pswWorldInstance; + m_pmgStepperMutexGroup = pmbStepperMutexGroup; + m_pcwIslandsSteppingWait = pcwIslandsSteppingWait; + } + + bResult = true; + } + while (false); + + if (!bResult) + { + if (bStepperMutexGroupAllocated) + { + pswWorldInstance->FreeMutexGroup(pmbStepperMutexGroup); + } + } + + return bResult; +} + + +dxWorldProcessMemArena *dxWorldProcessContext::ObtainStepperMemArena() +{ + dxWorldProcessMemArena *pmaArenaInstance = NULL; + + while (true) + { + dxWorldProcessMemArena *pmaRawArenasHead = GetStepperArenasHead(); + if (pmaRawArenasHead == NULL) + { + break; + } + + // Extraction must be locked so that other thread does not "steal" head arena, + // use it and then reinsert back with a different "next" + dxMutexGroupLockHelper lhLockHelper(m_pswObjectsAllocWorld, m_pmgStepperMutexGroup, dxPCM_STEPPER_ARENA_OBTAIN); + + dxWorldProcessMemArena *pmaArenasHead = GetStepperArenasHead(); // Arenas head must be re-extracted after mutex has been locked + bool bExchangeResult = pmaArenasHead != NULL && TryExtractingStepperArenasHead(pmaArenasHead); + + lhLockHelper.UnlockMutex(); + + if (bExchangeResult) + { + pmaArenasHead->ResetState(); + pmaArenaInstance = pmaArenasHead; + break; + } + } + + return pmaArenaInstance; +} + +void dxWorldProcessContext::ReturnStepperMemArena(dxWorldProcessMemArena *pmaArenaInstance) +{ + while (true) + { + dxWorldProcessMemArena *pmaArenasHead = GetStepperArenasHead(); + pmaArenaInstance->SetNextMemArena(pmaArenasHead); + + if (TryInsertingStepperArenasHead(pmaArenaInstance, pmaArenasHead)) + { + break; + } + } +} + + +dxWorldProcessMemArena *dxWorldProcessContext::ReallocateIslandsMemArena(sizeint nMemoryRequirement, + const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum) +{ + dxWorldProcessMemArena *pmaExistingArena = GetIslandsMemArena(); + dxWorldProcessMemArena *pmaNewMemArena = dxWorldProcessMemArena::ReallocateMemArena(pmaExistingArena, nMemoryRequirement, pmmMemortManager, fReserveFactor, uiReserveMinimum); + SetIslandsMemArena(pmaNewMemArena); + + pmaNewMemArena->ResetState(); + + return pmaNewMemArena; +} + +bool dxWorldProcessContext::ReallocateStepperMemArenas( + dxWorld *world, unsigned nIslandThreadsCount, sizeint nMemoryRequirement, + const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum) +{ + dxWorldProcessMemArena *pmaRebuiltArenasHead = NULL, *pmaRebuiltArenasTail = NULL; + dxWorldProcessMemArena *pmaExistingArenas = GetStepperArenasList(); + unsigned nArenasToProcess = nIslandThreadsCount; + + (void)world; // unused + + // NOTE! + // The list is reallocated in a way to assure the largest arenas are at end + // and if number of threads decreases they will be freed first of all. + + while (true) + { + if (nArenasToProcess == 0) + { + FreeArenasList(pmaExistingArenas); + break; + } + + dxWorldProcessMemArena *pmaOldMemArena = pmaExistingArenas; + + if (pmaExistingArenas != NULL) + { + pmaExistingArenas = pmaExistingArenas->GetNextMemArena(); + } + else + { + // If existing arenas ended, terminate and erase tail so that new arenas + // would be appended to list head. + if (pmaRebuiltArenasTail != NULL) + { + pmaRebuiltArenasTail->SetNextMemArena(NULL); + pmaRebuiltArenasTail = NULL; + } + } + + dxWorldProcessMemArena *pmaNewMemArena = dxWorldProcessMemArena::ReallocateMemArena(pmaOldMemArena, nMemoryRequirement, pmmMemortManager, fReserveFactor, uiReserveMinimum); + + if (pmaNewMemArena != NULL) + { + // Append reallocated arenas to list tail while old arenas still exist... + if (pmaRebuiltArenasTail != NULL) + { + pmaRebuiltArenasTail->SetNextMemArena(pmaNewMemArena); + pmaRebuiltArenasTail = pmaNewMemArena; + } + else if (pmaRebuiltArenasHead == NULL) + { + pmaRebuiltArenasHead = pmaNewMemArena; + pmaRebuiltArenasTail = pmaNewMemArena; + } + // ...and append them to list head if those are additional arenas created + else + { + pmaNewMemArena->SetNextMemArena(pmaRebuiltArenasHead); + pmaRebuiltArenasHead = pmaNewMemArena; + } + + --nArenasToProcess; + } + else if (pmaOldMemArena == NULL) + { + break; + } + } + + if (pmaRebuiltArenasTail != NULL) + { + pmaRebuiltArenasTail->SetNextMemArena(NULL); + } + + SetStepperArenasList(pmaRebuiltArenasHead); + + bool bResult = nArenasToProcess == 0; + return bResult; +} + +void dxWorldProcessContext::FreeArenasList(dxWorldProcessMemArena *pmaExistingArenas) +{ + while (pmaExistingArenas != NULL) + { + dxWorldProcessMemArena *pmaCurrentMemArena = pmaExistingArenas; + pmaExistingArenas = pmaExistingArenas->GetNextMemArena(); + + dxWorldProcessMemArena::FreeMemArena(pmaCurrentMemArena); + } +} + +dxWorldProcessMemArena *dxWorldProcessContext::GetStepperArenasHead() const +{ + return m_pmaStepperArenas; +} + +bool dxWorldProcessContext::TryExtractingStepperArenasHead(dxWorldProcessMemArena *pmaHeadInstance) +{ + dxWorldProcessMemArena *pmaNextInstance = pmaHeadInstance->GetNextMemArena(); + return ThrsafeCompareExchangePointer((volatile atomicptr *)&m_pmaStepperArenas, (atomicptr)pmaHeadInstance, (atomicptr)pmaNextInstance); +} + +bool dxWorldProcessContext::TryInsertingStepperArenasHead(dxWorldProcessMemArena *pmaArenaInstance, dxWorldProcessMemArena *pmaExistingHead) +{ + return ThrsafeCompareExchangePointer((volatile atomicptr *)&m_pmaStepperArenas, (atomicptr)pmaExistingHead, (atomicptr)pmaArenaInstance); +} + + +void dxWorldProcessContext::LockForAddLimotSerialization() +{ + m_pswObjectsAllocWorld->LockMutexGroupMutex(m_pmgStepperMutexGroup, dxPCM_STEPPER_ADDLIMOT_SERIALIZE); +} + +void dxWorldProcessContext::UnlockForAddLimotSerialization() +{ + m_pswObjectsAllocWorld->UnlockMutexGroupMutex(m_pmgStepperMutexGroup, dxPCM_STEPPER_ADDLIMOT_SERIALIZE); +} + + +void dxWorldProcessContext::LockForStepbodySerialization() +{ + m_pswObjectsAllocWorld->LockMutexGroupMutex(m_pmgStepperMutexGroup, dxPCM_STEPPER_STEPBODY_SERIALIZE); +} + +void dxWorldProcessContext::UnlockForStepbodySerialization() +{ + m_pswObjectsAllocWorld->UnlockMutexGroupMutex(m_pmgStepperMutexGroup, dxPCM_STEPPER_STEPBODY_SERIALIZE); +} + + +//**************************************************************************** +// Threading call contexts + +struct dxSingleIslandCallContext; + +struct dxIslandsProcessingCallContext +{ + dxIslandsProcessingCallContext(dxWorld *world, const dxWorldProcessIslandsInfo &islandsInfo, dReal stepSize, dstepper_fn_t stepper): + m_world(world), m_islandsInfo(islandsInfo), m_stepSize(stepSize), m_stepper(stepper), + m_groupReleasee(NULL), m_islandToProcessStorage(0), m_stepperAllowedThreads(0) + { + } + + void AssignGroupReleasee(dCallReleaseeID groupReleasee) { m_groupReleasee = groupReleasee; } + void SetStepperAllowedThreads(unsigned allowedThreadsLimit) { m_stepperAllowedThreads = allowedThreadsLimit; } + + static int ThreadedProcessGroup_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + bool ThreadedProcessGroup(); + + static int ThreadedProcessJobStart_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + void ThreadedProcessJobStart(); + + static int ThreadedProcessIslandSearch_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + void ThreadedProcessIslandSearch(dxSingleIslandCallContext *stepperCallContext); + + static int ThreadedProcessIslandStepper_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee); + void ThreadedProcessIslandStepper(dxSingleIslandCallContext *stepperCallContext); + + sizeint ObtainNextIslandToBeProcessed(sizeint islandsCount); + + dxWorld *const m_world; + dxWorldProcessIslandsInfo const &m_islandsInfo; + dReal const m_stepSize; + dstepper_fn_t const m_stepper; + dCallReleaseeID m_groupReleasee; + sizeint volatile m_islandToProcessStorage; + unsigned m_stepperAllowedThreads; +}; + + +struct dxSingleIslandCallContext +{ + dxSingleIslandCallContext(dxIslandsProcessingCallContext *islandsProcessingContext, + dxWorldProcessMemArena *stepperArena, void *arenaInitialState, + dxBody *const *islandBodiesStart, dxJoint *const *islandJointsStart): + m_islandsProcessingContext(islandsProcessingContext), m_islandIndex(0), + m_stepperArena(stepperArena), m_arenaInitialState(arenaInitialState), + m_stepperCallContext(islandsProcessingContext->m_world, islandsProcessingContext->m_stepSize, islandsProcessingContext->m_stepperAllowedThreads, stepperArena, islandBodiesStart, islandJointsStart) + { + } + + void AssignIslandSearchProgress(sizeint islandIndex) + { + m_islandIndex = islandIndex; + } + + void AssignIslandSelection(dxBody *const *islandBodiesStart, dxJoint *const *islandJointsStart, + unsigned islandBodiesCount, unsigned islandJointsCount) + { + m_stepperCallContext.AssignIslandSelection(islandBodiesStart, islandJointsStart, islandBodiesCount, islandJointsCount); + } + + dxBody *const *GetSelectedIslandBodiesEnd() const { return m_stepperCallContext.GetSelectedIslandBodiesEnd(); } + dxJoint *const *GetSelectedIslandJointsEnd() const { return m_stepperCallContext.GetSelectedIslandJointsEnd(); } + + void RestoreSavedMemArenaStateForStepper() + { + m_stepperArena->RestoreState(m_arenaInitialState); + } + + void AssignStepperCallFinalReleasee(dCallReleaseeID finalReleasee) + { + m_stepperCallContext.AssignStepperCallFinalReleasee(finalReleasee); + } + + dxIslandsProcessingCallContext *m_islandsProcessingContext; + sizeint m_islandIndex; + dxWorldProcessMemArena *m_stepperArena; + void *m_arenaInitialState; + dxStepperProcessingCallContext m_stepperCallContext; +}; + + +//**************************************************************************** +// Auto disabling + +void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize) +{ + dxBody *bb; + for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next ) + { + // don't freeze objects mid-air (patch 1586738) + if ( bb->firstjoint == NULL ) continue; + + // nothing to do unless this body is currently enabled and has + // the auto-disable flag set + if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue; + + // if sampling / threshold testing is disabled, we can never sleep. + if ( bb->adis.average_samples == 0 ) continue; + + // + // see if the body is idle + // + +#ifndef dNODEBUG + // sanity check + if ( bb->average_counter >= bb->adis.average_samples ) + { + dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" ); + + // something is going wrong, reset the average-calculations + bb->average_ready = 0; // not ready for average calculation + bb->average_counter = 0; // reset the buffer index + } +#endif // dNODEBUG + + // sample the linear and angular velocity + bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0]; + bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1]; + bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2]; + bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0]; + bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1]; + bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2]; + bb->average_counter++; + + // buffer ready test + if ( bb->average_counter >= bb->adis.average_samples ) + { + bb->average_counter = 0; // fill the buffer from the beginning + bb->average_ready = 1; // this body is ready now for average calculation + } + + int idle = 0; // Assume it's in motion unless we have samples to disprove it. + + // enough samples? + if ( bb->average_ready ) + { + idle = 1; // Initial assumption: IDLE + + // the sample buffers are filled and ready for calculation + dVector3 average_lvel, average_avel; + + // Store first velocity samples + average_lvel[0] = bb->average_lvel_buffer[0][0]; + average_avel[0] = bb->average_avel_buffer[0][0]; + average_lvel[1] = bb->average_lvel_buffer[0][1]; + average_avel[1] = bb->average_avel_buffer[0][1]; + average_lvel[2] = bb->average_lvel_buffer[0][2]; + average_avel[2] = bb->average_avel_buffer[0][2]; + + // If we're not in "instantaneous mode" + if ( bb->adis.average_samples > 1 ) + { + // add remaining velocities together + for ( unsigned int i = 1; i < bb->adis.average_samples; ++i ) + { + average_lvel[0] += bb->average_lvel_buffer[i][0]; + average_avel[0] += bb->average_avel_buffer[i][0]; + average_lvel[1] += bb->average_lvel_buffer[i][1]; + average_avel[1] += bb->average_avel_buffer[i][1]; + average_lvel[2] += bb->average_lvel_buffer[i][2]; + average_avel[2] += bb->average_avel_buffer[i][2]; + } + + // make average + dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples ); + + average_lvel[0] *= r1; + average_avel[0] *= r1; + average_lvel[1] *= r1; + average_avel[1] *= r1; + average_lvel[2] *= r1; + average_avel[2] *= r1; + } + + // threshold test + dReal av_lspeed, av_aspeed; + av_lspeed = dCalcVectorDot3( average_lvel, average_lvel ); + if ( av_lspeed > bb->adis.linear_average_threshold ) + { + idle = 0; // average linear velocity is too high for idle + } + else + { + av_aspeed = dCalcVectorDot3( average_avel, average_avel ); + if ( av_aspeed > bb->adis.angular_average_threshold ) + { + idle = 0; // average angular velocity is too high for idle + } + } + } + + // if it's idle, accumulate steps and time. + // these counters won't overflow because this code doesn't run for disabled bodies. + if (idle) { + bb->adis_stepsleft--; + bb->adis_timeleft -= stepsize; + } + else { + // Reset countdowns + bb->adis_stepsleft = bb->adis.idle_steps; + bb->adis_timeleft = bb->adis.idle_time; + } + + // disable the body if it's idle for a long enough time + if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 ) + { + bb->flags |= dxBodyDisabled; // set the disable flag + + // disabling bodies should also include resetting the velocity + // should prevent jittering in big "islands" + bb->lvel[0] = 0; + bb->lvel[1] = 0; + bb->lvel[2] = 0; + bb->avel[0] = 0; + bb->avel[1] = 0; + bb->avel[2] = 0; + } + } +} + + +//**************************************************************************** +// body rotation + +// return sin(x)/x. this has a singularity at 0 so special handling is needed +// for small arguments. + +static inline dReal sinc (dReal x) +{ + // if |x| < 1e-4 then use a taylor series expansion. this two term expansion + // is actually accurate to one LS bit within this range if double precision + // is being used - so don't worry! + if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667); + else return dSin(x)/x; +} + + +// given a body b, apply its linear and angular rotation over the time +// interval h, thereby adjusting its position and orientation. + +void dxStepBody (dxBody *b, dReal h) +{ + // cap the angular velocity + if (b->flags & dxBodyMaxAngularSpeed) { + const dReal max_ang_speed = b->max_angular_speed; + const dReal aspeed = dCalcVectorDot3( b->avel, b->avel ); + if (aspeed > max_ang_speed*max_ang_speed) { + const dReal coef = max_ang_speed/dSqrt(aspeed); + dScaleVector3(b->avel, coef); + } + } + // end of angular velocity cap + + + // handle linear velocity + for (unsigned int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv; // finite rotation vector + dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL(0.5); + dReal theta = k * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + + b->avel[2]*b->avel[2]); + h *= REAL(0.5); + dReal theta = wlen * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2,q,b->q); + for (unsigned int j=0; j<4; j++) b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) { + dReal dq[4]; + dWtoDQ (irv,b->q,dq); + for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j]; + } + } + else { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel,b->q,dq); + for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q,b->posr.R); + + // notify all attached geoms that this body has moved + dxWorldProcessContext *world_process_context = b->world->unsafeGetWorldProcessingContext(); + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) { + world_process_context->LockForStepbodySerialization(); + dGeomMoved (geom); + world_process_context->UnlockForStepbodySerialization(); + } + + // notify the user + if (b->moved_callback != NULL) { + b->moved_callback(b); + } + + // damping + if (b->flags & dxBodyLinearDamping) { + const dReal lin_threshold = b->dampingp.linear_threshold; + const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel ); + if ( lin_speed > lin_threshold) { + const dReal k = 1 - b->dampingp.linear_scale; + dScaleVector3(b->lvel, k); + } + } + if (b->flags & dxBodyAngularDamping) { + const dReal ang_threshold = b->dampingp.angular_threshold; + const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel ); + if ( ang_speed > ang_threshold) { + const dReal k = 1 - b->dampingp.angular_scale; + dScaleVector3(b->avel, k); + } + } +} + + +//**************************************************************************** +// island processing + +enum dxISLANDSIZESELEMENT +{ + dxISE_BODIES_COUNT, + dxISE_JOINTS_COUNT, + + dxISE__MAX +}; + +// This estimates dynamic memory requirements for dxProcessIslands +static sizeint EstimateIslandProcessingMemoryRequirements(dxWorld *world) +{ + sizeint res = 0; + + sizeint islandcounts = dEFFICIENT_SIZE((sizeint)(unsigned)world->nb * 2 * sizeof(int)); + res += islandcounts; + + sizeint bodiessize = dEFFICIENT_SIZE((sizeint)(unsigned)world->nb * sizeof(dxBody*)); + sizeint jointssize = dEFFICIENT_SIZE((sizeint)(unsigned)world->nj * sizeof(dxJoint*)); + res += bodiessize + jointssize; + + sizeint sesize = (bodiessize < jointssize) ? bodiessize : jointssize; + res += sesize; + + return res; +} + +static sizeint BuildIslandsAndEstimateStepperMemoryRequirements( + dxWorldProcessIslandsInfo &islandsinfo, dxWorldProcessMemArena *memarena, + dxWorld *world, dReal stepsize, dmemestimate_fn_t stepperestimate) +{ + sizeint maxreq = 0; + + // handle auto-disabling of bodies + dInternalHandleAutoDisabling (world,stepsize); + + unsigned int nb = world->nb, nj = world->nj; + // Make array for island body/joint counts + unsigned int *islandsizes = memarena->AllocateArray<unsigned int>(2 * (sizeint)nb); + unsigned int *sizescurr; + + // make arrays for body and joint lists (for a single island) to go into + dxBody **body = memarena->AllocateArray<dxBody *>(nb); + dxJoint **joint = memarena->AllocateArray<dxJoint *>(nj); + + BEGIN_STATE_SAVE(memarena, stackstate) { + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + unsigned int stackalloc = (nj < nb) ? nj : nb; + dxBody **stack = memarena->AllocateArray<dxBody *>(stackalloc); + + { + // set all body/joint tags to 0 + for (dxBody *b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; + for (dxJoint *j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; + } + + sizescurr = islandsizes; + dxBody **bodystart = body; + dxJoint **jointstart = joint; + for (dxBody *bb=world->firstbody; bb; bb=(dxBody*)bb->next) { + // get bb = the next enabled, untagged body, and tag it + if (!bb->tag) { + if (!(bb->flags & dxBodyDisabled)) { + bb->tag = 1; + + dxBody **bodycurr = bodystart; + dxJoint **jointcurr = jointstart; + + // tag all bodies and joints starting from bb. + *bodycurr++ = bb; + + unsigned int stacksize = 0; + dxBody *b = bb; + + while (true) { + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + dxJoint *njoint = n->joint; + if (!njoint->tag) { + if (njoint->isEnabled()) { + njoint->tag = 1; + *jointcurr++ = njoint; + + dxBody *nbody = n->body; + // Body disabled flag is not checked here. This is how auto-enable works. + if (nbody && nbody->tag <= 0) { + nbody->tag = 1; + // Make sure all bodies are in the enabled state. + nbody->flags &= ~dxBodyDisabled; + stack[stacksize++] = nbody; + } + } else { + njoint->tag = -1; // Used in Step to prevent search over disabled joints (not needed for QuickStep so far) + } + } + } + dIASSERT(stacksize <= (unsigned int)world->nb); + dIASSERT(stacksize <= (unsigned int)world->nj); + + if (stacksize == 0) { + break; + } + + b = stack[--stacksize]; // pop body off stack + *bodycurr++ = b; // put body on body list + } + + unsigned int bcount = (unsigned int)(bodycurr - bodystart); + unsigned int jcount = (unsigned int)(jointcurr - jointstart); + dIASSERT((sizeint)(bodycurr - bodystart) <= (sizeint)UINT_MAX); + dIASSERT((sizeint)(jointcurr - jointstart) <= (sizeint)UINT_MAX); + + sizescurr[dxISE_BODIES_COUNT] = bcount; + sizescurr[dxISE_JOINTS_COUNT] = jcount; + sizescurr += dxISE__MAX; + + sizeint islandreq = stepperestimate(bodystart, bcount, jointstart, jcount); + maxreq = (maxreq > islandreq) ? maxreq : islandreq; + + bodystart = bodycurr; + jointstart = jointcurr; + } else { + bb->tag = -1; // Not used so far (assigned to retain consistency with joints) + } + } + } + } END_STATE_SAVE(memarena, stackstate); + +# ifndef dNODEBUG + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. + { + for (dxBody *b=world->firstbody; b; b=(dxBody*)b->next) { + if (b->flags & dxBodyDisabled) { + if (b->tag > 0) dDebug (0,"disabled body tagged"); + } + else { + if (b->tag <= 0) dDebug (0,"enabled body not tagged"); + } + } + for (dxJoint *j=world->firstjoint; j; j=(dxJoint*)j->next) { + if ( (( j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0 ) || + (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0) ) + && + j->isEnabled() ) { + if (j->tag <= 0) dDebug (0,"attached enabled joint not tagged"); + } + else { + if (j->tag > 0) dDebug (0,"unattached or disabled joint tagged"); + } + } + } +# endif + + sizeint islandcount = ((sizeint)(sizescurr - islandsizes) / dxISE__MAX); + islandsinfo.AssignInfo(islandcount, islandsizes, body, joint); + + return maxreq; +} + +static unsigned EstimateIslandProcessingSimultaneousCallsMaximumCount(unsigned activeThreadCount, unsigned islandsAllowedThreadCount, + unsigned stepperAllowedThreadCount, dmaxcallcountestimate_fn_t maxCallCountEstimator) +{ + unsigned stepperCallsMaximum = maxCallCountEstimator(activeThreadCount, stepperAllowedThreadCount); + unsigned islandsIntermediateCallsMaximum = (1 + 2); // ThreadedProcessIslandSearch_Callback + (ThreadedProcessIslandStepper_Callback && ThreadedProcessIslandSearch_Callback) + + unsigned result = + 1 // ThreadedProcessGroup_Callback + + islandsAllowedThreadCount * dMAX(stepperCallsMaximum, islandsIntermediateCallsMaximum) + + dMIN(islandsAllowedThreadCount, (unsigned)(activeThreadCount - islandsAllowedThreadCount)) // ThreadedProcessJobStart_Callback + /*...the end*/; + return result; +} + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. +bool dxProcessIslands (dxWorld *world, const dxWorldProcessIslandsInfo &islandsInfo, + dReal stepSize, dstepper_fn_t stepper, dmaxcallcountestimate_fn_t maxCallCountEstimator) +{ + bool result = false; + + dxIslandsProcessingCallContext callContext(world, islandsInfo, stepSize, stepper); + + do { + dxStepWorkingMemory *wmem = world->wmem; + dIASSERT(wmem != NULL); + dxWorldProcessContext *context = wmem->GetWorldProcessingContext(); + dIASSERT(context != NULL); + dCallWaitID pcwGroupCallWait = context->GetIslandsSteppingWait(); + + int summaryFault = 0; + + unsigned activeThreadCount; + const unsigned islandsAllowedThreadCount = world->calculateIslandProcessingMaxThreadCount(&activeThreadCount); + dIASSERT(islandsAllowedThreadCount != 0); + dIASSERT(activeThreadCount >= islandsAllowedThreadCount); + + unsigned stepperAllowedThreadCount = islandsAllowedThreadCount; // For now, set stepper allowed threads equal to island stepping threads + + unsigned simultaneousCallsCount = EstimateIslandProcessingSimultaneousCallsMaximumCount(activeThreadCount, islandsAllowedThreadCount, stepperAllowedThreadCount, maxCallCountEstimator); + if (!world->PreallocateResourcesForThreadedCalls(simultaneousCallsCount)) { + break; + } + + dCallReleaseeID groupReleasee; + // First post a group call with dependency count set to number of expected threads + world->PostThreadedCall(&summaryFault, &groupReleasee, islandsAllowedThreadCount, NULL, pcwGroupCallWait, + &dxIslandsProcessingCallContext::ThreadedProcessGroup_Callback, (void *)&callContext, 0, "World Islands Stepping Group"); + + callContext.AssignGroupReleasee(groupReleasee); + callContext.SetStepperAllowedThreads(stepperAllowedThreadCount); + + // Summary fault flag may be omitted as any failures will automatically propagate to dependent releasee (i.e. to groupReleasee) + world->PostThreadedCallsGroup(NULL, islandsAllowedThreadCount, groupReleasee, + &dxIslandsProcessingCallContext::ThreadedProcessJobStart_Callback, (void *)&callContext, "World Islands Stepping Start"); + + // Wait until group completes (since jobs were the dependencies of the group the group is going to complete only after all the jobs end) + world->WaitThreadedCallExclusively(NULL, pcwGroupCallWait, NULL, "World Islands Stepping Wait"); + + if (summaryFault != 0) { + break; + } + + result = true; + } + while (false); + + return result; +} + + +int dxIslandsProcessingCallContext::ThreadedProcessGroup_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee) +{ + (void)callInstanceIndex; // unused + (void)callThisReleasee; // unused + return static_cast<dxIslandsProcessingCallContext *>(callContext)->ThreadedProcessGroup(); +} + +bool dxIslandsProcessingCallContext::ThreadedProcessGroup() +{ + // Do nothing - it's just a wrapper call + return true; +} + +int dxIslandsProcessingCallContext::ThreadedProcessJobStart_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee) +{ + (void)callInstanceIndex; // unused + (void)callThisReleasee; // unused + static_cast<dxIslandsProcessingCallContext *>(callContext)->ThreadedProcessJobStart(); + return true; +} + +void dxIslandsProcessingCallContext::ThreadedProcessJobStart() +{ + dxWorldProcessContext *context = m_world->unsafeGetWorldProcessingContext(); + + dxWorldProcessMemArena *stepperArena = context->ObtainStepperMemArena(); + dIASSERT(stepperArena != NULL && stepperArena->IsStructureValid()); + + const dxWorldProcessIslandsInfo &islandsInfo = m_islandsInfo; + dxBody *const *islandBodiesStart = islandsInfo.GetBodiesArray(); + dxJoint *const *islandJointsStart = islandsInfo.GetJointsArray(); + + dxSingleIslandCallContext *stepperCallContext = (dxSingleIslandCallContext *)stepperArena->AllocateBlock(sizeof(dxSingleIslandCallContext)); + // Save area state after context allocation to be restored for the stepper + void *arenaState = stepperArena->SaveState(); + new(stepperCallContext) dxSingleIslandCallContext(this, stepperArena, arenaState, islandBodiesStart, islandJointsStart); + + // Summary fault flag may be omitted as any failures will automatically propagate to dependent releasee (i.e. to m_groupReleasee) + m_world->PostThreadedCallForUnawareReleasee(NULL, NULL, 0, m_groupReleasee, NULL, + &dxIslandsProcessingCallContext::ThreadedProcessIslandSearch_Callback, (void *)stepperCallContext, 0, "World Islands Stepping Selection"); +} + +int dxIslandsProcessingCallContext::ThreadedProcessIslandSearch_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee) +{ + (void)callInstanceIndex; // unused + (void)callThisReleasee; // unused + dxSingleIslandCallContext *stepperCallContext = static_cast<dxSingleIslandCallContext *>(callContext); + stepperCallContext->m_islandsProcessingContext->ThreadedProcessIslandSearch(stepperCallContext); + return true; +} + +void dxIslandsProcessingCallContext::ThreadedProcessIslandSearch(dxSingleIslandCallContext *stepperCallContext) +{ + bool finalizeJob = false; + + const dxWorldProcessIslandsInfo &islandsInfo = m_islandsInfo; + unsigned int const *islandSizes = islandsInfo.GetIslandSizes(); + + const sizeint islandsCount = islandsInfo.GetIslandsCount(); + sizeint islandToProcess = ObtainNextIslandToBeProcessed(islandsCount); + + if (islandToProcess != islandsCount) { + // First time, the counts are zeros and on next passes, adding counts will skip island that has just been processed by stepper + dxBody *const *islandBodiesStart = stepperCallContext->GetSelectedIslandBodiesEnd(); + dxJoint *const *islandJointsStart = stepperCallContext->GetSelectedIslandJointsEnd(); + sizeint islandIndex = stepperCallContext->m_islandIndex; + + for (; ; ++islandIndex) { + unsigned int bcount = islandSizes[islandIndex * dxISE__MAX + dxISE_BODIES_COUNT]; + unsigned int jcount = islandSizes[islandIndex * dxISE__MAX + dxISE_JOINTS_COUNT]; + + if (islandIndex == islandToProcess) { + // Store selected island details + stepperCallContext->AssignIslandSelection(islandBodiesStart, islandJointsStart, bcount, jcount); + + // Store next island index to continue search from + ++islandIndex; + stepperCallContext->AssignIslandSearchProgress(islandIndex); + + // Restore saved stepper memory arena position + stepperCallContext->RestoreSavedMemArenaStateForStepper(); + + dCallReleaseeID nextSearchReleasee; + + // Summary fault flag may be omitted as any failures will automatically propagate to dependent releasee (i.e. to m_groupReleasee) + m_world->PostThreadedCallForUnawareReleasee(NULL, &nextSearchReleasee, 1, m_groupReleasee, NULL, + &dxIslandsProcessingCallContext::ThreadedProcessIslandSearch_Callback, (void *)stepperCallContext, 0, "World Islands Stepping Selection"); + + stepperCallContext->AssignStepperCallFinalReleasee(nextSearchReleasee); + + m_world->PostThreadedCall(NULL, NULL, 0, nextSearchReleasee, NULL, + &dxIslandsProcessingCallContext::ThreadedProcessIslandStepper_Callback, (void *)stepperCallContext, 0, "Island Stepping Job Start"); + + break; + } + + islandBodiesStart += bcount; + islandJointsStart += jcount; + } + } + else { + finalizeJob = true; + } + + if (finalizeJob) { + dxWorldProcessMemArena *stepperArena = stepperCallContext->m_stepperArena; + stepperCallContext->dxSingleIslandCallContext::~dxSingleIslandCallContext(); + + dxWorldProcessContext *context = m_world->unsafeGetWorldProcessingContext(); + context->ReturnStepperMemArena(stepperArena); + } +} + +int dxIslandsProcessingCallContext::ThreadedProcessIslandStepper_Callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID callThisReleasee) +{ + (void)callInstanceIndex; // unused + (void)callThisReleasee; // unused + dxSingleIslandCallContext *stepperCallContext = static_cast<dxSingleIslandCallContext *>(callContext); + stepperCallContext->m_islandsProcessingContext->ThreadedProcessIslandStepper(stepperCallContext); + return true; +} + +void dxIslandsProcessingCallContext::ThreadedProcessIslandStepper(dxSingleIslandCallContext *stepperCallContext) +{ + m_stepper(&stepperCallContext->m_stepperCallContext); +} + +sizeint dxIslandsProcessingCallContext::ObtainNextIslandToBeProcessed(sizeint islandsCount) +{ + return ThrsafeIncrementSizeUpToLimit(&m_islandToProcessStorage, islandsCount); +} + + +//**************************************************************************** +// World processing context management + +dxWorldProcessMemArena *dxWorldProcessMemArena::ReallocateMemArena ( + dxWorldProcessMemArena *oldarena, sizeint memreq, + const dxWorldProcessMemoryManager *memmgr, float rsrvfactor, unsigned rsrvminimum) +{ + dxWorldProcessMemArena *arena = oldarena; + bool allocsuccess = false; + + sizeint nOldArenaSize; + void *pOldArenaBuffer; + + do { + sizeint oldmemsize = oldarena ? oldarena->GetMemorySize() : 0; + if (oldarena == NULL || oldmemsize < memreq) { + nOldArenaSize = oldarena ? dxWorldProcessMemArena::MakeArenaSize(oldmemsize) : 0; + pOldArenaBuffer = oldarena ? oldarena->m_pArenaBegin : NULL; + + if (!dxWorldProcessMemArena::IsArenaPossible(memreq)) { + break; + } + + sizeint arenareq = dxWorldProcessMemArena::MakeArenaSize(memreq); + sizeint arenareq_with_reserve = AdjustArenaSizeForReserveRequirements(arenareq, rsrvfactor, rsrvminimum); + sizeint memreq_with_reserve = memreq + (arenareq_with_reserve - arenareq); + + if (oldarena != NULL) { + oldarena->m_pArenaMemMgr->m_fnFree(pOldArenaBuffer, nOldArenaSize); + oldarena = NULL; + + // Zero variables to avoid another freeing on exit + pOldArenaBuffer = NULL; + nOldArenaSize = 0; + } + + // Allocate new arena + void *pNewArenaBuffer = memmgr->m_fnAlloc(arenareq_with_reserve); + if (pNewArenaBuffer == NULL) { + break; + } + + arena = (dxWorldProcessMemArena *)dEFFICIENT_PTR(pNewArenaBuffer); + + void *blockbegin = dEFFICIENT_PTR(arena + 1); + void *blockend = dOFFSET_EFFICIENTLY(blockbegin, memreq_with_reserve); + + arena->m_pAllocBegin = blockbegin; + arena->m_pAllocEnd = blockend; + arena->m_pArenaBegin = pNewArenaBuffer; + arena->m_pAllocCurrentOrNextArena = NULL; + arena->m_pArenaMemMgr = memmgr; + } + + allocsuccess = true; + } + while (false); + + if (!allocsuccess) { + if (pOldArenaBuffer != NULL) { + dIASSERT(oldarena != NULL); + oldarena->m_pArenaMemMgr->m_fnFree(pOldArenaBuffer, nOldArenaSize); + } + arena = NULL; + } + + return arena; +} + +void dxWorldProcessMemArena::FreeMemArena (dxWorldProcessMemArena *arena) +{ + sizeint memsize = arena->GetMemorySize(); + sizeint arenasize = dxWorldProcessMemArena::MakeArenaSize(memsize); + + void *pArenaBegin = arena->m_pArenaBegin; + arena->m_pArenaMemMgr->m_fnFree(pArenaBegin, arenasize); +} + + +sizeint dxWorldProcessMemArena::AdjustArenaSizeForReserveRequirements(sizeint arenareq, float rsrvfactor, unsigned rsrvminimum) +{ + float scaledarena = arenareq * rsrvfactor; + sizeint adjustedarena = (scaledarena < SIZE_MAX) ? (sizeint)scaledarena : SIZE_MAX; + sizeint boundedarena = (adjustedarena > rsrvminimum) ? adjustedarena : (sizeint)rsrvminimum; + return dEFFICIENT_SIZE(boundedarena); +} + + +bool dxReallocateWorldProcessContext (dxWorld *world, dxWorldProcessIslandsInfo &islandsInfo, + dReal stepSize, dmemestimate_fn_t stepperEstimate) +{ + bool result = false; + + do + { + dxStepWorkingMemory *wmem = AllocateOnDemand(world->wmem); + if (wmem == NULL) + { + break; + } + + dxWorldProcessContext *context = wmem->SureGetWorldProcessingContext(); + if (context == NULL) + { + break; + } + + if (!context->EnsureStepperSyncObjectsAreAllocated(world)) + { + break; + } + + const dxWorldProcessMemoryReserveInfo *reserveInfo = wmem->SureGetMemoryReserveInfo(); + const dxWorldProcessMemoryManager *memmgr = wmem->SureGetMemoryManager(); + + sizeint islandsReq = EstimateIslandProcessingMemoryRequirements(world); + dIASSERT(islandsReq == dEFFICIENT_SIZE(islandsReq)); + + dxWorldProcessMemArena *islandsArena = context->ReallocateIslandsMemArena(islandsReq, memmgr, 1.0f, reserveInfo->m_uiReserveMinimum); + if (islandsArena == NULL) + { + break; + } + dIASSERT(islandsArena->IsStructureValid()); + + sizeint stepperReq = BuildIslandsAndEstimateStepperMemoryRequirements(islandsInfo, islandsArena, world, stepSize, stepperEstimate); + dIASSERT(stepperReq == dEFFICIENT_SIZE(stepperReq)); + + sizeint stepperReqWithCallContext = stepperReq + dEFFICIENT_SIZE(sizeof(dxSingleIslandCallContext)); + + unsigned islandThreadsCount = world->calculateIslandProcessingMaxThreadCount(); + if (!context->ReallocateStepperMemArenas(world, islandThreadsCount, stepperReqWithCallContext, + memmgr, reserveInfo->m_fReserveFactor, reserveInfo->m_uiReserveMinimum)) + { + break; + } + + result = true; + } + while (false); + + return result; +} + +dxWorldProcessMemArena *dxAllocateTemporaryWorldProcessMemArena( + sizeint memreq, const dxWorldProcessMemoryManager *memmgr/*=NULL*/, const dxWorldProcessMemoryReserveInfo *reserveinfo/*=NULL*/) +{ + const dxWorldProcessMemoryManager *surememmgr = memmgr ? memmgr : &g_WorldProcessMallocMemoryManager; + const dxWorldProcessMemoryReserveInfo *surereserveinfo = reserveinfo ? reserveinfo : &g_WorldProcessDefaultReserveInfo; + dxWorldProcessMemArena *arena = dxWorldProcessMemArena::ReallocateMemArena(NULL, memreq, surememmgr, surereserveinfo->m_fReserveFactor, surereserveinfo->m_uiReserveMinimum); + return arena; +} + +void dxFreeTemporaryWorldProcessMemArena(dxWorldProcessMemArena *arena) +{ + dxWorldProcessMemArena::FreeMemArena(arena); +} + diff --git a/libs/ode-0.16.1/ode/src/util.h b/libs/ode-0.16.1/ode/src/util.h new file mode 100644 index 0000000..ca222ac --- /dev/null +++ b/libs/ode-0.16.1/ode/src/util.h @@ -0,0 +1,440 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_UTIL_H_ +#define _ODE_UTIL_H_ + +#include "objects.h" +#include "common.h" + + +/* utility */ + +void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize); +void dxStepBody (dxBody *b, dReal h); + + +struct dxWorldProcessMemoryManager: + public dBase +{ + typedef void *(*alloc_block_fn_t)(sizeint block_size); + typedef void *(*shrink_block_fn_t)(void *block_pointer, sizeint block_current_size, sizeint block_smaller_size); + typedef void (*free_block_fn_t)(void *block_pointer, sizeint block_current_size); + + dxWorldProcessMemoryManager(alloc_block_fn_t fnAlloc, shrink_block_fn_t fnShrink, free_block_fn_t fnFree) + { + Assign(fnAlloc, fnShrink, fnFree); + } + + void Assign(alloc_block_fn_t fnAlloc, shrink_block_fn_t fnShrink, free_block_fn_t fnFree) + { + m_fnAlloc = fnAlloc; + m_fnShrink = fnShrink; + m_fnFree = fnFree; + } + + alloc_block_fn_t m_fnAlloc; + shrink_block_fn_t m_fnShrink; + free_block_fn_t m_fnFree; +}; + +extern dxWorldProcessMemoryManager g_WorldProcessMallocMemoryManager; + +struct dxWorldProcessMemoryReserveInfo: + public dBase +{ + dxWorldProcessMemoryReserveInfo(float fReserveFactor, unsigned uiReserveMinimum) + { + Assign(fReserveFactor, uiReserveMinimum); + } + + void Assign(float fReserveFactor, unsigned uiReserveMinimum) + { + m_fReserveFactor = fReserveFactor; + m_uiReserveMinimum = uiReserveMinimum; + } + + float m_fReserveFactor; // Use float as precision does not matter here + unsigned m_uiReserveMinimum; +}; + +extern dxWorldProcessMemoryReserveInfo g_WorldProcessDefaultReserveInfo; + + +class dxWorldProcessMemArena: + private dBase // new/delete must not be called for this class +{ +public: +#define BUFFER_TO_ARENA_EXTRA (EFFICIENT_ALIGNMENT + dEFFICIENT_SIZE(sizeof(dxWorldProcessMemArena))) + static bool IsArenaPossible(sizeint nBufferSize) + { + return SIZE_MAX - BUFFER_TO_ARENA_EXTRA >= nBufferSize; // This ensures there will be no overflow + } + + static sizeint MakeBufferSize(sizeint nArenaSize) + { + return nArenaSize - BUFFER_TO_ARENA_EXTRA; + } + + static sizeint MakeArenaSize(sizeint nBufferSize) + { + return BUFFER_TO_ARENA_EXTRA + nBufferSize; + } +#undef BUFFER_TO_ARENA_EXTRA + + bool IsStructureValid() const + { + return m_pAllocBegin != NULL && m_pAllocEnd != NULL && m_pAllocBegin <= m_pAllocEnd + && (m_pAllocCurrentOrNextArena == NULL || m_pAllocCurrentOrNextArena == m_pAllocBegin) + && m_pArenaBegin != NULL && m_pArenaBegin <= m_pAllocBegin; + } + + sizeint GetMemorySize() const + { + return (sizeint)m_pAllocEnd - (sizeint)m_pAllocBegin; + } + + void *SaveState() const + { + return m_pAllocCurrentOrNextArena; + } + + void RestoreState(void *state) + { + m_pAllocCurrentOrNextArena = state; + } + + void ResetState() + { + m_pAllocCurrentOrNextArena = m_pAllocBegin; + } + + void *PeekBufferRemainder() const + { + return m_pAllocCurrentOrNextArena; + } + + void *AllocateBlock(sizeint size) + { + void *arena = m_pAllocCurrentOrNextArena; + m_pAllocCurrentOrNextArena = dOFFSET_EFFICIENTLY(arena, size); + dIASSERT(m_pAllocCurrentOrNextArena <= m_pAllocEnd); + dIASSERT(dEFFICIENT_PTR(arena) == arena); + + return arena; + } + + void *AllocateOveralignedBlock(sizeint size, unsigned alignment) + { + void *arena = m_pAllocCurrentOrNextArena; + m_pAllocCurrentOrNextArena = dOFFSET_OVERALIGNEDLY(arena, size, alignment); + dIASSERT(m_pAllocCurrentOrNextArena <= m_pAllocEnd); + + void *block = dOVERALIGNED_PTR(arena, alignment); + return block; + } + + template<typename ElementType> + ElementType *AllocateArray(sizeint count) + { + return (ElementType *)AllocateBlock(count * sizeof(ElementType)); + } + + template<typename ElementType> + ElementType *AllocateOveralignedArray(sizeint count, unsigned alignment) + { + return (ElementType *)AllocateOveralignedBlock(count * sizeof(ElementType), alignment); + } + + template<typename ElementType> + void ShrinkArray(ElementType *arr, sizeint oldcount, sizeint newcount) + { + dIASSERT(newcount <= oldcount); + dIASSERT(dOFFSET_EFFICIENTLY(arr, oldcount * sizeof(ElementType)) == m_pAllocCurrentOrNextArena); + m_pAllocCurrentOrNextArena = dOFFSET_EFFICIENTLY(arr, newcount * sizeof(ElementType)); + } + +public: + static dxWorldProcessMemArena *ReallocateMemArena ( + dxWorldProcessMemArena *oldarena, sizeint memreq, + const dxWorldProcessMemoryManager *memmgr, float rsrvfactor, unsigned rsrvminimum); + static void FreeMemArena (dxWorldProcessMemArena *arena); + + dxWorldProcessMemArena *GetNextMemArena() const { return (dxWorldProcessMemArena *)m_pAllocCurrentOrNextArena; } + void SetNextMemArena(dxWorldProcessMemArena *pArenaInstance) { m_pAllocCurrentOrNextArena = pArenaInstance; } + +private: + static sizeint AdjustArenaSizeForReserveRequirements(sizeint arenareq, float rsrvfactor, unsigned rsrvminimum); + +private: + void *m_pAllocCurrentOrNextArena; + void *m_pAllocBegin; + void *m_pAllocEnd; + void *m_pArenaBegin; + + const dxWorldProcessMemoryManager *m_pArenaMemMgr; +}; + +class dxWorldProcessContext: + public dBase +{ +public: + dxWorldProcessContext(); + ~dxWorldProcessContext(); + + void CleanupWorldReferences(dxWorld *pswWorldInstance); + +public: + bool EnsureStepperSyncObjectsAreAllocated(dxWorld *pswWorldInstance); + dCallWaitID GetIslandsSteppingWait() const { return m_pcwIslandsSteppingWait; } + +public: + dxWorldProcessMemArena *ObtainStepperMemArena(); + void ReturnStepperMemArena(dxWorldProcessMemArena *pmaArenaInstance); + + dxWorldProcessMemArena *ReallocateIslandsMemArena(sizeint nMemoryRequirement, + const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum); + bool ReallocateStepperMemArenas(dxWorld *world, unsigned nIslandThreadsCount, sizeint nMemoryRequirement, + const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum); + +private: + static void FreeArenasList(dxWorldProcessMemArena *pmaExistingArenas); + +private: + void SetIslandsMemArena(dxWorldProcessMemArena *pmaInstance) { m_pmaIslandsArena = pmaInstance; } + dxWorldProcessMemArena *GetIslandsMemArena() const { return m_pmaIslandsArena; } + + void SetStepperArenasList(dxWorldProcessMemArena *pmaInstance) { m_pmaStepperArenas = pmaInstance; } + dxWorldProcessMemArena *GetStepperArenasList() const { return m_pmaStepperArenas; } + + inline dxWorldProcessMemArena *GetStepperArenasHead() const; + inline bool TryExtractingStepperArenasHead(dxWorldProcessMemArena *pmaHeadInstance); + inline bool TryInsertingStepperArenasHead(dxWorldProcessMemArena *pmaArenaInstance, dxWorldProcessMemArena *pmaExistingHead); + +public: + void LockForAddLimotSerialization(); + void UnlockForAddLimotSerialization(); + void LockForStepbodySerialization(); + void UnlockForStepbodySerialization(); + +private: + enum dxProcessContextMutex + { + dxPCM_STEPPER_ARENA_OBTAIN, + dxPCM_STEPPER_ADDLIMOT_SERIALIZE, + dxPCM_STEPPER_STEPBODY_SERIALIZE, + + dxPCM__MAX + }; + + static const char *const m_aszContextMutexNames[dxPCM__MAX]; + +private: + dxWorldProcessMemArena *m_pmaIslandsArena; + dxWorldProcessMemArena *volatile m_pmaStepperArenas; + dxWorld *m_pswObjectsAllocWorld; + dMutexGroupID m_pmgStepperMutexGroup; + dCallWaitID m_pcwIslandsSteppingWait; +}; + +struct dxWorldProcessIslandsInfo +{ + void AssignInfo(sizeint islandcount, unsigned int const *islandsizes, dxBody *const *bodies, dxJoint *const *joints) + { + m_IslandCount = islandcount; + m_pIslandSizes = islandsizes; + m_pBodies = bodies; + m_pJoints = joints; + } + + sizeint GetIslandsCount() const { return m_IslandCount; } + unsigned int const *GetIslandSizes() const { return m_pIslandSizes; } + dxBody *const *GetBodiesArray() const { return m_pBodies; } + dxJoint *const *GetJointsArray() const { return m_pJoints; } + +private: + sizeint m_IslandCount; + unsigned int const *m_pIslandSizes; + dxBody *const *m_pBodies; + dxJoint *const *m_pJoints; +}; + +struct dxStepperProcessingCallContext +{ + dxStepperProcessingCallContext(dxWorld *world, dReal stepSize, unsigned stepperAllowedThreads, + dxWorldProcessMemArena *stepperArena, dxBody *const *islandBodiesStart, dxJoint *const *islandJointsStart): + m_world(world), m_stepSize(stepSize), m_stepperArena(stepperArena), m_finalReleasee(NULL), + m_islandBodiesStart(islandBodiesStart), m_islandJointsStart(islandJointsStart), m_islandBodiesCount(0), m_islandJointsCount(0), + m_stepperAllowedThreads(stepperAllowedThreads) + { + } + + void AssignIslandSelection(dxBody *const *islandBodiesStart, dxJoint *const *islandJointsStart, + unsigned islandBodiesCount, unsigned islandJointsCount) + { + m_islandBodiesStart = islandBodiesStart; + m_islandJointsStart = islandJointsStart; + m_islandBodiesCount = islandBodiesCount; + m_islandJointsCount = islandJointsCount; + } + + dxBody *const *GetSelectedIslandBodiesEnd() const { return m_islandBodiesStart + m_islandBodiesCount; } + dxJoint *const *GetSelectedIslandJointsEnd() const { return m_islandJointsStart + m_islandJointsCount; } + + void AssignStepperCallFinalReleasee(dCallReleaseeID finalReleasee) + { + m_finalReleasee = finalReleasee; + } + + dxWorld *const m_world; + dReal const m_stepSize; + dxWorldProcessMemArena *m_stepperArena; + dCallReleaseeID m_finalReleasee; + dxBody *const *m_islandBodiesStart; + dxJoint *const *m_islandJointsStart; + unsigned m_islandBodiesCount; + unsigned m_islandJointsCount; + unsigned m_stepperAllowedThreads; +}; + +#define BEGIN_STATE_SAVE(memarena, state) void *state = memarena->SaveState(); +#define END_STATE_SAVE(memarena, state) memarena->RestoreState(state) + +typedef void (*dstepper_fn_t) (const dxStepperProcessingCallContext *callContext); +typedef unsigned (*dmaxcallcountestimate_fn_t) (unsigned activeThreadCount, unsigned allowedThreadCount); + +bool dxProcessIslands (dxWorld *world, const dxWorldProcessIslandsInfo &islandsInfo, + dReal stepSize, dstepper_fn_t stepper, dmaxcallcountestimate_fn_t maxCallCountEstimator); + + +typedef sizeint (*dmemestimate_fn_t) (dxBody * const *body, unsigned int nb, + dxJoint * const *_joint, unsigned int _nj); + +bool dxReallocateWorldProcessContext (dxWorld *world, dxWorldProcessIslandsInfo &islandsinfo, + dReal stepsize, dmemestimate_fn_t stepperestimate); + +dxWorldProcessMemArena *dxAllocateTemporaryWorldProcessMemArena( + sizeint memreq, const dxWorldProcessMemoryManager *memmgr/*=NULL*/, const dxWorldProcessMemoryReserveInfo *reserveinfo/*=NULL*/); +void dxFreeTemporaryWorldProcessMemArena(dxWorldProcessMemArena *arena); + + +template<class ClassType> +inline ClassType *AllocateOnDemand(ClassType *&pctStorage) +{ + ClassType *pctCurrentInstance = pctStorage; + + if (!pctCurrentInstance) + { + pctCurrentInstance = new ClassType(); + pctStorage = pctCurrentInstance; + } + + return pctCurrentInstance; +} + + +// World stepping working memory object +class dxStepWorkingMemory: + public dBase +{ +public: + dxStepWorkingMemory(): m_uiRefCount(1), m_ppcProcessingContext(NULL), m_priReserveInfo(NULL), m_pmmMemoryManager(NULL) {} + +private: + friend struct dBase; // To avoid GCC warning regarding private destructor + ~dxStepWorkingMemory() // Use Release() instead + { + delete m_ppcProcessingContext; + delete m_priReserveInfo; + delete m_pmmMemoryManager; + } + +public: + void Addref() + { + dIASSERT(~m_uiRefCount != 0); + ++m_uiRefCount; + } + + void Release() + { + dIASSERT(m_uiRefCount != 0); + if (--m_uiRefCount == 0) + { + delete this; + } + } + +public: + void CleanupMemory() + { + delete m_ppcProcessingContext; + m_ppcProcessingContext = NULL; + } + + void CleanupWorldReferences(dxWorld *world) + { + if (m_ppcProcessingContext != NULL) + { + m_ppcProcessingContext->CleanupWorldReferences(world); + } + } + +public: + dxWorldProcessContext *SureGetWorldProcessingContext() { return AllocateOnDemand(m_ppcProcessingContext); } + dxWorldProcessContext *GetWorldProcessingContext() const { return m_ppcProcessingContext; } + + const dxWorldProcessMemoryReserveInfo *GetMemoryReserveInfo() const { return m_priReserveInfo; } + const dxWorldProcessMemoryReserveInfo *SureGetMemoryReserveInfo() const { return m_priReserveInfo ? m_priReserveInfo : &g_WorldProcessDefaultReserveInfo; } + void SetMemoryReserveInfo(float fReserveFactor, unsigned uiReserveMinimum) + { + if (m_priReserveInfo) { m_priReserveInfo->Assign(fReserveFactor, uiReserveMinimum); } + else { m_priReserveInfo = new dxWorldProcessMemoryReserveInfo(fReserveFactor, uiReserveMinimum); } + } + void ResetMemoryReserveInfoToDefault() + { + if (m_priReserveInfo) { delete m_priReserveInfo; m_priReserveInfo = NULL; } + } + + const dxWorldProcessMemoryManager *GetMemoryManager() const { return m_pmmMemoryManager; } + const dxWorldProcessMemoryManager *SureGetMemoryManager() const { return m_pmmMemoryManager ? m_pmmMemoryManager : &g_WorldProcessMallocMemoryManager; } + void SetMemoryManager(dxWorldProcessMemoryManager::alloc_block_fn_t fnAlloc, + dxWorldProcessMemoryManager::shrink_block_fn_t fnShrink, + dxWorldProcessMemoryManager::free_block_fn_t fnFree) + { + if (m_pmmMemoryManager) { m_pmmMemoryManager->Assign(fnAlloc, fnShrink, fnFree); } + else { m_pmmMemoryManager = new dxWorldProcessMemoryManager(fnAlloc, fnShrink, fnFree); } + } + void ResetMemoryManagerToDefault() + { + if (m_pmmMemoryManager) { delete m_pmmMemoryManager; m_pmmMemoryManager = NULL; } + } + +private: + unsigned m_uiRefCount; + dxWorldProcessContext *m_ppcProcessingContext; + dxWorldProcessMemoryReserveInfo *m_priReserveInfo; + dxWorldProcessMemoryManager *m_pmmMemoryManager; +}; + + +#endif |
