From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/src/joints/Makefile.am | 37 + libs/ode-0.16.1/ode/src/joints/Makefile.in | 668 +++++++++++++++++ libs/ode-0.16.1/ode/src/joints/amotor.cpp | 810 +++++++++++++++++++++ libs/ode-0.16.1/ode/src/joints/amotor.h | 105 +++ libs/ode-0.16.1/ode/src/joints/ball.cpp | 186 +++++ libs/ode-0.16.1/ode/src/joints/ball.h | 54 ++ libs/ode-0.16.1/ode/src/joints/contact.cpp | 361 +++++++++ libs/ode-0.16.1/ode/src/joints/contact.h | 48 ++ libs/ode-0.16.1/ode/src/joints/dball.cpp | 314 ++++++++ libs/ode-0.16.1/ode/src/joints/dball.h | 58 ++ libs/ode-0.16.1/ode/src/joints/dhinge.cpp | 220 ++++++ libs/ode-0.16.1/ode/src/joints/dhinge.h | 46 ++ libs/ode-0.16.1/ode/src/joints/fixed.cpp | 216 ++++++ libs/ode-0.16.1/ode/src/joints/fixed.h | 54 ++ libs/ode-0.16.1/ode/src/joints/hinge.cpp | 394 ++++++++++ libs/ode-0.16.1/ode/src/joints/hinge.h | 57 ++ libs/ode-0.16.1/ode/src/joints/hinge2.cpp | 546 ++++++++++++++ libs/ode-0.16.1/ode/src/joints/hinge2.h | 71 ++ libs/ode-0.16.1/ode/src/joints/joint.cpp | 931 ++++++++++++++++++++++++ libs/ode-0.16.1/ode/src/joints/joint.h | 326 +++++++++ libs/ode-0.16.1/ode/src/joints/joint_internal.h | 70 ++ libs/ode-0.16.1/ode/src/joints/joints.h | 48 ++ libs/ode-0.16.1/ode/src/joints/lmotor.cpp | 214 ++++++ libs/ode-0.16.1/ode/src/joints/lmotor.h | 51 ++ libs/ode-0.16.1/ode/src/joints/null.cpp | 74 ++ libs/ode-0.16.1/ode/src/joints/null.h | 46 ++ libs/ode-0.16.1/ode/src/joints/piston.cpp | 729 +++++++++++++++++++ libs/ode-0.16.1/ode/src/joints/piston.h | 112 +++ libs/ode-0.16.1/ode/src/joints/plane2d.cpp | 195 +++++ libs/ode-0.16.1/ode/src/joints/plane2d.h | 54 ++ libs/ode-0.16.1/ode/src/joints/pr.cpp | 613 ++++++++++++++++ libs/ode-0.16.1/ode/src/joints/pr.h | 100 +++ libs/ode-0.16.1/ode/src/joints/pu.cpp | 756 +++++++++++++++++++ libs/ode-0.16.1/ode/src/joints/pu.h | 88 +++ libs/ode-0.16.1/ode/src/joints/slider.cpp | 423 +++++++++++ libs/ode-0.16.1/ode/src/joints/slider.h | 59 ++ libs/ode-0.16.1/ode/src/joints/transmission.cpp | 698 ++++++++++++++++++ libs/ode-0.16.1/ode/src/joints/transmission.h | 51 ++ libs/ode-0.16.1/ode/src/joints/universal.cpp | 803 ++++++++++++++++++++ libs/ode-0.16.1/ode/src/joints/universal.h | 64 ++ 40 files changed, 10750 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/joints/Makefile.am create mode 100644 libs/ode-0.16.1/ode/src/joints/Makefile.in create mode 100644 libs/ode-0.16.1/ode/src/joints/amotor.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/amotor.h create mode 100644 libs/ode-0.16.1/ode/src/joints/ball.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/ball.h create mode 100644 libs/ode-0.16.1/ode/src/joints/contact.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/contact.h create mode 100644 libs/ode-0.16.1/ode/src/joints/dball.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/dball.h create mode 100644 libs/ode-0.16.1/ode/src/joints/dhinge.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/dhinge.h create mode 100644 libs/ode-0.16.1/ode/src/joints/fixed.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/fixed.h create mode 100644 libs/ode-0.16.1/ode/src/joints/hinge.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/hinge.h create mode 100644 libs/ode-0.16.1/ode/src/joints/hinge2.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/hinge2.h create mode 100644 libs/ode-0.16.1/ode/src/joints/joint.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/joint.h create mode 100644 libs/ode-0.16.1/ode/src/joints/joint_internal.h create mode 100644 libs/ode-0.16.1/ode/src/joints/joints.h create mode 100644 libs/ode-0.16.1/ode/src/joints/lmotor.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/lmotor.h create mode 100644 libs/ode-0.16.1/ode/src/joints/null.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/null.h create mode 100644 libs/ode-0.16.1/ode/src/joints/piston.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/piston.h create mode 100644 libs/ode-0.16.1/ode/src/joints/plane2d.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/plane2d.h create mode 100644 libs/ode-0.16.1/ode/src/joints/pr.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/pr.h create mode 100644 libs/ode-0.16.1/ode/src/joints/pu.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/pu.h create mode 100644 libs/ode-0.16.1/ode/src/joints/slider.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/slider.h create mode 100644 libs/ode-0.16.1/ode/src/joints/transmission.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/transmission.h create mode 100644 libs/ode-0.16.1/ode/src/joints/universal.cpp create mode 100644 libs/ode-0.16.1/ode/src/joints/universal.h (limited to 'libs/ode-0.16.1/ode/src/joints') 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 +#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::m_aetElementArray[] = +{ + dJBR_BODY1, // dSA_X, + dJBR_GLOBAL, // dSA_Y, + dJBR_BODY2, // dSA_Z, +}; +END_NAMESPACE_OU(); +static const CEnumUnsortedElementArray 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 +#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 +#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 +#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(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(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(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->targetDistance; +} + +void dJointSetDBallDistance(dJointID j, dReal dist) +{ + dxJointDBall* joint = static_cast(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(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(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(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(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 +#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(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(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 +#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 +#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 +#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 +#include +#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 +#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 + 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 +#include +#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 + +#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 +#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 +#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 +#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 +/// 
+///                              |- Anchor point
+///      Body_1                  |                       Body_2
+///      +---------------+       V                       +------------------+
+///     /               /|                             /                  /|
+///    /               / +       |--      ______      /                  / +
+///   /      x        /./........x.......(_____()..../         x        /.......> axis
+///  +---------------+ /         |--                +------------------+ /
+///  |               |/                             |                  |/
+///  +---------------+                              +------------------+
+///          |                                                 |
+///          |                                                 |
+///          |------------------> <----------------------------|
+///              anchor1                  anchor2
+///
+///
+///
+/// +/// When the prismatic joint as been elongated (i.e. dJointGetPistonPosition) +/// return a value > 0 +/// 
+///                                   |- Anchor point
+///      Body_1                       |                       Body_2
+///      +---------------+            V                       +------------------+
+///     /               /|                                  /                  /|
+///    /               / +            |--      ______      /                  / +
+///   /      x        /./........_____x.......(_____()..../         x        /.......> axis
+///  +---------------+ /              |--                +------------------+ /
+///  |               |/                                  |                  |/
+///  +---------------+                                   +------------------+
+///          |                                                      |
+///          |                                                      |
+///          |------------------>      <----------------------------|
+///              anchor1         |----|         anchor2
+///                                ^
+///                                |-- This is what dJointGetPistonPosition will
+///                                    return
+///
+//////////////////////////////////////////////////////////////////////////////// +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 +#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 +#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 + * 
+ *                                        +-------------+
+ *                                        |      x      |
+ *                                        +------------\+
+ * Prismatic articulation                   ..     ..
+ *                       |                ..     ..
+ *                      \/              ..      ..
+ * +--------------+    --|        __..      ..  anchor2
+ * |      x       | .....|.......(__)     ..
+ * +--------------+    --|         ^     <
+ *        |----------------------->|
+ *            Offset               |--- Rotoide articulation
+ *
+ */ +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 +#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: + * 
+ * 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
+ *
+ + * @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: + * 
+ * 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
+ *
+ + * @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. + * 
+ *                                                 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)
+ *
+ */ +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 +#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 +#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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->mode; +} + +void dJointSetTransmissionRatio( dJointID j, dReal ratio ) +{ + dxJointTransmission* joint = static_cast(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(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->ratio; +} + +dReal dJointGetTransmissionAngle1( dJointID j ) +{ + dxJointTransmission* joint = static_cast(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->phase[0]; +} + +dReal dJointGetTransmissionAngle2( dJointID j ) +{ + dxJointTransmission* joint = static_cast(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->phase[1]; +} + +dReal dJointGetTransmissionRadius1( dJointID j ) +{ + dxJointTransmission* joint = static_cast(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->radii[0]; +} + +dReal dJointGetTransmissionRadius2( dJointID j ) +{ + dxJointTransmission* joint = static_cast(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->radii[1]; +} + +void dJointSetTransmissionRadius1( dJointID j, dReal radius ) +{ + dxJointTransmission* joint = static_cast(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(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(j); + dUASSERT( joint, "bad joint argument" ); + + return joint->backlash; +} + +void dJointSetTransmissionBacklash( dJointID j, dReal backlash ) +{ + dxJointTransmission* joint = static_cast(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 +#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 + -- cgit v1.2.1