diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/joints')
40 files changed, 10750 insertions, 0 deletions
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 + |