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/tests/joints/Makefile.am | 21 + libs/ode-0.16.1/tests/joints/Makefile.in | 638 +++++++++ libs/ode-0.16.1/tests/joints/amotor.cpp | 324 +++++ libs/ode-0.16.1/tests/joints/ball.cpp | 160 +++ libs/ode-0.16.1/tests/joints/dball.cpp | 81 ++ libs/ode-0.16.1/tests/joints/fixed.cpp | 149 ++ libs/ode-0.16.1/tests/joints/hinge.cpp | 928 ++++++++++++ libs/ode-0.16.1/tests/joints/hinge2.cpp | 167 +++ libs/ode-0.16.1/tests/joints/piston.cpp | 1456 +++++++++++++++++++ libs/ode-0.16.1/tests/joints/pr.cpp | 1160 +++++++++++++++ libs/ode-0.16.1/tests/joints/pu.cpp | 920 ++++++++++++ libs/ode-0.16.1/tests/joints/slider.cpp | 1332 +++++++++++++++++ libs/ode-0.16.1/tests/joints/universal.cpp | 2119 ++++++++++++++++++++++++++++ 13 files changed, 9455 insertions(+) create mode 100644 libs/ode-0.16.1/tests/joints/Makefile.am create mode 100644 libs/ode-0.16.1/tests/joints/Makefile.in create mode 100644 libs/ode-0.16.1/tests/joints/amotor.cpp create mode 100644 libs/ode-0.16.1/tests/joints/ball.cpp create mode 100644 libs/ode-0.16.1/tests/joints/dball.cpp create mode 100644 libs/ode-0.16.1/tests/joints/fixed.cpp create mode 100644 libs/ode-0.16.1/tests/joints/hinge.cpp create mode 100644 libs/ode-0.16.1/tests/joints/hinge2.cpp create mode 100644 libs/ode-0.16.1/tests/joints/piston.cpp create mode 100644 libs/ode-0.16.1/tests/joints/pr.cpp create mode 100644 libs/ode-0.16.1/tests/joints/pu.cpp create mode 100644 libs/ode-0.16.1/tests/joints/slider.cpp create mode 100644 libs/ode-0.16.1/tests/joints/universal.cpp (limited to 'libs/ode-0.16.1/tests/joints') diff --git a/libs/ode-0.16.1/tests/joints/Makefile.am b/libs/ode-0.16.1/tests/joints/Makefile.am new file mode 100644 index 0000000..5b8f743 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/Makefile.am @@ -0,0 +1,21 @@ +AM_CPPFLAGS = -I$(srcdir)/../UnitTest++/src \ + -I$(top_srcdir)/include \ + -I$(top_builddir)/include \ + -I$(top_srcdir)/ode/src + +check_LTLIBRARIES = libjoints.la + +libjoints_la_LDFLAGS = -static + +libjoints_la_SOURCES = \ + amotor.cpp \ + ball.cpp \ + dball.cpp \ + fixed.cpp \ + hinge.cpp \ + hinge2.cpp \ + piston.cpp \ + pr.cpp \ + pu.cpp \ + slider.cpp \ + universal.cpp diff --git a/libs/ode-0.16.1/tests/joints/Makefile.in b/libs/ode-0.16.1/tests/joints/Makefile.in new file mode 100644 index 0000000..edbb187 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/Makefile.in @@ -0,0 +1,638 @@ +# Makefile.in generated by automake 1.15 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994-2014 Free Software Foundation, Inc. + +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ +VPATH = @srcdir@ +am__is_gnu_make = { \ + if test -z '$(MAKELEVEL)'; then \ + false; \ + elif test -n '$(MAKE_HOST)'; then \ + true; \ + elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \ + true; \ + else \ + false; \ + fi; \ +} +am__make_running_with_option = \ + case $${target_option-} in \ + ?) ;; \ + *) echo "am__make_running_with_option: internal error: invalid" \ + "target option '$${target_option-}' specified" >&2; \ + exit 1;; \ + esac; \ + has_opt=no; \ + sane_makeflags=$$MAKEFLAGS; \ + if $(am__is_gnu_make); then \ + sane_makeflags=$$MFLAGS; \ + else \ + case $$MAKEFLAGS in \ + *\\[\ \ ]*) \ + bs=\\; \ + sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \ + | sed "s/$$bs$$bs[$$bs $$bs ]*//g"`;; \ + esac; \ + fi; \ + skip_next=no; \ + strip_trailopt () \ + { \ + flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \ + }; \ + for flg in $$sane_makeflags; do \ + test $$skip_next = yes && { skip_next=no; continue; }; \ + case $$flg in \ + *=*|--*) continue;; \ + -*I) strip_trailopt 'I'; skip_next=yes;; \ + -*I?*) strip_trailopt 'I';; \ + -*O) strip_trailopt 'O'; skip_next=yes;; \ + -*O?*) strip_trailopt 'O';; \ + -*l) strip_trailopt 'l'; skip_next=yes;; \ + -*l?*) strip_trailopt 'l';; \ + -[dEDm]) skip_next=yes;; \ + -[JT]) skip_next=yes;; \ + esac; \ + case $$flg in \ + *$$target_option*) has_opt=yes; break;; \ + esac; \ + done; \ + test $$has_opt = yes +am__make_dryrun = (target_option=n; $(am__make_running_with_option)) +am__make_keepgoing = (target_option=k; $(am__make_running_with_option)) +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ +subdir = tests/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 = +libjoints_la_LIBADD = +am_libjoints_la_OBJECTS = amotor.lo ball.lo dball.lo fixed.lo hinge.lo \ + hinge2.lo piston.lo pr.lo pu.lo slider.lo universal.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 = +libjoints_la_LINK = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CXXLD) $(AM_CXXFLAGS) \ + $(CXXFLAGS) $(libjoints_la_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_P = $(am__v_P_@AM_V@) +am__v_P_ = $(am__v_P_@AM_DEFAULT_V@) +am__v_P_0 = false +am__v_P_1 = : +AM_V_GEN = $(am__v_GEN_@AM_V@) +am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@) +am__v_GEN_0 = @echo " GEN " $@; +am__v_GEN_1 = +AM_V_at = $(am__v_at_@AM_V@) +am__v_at_ = $(am__v_at_@AM_DEFAULT_V@) +am__v_at_0 = @ +am__v_at_1 = +DEFAULT_INCLUDES = -I.@am__isrc@ -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 = +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$(srcdir)/../UnitTest++/src \ + -I$(top_srcdir)/include \ + -I$(top_builddir)/include \ + -I$(top_srcdir)/ode/src + +check_LTLIBRARIES = libjoints.la +libjoints_la_LDFLAGS = -static +libjoints_la_SOURCES = \ + amotor.cpp \ + ball.cpp \ + dball.cpp \ + fixed.cpp \ + hinge.cpp \ + hinge2.cpp \ + piston.cpp \ + pr.cpp \ + pu.cpp \ + slider.cpp \ + universal.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 tests/joints/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --foreign tests/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-checkLTLIBRARIES: + -test -z "$(check_LTLIBRARIES)" || rm -f $(check_LTLIBRARIES) + @list='$(check_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)$(libjoints_la_LINK) $(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)/dball.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)/piston.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)/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 + $(MAKE) $(AM_MAKEFLAGS) $(check_LTLIBRARIES) +check: check-am +all-am: Makefile +installdirs: +install: install-am +install-exec: install-exec-am +install-data: install-data-am +uninstall: uninstall-am + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-am +install-strip: + if test -z '$(STRIP)'; then \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + install; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \ + fi +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-am + +clean-am: clean-checkLTLIBRARIES clean-generic clean-libtool \ + 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: check-am install-am install-strip + +.PHONY: CTAGS GTAGS TAGS all all-am check check-am clean \ + clean-checkLTLIBRARIES clean-generic clean-libtool \ + 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/tests/joints/amotor.cpp b/libs/ode-0.16.1/tests/joints/amotor.cpp new file mode 100644 index 0000000..0dc1c2d --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/amotor.cpp @@ -0,0 +1,324 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/fixed.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "config.h" +#include "../../ode/src/joints/amotor.h" + +const dReal tol = 1e-5; + +SUITE (TestdxJointAMotor) +{ + struct FixtureBase { + dWorldID world; + dBodyID body; + dJointID joint; + + FixtureBase() + { + world = dWorldCreate(); + body = dBodyCreate(world); + joint = dJointCreateAMotor(world, 0); + } + + ~FixtureBase() + { + dJointDestroy(joint); + dBodyDestroy(body); + dWorldDestroy(world); + } + }; + + + struct FixtureXUser: FixtureBase { + FixtureXUser() + { + // body only allowed to rotate around X axis + dBodySetFiniteRotationMode(body, 1); + dBodySetFiniteRotationAxis(body, 1, 0, 0); + dJointAttach(joint, body, 0); + dJointSetAMotorNumAxes(joint, 2); + dJointSetAMotorAxis(joint, 0, 2, 0, 1, 0); + dJointSetAMotorAxis(joint, 1, 2, 0, 0, 1); + dJointSetAMotorParam(joint, dParamVel, 0); + dJointSetAMotorParam(joint, dParamFMax, dInfinity); + dJointSetAMotorParam(joint, dParamVel2, 0); + dJointSetAMotorParam(joint, dParamFMax2, dInfinity); + } + }; + + TEST_FIXTURE(FixtureXUser, rotate_x) + { + const dReal h = 1; + const dReal v = 1; + dMatrix3 identity = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0}; + dBodySetRotation(body, identity); + dBodySetAngularVel(body, v, 0, 0); + dWorldQuickStep(world, h); + const dReal* rot = dBodyGetRotation(body); + CHECK_CLOSE(1, rot[0], tol); + CHECK_CLOSE(0, rot[4], tol); + CHECK_CLOSE(0, rot[8], tol); + + CHECK_CLOSE(0, rot[1], tol); + CHECK_CLOSE(dCos(v*h), rot[5], tol); + CHECK_CLOSE(dSin(v*h), rot[9], tol); + + CHECK_CLOSE(0, rot[2], tol); + CHECK_CLOSE(-dSin(v*h), rot[6], tol); + CHECK_CLOSE( dCos(v*h), rot[10], tol); + } + + TEST_FIXTURE(FixtureXUser, rotate_yz) + { + const dReal h = 1; + const dReal v = 1; + dMatrix3 identity = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0}; + dBodySetRotation(body, identity); + + dVector3 axis_y; + dJointGetAMotorAxis(joint, 0, axis_y); + CHECK_CLOSE(0, axis_y[0], tol); + CHECK_CLOSE(1, axis_y[1], tol); + CHECK_CLOSE(0, axis_y[2], tol); + + dVector3 axis_z; + dJointGetAMotorAxis(joint, 1, axis_z); + CHECK_CLOSE(0, axis_z[0], tol); + CHECK_CLOSE(0, axis_z[1], tol); + CHECK_CLOSE(1, axis_z[2], tol); + + dBodySetAngularVel(body, 0, v, v); + dWorldStep(world, h); + const dReal* rot = dBodyGetRotation(body); + CHECK_CLOSE(1, rot[0], tol); + CHECK_CLOSE(0, rot[4], tol); + CHECK_CLOSE(0, rot[8], tol); + + CHECK_CLOSE(0, rot[1], tol); + CHECK_CLOSE(1, rot[5], tol); + CHECK_CLOSE(0, rot[9], tol); + + CHECK_CLOSE(0, rot[2], tol); + CHECK_CLOSE(0, rot[6], tol); + CHECK_CLOSE(1, rot[10], tol); + } + + + TEST_FIXTURE(FixtureBase, sanity_check) + { + dMatrix3 R; + dRFromAxisAndAngle(R, 1, 1, 1, 10*M_PI/180); + dBodySetRotation(body, R); + + dVector3 res; + + dJointAttach(joint, body, 0); + dJointSetAMotorNumAxes(joint, 3); + CHECK_EQUAL(3, dJointGetAMotorNumAxes(joint)); + + // axes relative to world + dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0); + dJointGetAMotorAxis(joint, 0, res); + CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0)); + CHECK_CLOSE(1, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0); + dJointGetAMotorAxis(joint, 1, res); + CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(1, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1); + dJointGetAMotorAxis(joint, 2, res); + CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(1, res[2], tol); + + // axes relative to body1 + dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0); + dJointGetAMotorAxis(joint, 0, res); + CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0)); + CHECK_CLOSE(1, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0); + dJointGetAMotorAxis(joint, 1, res); + CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(1, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1); + dJointGetAMotorAxis(joint, 2, res); + CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(1, res[2], tol); + + // axes relative to body2 + dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0); + dJointGetAMotorAxis(joint, 0, res); + CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0)); + CHECK_CLOSE(1, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0); + dJointGetAMotorAxis(joint, 1, res); + CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(1, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1); + dJointGetAMotorAxis(joint, 2, res); + CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(1, res[2], tol); + + // reverse attachment to force internal reversal + dJointAttach(joint, 0, body); + // axes relative to world + dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0); + dJointGetAMotorAxis(joint, 0, res); + CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0)); + CHECK_CLOSE(1, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0); + dJointGetAMotorAxis(joint, 1, res); + CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(1, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1); + dJointGetAMotorAxis(joint, 2, res); + CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(1, res[2], tol); + + // axes relative to body1 + dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0); + dJointGetAMotorAxis(joint, 0, res); + CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0)); + CHECK_CLOSE(1, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0); + dJointGetAMotorAxis(joint, 1, res); + CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(1, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1); + dJointGetAMotorAxis(joint, 2, res); + CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(1, res[2], tol); + + // axes relative to body2 + dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0); + dJointGetAMotorAxis(joint, 0, res); + CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0)); + CHECK_CLOSE(1, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0); + dJointGetAMotorAxis(joint, 1, res); + CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(1, res[1], tol); + CHECK_CLOSE(0, res[2], tol); + + dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1); + dJointGetAMotorAxis(joint, 2, res); + CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2)); + CHECK_CLOSE(0, res[0], tol); + CHECK_CLOSE(0, res[1], tol); + CHECK_CLOSE(1, res[2], tol); + } + + + struct FixtureXEuler : FixtureBase { + FixtureXEuler() + { + // body only allowed to rotate around X axis + dJointAttach(joint, 0, body); + dJointSetAMotorMode(joint, dAMotorEuler); + dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0); + dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1); + } + }; + + + TEST_FIXTURE(FixtureXEuler, check_axes) + { + // test patch #181 bug fix + dVector3 axis_x; + dJointGetAMotorAxis(joint, 0, axis_x); + CHECK_CLOSE(1, axis_x[0], tol); + CHECK_CLOSE(0, axis_x[1], tol); + CHECK_CLOSE(0, axis_x[2], tol); + + dVector3 axis_y; + dJointGetAMotorAxis(joint, 1, axis_y); + CHECK_CLOSE(0, axis_y[0], tol); + CHECK_CLOSE(1, axis_y[1], tol); + CHECK_CLOSE(0, axis_y[2], tol); + + dVector3 axis_z; + dJointGetAMotorAxis(joint, 2, axis_z); + CHECK_CLOSE(0, axis_z[0], tol); + CHECK_CLOSE(0, axis_z[1], tol); + CHECK_CLOSE(1, axis_z[2], tol); + } + +} // End of SUITE TestdxJointAMotor diff --git a/libs/ode-0.16.1/tests/joints/ball.cpp b/libs/ode-0.16.1/tests/joints/ball.cpp new file mode 100644 index 0000000..edbb243 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/ball.cpp @@ -0,0 +1,160 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/ball.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/ball.h" + + +using namespace std; + +SUITE (TestdxJointBall) +{ + // The 2 bodies are positionned at (-1, -2, -3), and (11, 22, 33) + // The bodis have rotation of 27deg around some axis. + // The joint is a Ball Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct dxJointBall_Fixture_B1_and_B2_At_Zero_Axis_Along_X { + dxJointBall_Fixture_B1_and_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + for (int j=0; j<2; ++j) { + bId[j][0] = dBodyCreate (wId); + dBodySetPosition (bId[j][0], -1, -2, -3); + + bId[j][1] = dBodyCreate (wId); + dBodySetPosition (bId[j][1], 11, 22, 33); + + + dMatrix3 R; + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][0], R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][1], R); + + jId[j] = dJointCreateBall (wId, 0); + dJointAttach (jId[j], bId[j][0], bId[j][1]); + } + } + + ~dxJointBall_Fixture_B1_and_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId[2][2]; + + + dJointID jId[2]; + }; + + // Rotate 2nd body 90deg around X then back to original position + // + // ^ ^ ^ + // | | => | <--- + // | | | + // B1 B2 B1 B2 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | <--- => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointBall_Fixture_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetBallAxisOffset_B2_90deg) { + + dVector3 anchor; + dJointGetBallAnchor(jId[1], anchor); + dJointSetBallAnchor(jId[1], anchor[0], anchor[1], anchor[2]); + + + for (int b=0; b<2; ++b) { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + } + + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + + for (int b=0; b<2; ++b) { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + + + const dReal *posA = dBodyGetPosition(bId[0][b]); + const dReal *posB = dBodyGetPosition(bId[1][b]); + CHECK_CLOSE (posA[0], posB[0], 1e-4); + CHECK_CLOSE (posA[1], posB[1], 1e-4); + CHECK_CLOSE (posA[2], posB[2], 1e-4); + CHECK_CLOSE (posA[3], posB[3], 1e-4); + } + } + + + + +} // End of SUITE TestdxJointBall + + diff --git a/libs/ode-0.16.1/tests/joints/dball.cpp b/libs/ode-0.16.1/tests/joints/dball.cpp new file mode 100644 index 0000000..0e82c8d --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/dball.cpp @@ -0,0 +1,81 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/dball.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" + + +using namespace std; + +SUITE (TestdxJointDBall) +{ + struct SimpleFixture { + dWorldID w; + dBodyID b1, b2; + dJointID j; + + SimpleFixture() : + w(dWorldCreate()), + b1(dBodyCreate(w)), + b2(dBodyCreate(w)), + j(dJointCreateDBall(w, 0)) + { + dJointAttach(j, b1, b2); + } + + ~SimpleFixture() + { + dJointDestroy(j); + dBodyDestroy(b1); + dBodyDestroy(b2); + dWorldDestroy(w); + } + }; + + TEST_FIXTURE(SimpleFixture, testTargetDistance) + { + dBodySetPosition(b1, -1, -2, -3); + dBodySetPosition(b2, 3, 5, 7); + dJointAttach(j, b1, b2); // this recomputes the deduced target distance + CHECK_CLOSE(dJointGetDBallDistance(j), dSqrt(REAL(165.0)), 1e-4); + + // moving body should not change target distance + dBodySetPosition(b1, 2,3,4); + CHECK_CLOSE(dJointGetDBallDistance(j), dSqrt(REAL(165.0)), 1e-4); + + // setting target distance manually should override the deduced one + dJointSetDBallDistance(j, REAL(6.0)); + CHECK_EQUAL(dJointGetDBallDistance(j), REAL(6.0)); + } + +} diff --git a/libs/ode-0.16.1/tests/joints/fixed.cpp b/libs/ode-0.16.1/tests/joints/fixed.cpp new file mode 100644 index 0000000..c069073 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/fixed.cpp @@ -0,0 +1,149 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/fixed.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/fixed.h" + +SUITE (TestdxJointFixed) +{ + struct dxJointFixed_Fixture_1 + { + dxJointFixed_Fixture_1() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, -1, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 1, 0); + + jId = dJointCreateFixed (wId, 0); + joint = (dxJointFixed*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~dxJointFixed_Fixture_1() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointFixed* joint; + }; + + TEST_FIXTURE (dxJointFixed_Fixture_1, test_dJointSetFixed) + { + // the 2 bodies are align + dJointSetFixed (jId); + CHECK_CLOSE (joint->qrel[0], 1.0, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + dMatrix3 R; + // Rotate 2nd body 90deg around X + dBodySetPosition (bId2, 0, 0, 1); + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId2, R); + + dJointSetFixed (jId); + CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + + // Rotate 2nd body -90deg around X + dBodySetPosition (bId2, 0, 0, -1); + dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0); + dBodySetRotation (bId2, R); + + dJointSetFixed (jId); + CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[1], -0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + + // Rotate 2nd body 90deg around Z + dBodySetPosition (bId2, 0, 1, 0); + dRFromAxisAndAngle (R, 0, 0, 1, M_PI/2.0); + dBodySetRotation (bId2, R); + + dJointSetFixed (jId); + CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.70710678118654757, 1e-4); + + + // Rotate 2nd body 45deg around Y + dBodySetPosition (bId2, 0, 1, 0); + dRFromAxisAndAngle (R, 0, 1, 0, M_PI/4.0); + dBodySetRotation (bId2, R); + + dJointSetFixed (jId); + CHECK_CLOSE (joint->qrel[0], 0.92387953251128674, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.38268343236508984, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + // Rotate in a strange manner + // Both bodies at origin + dRFromEulerAngles (R, REAL(0.23), REAL(3.1), REAL(-0.73)); + dBodySetPosition (bId1, 0, 0, 0); + dBodySetRotation (bId1, R); + + dRFromEulerAngles (R, REAL(-0.57), REAL(1.49), REAL(0.81)); + dBodySetPosition (bId2, 0, 0, 0); + dBodySetRotation (bId2, R); + + dJointSetFixed (jId); + CHECK_CLOSE (joint->qrel[0], -0.25526036263124319, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.28434861188441968, 1e-4); + CHECK_CLOSE (joint->qrel[2], -0.65308047160141625, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.65381489108282143, 1e-4); + } + + +} // End of SUITE TestdxJointFixed diff --git a/libs/ode-0.16.1/tests/joints/hinge.cpp b/libs/ode-0.16.1/tests/joints/hinge.cpp new file mode 100644 index 0000000..f715a6e --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/hinge.cpp @@ -0,0 +1,928 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/hinge.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/hinge.h" + +SUITE (TestdxJointHinge) +{ + // The 2 bodies are positionned at (0, 0, 0), with no rotation + // The joint is an Hinge Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + // ^Y + // | + // | + // | + // | + // | + // Z <---- . (X going out of the page) + struct dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X { + dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateHinge (wId, 0); + joint = (dxJointHinge*) jId; + + + dJointAttach (jId, bId1, bId2); + dJointSetHingeAnchor (jId, 0, 0, 0); + + axis[0] = 1; + axis[1] = 0; + axis[2] = 0; + } + + ~dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointHinge* joint; + + dVector3 axis; + }; + + // Rotate 2nd body 90deg around X then back to original position + // + // ^ ^ ^ + // | | => | <--- + // | | | + // B1 B2 B1 B2 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | <--- => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_B2_90deg) { + dMatrix3 R; + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], -M_PI/2.0); + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // Rotate 2nd body -90deg around X then back to original position + // + // ^ ^ ^ + // | | => | ---> + // | | | + // B1 B2 B1 B2 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | ---> => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_B2_Minus90deg) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], M_PI/2.0); + CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // Rotate 1st body 0.23rad around X then back to original position + // + // ^ ^ ^ ^ + // | | => \ | + // | | \ | + // B1 B2 B1 B2 + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // \ | => | | + // \ | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_B1_0_23rad) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, REAL(0.23) ); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], REAL(0.23)); + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // Rotate 1st body -0.23rad around Z then back to original position + // + // ^ ^ ^ ^ + // | | => / | + // | | / | + // B1 B2 B1 B2 + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // / | => | | + // / | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_B1_Minus0_23rad) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], -REAL(0.23)); + CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // The 2 bodies are positionned at (0, 0, 0), with no rotation + // The joint is an Hinge Joint. + // Axis in the inverse direction of the X axis + // Anchor at (0, 0, 0) + // ^Y + // | + // | + // | + // | + // | + // Z <---- x (X going out of the page) + struct dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X { + dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, -1, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 1, 0); + + jId = dJointCreateHinge (wId, 0); + joint = (dxJointHinge*) jId; + + + dJointAttach (jId, bId1, bId2); + dJointSetHingeAnchor (jId, 0, 0, 0); + + axis[0] = -1; + axis[1] = 0; + axis[2] = 0; + } + + ~dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointHinge* joint; + + dVector3 axis; + }; + + // Rotate 2nd body 90deg around X then back to original position + // + // ^ ^ ^ + // | | => | <--- + // | | | + // B1 B2 B1 B2 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | <--- => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetHingeAxisOffset_B2_90Deg) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], M_PI/2.0); + CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // Rotate 2nd body -90deg around X then back to original position + // + // ^ ^ ^ + // | | => | ---> + // | | | + // B1 B2 B1 B2 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | ---> => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetHingeAxisOffset_B2_Minus90Deg) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], -M_PI/2.0); + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // Rotate 1st body 0.23rad around X then back to original position + // + // ^ ^ ^ ^ + // | | => \ | + // | | \ | + // B1 B2 B1 B2 + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // \ | => | | + // \ | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetHingeAxisOffset_B1_0_23rad) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], -REAL(0.23)); + CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // Rotate 2nd body -0.23rad around Z then back to original position + // + // ^ ^ ^ ^ + // | | => / | + // | | / | + // B1 B2 B1 B2 + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // / | => | | + // / | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetHingeAxisOffset_B1_Minus0_23rad) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], REAL(0.23)); + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4); + } + + + // Only one body body1 at (0,0,0) + // The joint is an Hinge Joint. + // Axis is along the X axis + // Anchor at (0, 0, 0) + // + // ^Y + // | + // | + // | + // | + // | + // Z <-- X + struct dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X { + dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreateHinge (wId, 0); + joint = (dxJointHinge*) jId; + + + dJointAttach (jId, bId1, NULL); + dJointSetHingeAnchor (jId, 0, 0, 0); + + axis[0] = 1; + axis[1] = 0; + axis[2] = 0; + } + + ~dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + + dJointID jId; + dxJointHinge* joint; + + dVector3 axis; + }; + + // Rotate B1 by 90deg around X then back to original position + // + // ^ + // | => <--- + // | + // B1 B1 + // + // Start with a Delta of 90deg + // ^ + // <--- => | + // | + // B1 B1 + TEST_FIXTURE (dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_1Body_B1_90Deg) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], M_PI/2.0); + CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4); + } + + // Rotate B1 by -0.23rad around X then back to original position + // + // ^ ^ + // | => / + // | / + // B1 B1 + // + // Start with a Delta of -0.23rad + // ^ ^ + // / => | + // / | + // B1 B1 + TEST_FIXTURE (dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_1Body_B1_Minus0_23rad) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], -REAL(0.23)); + CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4); + } + + + + // Only one body body1 at (0,0,0) + // The joint is an Hinge Joint. + // Axis the inverse of the X axis + // Anchor at (0, 0, 0) + // + // ^Y + // | + // | + // | + // | + // | + // Z <-- X + struct dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X { + dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreateHinge (wId, 0); + joint = (dxJointHinge*) jId; + + + dJointAttach (jId, bId1, NULL); + dJointSetHingeAnchor (jId, 0, 0, 0); + + axis[0] = -1; + axis[1] = 0; + axis[2] = 0; + } + + ~dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + + dJointID jId; + dxJointHinge* joint; + + dVector3 axis; + }; + + // Rotate B1 by 90deg around X then back to original position + // + // ^ + // | => <--- + // | + // B1 B1 + // + // Start with a Delta of 90deg + // ^ + // <--- => | + // | + // B1 B1 + TEST_FIXTURE (dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetHingeAxisOffset_1Body_B1_90Deg) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], -M_PI/2.0); + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4); + } + + // Rotate B1 by -0.23rad around X then back to original position + // + // ^ ^ + // | => / + // | / + // B1 B1 + // + // Start with a Delta of -0.23rad + // ^ ^ + // / => | + // / | + // B1 B1 + TEST_FIXTURE (dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetHingeAxisOffset_1Body_B1_Minus0_23rad) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], REAL(0.23)); + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4); + } + + + + + + // Only one body body2 at (0,0,0) + // The joint is an Hinge Joint. + // Axis is along the X axis + // Anchor at (0, 0, 0) + // + // ^Y + // | + // | + // | + // | + // | + // Z <-- X + struct dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X { + dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateHinge (wId, 0); + joint = (dxJointHinge*) jId; + + + dJointAttach (jId, NULL, bId2); + dJointSetHingeAnchor (jId, 0, 0, 0); + + axis[0] = 1; + axis[1] = 0; + axis[2] = 0; + } + + ~dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + + dJointID jId; + dxJointHinge* joint; + + dVector3 axis; + }; + + // Rotate B2 by 90deg around X then back to original position + // + // ^ + // | => <--- + // | + // B2 B2 + // + // Start with a Delta of 90deg + // ^ + // <--- => | + // | + // B2 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_1Body_B2_90Deg) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], -M_PI/2.0); + CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4); + } + + // Rotate B2 by -0.23rad around X then back to original position + // + // ^ ^ + // | => / + // | / + // B2 B2 + // + // Start with a Delta of -0.23rad + // ^ ^ + // / => | + // / | + // B2 B2 + TEST_FIXTURE (dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X, + test_dJointSetHingeAxisOffset_1Body_B2_Minus0_23rad) { + dMatrix3 R; + + dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]); + + CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2], REAL(0.23)); + CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4); + } + + + + // Create 2 bodies attached by a Hinge joint + // Axis is along the X axis (Default value + // Anchor at (0, 0, 0) (Default value) + // + // ^Y + // | + // * Body2 + // | + // | + // Body1 | + // * Z--------> + struct dxJointHinge_Test_Initialization { + dxJointHinge_Test_Initialization() + { + wId = dWorldCreate(); + + // Remove gravity to have the only force be the force of the joint + dWorldSetGravity(wId, 0,0,0); + + for (int j=0; j<2; ++j) { + bId[j][0] = dBodyCreate (wId); + dBodySetPosition (bId[j][0], -1, -2, -3); + + bId[j][1] = dBodyCreate (wId); + dBodySetPosition (bId[j][1], 11, 22, 33); + + + dMatrix3 R; + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][0], R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][1], R); + + jId[j] = dJointCreateHinge (wId, 0); + dJointAttach (jId[j], bId[j][0], bId[j][1]); + // dJointSetHingeParam(jId[j], dParamLoStop, 1); + // dJointSetHingeParam(jId[j], dParamHiStop, 2); + // dJointSetHingeParam(jId[j], dParamFMax, 200); + } + } + + ~dxJointHinge_Test_Initialization() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId[2][2]; + + + dJointID jId[2]; + + }; + + + // Test if setting a Hinge with its default values + // will behave the same as a default Hinge joint + TEST_FIXTURE (dxJointHinge_Test_Initialization, + test_Hinge_Initialization) { + using namespace std; + + dVector3 axis; + dJointGetHingeAxis(jId[1], axis); + dJointSetHingeAxis(jId[1], axis[0], axis[1], axis[2]); + + + dVector3 anchor; + dJointGetHingeAnchor(jId[1], anchor); + dJointSetHingeAnchor(jId[1], anchor[0], anchor[1], anchor[2]); + + + for (int b=0; b<2; ++b) { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-6); + CHECK_CLOSE (qA[1], qB[1], 1e-6); + CHECK_CLOSE (qA[2], qB[2], 1e-6); + CHECK_CLOSE (qA[3], qB[3], 1e-6); + } + + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + + for (int b=0; b<2; ++b) { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-6); + CHECK_CLOSE (qA[1], qB[1], 1e-6); + CHECK_CLOSE (qA[2], qB[2], 1e-6); + CHECK_CLOSE (qA[3], qB[3], 1e-6); + + + const dReal *posA = dBodyGetPosition(bId[0][b]); + const dReal *posB = dBodyGetPosition(bId[1][b]); + CHECK_CLOSE (posA[0], posB[0], 1e-6); + CHECK_CLOSE (posA[1], posB[1], 1e-6); + CHECK_CLOSE (posA[2], posB[2], 1e-6); + CHECK_CLOSE (posA[3], posB[3], 1e-6); + } + } + + + TEST_FIXTURE(dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X, + test_Hinge_dParamVel) + { + const dReal targetvel = 100; + const dReal tolerance = targetvel * +#ifdef dSINGLE + 1e-2 +#else + 1e-6 +#endif + ; + + dJointSetHingeParam(jId, dParamFMax, dInfinity); + dJointSetHingeParam(jId, dParamVel, targetvel); + + dWorldStep(wId, 0.001); + + const dReal *v1 = dBodyGetAngularVel(bId1); + const dReal *v2 = dBodyGetAngularVel(bId2); + dVector3 rvel = { v1[0]-v2[0], v1[1]-v2[1], v1[2]-v2[2] }; + CHECK_CLOSE(rvel[0], targetvel, tolerance); + CHECK_CLOSE(rvel[1], 0, tolerance); + CHECK_CLOSE(rvel[2], 0, tolerance); + } + + + +} // End of SUITE TestdxJointHinge + + diff --git a/libs/ode-0.16.1/tests/joints/hinge2.cpp b/libs/ode-0.16.1/tests/joints/hinge2.cpp new file mode 100644 index 0000000..3f9007c --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/hinge2.cpp @@ -0,0 +1,167 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/hinge2.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/hinge2.h" + + +using namespace std; + +SUITE (TestdxJointHinge2) +{ + // The 2 bodies are positionned at (-1, -2, -3), and (11, 22, 33) + // The bodis have rotation of 27deg around some axis. + // The joint is a Hinge2 Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct dxJointHinge2_Fixture_B1_and_B2_At_Zero_Axis_Along_X { + dxJointHinge2_Fixture_B1_and_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + for (int j=0; j<2; ++j) { + bId[j][0] = dBodyCreate (wId); + dBodySetPosition (bId[j][0], -1, -2, -3); + + bId[j][1] = dBodyCreate (wId); + dBodySetPosition (bId[j][1], 11, 22, 33); + + + dMatrix3 R; + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][0], R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][1], R); + + jId[j] = dJointCreateHinge2 (wId, 0); + dJointAttach (jId[j], bId[j][0], bId[j][1]); + } + } + + ~dxJointHinge2_Fixture_B1_and_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId[2][2]; + + + dJointID jId[2]; + }; + + // Rotate 2nd body 90deg around X then back to original position + // + // ^ ^ ^ + // | | => | <--- + // | | | + // B1 B2 B1 B2 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | <--- => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (dxJointHinge2_Fixture_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetHinge2AxisOffset_B2_90deg) { + + dVector3 anchor; + dJointGetHinge2Anchor(jId[1], anchor); + dJointSetHinge2Anchor(jId[1], anchor[0], anchor[1], anchor[2]); + + dVector3 axis1, axis2; + dJointGetHinge2Axis1(jId[1], axis1); + dJointGetHinge2Axis2(jId[1], axis2); + dJointSetHinge2Axes(jId[1], axis1, axis2); + dJointSetHinge2Axes(jId[1], axis1, NULL); + dJointSetHinge2Axes(jId[1], NULL, axis2); + + + for (int b=0; b<2; ++b) { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + } + + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + + for (int b=0; b<2; ++b) { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + + + const dReal *posA = dBodyGetPosition(bId[0][b]); + const dReal *posB = dBodyGetPosition(bId[1][b]); + CHECK_CLOSE (posA[0], posB[0], 1e-4); + CHECK_CLOSE (posA[1], posB[1], 1e-4); + CHECK_CLOSE (posA[2], posB[2], 1e-4); + CHECK_CLOSE (posA[3], posB[3], 1e-4); + } + } + + + + +} // End of SUITE TestdxJointHinge2 + + diff --git a/libs/ode-0.16.1/tests/joints/piston.cpp b/libs/ode-0.16.1/tests/joints/piston.cpp new file mode 100644 index 0000000..9422180 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/piston.cpp @@ -0,0 +1,1456 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/piston.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/piston.h" + +SUITE (TestdxJointPiston) +{ + // The 2 bodies are positionned at (0, 0, 0), with no rotation + // The joint is a Piston Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X + { + Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePiston (wId, 0); + joint = (dxJointPiston*) jId; + + + dJointAttach (jId, bId1, bId2); + + dJointSetPistonAxis (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointPiston* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X::axis = + { + 1, 0, 0 + }; + const dReal Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X::offset = REAL (3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B1_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + // Only here to test a deprecated warning + #if 0 // the deprecated warning is not a functional part of the API, no need to test it. + dJointSetPistonAxisDelta (jId, 1, 0, 0, 0, 0, 0); + #endif + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B1_Minus_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B2_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B2_Minus_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + + + // The 2 bodies are positionned at (0, 0, 0), with no rotation + // The joint is a Piston Joint + // Axis is the opposite of the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePiston (wId, 0); + joint = (dxJointPiston*) jId; + + + dJointAttach (jId, bId1, bId2); + + + dJointSetPistonAxis (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointPiston* joint; + + static const dVector3 axis; + static const dReal offset; + }; + const dVector3 Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X::axis = + { + -1, 0, 0 + }; + const dReal Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X::offset = REAL (3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B1_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B1_Minus_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B2_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B2_Minus_3Unit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + + // Only body 1 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Piston Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X + { + Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreatePiston (wId, 0); + joint = (dxJointPiston*) jId; + + + dJointAttach (jId, bId1, NULL); + + dJointSetPistonAxis (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + dJointID jId; + dxJointPiston* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X::axis = + { + 1, 0, 0 + }; + const dReal Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X::offset = REAL (3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B1_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B1_Minus_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Only body 1 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Piston Joint + // Axis is in the oppsite X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreatePiston (wId, 0); + joint = (dxJointPiston*) jId; + + + dJointAttach (jId, bId1, NULL); + + dJointSetPistonAxis (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + dJointID jId; + dxJointPiston* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X::axis = + { + -1, 0, 0 + }; + const dReal Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X::offset = REAL (3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <--- Axis + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B1_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <--- Axis + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B1_Minus_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + + + + + + + + + // Only body 2 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Piston Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X + { + Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePiston (wId, 0); + joint = (dxJointPiston*) jId; + + + dJointAttach (jId, NULL, bId2); + + dJointSetPistonAxis (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + dJointID jId; + dxJointPiston* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X::axis = + { + 1, 0, 0 + }; + const dReal Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X::offset = REAL (3.1); + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B2_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonAxisOffset_B2_Minus_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // Only body 2 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Piston Joint + // Axis is in the opposite X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePiston (wId, 0); + joint = (dxJointPiston*) jId; + + + dJointAttach (jId, NULL, bId2); + + dJointSetPistonAxis (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + dJointID jId; + dxJointPiston* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X::axis = + { + -1, 0, 0 + }; + const dReal Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X::offset = REAL (3.1); + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <--- Axis + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B2_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + #if 0 // another deprecated warning test? + dJointSetPistonAxisDelta (jId, 1, 0, 0, 0, 0, 0); + #endif + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <--- Axis + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonAxisOffset_B2_Minus_OffsetUnit) + { + dJointSetPistonAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dJointSetPistonAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPistonPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + } + + // ========================================================================== + // Test Position Rate + // ========================================================================== + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B1 F-> => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Along_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> Axis --> + // B1 <-F => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Inverse_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B1 F-> => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Inverse_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> <-- Axis + // B1 <-F => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Along_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Along_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Inverse_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Inverse_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Along_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B1 F-> => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Along_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> Axis --> + // B1 <-F => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Inverse_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B1 F-> => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Inverse_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> <-- Axis + // B1 <-F => B1 + TEST_FIXTURE (Fixture_dxJointPiston_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Along_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + + // Apply force on body 2 in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Along_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on body 2 in the inverse X direction + // + // X-------> X---------> Axis --> + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Along_X, + test_dJointSetPistonPositionRate_Force_Inverse_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + + // Apply force on body 2 in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Inverse_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetPistonPositionRate (jId), 1e-4); + } + + // Apply force on body 2 in the inverse X direction + // + // X-------> X---------> <-- Axis + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointPiston_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPistonPositionRate_Force_Along_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetPistonPosition (jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetPistonPositionRate (jId), 1e-4); + + dBodyAddForce (bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetPistonPositionRate (jId), 1e-4); + } + + + + +// Create 2 bodies attached by a Piston joint + // Axis is along the X axis (Default value + // Anchor at (0, 0, 0) (Default value) + // + // ^Y + // | + // * Body2 + // | + // | + // Body1 | + // * Z--------> + struct dxJointPiston_Test_Initialization + { + dxJointPiston_Test_Initialization() + { + wId = dWorldCreate(); + + // Remove gravity to have the only force be the force of the joint + dWorldSetGravity(wId, 0,0,0); + + for (int j=0; j<2; ++j) + { + bId[j][0] = dBodyCreate (wId); + dBodySetPosition (bId[j][0], -1, -2, -3); + + bId[j][1] = dBodyCreate (wId); + dBodySetPosition (bId[j][1], 11, 22, 33); + + + dMatrix3 R; + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][0], R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][1], R); + + jId[j] = dJointCreatePiston (wId, 0); + dJointAttach (jId[j], bId[j][0], bId[j][1]); + } + } + + ~dxJointPiston_Test_Initialization() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId[2][2]; + + + dJointID jId[2]; + + }; + + + // Test if setting a Piston with its default values + // will behave the same as a default Piston joint + TEST_FIXTURE (dxJointPiston_Test_Initialization, + test_Piston_Initialization) + { + using namespace std; + + dVector3 axis; + dJointGetPistonAxis(jId[1], axis); + dJointSetPistonAxis(jId[1], axis[0], axis[1], axis[2]); + + + dVector3 anchor; + dJointGetPistonAnchor(jId[1], anchor); + dJointSetPistonAnchor(jId[1], anchor[0], anchor[1], anchor[2]); + + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-6); + CHECK_CLOSE (qA[1], qB[1], 1e-6); + CHECK_CLOSE (qA[2], qB[2], 1e-6); + CHECK_CLOSE (qA[3], qB[3], 1e-6); + } + + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-6); + CHECK_CLOSE (qA[1], qB[1], 1e-6); + CHECK_CLOSE (qA[2], qB[2], 1e-6); + CHECK_CLOSE (qA[3], qB[3], 1e-6); + + + const dReal *posA = dBodyGetPosition(bId[0][b]); + const dReal *posB = dBodyGetPosition(bId[1][b]); + CHECK_CLOSE (posA[0], posB[0], 1e-6); + CHECK_CLOSE (posA[1], posB[1], 1e-6); + CHECK_CLOSE (posA[2], posB[2], 1e-6); + CHECK_CLOSE (posA[3], posB[3], 1e-6); + } + + + } + + + + + + // Compare only one body to 2 bodies with one fixed. + // + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Piston Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPiston_Compare_Body_At_Zero_Axis_Along_X + { + Fixture_dxJointPiston_Compare_Body_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1_12 = dBodyCreate (wId); + dBodySetPosition (bId1_12, 0, 0, 0); + + bId2_12 = dBodyCreate (wId); + dBodySetPosition (bId2_12, 0, 0, 0); + // The force will be added in the function since it is not + // always on the same body + + jId_12 = dJointCreatePiston (wId, 0); + dJointAttach(jId_12, bId1_12, bId2_12); + + fixed = dJointCreateFixed (wId, 0); + + + + bId = dBodyCreate (wId); + dBodySetPosition (bId, 0, 0, 0); + + dBodyAddForce (bId, 4, 0, 0); + + jId = dJointCreatePiston (wId, 0); + } + + ~Fixture_dxJointPiston_Compare_Body_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1_12; + dBodyID bId2_12; + + dJointID jId_12; // Joint with 2 bodies + + dJointID fixed; + + + + dBodyID bId; + dJointID jId; // Joint with one body + }; + + // This test compare the result of a slider with 2 bodies where body body 2 is + // fixed to the world to a slider with only one body at position 1. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + TEST_FIXTURE(Fixture_dxJointPiston_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_minus1_025_One_Body_on_left) + { + dBodyAddForce (bId1_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPistonParam(jId_12, dParamLoStop, -1); + dJointSetPistonParam(jId_12, dParamHiStop, 0.25); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + dJointAttach(jId, bId, 0); + dJointSetPistonParam(jId, dParamLoStop, -1); + dJointSetPistonParam(jId, dParamHiStop, 0.25); + + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos1_12[0], pos[0], 1e-2); + CHECK_CLOSE (pos1_12[1], pos[1], 1e-2); + CHECK_CLOSE (pos1_12[2], pos[2], 1e-2); + + const dReal *q1_12 = dBodyGetQuaternion(bId1_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q1_12[0], q[0], 1e-4); + CHECK_CLOSE (q1_12[1], q[1], 1e-4); + CHECK_CLOSE (q1_12[2], q[2], 1e-4); + CHECK_CLOSE (q1_12[3], q[3], 1e-4); + } + + + + // This test compare the result of a slider with 2 bodies where body body 1 is + // fixed to the world to a slider with only one body at position 2. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + TEST_FIXTURE(Fixture_dxJointPiston_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_minus1_025_One_Body_on_right) + { + dBodyAddForce (bId2_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPistonParam(jId_12, dParamLoStop, -1); + dJointSetPistonParam(jId_12, dParamHiStop, 0.25); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetPistonParam(jId, dParamLoStop, -1); + dJointSetPistonParam(jId, dParamHiStop, 0.25); + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos2_12[0], pos[0], 1e-2); + CHECK_CLOSE (pos2_12[1], pos[1], 1e-2); + CHECK_CLOSE (pos2_12[2], pos[2], 1e-2); + + + const dReal *q2_12 = dBodyGetQuaternion(bId2_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q2_12[0], q[0], 1e-4); + CHECK_CLOSE (q2_12[1], q[1], 1e-4); + CHECK_CLOSE (q2_12[2], q[2], 1e-4); + CHECK_CLOSE (q2_12[3], q[3], 1e-4); + } + + + + // This test compare the result of a slider with 2 bodies where body body 2 is + // fixed to the world to a slider with only one body at position 1. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointPiston_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_0_0_One_Body_on_left) + { + dBodyAddForce (bId1_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPistonParam(jId_12, dParamLoStop, 0); + dJointSetPistonParam(jId_12, dParamHiStop, 0); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + + dJointAttach(jId, bId, 0); + dJointSetPistonParam(jId, dParamLoStop, 0); + dJointSetPistonParam(jId, dParamHiStop, 0); + + for (int i=0; i<500; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos1_12[0], pos[0], 1e-4); + CHECK_CLOSE (pos1_12[1], pos[1], 1e-4); + CHECK_CLOSE (pos1_12[2], pos[2], 1e-4); + + CHECK_CLOSE (0, pos[0], 1e-4); + CHECK_CLOSE (0, pos[1], 1e-4); + CHECK_CLOSE (0, pos[2], 1e-4); + + + const dReal *q1_12 = dBodyGetQuaternion(bId1_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q1_12[0], q[0], 1e-4); + CHECK_CLOSE (q1_12[1], q[1], 1e-4); + CHECK_CLOSE (q1_12[2], q[2], 1e-4); + CHECK_CLOSE (q1_12[3], q[3], 1e-4); + } + + + // This test compare the result of a slider with 2 bodies where body body 1 is + // fixed to the world to a slider with only one body at position 2. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointPiston_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_0_0_One_Body_on_right) + { + dBodyAddForce (bId2_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPistonParam(jId_12, dParamLoStop, 0); + dJointSetPistonParam(jId_12, dParamHiStop, 0); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetPistonParam(jId, dParamLoStop, 0); + dJointSetPistonParam(jId, dParamHiStop, 0); + + for (int i=0; i<500; ++i) + dWorldStep(wId, 1.0); + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos2_12[0], pos[0], 1e-4); + CHECK_CLOSE (pos2_12[1], pos[1], 1e-4); + CHECK_CLOSE (pos2_12[2], pos[2], 1e-4); + + CHECK_CLOSE (0, pos[0], 1e-4); + CHECK_CLOSE (0, pos[1], 1e-4); + CHECK_CLOSE (0, pos[2], 1e-4); + + + const dReal *q2_12 = dBodyGetQuaternion(bId2_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q2_12[0], q[0], 1e-4); + CHECK_CLOSE (q2_12[1], q[1], 1e-4); + CHECK_CLOSE (q2_12[2], q[2], 1e-4); + CHECK_CLOSE (q2_12[3], q[3], 1e-4); + } + + +} // End of SUITE TestdxJointPiston diff --git a/libs/ode-0.16.1/tests/joints/pr.cpp b/libs/ode-0.16.1/tests/joints/pr.cpp new file mode 100644 index 0000000..30995c7 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/pr.cpp @@ -0,0 +1,1160 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/pr.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/pr.h" + +SUITE (TestdxJointPR) +{ + // The 2 bodies are positionned at (0, 0, 0), with no rotation + // The joint is a PR Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X + { + Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePR (wId, 0); + joint = (dxJointPR*) jId; + + + dJointAttach (jId, bId1, bId2); + + dJointSetPRAxis1 (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointPR* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X::axis = + { + 1, 0, 0 + }; + const dReal Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X::offset = REAL (3.1); + + + + + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B1_3Unit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// offset*axis[0],offset*axis[1],offset*axis[2]); +// CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId1, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + +// // Only here to test a deprecated warning +// dJointSetPRAxisDelta (jId, 1, 0, 0, 0, 0, 0); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B1_Minus_3Unit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// -offset*axis[0],-offset*axis[1],-offset*axis[2]); +// CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId1, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B2_3Unit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// -offset*axis[0],-offset*axis[1],-offset*axis[2]); +// CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId2, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPR_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B2_Minus_3Unit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// offset*axis[0],offset*axis[1],offset*axis[2]); +// CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId2, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + + + + + + // Only body 1 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a PR Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPR_B1_At_Zero_Axis_Along_X + { + Fixture_dxJointPR_B1_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreatePR (wId, 0); + joint = (dxJointPR*) jId; + + + dJointAttach (jId, bId1, NULL); + + dJointSetPRAxis1 (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPR_B1_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + dJointID jId; + dxJointPR* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPR_B1_At_Zero_Axis_Along_X::axis = + { + 1, 0, 0 + }; + const dReal Fixture_dxJointPR_B1_At_Zero_Axis_Along_X::offset = REAL (3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPR_B1_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B1_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// offset*axis[0],offset*axis[1],offset*axis[2]); +// CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId1, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPR_B1_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B1_Minus_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// -offset*axis[0],-offset*axis[1],-offset*axis[2]); +// CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId1, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + // Only body 1 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a PR Joint + // Axis is in the oppsite X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPR_B1_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointPR_B1_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreatePR (wId, 0); + joint = (dxJointPR*) jId; + + + dJointAttach (jId, bId1, NULL); + + dJointSetPRAxis1 (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPR_B1_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + dJointID jId; + dxJointPR* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPR_B1_At_Zero_Axis_Inverse_of_X::axis = + { + -1, 0, 0 + }; + const dReal Fixture_dxJointPR_B1_At_Zero_Axis_Inverse_of_X::offset = REAL (3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <--- Axis + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPR_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetPRAxisOffset_B1_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// -offset*axis[0],-offset*axis[1],-offset*axis[2]); +// CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId1, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <--- Axis + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPR_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetPRAxisOffset_B1_Minus_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// offset*axis[0],offset*axis[1],offset*axis[2]); +// CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId1, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + + + + // Compare only one body to 2 bodies with one fixed. + // + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a PR Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y + { + Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y() + { + wId = dWorldCreate(); + + bId1_12 = dBodyCreate (wId); + dBodySetPosition (bId1_12, 0, 0, 0); + + bId2_12 = dBodyCreate (wId); + dBodySetPosition (bId2_12, 0, 0, 0); + // The force will be added in the function since it is not + // always on the same body + + jId_12 = dJointCreatePR (wId, 0); + dJointAttach(jId_12, bId1_12, bId2_12); + + fixed = dJointCreateFixed (wId, 0); + + + + jId = dJointCreatePR (wId, 0); + + bId = dBodyCreate (wId); + dBodySetPosition (bId, 0, 0, 0); + + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPRAxis1(jId_12, axis); + dJointSetPRAxis1(jId, axis[0], axis[1], axis[2]); + dBodySetLinearVel (bId, 4*axis[0], 4*axis[1], 4*axis[2]); + } + + ~Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1_12; + dBodyID bId2_12; + + dJointID jId_12; // Joint with 2 bodies + + dJointID fixed; + + + + dBodyID bId; + dJointID jId; // Joint with one body + }; + + + TEST_FIXTURE (Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y, + test_dJointSetPRPositionRate_Only_B1) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPRAxis1(jId_12, axis); + dBodySetLinearVel (bId1_12, 4*axis[0], 4*axis[1], 4*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + dJointAttach(jId, bId, 0); + + CHECK_CLOSE(dJointGetPRPositionRate(jId_12), dJointGetPRPositionRate(jId), 1e-2); + + CHECK_CLOSE(dJointGetPRAngleRate(jId_12), dJointGetPRAngleRate(jId), 1e-2); + } + + + TEST_FIXTURE (Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y, + test_dJointSetPRPositionRate_Only_B2) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPRAxis1(jId_12, axis); + dBodySetLinearVel (bId2_12, 4*axis[0], 4*axis[1], 4*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + dJointAttach(jId, 0, bId); + + CHECK_CLOSE(dJointGetPRPositionRate(jId_12), dJointGetPRPositionRate(jId), 1e-2); + CHECK_CLOSE(dJointGetPRAngleRate(jId_12), dJointGetPRAngleRate(jId), 1e-2); + } + + + + + + // Only body 2 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a PR Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPR_B2_At_Zero_Axis_Along_X + { + Fixture_dxJointPR_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePR (wId, 0); + joint = (dxJointPR*) jId; + + + dJointAttach (jId, NULL, bId2); + + dJointSetPRAxis1 (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPR_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + dJointID jId; + dxJointPR* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPR_B2_At_Zero_Axis_Along_X::axis = + { + 1, 0, 0 + }; + const dReal Fixture_dxJointPR_B2_At_Zero_Axis_Along_X::offset = REAL (3.1); + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPR_B2_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B2_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// -offset*axis[0],-offset*axis[1],-offset*axis[2]); +// CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId2, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPR_B2_At_Zero_Axis_Along_X, + test_dJointSetPRAxisOffset_B2_Minus_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// offset*axis[0],offset*axis[1],offset*axis[2]); +// CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId2, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + // Only body 2 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a PR Joint + // Axis is in the opposite X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPR_B2_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointPR_B2_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePR (wId, 0); + joint = (dxJointPR*) jId; + + + dJointAttach (jId, NULL, bId2); + + dJointSetPRAxis1 (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPR_B2_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + dJointID jId; + dxJointPR* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPR_B2_At_Zero_Axis_Inverse_of_X::axis = + { + -1, 0, 0 + }; + const dReal Fixture_dxJointPR_B2_At_Zero_Axis_Inverse_of_X::offset = REAL (3.1); + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <--- Axis + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPR_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPRAxisOffset_B2_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// offset*axis[0],offset*axis[1],offset*axis[2]); +// CHECK_CLOSE (offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId2, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); +// dJointSetPRAxisDelta (jId, 1, 0, 0, 0, 0, 0); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <--- Axis + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPR_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetPRAxisOffset_B2_Minus_OffsetUnit) + { + dJointSetPRAnchor (jId, 0, 0, 0); + + CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dJointSetPRAnchorOffset (jId, 0, 0, 0, +// -offset*axis[0],-offset*axis[1],-offset*axis[2]); +// CHECK_CLOSE (-offset, dJointGetPRPosition (jId), 1e-4); + +// dBodySetPosition (bId2, 0, 0, 0); +// CHECK_CLOSE (0.0, dJointGetPRPosition (jId), 1e-4); + } + + + // The 2 bodies are positionned at (0, 0, 0), and (0, 0, 0) + // The bodis have rotation of 27deg around some axis. + // The joint is a PR Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPR_B1_and_B2_Random_Orientation_At_Zero_Axis_Along_X + { + Fixture_dxJointPR_B1_and_B2_Random_Orientation_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + dMatrix3 R; + + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId1, R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId2, R); + + jId = dJointCreatePR (wId, 0); + joint = (dxJointPR*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~Fixture_dxJointPR_B1_and_B2_Random_Orientation_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointPR* joint; + }; + + // Test is dJointSetPRAxis and dJointGetPRAxis return same value + TEST_FIXTURE (Fixture_dxJointPR_B1_and_B2_Random_Orientation_At_Zero_Axis_Along_X, + test_dJointSetGetPRAxis) + { + dVector3 axisOrig, axis; + + + dJointGetPRAxis1 (jId, axisOrig); + dJointGetPRAxis1 (jId, axis); + dJointSetPRAxis1 (jId, axis[0], axis[1], axis[2]); + dJointGetPRAxis1 (jId, axis); + CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4); + CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4); + CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4); + + + dJointGetPRAxis2 (jId, axisOrig); + dJointGetPRAxis2(jId, axis); + dJointSetPRAxis2 (jId, axis[0], axis[1], axis[2]); + dJointGetPRAxis2 (jId, axis); + CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4); + CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4); + CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4); + } + + + + // Create 2 bodies attached by a PR joint + // Axis is along the X axis (Default value + // Anchor at (0, 0, 0) (Default value) + // + // ^Y + // | + // * Body2 + // | + // | + // Body1 | + // * Z--------> + struct dxJointPR_Test_Initialization + { + dxJointPR_Test_Initialization() + { + wId = dWorldCreate(); + + // Remove gravity to have the only force be the force of the joint + dWorldSetGravity(wId, 0,0,0); + + for (int j=0; j<2; ++j) + { + bId[j][0] = dBodyCreate (wId); + dBodySetPosition (bId[j][0], -1, -2, -3); + + bId[j][1] = dBodyCreate (wId); + dBodySetPosition (bId[j][1], 11, 22, 33); + + + dMatrix3 R; + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][0], R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][1], R); + + + jId[j] = dJointCreatePR (wId, 0); + dJointAttach (jId[j], bId[j][0], bId[j][1]); + } + } + + ~dxJointPR_Test_Initialization() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId[2][2]; + + + dJointID jId[2]; + + }; + + + // Test if setting a PR with its default values + // will behave the same as a default PR joint + TEST_FIXTURE (dxJointPR_Test_Initialization, + test_PR_Initialization) + { + using namespace std; + + dVector3 axis; + dJointGetPRAxis1(jId[1], axis); + dJointSetPRAxis1(jId[1], axis[0], axis[1], axis[2]); + + dJointGetPRAxis2(jId[1], axis); + dJointSetPRAxis2(jId[1], axis[0], axis[1], axis[2]); + + dVector3 anchor; + dJointGetPRAnchor(jId[1], anchor); + dJointSetPRAnchor(jId[1], anchor[0], anchor[1], anchor[2]); + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + } + + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + + + const dReal *posA = dBodyGetPosition(bId[0][b]); + const dReal *posB = dBodyGetPosition(bId[1][b]); + CHECK_CLOSE (posA[0], posB[0], 1e-4); + CHECK_CLOSE (posA[1], posB[1], 1e-4); + CHECK_CLOSE (posA[2], posB[2], 1e-4); + CHECK_CLOSE (posA[3], posB[3], 1e-4); + } + } + + + + + + + // This test compare the result of a slider with 2 bodies where body body 2 is + // fixed to the world to a slider with only one body at position 1. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + TEST_FIXTURE(Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y, + test_Limit_minus1_025_One_Body_on_left) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPRAxis1(jId_12, axis); + dBodySetLinearVel (bId1_12, 4*axis[0], 4*axis[1], 4*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPRParam(jId_12, dParamLoStop, -1); + dJointSetPRParam(jId_12, dParamHiStop, 0.25); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + dJointAttach(jId, bId, 0); + dJointSetPRParam(jId, dParamLoStop, -1); + dJointSetPRParam(jId, dParamHiStop, 0.25); + + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos1_12[0], pos[0], 1e-2); + CHECK_CLOSE (pos1_12[1], pos[1], 1e-2); + CHECK_CLOSE (pos1_12[2], pos[2], 1e-2); + + const dReal *q1_12 = dBodyGetQuaternion(bId1_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q1_12[0], q[0], 1e-4); + CHECK_CLOSE (q1_12[1], q[1], 1e-4); + CHECK_CLOSE (q1_12[2], q[2], 1e-4); + CHECK_CLOSE (q1_12[3], q[3], 1e-4); + } + + + + // This test compare the result of a slider with 2 bodies where body body 1 is + // fixed to the world to a slider with only one body at position 2. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + TEST_FIXTURE(Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y, + test_Limit_minus1_025_One_Body_on_right) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPRAxis1(jId_12, axis); + dBodySetLinearVel (bId2_12, 4*axis[0], 4*axis[1], 4*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPRParam(jId_12, dParamLoStop, -1); + dJointSetPRParam(jId_12, dParamHiStop, 0.25); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetPRParam(jId, dParamLoStop, -1); + dJointSetPRParam(jId, dParamHiStop, 0.25); + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos2_12[0], pos[0], 1e-2); + CHECK_CLOSE (pos2_12[1], pos[1], 1e-2); + CHECK_CLOSE (pos2_12[2], pos[2], 1e-2); + + + const dReal *q2_12 = dBodyGetQuaternion(bId2_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q2_12[0], q[0], 1e-4); + CHECK_CLOSE (q2_12[1], q[1], 1e-4); + CHECK_CLOSE (q2_12[2], q[2], 1e-4); + CHECK_CLOSE (q2_12[3], q[3], 1e-4); + } + + + + // This test compare the result of a slider with 2 bodies where body body 2 is + // fixed to the world to a slider with only one body at position 1. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y, + test_Limit_0_0_One_Body_on_left) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPRAxis1(jId_12, axis); + dBodySetLinearVel (bId1_12, 4*axis[0], 4*axis[1], 4*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPRParam(jId_12, dParamLoStop, 0); + dJointSetPRParam(jId_12, dParamHiStop, 0); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + + dJointAttach(jId, bId, 0); + dJointSetPRParam(jId, dParamLoStop, 0); + dJointSetPRParam(jId, dParamHiStop, 0); + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos1_12[0], pos[0], 1e-4); + CHECK_CLOSE (pos1_12[1], pos[1], 1e-4); + CHECK_CLOSE (pos1_12[2], pos[2], 1e-4); + + CHECK_CLOSE (0, pos[0], 1e-4); + CHECK_CLOSE (0, pos[1], 1e-4); + CHECK_CLOSE (0, pos[2], 1e-4); + + + const dReal *q1_12 = dBodyGetQuaternion(bId1_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q1_12[0], q[0], 1e-4); + CHECK_CLOSE (q1_12[1], q[1], 1e-4); + CHECK_CLOSE (q1_12[2], q[2], 1e-4); + CHECK_CLOSE (q1_12[3], q[3], 1e-4); + } + + + // This test compare the result of a slider with 2 bodies where body body 1 is + // fixed to the world to a slider with only one body at position 2. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointPR_Compare_Body_At_Zero_AxisP_Along_Y, + test_Limit_0_0_One_Body_on_right) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPRAxis1(jId_12, axis); + dBodySetLinearVel (bId2_12, 4*axis[0], 4*axis[1], 4*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPRParam(jId_12, dParamLoStop, 0); + dJointSetPRParam(jId_12, dParamHiStop, 0); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetPRParam(jId, dParamLoStop, 0); + dJointSetPRParam(jId, dParamHiStop, 0); + + for (int i=0; i<50; ++i) + { + dWorldStep(wId, 1.0); + } + + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos2_12[0], pos[0], 1e-4); + CHECK_CLOSE (pos2_12[1], pos[1], 1e-4); + CHECK_CLOSE (pos2_12[2], pos[2], 1e-4); + + CHECK_CLOSE (0, pos[0], 1e-4); + CHECK_CLOSE (0, pos[1], 1e-4); + CHECK_CLOSE (0, pos[2], 1e-4); + + + const dReal *q2_12 = dBodyGetQuaternion(bId2_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q2_12[0], q[0], 1e-4); + CHECK_CLOSE (q2_12[1], q[1], 1e-4); + CHECK_CLOSE (q2_12[2], q[2], 1e-4); + CHECK_CLOSE (q2_12[3], q[3], 1e-4); + } + +} // End of SUITE TestdxJointPR + diff --git a/libs/ode-0.16.1/tests/joints/pu.cpp b/libs/ode-0.16.1/tests/joints/pu.cpp new file mode 100644 index 0000000..6d97a07 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/pu.cpp @@ -0,0 +1,920 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/pu.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/pu.h" + +SUITE (TestdxJointPU) +{ + // The 2 bodies are positionned at (0, 0, 0), and (0, 0, 0) + // The second body has a rotation of 27deg around X axis. + // The joint is a PU Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointPU_B1_and_B2_At_Zero_Axis_Along_X + { + Fixture_dxJointPU_B1_and_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + dMatrix3 R; + + dRFromAxisAndAngle (R, 1, 0, 0, REAL(0.47123)); // 27deg + dBodySetRotation (bId2, R); + + jId = dJointCreatePU (wId, 0); + joint = (dxJointPU*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~Fixture_dxJointPU_B1_and_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointPU* joint; + }; + + // Test is dJointSetPUAxis and dJointGetPUAxis return same value + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetGetPUAxis) + { + dVector3 axisOrig, axis; + + + dJointGetPUAxis1 (jId, axisOrig); + dJointGetPUAxis1 (jId, axis); + dJointSetPUAxis1 (jId, axis[0], axis[1], axis[2]); + dJointGetPUAxis1 (jId, axis); + CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4); + CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4); + CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4); + + + dJointGetPUAxis2 (jId, axisOrig); + dJointGetPUAxis2(jId, axis); + dJointSetPUAxis2 (jId, axis[0], axis[1], axis[2]); + dJointGetPUAxis2 (jId, axis); + CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4); + CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4); + CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4); + + + dJointGetPUAxis3 (jId, axisOrig); + dJointGetPUAxis3(jId, axis); + dJointSetPUAxis3 (jId, axis[0], axis[1], axis[2]); + dJointGetPUAxis3 (jId, axis); + CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4); + CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4); + CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4); + } + + + + + + + + + + + + + + + // The joint is a PU Joint + // Default joint value + // The two bodies at at (0, 0, 0) + struct Fixture_dxJointPU_B1_and_B2_At_Zero + { + Fixture_dxJointPU_B1_and_B2_At_Zero() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreatePU (wId, 0); + joint = (dxJointPU*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~Fixture_dxJointPU_B1_and_B2_At_Zero() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointPU* joint; + + static const dReal offset; + }; + const dReal Fixture_dxJointPU_B1_and_B2_At_Zero::offset = REAL (3.1); + + + + + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B1_3Unit) + { + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + + dJointSetPUAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B1_Minus_3Unit) + { + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + dJointSetPUAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B2_3Unit) + { + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + dJointSetPUAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B2_Minus_3Unit) + { + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + dJointSetPUAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + + + // Attach only one body at position 1 to the joint dJointAttach (jId, bId, 0) + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B1_OffsetUnit) + { + dJointAttach (jId, bId1, 0); + + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + dJointSetPUAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + // Attache only one body at position 1 to the joint dJointAttach (jId, bId, 0) + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B1_Minus_OffsetUnit) + { + dJointAttach (jId, bId1, 0); + + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + dJointSetPUAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId1, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + + + // Attache only one body at position 2 to the joint dJointAttach (jId, 0, bId) + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B2_OffsetUnit) + { + dJointAttach (jId, 0, bId2); + + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + dJointSetPUAnchorOffset (jId, 0, 0, 0, + -offset*axis[0], -offset*axis[1], -offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + // Attache only one body at position 2 to the joint dJointAttach (jId, 0, bId) + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPU_B1_and_B2_At_Zero, + test_dJointSetPUAxisOffset_B2_Minus_OffsetUnit) + { + dJointAttach (jId, 0, bId2); + + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dVector3 axis; + dJointGetPUAxisP (jId, axis); + dJointSetPUAnchorOffset (jId, 0, 0, 0, + offset*axis[0], offset*axis[1], offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId2, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + + + + // Only one body + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a PU Joint + // Axis is in the oppsite X axis + // Anchor at (0, 0, 0) + // N.B. By default the body is attached at position 1 on the joint + // dJointAttach (jId, bId, 0); + struct Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId = dBodyCreate (wId); + dBodySetPosition (bId, 0, 0, 0); + + jId = dJointCreatePU (wId, 0); + joint = (dxJointPU*) jId; + + + dJointAttach (jId, bId, NULL); + + dJointSetPUAxisP (jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId; + + dJointID jId; + dxJointPU* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X::axis = + { + -1, 0, 0 + }; + const dReal Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X::offset = REAL (3.1); + + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <--- Axis + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X, + test_dJointSetPUAxisOffset_B1_At_Position_1_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dJointSetPUAnchorOffset (jId, 0, 0, 0, + -offset*axis[0],-offset*axis[1],-offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <--- Axis + // B1 => B1 + TEST_FIXTURE (Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X, + test_dJointSetPUAxisOffset_B1_Minus_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dJointSetPUAnchorOffset (jId, 0, 0, 0, + offset*axis[0],offset*axis[1],offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <--- Axis + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X, + test_dJointSetPUAxisOffset_B2_OffsetUnit) + { + // By default it is attached to position 1 + // Now attach the body at positiojn 2 + dJointAttach(jId, 0, bId); + + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dJointSetPUAnchorOffset (jId, 0, 0, 0, + offset*axis[0], offset*axis[1], offset*axis[2]); + CHECK_CLOSE (offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <--- Axis + // B2 => B2 + TEST_FIXTURE (Fixture_dxJointPU_One_Body_At_Zero_Axis_Inverse_of_X, + test_dJointSetPUAxisOffset_B2_Minus_OffsetUnit) + { + // By default it is attached to position 1 + // Now attach the body at positiojn 2 + dJointAttach(jId, 0, bId); + + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dJointSetPUAnchorOffset (jId, 0, 0, 0, + -offset*axis[0], -offset*axis[1], -offset*axis[2]); + CHECK_CLOSE (-offset, dJointGetPUPosition (jId), 1e-4); + + dBodySetPosition (bId, 0, 0, 0); + CHECK_CLOSE (0.0, dJointGetPUPosition (jId), 1e-4); + } + + + + + + + + + + // Compare only one body to 2 bodies with one fixed. + // + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a PU Joint with default values + struct Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero + { + Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero() + { + wId = dWorldCreate(); + + bId1_12 = dBodyCreate (wId); + dBodySetPosition (bId1_12, 0, 0, 0); + + bId2_12 = dBodyCreate (wId); + dBodySetPosition (bId2_12, 0, 0, 0); + // The force will be added in the function since it is not + // always on the same body + + jId_12 = dJointCreatePU (wId, 0); + dJointAttach(jId_12, bId1_12, bId2_12); + + fixed = dJointCreateFixed (wId, 0); + + + + jId = dJointCreatePU (wId, 0); + + bId = dBodyCreate (wId); + dBodySetPosition (bId, 0, 0, 0); + + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPUAxisP(jId_12, axis); + dJointSetPUAxisP(jId, axis[0], axis[1], axis[2]); + dBodySetLinearVel (bId, magnitude*axis[0], magnitude*axis[1], magnitude*axis[2]); + } + + ~Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1_12; + dBodyID bId2_12; + + dJointID jId_12; // Joint with 2 bodies + + dJointID fixed; + + + + dBodyID bId; + dJointID jId; // Joint with one body + + static const dReal magnitude; + }; + const dReal Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero::magnitude = REAL (4.27); + + + TEST_FIXTURE (Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero, + test_dJointSetPUPositionRate_Only_B1) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPUAxisP(jId_12, axis); + dBodySetLinearVel (bId1_12, magnitude*axis[0], magnitude*axis[1], magnitude*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + dJointAttach(jId, bId, 0); + + CHECK_CLOSE(dJointGetPUPositionRate(jId_12), dJointGetPUPositionRate(jId), 1e-2); + } + + + TEST_FIXTURE (Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero, + test_dJointSetPUPositionRate_Only_B2) + { + // Linear velocity along the prismatic axis; + dVector3 axis; + dJointGetPUAxisP(jId_12, axis); + dBodySetLinearVel (bId2_12, magnitude*axis[0], magnitude*axis[1], magnitude*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + dJointAttach(jId, 0, bId); + + CHECK_CLOSE(dJointGetPUPositionRate(jId_12), dJointGetPUPositionRate(jId), 1e-2); + } + + + + + + + + + // This test compare the result of a pu joint with 2 bodies where body body 2 is + // fixed to the world to a pu joint with only one body at position 1. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + TEST_FIXTURE(Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero, + test_Limit_minus1_025_One_Body_on_left) + { + dVector3 axis; + dJointGetPUAxisP(jId_12, axis); + dJointSetPUAxisP(jId, axis[0], axis[1], axis[2]); + dBodySetLinearVel (bId1_12, magnitude*axis[0], magnitude*axis[1], magnitude*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPUParam(jId_12, dParamLoStop3, -1); + dJointSetPUParam(jId_12, dParamHiStop3, 0.25); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + dJointAttach(jId, bId, 0); + dJointSetPUParam(jId, dParamLoStop3, -1); + dJointSetPUParam(jId, dParamHiStop3, 0.25); + + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos1_12[0], pos[0], 1e-2); + CHECK_CLOSE (pos1_12[1], pos[1], 1e-2); + CHECK_CLOSE (pos1_12[2], pos[2], 1e-2); + + const dReal *q1_12 = dBodyGetQuaternion(bId1_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q1_12[0], q[0], 1e-4); + CHECK_CLOSE (q1_12[1], q[1], 1e-4); + CHECK_CLOSE (q1_12[2], q[2], 1e-4); + CHECK_CLOSE (q1_12[3], q[3], 1e-4); + + // Should be different than zero + CHECK( dJointGetPUPosition(jId_12) ); + CHECK( dJointGetPUPosition(jId) ); + + CHECK( dJointGetPUPositionRate(jId_12) ); + CHECK( dJointGetPUPositionRate(jId) ); + } + + + + // This test compare the result of a pu joint with 2 bodies where body body 1 is + // fixed to the world to a pu joint with only one body at position 2. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + TEST_FIXTURE(Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero, + test_Limit_minus1_025_One_Body_on_right) + { + dVector3 axis; + dJointGetPUAxisP(jId_12, axis); + dJointSetPUAxisP(jId, axis[0], axis[1], axis[2]); + dBodySetLinearVel (bId2_12, magnitude*axis[0], magnitude*axis[1], magnitude*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPUParam(jId_12, dParamLoStop3, -1); + dJointSetPUParam(jId_12, dParamHiStop3, 0.25); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetPUParam(jId, dParamLoStop3, -1); + dJointSetPUParam(jId, dParamHiStop3, 0.25); + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos2_12[0], pos[0], 1e-2); + CHECK_CLOSE (pos2_12[1], pos[1], 1e-2); + CHECK_CLOSE (pos2_12[2], pos[2], 1e-2); + + + const dReal *q2_12 = dBodyGetQuaternion(bId2_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q2_12[0], q[0], 1e-4); + CHECK_CLOSE (q2_12[1], q[1], 1e-4); + CHECK_CLOSE (q2_12[2], q[2], 1e-4); + CHECK_CLOSE (q2_12[3], q[3], 1e-4); + + // Should be different than zero + CHECK( dJointGetPUPosition(jId_12) ); + CHECK( dJointGetPUPosition(jId) ); + + CHECK( dJointGetPUPositionRate(jId_12) ); + CHECK( dJointGetPUPositionRate(jId) ); + } + + + + // This test compare the result of a pu joint with 2 bodies where body 2 is + // fixed to the world to a pu joint with only one body at position 1. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero, + test_Limit_0_0_One_Body_on_left) + { + dVector3 axis; + dJointGetPUAxisP(jId_12, axis); + dJointSetPUAxisP(jId, axis[0], axis[1], axis[2]); + dBodySetLinearVel (bId1_12, magnitude*axis[0], magnitude*axis[1], magnitude*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPUParam(jId_12, dParamLoStop3, 0); + dJointSetPUParam(jId_12, dParamHiStop3, 0); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + + dJointAttach(jId, bId, 0); + dJointSetPUParam(jId, dParamLoStop3, 0); + dJointSetPUParam(jId, dParamHiStop3, 0); + + for (int i=0; i<500; ++i) + dWorldStep(wId, 1.0); + + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos1_12[0], pos[0], 1e-4); + CHECK_CLOSE (pos1_12[1], pos[1], 1e-4); + CHECK_CLOSE (pos1_12[2], pos[2], 1e-4); + + CHECK_CLOSE (0, pos[0], 1e-4); + CHECK_CLOSE (0, pos[1], 1e-4); + CHECK_CLOSE (0, pos[2], 1e-4); + + + const dReal *q1_12 = dBodyGetQuaternion(bId1_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q1_12[0], q[0], 1e-4); + CHECK_CLOSE (q1_12[1], q[1], 1e-4); + CHECK_CLOSE (q1_12[2], q[2], 1e-4); + CHECK_CLOSE (q1_12[3], q[3], 1e-4); + } + + + // This test compare the result of a pu joint with 2 bodies where body body 1 is + // fixed to the world to a pu joint with only one body at position 2. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointPU_Compare_One_Body_To_Two_Bodies_At_Zero, + test_Limit_0_0_One_Body_on_right) + { + dVector3 axis; + dJointGetPUAxisP(jId_12, axis); + dJointSetPUAxisP(jId, axis[0], axis[1], axis[2]); + dBodySetLinearVel (bId2_12, magnitude*axis[0], magnitude*axis[1], magnitude*axis[2]); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetPUParam(jId_12, dParamLoStop3, 0); + dJointSetPUParam(jId_12, dParamHiStop3, 0); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetPUParam(jId, dParamLoStop3, 0); + dJointSetPUParam(jId, dParamHiStop3, 0); + + for (int i=0; i<500; ++i) + dWorldStep(wId, 1.0); + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + const dReal *pos = dBodyGetPosition(bId); + + CHECK_CLOSE (pos2_12[0], pos[0], 1e-4); + CHECK_CLOSE (pos2_12[1], pos[1], 1e-4); + CHECK_CLOSE (pos2_12[2], pos[2], 1e-4); + + CHECK_CLOSE (0, pos[0], 1e-4); + CHECK_CLOSE (0, pos[1], 1e-4); + CHECK_CLOSE (0, pos[2], 1e-4); + + + const dReal *q2_12 = dBodyGetQuaternion(bId2_12); + const dReal *q = dBodyGetQuaternion(bId); + + CHECK_CLOSE (q2_12[0], q[0], 1e-4); + CHECK_CLOSE (q2_12[1], q[1], 1e-4); + CHECK_CLOSE (q2_12[2], q[2], 1e-4); + CHECK_CLOSE (q2_12[3], q[3], 1e-4); + } + + +} // End of SUITE TestdxJointPU + diff --git a/libs/ode-0.16.1/tests/joints/slider.cpp b/libs/ode-0.16.1/tests/joints/slider.cpp new file mode 100644 index 0000000..fe57a55 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/slider.cpp @@ -0,0 +1,1332 @@ + +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/slider.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/slider.h" + +SUITE (TestdxJointSlider) +{ + struct dxJointSlider_Fixture_1 + { + dxJointSlider_Fixture_1() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, -1, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 1, 0); + + jId = dJointCreateSlider (wId, 0); + joint = (dxJointSlider*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~dxJointSlider_Fixture_1() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointSlider* joint; + }; + + TEST_FIXTURE (dxJointSlider_Fixture_1, test_dJointSetSlider) + { + // the 2 bodies are align + dJointSetSliderAxis (jId, 1, 0, 0); + CHECK_CLOSE (joint->qrel[0], 1.0, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + dMatrix3 R; + // Rotate 2nd body 90deg around X + dBodySetPosition (bId2, 0, 0, 1); + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId2, R); + + dJointSetSliderAxis (jId, 1, 0 ,0); + CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + + // Rotate 2nd body -90deg around X + dBodySetPosition (bId2, 0, 0, -1); + dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0); + dBodySetRotation (bId2, R); + + dJointSetSliderAxis (jId, 1, 0 ,0); + CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[1], -0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + + // Rotate 2nd body 90deg around Z + dBodySetPosition (bId2, 0, 1, 0); + dRFromAxisAndAngle (R, 0, 0, 1, M_PI/2.0); + dBodySetRotation (bId2, R); + + dJointSetSliderAxis (jId, 1, 0 ,0); + CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.70710678118654757, 1e-4); + + + // Rotate 2nd body 45deg around Y + dBodySetPosition (bId2, 0, 1, 0); + dRFromAxisAndAngle (R, 0, 1, 0, M_PI/4.0); + dBodySetRotation (bId2, R); + + dJointSetSliderAxis (jId, 1, 0 ,0); + CHECK_CLOSE (joint->qrel[0], 0.92387953251128674, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); + CHECK_CLOSE (joint->qrel[2], 0.38268343236508984, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); + + // Rotate in a strange manner + // Both bodies at origin + dRFromEulerAngles (R, REAL(0.23), REAL(3.1), REAL(-0.73)); + dBodySetPosition (bId1, 0, 0, 0); + dBodySetRotation (bId1, R); + + dRFromEulerAngles (R, REAL(-0.57), REAL(1.49), REAL(0.81)); + dBodySetPosition (bId2, 0, 0, 0); + dBodySetRotation (bId2, R); + + dJointSetSliderAxis (jId, 1, 0 ,0); + CHECK_CLOSE (joint->qrel[0], -0.25526036263124319, 1e-4); + CHECK_CLOSE (joint->qrel[1], 0.28434861188441968, 1e-4); + CHECK_CLOSE (joint->qrel[2], -0.65308047160141625, 1e-4); + CHECK_CLOSE (joint->qrel[3], 0.65381489108282143, 1e-4); + } + + + + // The 2 bodies are positionned at (0, 0, 0), with no rotation + // The joint is a Slider Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X + { + Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateSlider (wId, 0); + joint = (dxJointSlider*) jId; + + + dJointAttach (jId, bId1, bId2); + + dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointSlider* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X::axis = {1, 0, 0}; + const dReal Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X::offset = REAL(3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B1_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B1_Minus_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B2_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B2_Minus_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + + + // The 2 bodies are positionned at (0, 0, 0), with no rotation + // The joint is a Slider Joint + // Axis is the opposite of the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateSlider (wId, 0); + joint = (dxJointSlider*) jId; + + + dJointAttach (jId, bId1, bId2); + + + dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointSlider* joint; + + static const dVector3 axis; + static const dReal offset; + }; + const dVector3 Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X::axis = {-1, 0, 0}; + const dReal Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X::offset = REAL(3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B1_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B1_Minus_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B2_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + // + // Start with a Offset of -offset unit + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B2_Minus_3Unit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + + // Only body 1 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Slider Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X + { + Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreateSlider (wId, 0); + joint = (dxJointSlider*) jId; + + + dJointAttach (jId, bId1, NULL); + + dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + dJointID jId; + dxJointSlider* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X::axis = {1, 0, 0}; + const dReal Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X::offset = REAL(3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B1_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B1_Minus_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + + // Only body 1 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Slider Joint + // Axis is in the oppsite X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreateSlider (wId, 0); + joint = (dxJointSlider*) jId; + + + dJointAttach (jId, bId1, NULL); + + dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + dJointID jId; + dxJointSlider* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X::axis = {-1, 0, 0}; + const dReal Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X::offset = REAL(3.1); + + // Move 1st body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B1_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 1st body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B1 => B1 + // + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B1_Minus_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId1, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + + // Only body 2 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Slider Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X + { + Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateSlider (wId, 0); + joint = (dxJointSlider*) jId; + + + dJointAttach (jId, NULL, bId2); + + dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + dJointID jId; + dxJointSlider* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X::axis = {1, 0, 0}; + const dReal Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X::offset = REAL(3.1); + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B2_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> Axis --> + // B2 => B2 + // + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderAxisOffset_B2_Minus_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, -offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Only body 2 + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Slider Joint + // Axis is in the oppsite X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateSlider (wId, 0); + joint = (dxJointSlider*) jId; + + + dJointAttach (jId, NULL, bId2); + + dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + dJointID jId; + dxJointSlider* joint; + + static const dVector3 axis; + + static const dReal offset; + }; + const dVector3 Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X::axis = {-1, 0, 0}; + const dReal Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X::offset = REAL(3.1); + + // Move 2nd body offset unit in the X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B2_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, offset, 0, 0); + + CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); + } + + // Move 2nd body offset unit in the opposite X direction + // + // X-------> X---------> <--- Axis + // B2 => B2 + // + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderAxisOffset_B2_Minus_OffsetUnit) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + + dBodySetPosition(bId2, -offset, 0, 0); + + CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); + } + + // ========================================================================== + // Test Position Rate + // ========================================================================== + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B1 F-> => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Along_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> Axis --> + // B1 <-F => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B1 F-> => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> <-- Axis + // B1 <-F => B1 + // B2 B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Along_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> Axis --> + // B1 => B1 + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> <-- Axis + // B1 => B1 + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B1 F-> => B1 + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Along_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> Axis --> + // B1 <-F => B1 + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + + // Apply force on 1st body in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B1 F-> => B1 + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on 1st body in the inverse X direction + // + // X-------> X---------> <-- Axis + // B1 <-F => B1 + TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B1) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId1, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + + // Apply force on body 2 in the X direction also the Axis direction + // + // X-------> X---------> Axis --> + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Along_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on body 2 in the inverse X direction + // + // X-------> X---------> Axis --> + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, + test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + + // Apply force on body 2 in the X direction also the Axis direction + // + // X-------> X---------> <-- Axis + // B2 F-> B2 + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, 1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); + } + + // Apply force on body 2 in the inverse X direction + // + // X-------> X---------> <-- Axis + // B2 <-F B2 + TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, + test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B2) + { + CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); + CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); + + dBodyAddForce(bId2, -1.0, 0, 0); + dWorldQuickStep (wId, 1.0); + + CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); + } + + + + + + + // Create 2 bodies attached by a Slider joint + // Axis is along the X axis (Default value + // Anchor at (0, 0, 0) (Default value) + // + // ^Y + // | + // | + // | + // | + // Body1 | Body2 + // * Z-----*->x + struct dxJointSlider_Test_Initialization + { + dxJointSlider_Test_Initialization() + { + wId = dWorldCreate(); + + // Remove gravity to have the only force be the force of the joint + dWorldSetGravity(wId, 0,0,0); + + for (int j=0; j<2; ++j) + { + bId[j][0] = dBodyCreate (wId); + dBodySetPosition (bId[j][0], -1, -2, -3); + + bId[j][1] = dBodyCreate (wId); + dBodySetPosition (bId[j][1], 11, 22, 33); + + + dMatrix3 R; + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][0], R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][1], R); + + + jId[j] = dJointCreateSlider (wId, 0); + dJointAttach (jId[j], bId[j][0], bId[j][1]); + } + } + + ~dxJointSlider_Test_Initialization() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId[2][2]; + + + dJointID jId[2]; + + }; + + + // Test if setting a Slider joint with its default values + // will behave the same as a default Slider joint + TEST_FIXTURE (dxJointSlider_Test_Initialization, + test_Slider_Initialization) + { + using namespace std; + + dVector3 axis; + dJointGetSliderAxis(jId[1], axis); + dJointSetSliderAxis(jId[1], axis[0], axis[1], axis[2]); + + + CHECK_CLOSE (dJointGetSliderPosition(jId[0]), + dJointGetSliderPosition(jId[1]), 1e-6); + + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-6); + CHECK_CLOSE (qA[1], qB[1], 1e-6); + CHECK_CLOSE (qA[2], qB[2], 1e-6); + CHECK_CLOSE (qA[3], qB[3], 1e-6); + } + + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-6); + CHECK_CLOSE (qA[1], qB[1], 1e-6); + CHECK_CLOSE (qA[2], qB[2], 1e-6); + CHECK_CLOSE (qA[3], qB[3], 1e-6); + + + const dReal *posA = dBodyGetPosition(bId[0][b]); + const dReal *posB = dBodyGetPosition(bId[1][b]); + CHECK_CLOSE (posA[0], posB[0], 1e-6); + CHECK_CLOSE (posA[1], posB[1], 1e-6); + CHECK_CLOSE (posA[2], posB[2], 1e-6); + CHECK_CLOSE (posA[3], posB[3], 1e-6); + } + } + + + + // Compare Only body 1 to 2 bodies with one fixed. + // + // The body are positionned at (0, 0, 0), with no rotation + // The joint is a Slider Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X + { + Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1_12 = dBodyCreate (wId); + dBodySetPosition (bId1_12, 0, 0, 0); + + bId2_12 = dBodyCreate (wId); + dBodySetPosition (bId2_12, 0, 0, 0); + // The force will be added in the function since it is not + // always on the same body + + jId_12 = dJointCreateSlider (wId, 0); + dJointAttach(jId_12, bId1_12, bId2_12); + + fixed = dJointCreateFixed (wId, 0); + + + + bId = dBodyCreate (wId); + dBodySetPosition (bId, 0, 0, 0); + + dBodyAddForce (bId, 4, 0, 0); + + jId = dJointCreateSlider (wId, 0); + } + + ~Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1_12; + dBodyID bId2_12; + + dJointID jId_12; // Joint with 2 bodies + + dJointID fixed; + + + + dBodyID bId; + dJointID jId; // Joint with one body + }; + + // This test compare the result of a slider with 2 bodies where body body 2 is + // fixed to the world to a slider with only one body at position 1. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_minus1_025_One_Body_on_left) + { + dBodyAddForce (bId1_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetSliderParam(jId_12, dParamLoStop, -1); + dJointSetSliderParam(jId_12, dParamHiStop, 0.25); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + dJointAttach(jId, bId, 0); + dJointSetSliderParam(jId, dParamLoStop, -1); + dJointSetSliderParam(jId, dParamHiStop, 0.25); + + + for (int i=0; i<50; ++i) + dWorldStep(wId, 1.0); + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + + const dReal *pos = dBodyGetPosition(bId); + + + CHECK_CLOSE (pos[0], pos1_12[0], 1e-2); + CHECK_CLOSE (pos[1], pos1_12[1], 1e-2); + CHECK_CLOSE (pos[2], pos1_12[2], 1e-2); + } + + + + // This test compare the result of a slider with 2 bodies where body body 1 is + // fixed to the world to a slider with only one body at position 2. + // + // Test the limits [-1, 0.25] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_minus1_025_One_Body_on_right) + { + dBodyAddForce (bId2_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetSliderParam(jId_12, dParamLoStop, -1); + dJointSetSliderParam(jId_12, dParamHiStop, 0.25); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetSliderParam(jId, dParamLoStop, -1); + dJointSetSliderParam(jId, dParamHiStop, 0.25); + + for (int i=0; i<50; ++i) + { + dWorldStep(wId, 1.0); + } + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + + const dReal *pos = dBodyGetPosition(bId); + + + CHECK_CLOSE (pos[0], pos2_12[0], 1e-2); + CHECK_CLOSE (pos[1], pos2_12[1], 1e-2); + CHECK_CLOSE (pos[2], pos2_12[2], 1e-2); + } + + + + // This test compare the result of a slider with 2 bodies where body body 2 is + // fixed to the world to a slider with only one body at position 1. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, bId, 0); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_0_0_One_Body_on_left) + { + dBodyAddForce (bId1_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetSliderParam(jId_12, dParamLoStop, 0); + dJointSetSliderParam(jId_12, dParamHiStop, 0); + + dJointAttach(fixed, 0, bId2_12); + dJointSetFixed(fixed); + + + dJointAttach(jId, bId, 0); + dJointSetSliderParam(jId, dParamLoStop, 0); + dJointSetSliderParam(jId, dParamHiStop, 0); + + for (int i=0; i<500; ++i) + dWorldStep(wId, 1.0); + + const dReal *pos1_12 = dBodyGetPosition(bId1_12); + + const dReal *pos = dBodyGetPosition(bId); + + + CHECK_CLOSE (pos[0], pos1_12[0], 1e-4); + CHECK_CLOSE (pos[1], pos1_12[1], 1e-4); + CHECK_CLOSE (pos[2], pos1_12[2], 1e-4); + + CHECK_CLOSE (pos[0], 0, 1e-4); + CHECK_CLOSE (pos[1], 0, 1e-4); + CHECK_CLOSE (pos[2], 0, 1e-4); + } + + + // This test compare the result of a slider with 2 bodies where body body 1 is + // fixed to the world to a slider with only one body at position 2. + // + // Test the limits [0, 0] when only one body at is attached to the joint + // using dJointAttache(jId, 0, bId); + // + // The body should not move since their is no room between the two limits + // + TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, + test_Limit_0_0_One_Body_on_right) + { + dBodyAddForce (bId2_12, 4, 0, 0); + + dJointAttach(jId_12, bId1_12, bId2_12); + dJointSetSliderParam(jId_12, dParamLoStop, 0); + dJointSetSliderParam(jId_12, dParamHiStop, 0); + + dJointAttach(fixed, bId1_12, 0); + dJointSetFixed(fixed); + + + dJointAttach(jId, 0, bId); + dJointSetSliderParam(jId, dParamLoStop, 0); + dJointSetSliderParam(jId, dParamHiStop, 0); + + for (int i=0; i<500; ++i) + dWorldStep(wId, 1.0); + + const dReal *pos2_12 = dBodyGetPosition(bId2_12); + + const dReal *pos = dBodyGetPosition(bId); + + + CHECK_CLOSE (pos[0], pos2_12[0], 1e-4); + CHECK_CLOSE (pos[1], pos2_12[1], 1e-4); + CHECK_CLOSE (pos[2], pos2_12[2], 1e-4); + + CHECK_CLOSE (pos[0], 0, 1e-4); + CHECK_CLOSE (pos[1], 0, 1e-4); + CHECK_CLOSE (pos[2], 0, 1e-4); + } + + + + +} // End of SUITE TestdxJointSlider diff --git a/libs/ode-0.16.1/tests/joints/universal.cpp b/libs/ode-0.16.1/tests/joints/universal.cpp new file mode 100644 index 0000000..5a4c17b --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/universal.cpp @@ -0,0 +1,2119 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/universal.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/universal.h" + +dReal d2r(dReal degree) +{ + return degree * (dReal)(M_PI / 180.0); +} +dReal r2d(dReal degree) +{ + return degree * (dReal)(180.0/M_PI); +} + +SUITE (TestdxJointUniversal) +{ + // The 2 bodies are positionned at (0, 0, 0) + // The bodis have no rotation. + // The joint is a Universal Joint + // Axis1 is along the X axis + // Axis2 is along the Y axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y + { + Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y() + { + + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + + jId = dJointCreateUniversal (wId, 0); + joint = (dxJointUniversal*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointUniversal* joint; + }; + + + // The 2 bodies are positionned at (-1, -2, -3), and (11, 22, 33) + // The bodis have rotation of 27deg around some axis. + // The joint is a Universal Joint + // Axis is along the X axis + // Anchor at (0, 0, 0) + struct Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X + { + Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, -1, -2, -3); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 11, 22, 33); + + dMatrix3 R; + + dVector3 axis; + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId1, R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId2, R); + + jId = dJointCreateUniversal (wId, 0); + joint = (dxJointUniversal*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointUniversal* joint; + }; + + + // Only one body body1 at (0,0,0) + // The joint is an Universal Joint. + // Axis1 is along the X axis + // Axis2 is along the Y axis + // Anchor at (0, 0, 0) + // + // ^Y + // | + // | + // | + // | + // | + // Z <-- X + struct Fixture_dxJointUniversal_B1_At_Zero_Default_Axes + { + Fixture_dxJointUniversal_B1_At_Zero_Default_Axes() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreateUniversal (wId, 0); + + + dJointAttach (jId, bId1, NULL); + dJointSetUniversalAnchor (jId, 0, 0, 0); + } + + ~Fixture_dxJointUniversal_B1_At_Zero_Default_Axes() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + + dJointID jId; + }; + + + + // Only one body body2 at (0,0,0) + // The joint is an Universal Joint. + // Axis1 is along the X axis. + // Axis2 is along the Y axis. + // Anchor at (0, 0, 0) + // + // ^Y + // | + // | + // | + // | + // | + // Z <-- X + struct Fixture_dxJointUniversal_B2_At_Zero_Default_Axes + { + Fixture_dxJointUniversal_B2_At_Zero_Default_Axes() + { + wId = dWorldCreate(); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateUniversal (wId, 0); + + + dJointAttach (jId, NULL, bId2); + dJointSetUniversalAnchor (jId, 0, 0, 0); + } + + ~Fixture_dxJointUniversal_B2_At_Zero_Default_Axes() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId2; + + dJointID jId; + }; + + + // Test is dJointGetUniversalAngles versus + // dJointGetUniversalAngle1 and dJointGetUniversalAngle2 dJointGetUniversalAxis + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetGetUniversalAngles_Versus_Angle1_and_Angle2) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dMatrix3 R; + dReal ang1, ang2; + + + dVector3 axis1; + dJointGetUniversalAxis1 (jId, axis1); + + dVector3 axis2; + dJointGetUniversalAxis2 (jId, axis2); + + ang1 = d2r(REAL(23.0)); + dRFromAxisAndAngle (R, axis1[0], axis1[1], axis1[2], ang1); + dBodySetRotation (bId1, R); + + ang2 = d2r(REAL(17.0)); + dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + + + + + // ax1 and ax2 are pseudo-random axis. N.B. They are NOT the axis of the joints. + dVector3 ax1; + ax1[0] = REAL(0.2); + ax1[1] = -REAL(0.67); + ax1[2] = -REAL(0.81); + dNormalize3(ax1); + + dVector3 ax2; + ax2[0] = REAL(0.62); + ax2[1] = REAL(0.31); + ax2[2] = REAL(0.43); + dNormalize3(ax2); + + + ang1 = d2r(REAL(23.0)); + dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang1); + dBodySetRotation (bId1, R); + + ang2 = d2r(REAL(0.0)); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4); + + + + ang1 = d2r(REAL(0.0)); + + ang2 = d2r(REAL(23.0)); + dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2); + dBodySetRotation (bId1, R); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4); + + + ang1 = d2r(REAL(38.0)); + dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang2); + dBodySetRotation (bId1, R); + + ang2 = d2r(REAL(-43.0)); + dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2); + dBodySetRotation (bId1, R); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4); + + + // Try with random axis for the axis of the joints + dRSetIdentity(R); + dBodySetRotation (bId1, R); + dBodySetRotation (bId1, R); + + axis1[0] = REAL(0.32); + axis1[1] = -REAL(0.57); + axis1[2] = REAL(0.71); + dNormalize3(axis1); + + axis2[0] = -REAL(0.26); + axis2[1] = -REAL(0.31); + axis2[2] = REAL(0.69); + dNormalize3(axis2); + + dVector3 cross; + dCalcVectorCross3(cross, axis1, axis2); + dJointSetUniversalAxis1(jId, axis1[0], axis1[1], axis1[2]); + dJointSetUniversalAxis2(jId, cross[0], cross[1], cross[2]); + + + ang1 = d2r(REAL(23.0)); + dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang1); + dBodySetRotation (bId1, R); + + ang2 = d2r(REAL(0.0)); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4); + + + + ang1 = d2r(REAL(0.0)); + + ang2 = d2r(REAL(23.0)); + dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2); + dBodySetRotation (bId1, R); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4); + + + ang1 = d2r(REAL(38.0)); + dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang2); + dBodySetRotation (bId1, R); + + ang2 = d2r(REAL(-43.0)); + dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2); + dBodySetRotation (bId1, R); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4); + } + + + // ========================================================================= + // Test ONE BODY behavior + // ========================================================================= + + + // Test when there is only one body at position one on the joint + TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes, + test_dJointGetUniversalAngle1_1Body_B1) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis1; + dJointGetUniversalAxis1 (jId, axis1); + dVector3 axis2; + dJointGetUniversalAxis2 (jId, axis2); + + dMatrix3 R; + + dReal ang1 = REAL(0.23); + dRFromAxisAndAngle (R, axis1[0], axis1[1], axis1[2], ang1); + dBodySetRotation (bId1, R); + + dReal ang2 = REAL(0.0); + + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + + dMatrix3 I; + dRSetIdentity(I); // Set the rotation of the body to be the Identity (i.e. zero) + dBodySetRotation (bId1, I); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + + // Test the same rotation, when axis1 is inverted + dJointSetUniversalAxis1 (jId, -axis1[0], -axis1[1], -axis1[2]); + + dBodySetRotation (bId1, R); + + CHECK_CLOSE (-ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (-ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + // Test the same rotation, when axis1 is default and axis2 is inverted + dBodySetRotation (bId1, I); + + dJointSetUniversalAxis1 (jId, axis1[0], axis1[1], axis1[2]); + dJointSetUniversalAxis2 (jId, -axis2[0], -axis2[1], -axis2[2]); + + + dBodySetRotation (bId1, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + } + + + + + // Test when there is only one body at position two on the joint + TEST_FIXTURE (Fixture_dxJointUniversal_B2_At_Zero_Default_Axes, + test_dJointGetUniversalAngle1_1Body_B2) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis1; + dJointGetUniversalAxis1 (jId, axis1); + + dVector3 axis2; + dJointGetUniversalAxis2 (jId, axis2); + + dMatrix3 R; + + dReal ang1 = REAL(0.0); + + dReal ang2 = REAL(0.23); + dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2); + dBodySetRotation (bId2, R); + + + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + + dMatrix3 I; + dRSetIdentity(I); // Set the rotation of the body to be the Identity (i.e. zero) + dBodySetRotation (bId2, I); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis2 (jId, -axis2[0], -axis2[1], -axis2[2]); + + dBodySetRotation (bId2, R); + + CHECK_CLOSE (-ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (-ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + // Test the same rotation, when axis1 is inverted and axis2 is default + dBodySetRotation (bId2, I); + + dJointSetUniversalAxis1 (jId, -axis1[0], -axis1[1], -axis1[2]); + dJointSetUniversalAxis2 (jId, axis2[0], axis2[1], axis2[2]); + + + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + } + + + + + + + // ========================================================================= + // + // ========================================================================= + + + // Test is dJointSetUniversalAxis and dJointGetUniversalAxis return same value + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X, + test_dJointSetGetUniversalAxis) + { + dVector3 axisOrig, axis; + + + dJointGetUniversalAxis1 (jId, axisOrig); + dJointGetUniversalAxis1 (jId, axis); + dJointSetUniversalAxis1 (jId, axis[0], axis[1], axis[2]); + dJointGetUniversalAxis1 (jId, axis); + CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4); + CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4); + CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4); + + + dJointGetUniversalAxis2 (jId, axisOrig); + dJointGetUniversalAxis2(jId, axis); + dJointSetUniversalAxis2 (jId, axis[0], axis[1], axis[2]); + dJointGetUniversalAxis2 (jId, axis); + CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4); + CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4); + CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4); + + + dVector3 anchor1, anchor2, anchorOrig1, anchorOrig2; + dJointGetUniversalAnchor (jId, anchorOrig1); + dJointGetUniversalAnchor (jId, anchor1); + dJointGetUniversalAnchor2 (jId, anchorOrig2); + dJointGetUniversalAnchor2 (jId, anchor2); + + dJointSetUniversalAnchor (jId, anchor1[0], anchor1[1], anchor1[2]); + dJointGetUniversalAnchor (jId, anchor1); + dJointGetUniversalAnchor2 (jId, anchor2); + CHECK_CLOSE (anchor1[0], anchorOrig1[0] , 1e-4); + CHECK_CLOSE (anchor1[1], anchorOrig1[1] , 1e-4); + CHECK_CLOSE (anchor1[2], anchorOrig1[2] , 1e-4); + + CHECK_CLOSE (anchor2[0], anchorOrig2[0] , 1e-4); + CHECK_CLOSE (anchor2[1], anchorOrig2[1] , 1e-4); + CHECK_CLOSE (anchor2[2], anchorOrig2[2] , 1e-4); + } + + + + // Create 2 bodies attached by a Universal joint + // Axis is along the X axis (Default value + // Anchor at (0, 0, 0) (Default value) + // + // ^Y + // | + // * Body2 + // | + // | + // Body1 | + // * Z--------> + struct dxJointUniversal_Test_Initialization + { + dxJointUniversal_Test_Initialization() + { + wId = dWorldCreate(); + + // Remove gravity to have the only force be the force of the joint + dWorldSetGravity(wId, 0,0,0); + + for (int j=0; j<2; ++j) + { + bId[j][0] = dBodyCreate (wId); + dBodySetPosition (bId[j][0], -1, -2, -3); + + bId[j][1] = dBodyCreate (wId); + dBodySetPosition (bId[j][1], 11, 22, 33); + + + dMatrix3 R; + dVector3 axis; // Random axis + + axis[0] = REAL(0.53); + axis[1] = -REAL(0.71); + axis[2] = REAL(0.43); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][0], R); + + + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + dNormalize3(axis); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], + REAL(0.47123)); // 27deg + dBodySetRotation (bId[j][1], R); + + jId[j] = dJointCreateUniversal (wId, 0); + dJointAttach (jId[j], bId[j][0], bId[j][1]); + dJointSetUniversalParam(jId[j], dParamLoStop, 1); + dJointSetUniversalParam(jId[j], dParamHiStop, 2); + dJointSetUniversalParam(jId[j], dParamFMax, 200); + } + } + + ~dxJointUniversal_Test_Initialization() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId[2][2]; + + + dJointID jId[2]; + + }; + + + // Test if setting a Universal with its default values + // will behave the same as a default Universal joint + TEST_FIXTURE (dxJointUniversal_Test_Initialization, + test_Universal_Initialization) + { + using namespace std; + + dVector3 axis; + dJointGetUniversalAxis1(jId[1], axis); + dJointSetUniversalAxis1(jId[1], axis[0], axis[1], axis[2]); + + dJointGetUniversalAxis2(jId[1], axis); + dJointSetUniversalAxis2(jId[1], axis[0], axis[1], axis[2]); + + dVector3 anchor; + dJointGetUniversalAnchor(jId[1], anchor); + dJointSetUniversalAnchor(jId[1], anchor[0], anchor[1], anchor[2]); + + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + } + + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + dWorldStep (wId,0.5); + + for (int b=0; b<2; ++b) + { + // Compare body b of the first joint with its equivalent on the + // second joint + const dReal *qA = dBodyGetQuaternion(bId[0][b]); + const dReal *qB = dBodyGetQuaternion(bId[1][b]); + CHECK_CLOSE (qA[0], qB[0], 1e-4); + CHECK_CLOSE (qA[1], qB[1], 1e-4); + CHECK_CLOSE (qA[2], qB[2], 1e-4); + CHECK_CLOSE (qA[3], qB[3], 1e-4); + + + const dReal *posA = dBodyGetPosition(bId[0][b]); + const dReal *posB = dBodyGetPosition(bId[1][b]); + CHECK_CLOSE (posA[0], posB[0], 1e-4); + CHECK_CLOSE (posA[1], posB[1], 1e-4); + CHECK_CLOSE (posA[2], posB[2], 1e-4); + CHECK_CLOSE (posA[3], posB[3], 1e-4); + } + } + + + + + + + + + + + + + + // ========================================================================== + // Testing the offset + // TODO: + // - Test Axis1Offset(...., 0, ang2); + // ========================================================================== + + + // Rotate first body 90deg around X (Axis1) then back to original position + // + // ^ ^ ^ Z ^ + // | | => <--- | | + // | | | | + // B1 B2 B1 B2 .----->Y + // / + // / + // v X (N.B. X is going out of the screen) + // + // Set Axis1 with an Offset of 90deg + // ^ ^ ^ + // <--- | => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetUniversalAxis1Offset_B1_90deg) + { + dMatrix3 R; + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + + dReal ang1 = d2r(REAL(90.0)); + dReal ang2 = d2r(REAL(0.0)); + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + + + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], + ang1, ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + // Rotate 2nd body 90deg around (Axis2) then back to original position + // Offset when setting axis1 + // + // ^ ^ ^ Z ^ + // | | => <--- | | + // | | | | + // B1 B2 B1 B2 .----->Y + // / + // / + // v X (N.B. X is going out of the screen) + // + // Set Axis1 with an Offset of 90deg + // ^ ^ ^ + // <--- | => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetUniversalAxis1Offset_B2_90deg) + { + dMatrix3 R; + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + + dVector3 ax1, ax2; + dJointGetUniversalAxis1 (jId, ax1); + dJointGetUniversalAxis2 (jId, ax2); + + dReal ang1 = d2r(REAL(0.0)); + dReal ang2 = d2r(REAL(90.0)); + dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + + dJointSetUniversalAxis1Offset (jId, ax1[0], ax1[1], ax1[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + + + + + + + // Rotate second body 90deg around Y (Axis2) then back to original position + // + // ^ ^ ^ Z ^ + // | | => | . | + // | | | | + // B1 B2 B1 B2 .----->Y + // / + // / + // v X (N.B. X is going out of the screen) + // + // Set Axis2 with an Offset of 90deg + // ^ ^ ^ + // | . => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetUniversalAxisOffset_B2_90deg) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang1 = d2r(REAL(0.0)); + dReal ang2 = d2r(REAL(90.0)); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + + // Rotate 2nd body -90deg around Y (Axis2) then back to original position + // + // ^ ^ ^ Z ^ + // | | => | x | + // | | | | + // B1 B2 B1 B2 X .----> Y + // N.B. X is going out of the screen + // Start with a Delta of 90deg + // ^ ^ ^ + // | x => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetUniversalAxisOffset_B2_Minus90deg) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang1 = d2r(REAL(0.0)); + dReal ang2 = d2r(REAL(90.0)); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], -ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + + + dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], + ang1, ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], 0); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + // Rotate 1st body 0.23rad around X (Axis1) then back to original position + // + // ^ ^ ^ ^ Z ^ + // | | => \ | | + // | | \ | | + // B1 B2 B1 B2 .-------> Y + // / + // / + // v X (N.B. X is going out of the screen) + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // \ | => | | + // \ | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetUniversalAxis1Offset_B1_0_23rad) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + + dReal ang1 = REAL(0.23); + dReal ang2 = REAL(0.0); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], + ang1, ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + // Rotate 2nd body 0.23rad around Y (Axis2) then back to original position + // + // ^ ^ ^ ^ Z ^ ^ Y (N.B. Y is going in the screen) + // | | => | / | / + // | | | / | / + // B1 B2 B1 B2 .-------> X + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // | / => | | + // | / | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetUniversalAxisOffset_B2_0_23rad) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang1 = REAL(0.0); + dReal ang2 = REAL(0.23); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + // Rotate 1st body 0.23rad around X axis and 2nd body 0.37rad around Y (Axis2) + // then back to their original position. + // The Axis offset are set one at a time + // + // ^ ^ ^ ^ Z ^ ^ Y (N.B. Y is going in the screen) + // | | => \ / | / + // | | \ / | / + // B1 B2 B1 B2 .-------> X + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // \ / => | | + // \ / | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y, + test_dJointSetUniversalAxisOffset_B1_0_23rad_B2_0_37rad_One_by_One) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis1; + dJointGetUniversalAxis1 (jId, axis1); + dVector3 axis2; + dJointGetUniversalAxis2 (jId, axis2); + + dMatrix3 R; + + dReal ang1 = REAL(0.23); + dRFromAxisAndAngle (R, axis1[0], axis1[1], axis1[2], ang1); + dBodySetRotation (bId1, R); + + dReal ang2 = REAL(0.37); + dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2); + dBodySetRotation (bId2, R); + + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dJointSetUniversalAxis1Offset (jId, axis1[0], axis1[1], axis1[2], + ang1, -ang2 ); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + dJointGetUniversalAxis1 (jId, axis1); + dJointGetUniversalAxis2 (jId, axis2); + + dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2); + dBodySetRotation (bId2, R); + + dJointSetUniversalAxis2Offset (jId, axis2[0], axis2[1], axis2[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + +// The 2 bodies are positionned at (0, 0, 0), with no rotation +// The joint is an Universal Joint. +// Axis in the inverse direction of the X axis +// Anchor at (0, 0, 0) +// ^Y +// | +// | +// | +// | +// | +// Z <---- x (X going out of the page) + struct Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X + { + Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 0, 0); + + jId = dJointCreateUniversal (wId, 0); + joint = (dxJointUniversal*) jId; + + + dJointAttach (jId, bId1, bId2); + dJointSetUniversalAnchor (jId, 0, 0, 0); + + axis[0] = -1; + axis[1] = 0; + axis[2] = 0; + dJointSetUniversalAxis1(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointUniversal* joint; + + dVector3 axis; + }; + + + // No offset when setting the Axis1 offset + // x is a Symbol for lines pointing into the screen + // . is a Symbol for lines pointing out of the screen + // + // In 2D In 3D + // ^ ^ ^ ^ Z ^ ^ Y + // | | => | | | / + // | | | | | / + // B1 B2 B1 B2 .-------> X <-- Axis1 + // + // Start with a Delta of 90deg + // ^ ^ ^ ^ + // | | => | | + // | | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X, + test_dJointSetUniversalAxis1Offset_No_Offset_Axis1_Inverse_of_X) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + + dReal ang1 = REAL(0.0); + dReal ang2 = REAL(0.0); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], + ang1, ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + + // Rotate 1st body 90deg around axis1 then back to original position + // x is a Symbol for lines pointing into the screen + // . is a Symbol for lines pointing out of the screen + // + // In 2D In 3D + // ^ ^ ^ Z ^ ^ Y + // | | => x | | / + // | | | | / + // B1 B2 B1 B2 .-------> X <-- Axis1 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // x | => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X, + test_dJointSetUniversalAxis1Offset_B1_90Deg_Axis1_Inverse_of_X) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + + dReal ang1 = d2r(90); + dReal ang2 = REAL(0.0); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], + ang1, ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (ang2, angle2, 1e-4); + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + + // No offset when setting the Axis 2 offset + // x is a Symbol for lines pointing into the screen + // . is a Symbol for lines pointing out of the screen + // + // In 2D In 3D + // ^ ^ ^ ^ Z ^ ^ Y ^ Axis2 + // | | => | | | / / + // | | | | | / / + // B1 B2 B1 B2 . -------> <-- Axis1 + // + // Start with a Delta of 90deg + // ^ ^ ^ ^ + // | | => | | + // | | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X, + test_dJointSetUniversalAxis2Offset_No_Offset_Axis2_Inverse_of_X) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang1 = d2r(REAL(0.0)); + dReal ang2 = d2r(REAL(0.0)); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + // Rotate 2nd body 90deg around axis2 then back to original position + // + // In 2D In 3D + // ^ ^ ^ Z ^ ^ Y ^ Axis2 + // | | => | --> | / / + // | | | | / / + // B1 B2 B1 B2 . -------> <-- Axis1 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | <--- => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X, + test_dJointSetUniversalAxisOffset_B2_90Deg) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang1 = d2r(REAL(0.0)); + dReal ang2 = d2r(REAL(90.0)); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + // Rotate 2nd body -90deg around axis2 then back to original position + // + // ^ ^ ^ + // | | => | ---> + // | | | + // B1 B2 B1 B2 + // + // Start with a Delta of 90deg + // ^ ^ ^ + // | ---> => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X, + test_dJointSetUniversalAxis1Offset_B2_Minus90Deg) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang1 = d2r(0.0); + dReal ang2 = d2r(REAL(-90.0)); + + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dJointGetUniversalAxis1 (jId, axis); + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId2, R); + + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + // Rotate 1st body 0.23rad around X then back to original position + // + // ^ ^ ^ ^ + // | | => \ | + // | | \ | + // B1 B2 B1 B2 + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // \ | => | | + // \ | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X, + test_dJointSetUniversalAxis1Offset_B1_0_23rad) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + + dReal ang1 = REAL(0.23); + dReal ang2 = REAL(0.0); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], ang1, ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + // Rotate 2nd body -0.23rad around Z then back to original position + // + // ^ ^ ^ ^ + // | | => / | + // | | / | + // B1 B2 B1 B2 + // + // Start with a Delta of 0.23rad + // ^ ^ ^ ^ + // / | => | | + // / | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X, + test_dJointSetUniversalAxisOffset_B1_Minus0_23rad) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + + dMatrix3 R; + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], REAL(0.23), 0); + CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + } + + + + + // Rotate the body by 90deg around X then back to original position. + // The body is attached at the second position of the joint: + // dJointAttache(jId, 0, bId); + // + // ^ + // | => <--- + // | + // B1 B1 + // + // Start with a Delta of 90deg + // ^ + // <--- => | + // | + // B1 B1 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes, + test_dJointSetUniversalAxisOffset_1Body_B1_90Deg) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + + dMatrix3 R; + dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (M_PI/2.0, dJointGetUniversalAngle1 (jId), 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], M_PI/2.0, 0); + CHECK_CLOSE (M_PI/2.0, dJointGetUniversalAngle1 (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + } + + // Rotate the body by -0.23rad around X then back to original position. + // The body is attached at the second position of the joint: + // dJointAttache(jId, 0, bId); + // + // ^ ^ + // | => / + // | / + // B1 B1 + // + // Start with a Delta of -0.23rad + // ^ ^ + // / => | + // / | + // B1 B1 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes, + test_dJointSetUniversalAxisOffset_1Body_B1_Minus0_23rad) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + + dMatrix3 R; + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (-REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], -REAL(0.23), 0); + CHECK_CLOSE (-REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + } + + + + // Only one body body1 at (0,0,0) + // The joint is an Universal Joint. + // Axis the inverse of the X axis + // Anchor at (0, 0, 0) + // + // ^Y + // | + // | + // | + // | + // | + // Z <-- X + struct Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X + { + Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, 0, 0); + + jId = dJointCreateUniversal (wId, 0); + joint = (dxJointUniversal*) jId; + + + dJointAttach (jId, bId1, NULL); + dJointSetUniversalAnchor (jId, 0, 0, 0); + + axis[0] = -1; + axis[1] = 0; + axis[2] = 0; + dJointSetUniversalAxis1(jId, axis[0], axis[1], axis[2]); + } + + ~Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + + + dJointID jId; + dxJointUniversal* joint; + + dVector3 axis; + }; + + // Rotate B1 by 90deg around X then back to original position + // + // ^ + // | => <--- + // | + // B1 B1 + // + // Start with a Delta of 90deg + // ^ + // <--- => | + // | + // B1 B1 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetUniversalAxisOffset_1Body_B1_90Deg) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1(jId, axis); + + dReal ang1 = d2r(REAL(90.0)); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], ang1, 0); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + } + + // Rotate B1 by -0.23rad around X then back to original position + // + // ^ ^ + // | => / + // | / + // B1 B1 + // + // Start with a Delta of -0.23rad + // ^ ^ + // / => | + // / | + // B1 B1 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X, + test_dJointSetUniversalAxisOffset_1Body_B1_Minus0_23rad) + { + CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4); + + dMatrix3 R; + dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23)); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4); + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], REAL(0.23), 0); + CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4); + + dRFromAxisAndAngle (R, 1, 0, 0, 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + } + + + + + + + + // Rotate B2 by 90deg around X then back to original position + // + // ^ + // | => <--- + // | + // B2 B2 + // + // Start with a Delta of 90deg + // ^ + // <--- => | + // | + // B2 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B2_At_Zero_Default_Axes, + test_dJointSetUniversalAxisOffset_1Body_B2_90Deg) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang2 = d2r(REAL(90.0)); + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2); + dBodySetRotation (bId2, R); + + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], 0, -ang2); + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + } + + // Rotate B2 by -0.23rad around Y then back to original position + // + // ^ ^ + // | => / + // | / + // B2 B2 + // + // Start with an offset of -0.23rad + // ^ ^ + // / => | + // / | + // B2 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B2_At_Zero_Default_Axes, + test_dJointSetUniversalAxis2Offset_1Body_B2_Minus0_23rad) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + dVector3 axis; + dJointGetUniversalAxis2 (jId, axis); + + dReal ang1 = 0; + dReal ang2 = REAL(-0.23); + + + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2); + dBodySetRotation (bId2, R); + + + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (ang1, angle1, 1e-4); + CHECK_CLOSE (-ang2, angle2, 1e-4); + + + dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], + ang1, -ang2); + CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4); + + dRSetIdentity(R); // Set the rotation of the body to be zero + dBodySetRotation (bId2, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + + + + + + + + + + + + + // The 2 bodies are positionned at (0,0,0), and (0,0,0) + // The bodis have no rotation. + // The joint is a Universal Joint + // The axis of the joint are at random (Still at 90deg w.r.t each other) + // Anchor at (0, 0, 0) + struct Fixture_dxJointUniversal_B1_and_B2_Axis_Random + { + Fixture_dxJointUniversal_B1_and_B2_Axis_Random() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, -1, -2, -3); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 11, 22, 33); + + + jId = dJointCreateUniversal (wId, 0); + + + dJointAttach (jId, bId1, bId2); + + dVector3 axis1; + axis1[0] = REAL(0.53); + axis1[1] = -REAL(0.71); + axis1[2] = REAL(0.43); + dNormalize3(axis1); + + dVector3 axis; + axis[0] = REAL(1.2); + axis[1] = REAL(0.87); + axis[2] = -REAL(0.33); + + dVector3 axis2; + dCalcVectorCross3(axis2, axis1, axis); + + dJointSetUniversalAxis1(jId, axis1[0], axis1[1], axis1[2]); + dJointSetUniversalAxis2(jId, axis2[0], axis2[1], axis2[2]); + } + + ~Fixture_dxJointUniversal_B1_and_B2_Axis_Random() + { + dWorldDestroy (wId); + } + + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + }; + + + // Rotate first body 90deg around Axis1 then back to original position + // + // ^ ^ ^ Z ^ + // | | => <--- | | + // | | | | + // B1 B2 B1 B2 X .----->Y + // N.B. X is going out of the screen + // Set Axis1 with an Offset of 90deg + // ^ ^ ^ + // <--- | => | | + // | | | + // B1 B2 B1 B2 + TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_Axis_Random, + test_dJointSetUniversalAxisOffset_B1_90deg_Axis_Random) + { + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + dReal angle1, angle2; + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + + dVector3 axis; + dJointGetUniversalAxis1 (jId, axis); + + dReal angle = d2r(90); + dMatrix3 R; + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], angle); + dBodySetRotation (bId1, R); + + + CHECK_CLOSE (angle, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + + + dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], angle, 0); + CHECK_CLOSE (angle, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (angle, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + + + dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], 0); + dBodySetRotation (bId1, R); + + CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4); + CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4); + + dJointGetUniversalAngles(jId, &angle1, &angle2); + CHECK_CLOSE (0, angle1, 1e-4); + CHECK_CLOSE (0, angle2, 1e-4); + } + +} // End of SUITE TestdxJointUniversal + -- cgit v1.2.1