add ode

13 files changed, 9455 insertions, 0 deletions

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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <UnitTest++.h>
+#include <ode/ode.h>
+
+#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 <iostream>
+
+#include <UnitTest++.h>
+#include <ode/ode.h>
+
+#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
+