Changeset 8284

code/branches/kicklib2

Property svn:mergeinfo changed

/code/branches/kicklib	merged: 7979-7984,8057-8058,8061-8064,8066-8068,8071,8073-8074,8080-8082,8089,8094-8096
/code/branches/mac_osx (added)	merged: 7789,7933-7934,8042-8049,8054-8056,8059-8060,8065,8069-8070,8072
/code/branches/ois_update (added)	merged: 7506-7528,7530,7532,7553,7557-7560,7562-7564,7567,7570-7572,7575-7578,7582,7591-7592,7615,7618-7624,7660,7664,7669,7681-7683,7691,7701,7719,7767,7771,7782

code/branches/kicklib2/CMakeLists.txt

-                      r7383
+                      r8284
 OPTION(ORXONOX_RELEASE "Enable when building restributable releases" FALSE)
+IF(APPLE)
+  # Set 10.5 as the base SDK by default
+  SET(XCODE_ATTRIBUTE_SDKROOT macosx10.5)
+  # 10.6 sets x86_64 as the default architecture.
+  # Because Carbon isn't supported on 64-bit and we still need it, force the architectures to ppc and i386
+  IF(CMAKE_OSX_ARCHITECTURES MATCHES "x86_64")
+    SET(CMAKE_OSX_ARCHITECTURES "i386")
+  ENDIF()
+  IF(CMAKE_OSX_ARCHITECTURES MATCHES "ppc64")
+    SET(CMAKE_OSX_ARCHITECTURES "ppc")
+  ENDIF()
+  IF(NOT CMAKE_OSX_ARCHITECTURES)
+    SET(CMAKE_OSX_ARCHITECTURES "i386")
+  ENDIF()
+ENDIF()
 ########### Subfolders and Subscripts ###########
+# General build and compiler options and configurations
+INCLUDE(CompilerConfig)
 # Library finding
 INCLUDE(LibraryConfig)
-# General build and compiler options and configurations
-INCLUDE(CompilerConfig)
 # Configure installation paths and options

code/branches/kicklib2/cmake/CompilerConfig.cmake

-                      r5781
+                      r8284
   MESSAGE(STATUS "Warning: Your compiler is not officially supported.")
 ENDIF()
-SET(COMPILER_CONFIG_USER_SCRIPT "" CACHE FILEPATH
-    "Specify a CMake script if you wish to write your own compiler config.
-     See CompilerConfigGCC.cmake or CompilerConfigMSVC.cmake for examples.")
-IF(COMPILER_CONFIG_USER_SCRIPT)
-  IF(EXISTS ${CMAKE_MODULE_PATH}/${COMPILER_CONFIG_USER_SCRIPT})
-    INCLUDE(${CMAKE_MODULE_PATH}/${COMPILER_CONFIG_USER_SCRIPT})
-  ENDIF()
-ENDIF(COMPILER_CONFIG_USER_SCRIPT)

code/branches/kicklib2/cmake/CompilerConfigGCC.cmake

-                      r7458
+                      r8284
 INCLUDE(FlagUtilities)
 INCLUDE(CompareVersionStrings)
+INCLUDE(CheckCXXCompilerFlag)
 # Shortcut for CMAKE_COMPILER_IS_GNUCXX and ..._GNUC
 …
   OUTPUT_VARIABLE GCC_VERSION
+)
-# Complain about incompatibilities
-COMPARE_VERSION_STRINGS("${GCC_VERSION}" "4.4.0" _compare_result)
-IF(NOT _compare_result LESS 0)
-  IF(${Boost_VERSION} LESS 103700)
-    MESSAGE(STATUS "Warning: Boost versions earlier than 1.37 may not compile with GCC 4.4 or later!")
-  ENDIF()
-ENDIF()
 # GCC may not support #pragma GCC system_header correctly when using
 …
 # CMake doesn't seem to set the PIC flags right on certain 64 bit systems
 IF(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "x86_64")
+IF(NOT MINGW AND ${CMAKE_SYSTEM_PROCESSOR} STREQUAL "x86_64")
   ADD_COMPILER_FLAGS("-fPIC" CACHE)
+ENDIF()
+# Use SSE if possible
+# Commented because this might not work for cross compiling
+#CHECK_CXX_COMPILER_FLAG(-msse _gcc_have_sse)
+#IF(_gcc_have_sse)
+#  ADD_COMPILER_FLAGS("-msse" CACHE)
+#ENDIF()
+IF(NOT MINGW)
+  # Have GCC visibility?
+  CHECK_CXX_COMPILER_FLAG("-fvisibility=hidden" _gcc_have_visibility)
+  IF(_gcc_have_visibility)
+    # Note: There is a possible bug with the flag in gcc < 4.2 and Debug versions
+    COMPARE_VERSION_STRINGS("${GCC_VERSION}" "4.2.0" _compare_result)
+    IF(NOT CMAKE_BUILD_TYPE STREQUAL "Debug" OR _compare_result GREATER -1)
+      ADD_COMPILER_FLAGS("-DORXONOX_GCC_VISIBILITY -fvisibility=default -fvisibility-inlines-hidden" CACHE)
+    ENDIF()
+  ENDIF(_gcc_have_visibility)
 ENDIF()

code/branches/kicklib2/cmake/CompilerConfigMSVC.cmake

-                      r7818
+                      r8284
 # Overwrite CMake default flags here for the individual configurations
+SET_COMPILER_FLAGS("-MDd -Od -Zi -D_DEBUG -RTC1" Debug          CACHE)
+SET_COMPILER_FLAGS("-MD  -O2     -DNDEBUG"       Release        CACHE)
+SET_COMPILER_FLAGS("-MD  -O2 -Zi -DNDEBUG"       RelWithDebInfo CACHE)
+SET_COMPILER_FLAGS("-MD  -O1     -DNDEBUG"       MinSizeRel     CACHE)
+ADD_COMPILER_FLAGS("-D_SECURE_SCL=0"       MSVC9 ReleaseAll     CACHE)
+SET_COMPILER_FLAGS("-MDd -Od -Oi -Zi -D_DEBUG -RTC1" Debug          CACHE)
+SET_COMPILER_FLAGS("-MD  -O2         -DNDEBUG"       Release        CACHE)
+SET_COMPILER_FLAGS("-MD  -O2     -Zi -DNDEBUG"       RelWithDebInfo CACHE)
+SET_COMPILER_FLAGS("-MD  -O1         -DNDEBUG"       MinSizeRel     CACHE)
+# Enable non standard floating point optimisations
+# Note: It hasn't been checked yet whether we have code that might break
+#ADD_COMPILER_FLAGS("-fp:fast" CACHE)
+# No iterator checking for release builds (MSVC 8 dosn't understand this though)
+ADD_COMPILER_FLAGS("-D_SECURE_SCL=0" ReleaseAll CACHE)
+# Newer MSVC versions come with std::shared_ptr which conflicts with
+# boost::shared_ptr in cpptcl. And since we don't yet use the new C++ standard
+# anyway, disable it completely.
+ADD_COMPILER_FLAGS("-D_HAS_CPP0X=0" CACHE)
 # Use Link time code generation for Release config if ORXONOX_RELEASE is defined

code/branches/kicklib2/cmake/LibraryConfig.cmake

-                      r8283
+                      r8284
 # On Windows using a package causes way less problems
 SET(_option_msg "Set this to true to use precompiled dependecy archives")
 IF(WIN32)
+IF(WIN32 OR APPLE)
   OPTION(DEPENDENCY_PACKAGE_ENABLE "${_option_msg}" ON)
 ELSE(WIN32)
+ELSE()
   OPTION(DEPENDENCY_PACKAGE_ENABLE "${_option_msg}" FALSE)
 ENDIF(WIN32)
+ENDIF()
 # Scripts for specific library and CMake config
 INCLUDE(LibraryConfigTardis)
-INCLUDE(LibraryConfigApple)
 IF(DEPENDENCY_PACKAGE_ENABLE)
 …
   ELSEIF(MSVC90)
     SET(_compiler_prefix msvc9)
+  ELSEIF(MSVC100)
+    SET(_compiler_prefix msvc10)
   ENDIF()
   FIND_PATH(DEPENDENCY_PACKAGE_DIR
 …
                    "Disable LIBRARY_USE_PACKAGE if you have none intalled.")
   ELSE()
+    INCLUDE(PackageConfigMinGW)
+    INCLUDE(PackageConfigMSVC)
+    INCLUDE(PackageConfig) # For both msvc and mingw
+    IF(WIN32)
+      INCLUDE(PackageConfigMinGW)
+      INCLUDE(PackageConfigMSVC)
+      INCLUDE(PackageConfig) # For both msvc and mingw
+    ELSEIF(APPLE)
+      INCLUDE(PackageConfigOSX)
+    ENDIF(WIN32)
   ENDIF()
 ENDIF(DEPENDENCY_PACKAGE_ENABLE)
 …
 ##### Boost #####
 # Expand the next statement if newer boost versions than 1.36.1 are released
+# Expand the next statement if newer boost versions are released
 SET(Boost_ADDITIONAL_VERSIONS 1.37 1.37.0 1.38 1.38.0 1.39 1.39.0 1.40 1.40.0
+.41 1.41.0 1.42 1.42.0 1.43 1.43.0 1.44 1.44.0)
+.41 1.41.0 1.42 1.42.0 1.43 1.43.0 1.44 1.44.0
+.45 1.45.0 1.46 1.46.0 1.46.1)
 IF( NOT TARDIS )
   FIND_PACKAGE(Boost 1.35 REQUIRED thread filesystem system date_time)
 …
 # No auto linking, so this option is useless anyway
 MARK_AS_ADVANCED(Boost_LIB_DIAGNOSTIC_DEFINITIONS)
+# Complain about incompatibilities
+IF(GCC_VERSION)
+  COMPARE_VERSION_STRINGS("${GCC_VERSION}" "4.4.0" _compare_result)
+  IF(NOT _compare_result LESS 0)
+    IF(${Boost_VERSION} LESS 103700)
+      MESSAGE(STATUS "Warning: Boost versions earlier than 1.37 may not compile with GCC 4.4 or later!")
+    ENDIF()
+  ENDIF()
+ENDIF()

code/branches/kicklib2/cmake/PackageConfig.cmake

-                      r8283
+                      r8284
+ #
-# Check package version info
-# MAJOR: Breaking change
-# MINOR: No breaking changes by the dependency package
-#        For example any code running on 3.0 should still run on 3.1
-#        But you can specify that the code only runs on 3.1 and higher
-#        or 4.0 and higher (so both 3.1 and 4.0 will work).
-IF(MSVC)
-  SET(ALLOWED_MINIMUM_VERSIONS 4.3 5.1 6.0)
-ELSE()
-  SET(ALLOWED_MINIMUM_VERSIONS 4.1 5.2)
-ENDIF()
-IF(NOT EXISTS ${DEPENDENCY_PACKAGE_DIR}/version.txt)
-  SET(DEPENDENCY_VERSION 1.0)
-ELSE()
-  # Get version from file
-  FILE(READ ${DEPENDENCY_PACKAGE_DIR}/version.txt _file_content)
-  SET(_match)
-  STRING(REGEX MATCH "([0-9]+.[0-9]+)" _match ${_file_content})
-  IF(_match)
-    SET(DEPENDENCY_VERSION ${_match})
-  ELSE()
-    MESSAGE(FATAL_ERROR "The version.txt file in the dependency file has corrupt version information.")
-  ENDIF()
-ENDIF()
-INCLUDE(CompareVersionStrings)
-SET(_version_match FALSE)
-FOREACH(_version ${ALLOWED_MINIMUM_VERSIONS})
-  # Get major version
-  STRING(REGEX REPLACE "^([0-9]+)\\..*$" "\\1" _major_version "${_version}")
-  COMPARE_VERSION_STRINGS(${DEPENDENCY_VERSION} ${_major_version} _result TRUE)
-  IF(_result EQUAL 0)
-    COMPARE_VERSION_STRINGS(${DEPENDENCY_VERSION} ${_version} _result FALSE)
-    IF(NOT _result LESS 0)
-      SET(_version_match TRUE)
-    ENDIF()
-  ENDIF()
-ENDFOREACH(_version)
-IF(NOT _version_match)
-  MESSAGE(FATAL_ERROR "Your dependency package version is ${DEPENDENCY_VERSION}\n"
-          "Possible required versions: ${ALLOWED_MINIMUM_VERSIONS}\n"
-          "You can get a new version from www.orxonox.net")
-ENDIF()
 IF(NOT _INTERNAL_PACKAGE_MESSAGE)
   MESSAGE(STATUS "Using library package for the dependencies.")
 …
 # Ogre versions >= 1.7 require the POCO library on Windows with MSVC for threading
 COMPARE_VERSION_STRINGS(${DEPENDENCY_VERSION} 5 _result TRUE)
 IF(NOT _result EQUAL -1 AND NOT MINGW)
     SET(POCO_REQUIRED TRUE)
+IF(NOT _result EQUAL -1 AND NOT APPLE)
+  SET(POCO_REQUIRED TRUE)
 ENDIF()

code/branches/kicklib2/cmake/PackageConfigMSVC.cmake

-                      r7818
+                      r8284
 IF(MSVC)
+  INCLUDE(CheckPackageVersion)
+  CHECK_PACKAGE_VERSION(4.3 6.0)
   # 64 bit system?
   IF(CMAKE_SIZEOF_VOID_P EQUAL 8)
 …
   # Choose right MSVC version
   STRING(REGEX REPLACE "^Visual Studio ([0-9][0-9]?) .*$" "\\1"
+  STRING(REGEX REPLACE "^Visual Studio ([0-9][0-9]?).*$" "\\1"
          _msvc_version "${CMAKE_GENERATOR}")
 …
   SET(TCL_LIBRARY  ${DEP_LIBRARY_DIR}/tcl85.lib CACHE FILEPATH "")
   SET(ZLIB_LIBRARY ${DEP_LIBRARY_DIR}/zdll.lib  CACHE FILEPATH "")
+  # Part of Platform SDK and usually gets linked automatically
+  SET(WMI_LIBRARY  wbemuuid.lib)
 ENDIF(MSVC)

code/branches/kicklib2/cmake/PackageConfigMinGW.cmake

-                      r5781
+                      r8284
 IF(MINGW)
+  INCLUDE(CheckPackageVersion)
+  CHECK_PACKAGE_VERSION(6.0)
   # 64 bit system?
   IF(CMAKE_SIZEOF_VOID_P EQUAL 8)
 …
   # to specify the libraries ourselves.
   SET(TCL_LIBRARY  ${DEP_BINARY_DIR}/tcl85.dll CACHE FILEPATH "")
+  SET(ZLIB_LIBRARY ${DEP_BINARY_DIR}/zlib1.dll CACHE FILEPATH "")
+  SET(ZLIB_LIBRARY ${DEP_BINARY_DIR}/libzlib.dll CACHE FILEPATH "")
+  # Not included in MinGW, so we need to supply it for OIS
+  SET(WMI_INCLUDE_DIR ${DEP_INCLUDE_DIR}/wmi/include)
+  SET(WMI_LIBRARY     ${DEP_LIBRARY_DIR}/wbemuuid.lib)
 ENDIF(MINGW)

code/branches/kicklib2/cmake/tools/CheckOGREPlugins.cmake

-                      r8264
+                      r8284
       NAMES ${_plugin}
       PATHS $ENV{OGRE_HOME} $ENV{OGRE_PLUGIN_DIR}
       PATH_SUFFIXES bin/Release bin/release Release release lib lib/OGRE bin
+      PATH_SUFFIXES bin/Release bin/release Release release lib lib/OGRE bin Ogre.framework/Resources
+    )
     FIND_LIBRARY(OGRE_PLUGIN_${_plugin}_DEBUG
       NAMES ${_plugin}d ${_plugin}_d ${_plugin}
       PATHS $ENV{OGRE_HOME} $ENV{OGRE_PLUGIN_DIR}
       PATH_SUFFIXES bin/Debug bin/debug Debug debug lib lib/OGRE bin
+      PATH_SUFFIXES bin/Debug bin/debug Debug debug lib lib/OGRE bin Ogre.framework/Resources
+    )
     # We only need at least one render system. Check at the end.

code/branches/kicklib2/cmake/tools/FindALUT.cmake

-                      r7163
+                      r8284
+# - Locate FreeAlut
+# This module defines
+#  ALUT_LIBRARY
+#  ALUT_FOUND, if false, do not try to link against Alut
+#  ALUT_INCLUDE_DIR, where to find the headers
+#
+# $ALUTDIR is an environment variable that would
+# correspond to the ./configure --prefix=$ALUTDIR
+# used in building Alut.
+#
+# Created by Eric Wing. This was influenced by the FindSDL.cmake module.
+# On OSX, this will prefer the Framework version (if found) over others.
+# People will have to manually change the cache values of
+# ALUT_LIBRARY to override this selection.
+# Tiger will include OpenAL as part of the System.
+# But for now, we have to look around.
+# Other (Unix) systems should be able to utilize the non-framework paths.
+#
+# Several changes and additions by Fabian 'x3n' Landau
+# Some simplifications by Adrian Friedli and Reto Grieder
+#                 > www.orxonox.net <
+ #
+ #             ORXONOX - the hottest 3D action shooter ever to exist
+ #                             > www.orxonox.net <
+ #
+ #        This program is free software; you can redistribute it and/or
+ #         modify it under the terms of the GNU General Public License
+ #        as published by the Free Software Foundation; either version 2
+ #            of the License, or (at your option) any later version.
+ #
+ #       This program is distributed in the hope that it will be useful,
+ #        but WITHOUT ANY WARRANTY; without even the implied warranty of
+ #        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ #                 GNU General Public License for more details.
+ #
+ #   You should have received a copy of the GNU General Public License along
+ #      with this program; if not, write to the Free Software Foundation,
+ #     Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ #
+ #
+ #  Author:
+ #    Kevin Young
+ #  Description:
+ #    Variables defined:
+ #      ALUT_FOUND
+ #      ALUT_INCLUDE_DIR
+ #      ALUT_LIBRARY
+ #
 INCLUDE(FindPackageHandleStandardArgs)
 INCLUDE(HandleLibraryTypes)
+FIND_PATH(ALUT_INCLUDE_DIR AL/alut.h
+  PATHS
+  $ENV{ALUTDIR}
+  ~/Library/Frameworks/OpenAL.framework
+  /Library/Frameworks/OpenAL.framework
+  /System/Library/Frameworks/OpenAL.framework # Tiger
+  PATH_SUFFIXES include include/OpenAL include/AL Headers
+FIND_PATH(ALUT_INCLUDE_DIR alut.h
+  PATHS $ENV{ALUTDIR}
+  PATH_SUFFIXES include include/AL Headers Headers/AL
+)
+# I'm not sure if I should do a special casing for Apple. It is
+# unlikely that other Unix systems will find the framework path.
+# But if they do ([Next|Open|GNU]Step?),
+# do they want the -framework option also?
+IF(${ALUT_INCLUDE_DIR} MATCHES ".framework")
+  STRING(REGEX REPLACE "(.*)/.*\\.framework/.*" "\\1" ALUT_FRAMEWORK_PATH_TMP ${ALUT_INCLUDE_DIR})
+  IF("${ALUT_FRAMEWORK_PATH_TMP}" STREQUAL "/Library/Frameworks"
+      OR "${ALUT_FRAMEWORK_PATH_TMP}" STREQUAL "/System/Library/Frameworks"
+      )
+    # String is in default search path, don't need to use -F
+    SET (ALUT_LIBRARY_OPTIMIZED "-framework OpenAL" CACHE STRING "OpenAL framework for OSX")
+  ELSE()
+    # String is not /Library/Frameworks, need to use -F
+    SET(ALUT_LIBRARY_OPTIMIZED "-F${ALUT_FRAMEWORK_PATH_TMP} -framework OpenAL" CACHE STRING "OpenAL framework for OSX")
+  ENDIF()
+  # Clear the temp variable so nobody can see it
+  SET(ALUT_FRAMEWORK_PATH_TMP "" CACHE INTERNAL "")
+ELSE()
+  FIND_LIBRARY(ALUT_LIBRARY_OPTIMIZED
+    NAMES alut
+    PATHS $ENV{ALUTDIR}
+    PATH_SUFFIXES lib libs
+  )
+  FIND_LIBRARY(ALUT_LIBRARY_DEBUG
+    NAMES alutd alut_d alutD alut_D
+    PATHS $ENV{ALUTDIR}
+    PATH_SUFFIXES lib libs
+  )
+ENDIF()
+FIND_LIBRARY(ALUT_LIBRARY_OPTIMIZED
+  NAMES alut ALUT
+  PATHS $ENV{ALUTDIR}
+  PATH_SUFFIXES lib bin/Release bin/release Release release ALUT
+)
+FIND_LIBRARY(ALUT_LIBRARY_DEBUG
+  NAMES alutd alut_d alutD alut_D ALUTd ALUT_d ALUTD ALUT_D
+  PATHS $ENV{ALUTDIR}
+  PATH_SUFFIXES lib bin/Debug bin/debug Debug debug ALUT
+)
 # Handle the REQUIRED argument and set ALUT_FOUND
 FIND_PACKAGE_HANDLE_STANDARD_ARGS(ALUT DEFAULT_MSG
     ALUT_LIBRARY_OPTIMIZED
     ALUT_INCLUDE_DIR
+  ALUT_LIBRARY_OPTIMIZED
+  ALUT_INCLUDE_DIR
+)
 …
 MARK_AS_ADVANCED(
     ALUT_INCLUDE_DIR
     ALUT_LIBRARY_OPTIMIZED
     ALUT_LIBRARY_DEBUG
+  ALUT_INCLUDE_DIR
+  ALUT_LIBRARY_OPTIMIZED
+  ALUT_LIBRARY_DEBUG
+)

code/branches/kicklib2/cmake/tools/FindCEGUI.cmake

-                      r8283
+                      r8284
 INCLUDE(HandleLibraryTypes)
 # Find headers
+# Find CEGUI headers
 FIND_PATH(CEGUI_INCLUDE_DIR CEGUI.h
   PATHS $ENV{CEGUIDIR}
 …
   NAMES CEGUIBase CEGUI
   PATHS $ENV{CEGUIDIR}
   PATH_SUFFIXES lib bin
+  PATH_SUFFIXES lib bin CEGUIBase.framework CEGUI.framework
+)
 FIND_LIBRARY(CEGUI_LIBRARY_DEBUG
 …
+)
+# Find CEGUILua headers
+FIND_PATH(CEGUILUA_INCLUDE_DIR CEGUILua.h
+  PATHS $ENV{CEGUIDIR} ${CEGUI_INCLUDE_DIR}/ScriptingModules/LuaScriptModule
+  PATH_SUFFIXES include include/CEGUI CEGUILuaScriptModule.framework/Headers
+)
 # Find CEGUILua libraries
 FIND_LIBRARY(CEGUILUA_LIBRARY_OPTIMIZED
   NAMES CEGUILua CEGUILuaScriptModule
   PATHS $ENV{CEGUIDIR}
   PATH_SUFFIXES lib bin
+  PATH_SUFFIXES lib bin CEGUILuaScriptModule.framework
+)
 FIND_LIBRARY(CEGUILUA_LIBRARY_DEBUG
 …
+)
+# Find CEGUI Tolua++ include file
+# We only need to add this path since we use tolua++ like a normal
+# dependency but it is shipped with CEGUILua.
+FIND_PATH(CEGUI_TOLUA_INCLUDE_DIR tolua++.h
+  PATHS
+    ${CEGUILUA_INCLUDE_DIR}
+    # For newer CEGUI versions >= 0.7
+    ${CEGUILUA_INCLUDE_DIR}/support/tolua++
+    # For Apples
+    $ENV{CEGUIDIR}
+  PATH_SUFFIXES ceguitolua++.framework/Headers
+  NO_DEFAULT_PATH # MUST be in CEGUILUA_INCLUDE_DIR somewhere
+)
 # Find CEGUI Tolua++ libraries
 FIND_LIBRARY(CEGUI_TOLUA_LIBRARY_OPTIMIZED
   NAMES CEGUItoluapp tolua++
+  NAMES CEGUItoluapp tolua++ ceguitolua++
   PATHS $ENV{CEGUIDIR}
   PATH_SUFFIXES lib bin
+  PATH_SUFFIXES lib bin ceguitolua++.framework
+)
 FIND_LIBRARY(CEGUI_TOLUA_LIBRARY_DEBUG
 …
 COMPARE_VERSION_STRINGS("${CEGUI_VERSION}" "0.7" _version_compare TRUE)
 IF(_version_compare GREATER -1)
+  # Find CEGUI OGRE Renderer headers
+  FIND_PATH(CEGUI_OGRE_RENDERER_INCLUDE_DIR CEGUIOgreRenderer.h
+    PATHS $ENV{CEGUIDIR} ${CEGUI_INCLUDE_DIR}/RendererModules/Ogre
+    PATH_SUFFIXES include include/CEGUI CEGUI.framework/Headers
+  )
   # Find CEGUI OGRE Renderer libraries
   FIND_LIBRARY(CEGUI_OGRE_RENDERER_LIBRARY_OPTIMIZED
 …
     PATH_SUFFIXES lib bin
+  )
+  SET(CEGUI_OGRE_RENDERER_LIBRARY_NAME CEGUI_OGRE_RENDERER_LIBRARY_OPTIMIZED)
+  SET(CEGUI_OGRE_RENDERER_REQUIRED_VARIABLES
+    CEGUI_OGRE_RENDERER_INCLUDE_DIR
+    CEGUI_OGRE_RENDERER_LIBRARY_OPTIMIZED
+  )
 ELSE()
+  SET(CEGUI_OLD_VERSION TRUE)
   SET(CEGUI_OGRE_RENDERER_BUILD_REQUIRED TRUE)
 ENDIF()
 …
   CEGUI_INCLUDE_DIR
   CEGUI_LIBRARY_OPTIMIZED
+  CEGUILUA_INCLUDE_DIR
   CEGUILUA_LIBRARY_OPTIMIZED
+  CEGUI_TOLUA_INCLUDE_DIR
   CEGUI_TOLUA_LIBRARY_OPTIMIZED
   ${CEGUI_OGRE_RENDERER_LIBRARY_NAME}
+  ${CEGUI_OGRE_RENDERER_REQUIRED_VARIABLES}
+)
 …
   CEGUI_LIBRARY_OPTIMIZED
   CEGUI_LIBRARY_DEBUG
+  CEGUILUA_INCLUDE_DIR
   CEGUILUA_LIBRARY_OPTIMIZED
   CEGUILUA_LIBRARY_DEBUG
+  CEGUI_TOLUA_INCLUDE_DIR
   CEGUI_TOLUA_LIBRARY_OPTIMIZED
   CEGUI_TOLUA_LIBRARY_DEBUG
+  CEGUI_OGRE_RENDERER_INCLUDE_DIR
   CEGUI_OGRE_RENDERER_LIBRARY_OPTIMIZED
   CEGUI_OGRE_RENDERER_LIBRARY_DEBUG

code/branches/kicklib2/cmake/tools/FindOgg.cmake

r7163	r8284
22	22	)
23	23	FIND_LIBRARY(OGG_LIBRARY_OPTIMIZED
24		NAMES ogg
	24	NAMES ogg ogg-0
25	25	PATHS $ENV{OGGDIR}
26	26	PATH_SUFFIXES lib

code/branches/kicklib2/cmake/tools/FindPOCO.cmake

r7285	r8284
29	29	FIND_PATH(POCO_INCLUDE_DIR Poco/Poco.h
30	30	PATHS $ENV{POCODIR}
31		PATH_SUFFIXES include
	31	PATH_SUFFIXES include Foundation/include
32	32	)
33	33	FIND_LIBRARY(POCO_LIBRARY_OPTIMIZED

code/branches/kicklib2/cmake/tools/FindVorbis.cmake

-                      r7163
+                      r8284
+)
 FIND_LIBRARY(VORBIS_LIBRARY_OPTIMIZED
   NAMES vorbis
+  NAMES vorbis vorbis-0
   PATHS $ENV{VORBISDIR}
   PATH_SUFFIXES lib
 …
+)
 FIND_LIBRARY(VORBISFILE_LIBRARY_OPTIMIZED
   NAMES vorbisfile
+  NAMES vorbisfile vorbisfile-3
   PATHS $ENV{VORBISDIR}
   PATH_SUFFIXES lib

code/branches/kicklib2/cmake/tools/GenerateToluaBindings.cmake

-                      r7415
+                      r8284
  #    RUNTIME_LIBRARY_DIRECTORY - Working directory
+ #
+# Workaround for XCode: The folder where the bind files are written to has
+# to be present beforehand.
+# We have to do this here because the header files are all stored in a single
+# location per configuration.
+IF(CMAKE_CONFIGURATION_TYPES)
+  FOREACH(_dir ${CMAKE_CONFIGURATION_TYPES})
+    FILE(MAKE_DIRECTORY "${CMAKE_BINARY_DIR}/src/toluabind/${_dir}")
+  ENDFOREACH(_dir)
+ENDIF()
 FUNCTION(GENERATE_TOLUA_BINDINGS _tolua_package _target_source_files)

code/branches/kicklib2/cmake/tools/TargetUtilities.cmake

-                      r8079
+                      r8284
  #    This function also installs the target!
  #  Prerequisistes:
  #    ORXONOX_DEFAULT_LINK, ORXONOX_CONFIG_FILES
+ #    ORXONOX_DEFAULT_LINK, ORXONOX_CONFIG_FILES, ORXONOX_CONFIG_FILES_GENERATED
  #  Parameters:
  #    _target_name, ARGN for the macro arguments
 …
     GENERATE_TOLUA_BINDINGS(${_target_name_capitalised} _${_target_name}_files
                             INPUTFILES ${_arg_TOLUA_FILES})
+    # Workaround for XCode: The folder where the bind files are written to has
+    # to be present beforehand.
+    IF(CMAKE_CONFIGURATION_TYPES)
+      FOREACH(_dir ${CMAKE_CONFIGURATION_TYPES})
+        FILE(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/${_dir})
+      ENDFOREACH(_dir)
+    ENDIF()
   ENDIF()
 …
     IF(NOT _arg_ORXONOX_EXTERNAL)
       # Move the prereqs.h file to the config section
+      # Move the ...Prereqs.h and the PCH files to the 'Config' section
       IF(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${_target_name_capitalised}Prereqs.h)
         SOURCE_GROUP("Config" FILES ${_target_name_capitalised}Prereqs.h)
       ENDIF()
+      # Add config files to the config section
+      LIST(APPEND _${_target_name}_files ${ORXONOX_CONFIG_FILES})
+      IF(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${_arg_PCH_FILE})
+        SOURCE_GROUP("Config" FILES ${CMAKE_CURRENT_SOURCE_DIR}/${_arg_PCH_FILE})
+      ENDIF()
+      # Also include all config files
+      LIST(APPEND _${_target_name}_files ${ORXONOX_CONFIG_FILES} ${ORXONOX_CONFIG_FILES_GENERATED})
+      # Add unprocessed config files to the 'Config' section
       SOURCE_GROUP("Config" FILES ${ORXONOX_CONFIG_FILES})
+      # Add generated config files to the 'Generated' section
+      SOURCE_GROUP("Generated" FILES ${ORXONOX_CONFIG_FILES_GENERATED})
     ENDIF()
   ENDIF()
 …
   IF(_arg_ORXONOX_EXTERNAL)
     REMOVE_COMPILER_FLAGS("-W3 -W4" MSVC)
+    ADD_COMPILER_FLAGS("-w")
+    ADD_COMPILER_FLAGS("-w" NOT MSVC)
+    ADD_COMPILER_FLAGS("-W0" MSVC)
   ENDIF()
   # Don't compile header files
   FOREACH(_file ${_${_target_name}_files})
     IF(NOT _file MATCHES "\\.(c|cc|cpp|cxx)$")
+    IF(NOT _file MATCHES "\\.(c|cc|cpp|cxx|mm)$")
       SET_SOURCE_FILES_PROPERTIES(${_file} PROPERTIES HEADER_FILE_ONLY TRUE)
     ENDIF()

code/branches/kicklib2/data/gui/scripts/AudioMenu.lua

-                      r8079
+                      r8284
     table.insert(themeList, "Default")
     table.insert(themeList, "Drum n' Bass")
+    table.insert(themeList, "8-Bit Style")
+    table.insert(themeList, "Corny Jazz")
     for k,v in pairs(themeList) do
         item = CEGUI.createListboxTextItem(v)
 …
     if orxonox.getConfig("MoodManager", "mood_") == "dnb" then
         listboxwindow:setItemSelectState(1,true)
+    elseif orxonox.getConfig("MoodManager", "mood_") == "eightbit" then
+        listboxwindow:setItemSelectState(2,true)
+    elseif orxonox.getConfig("MoodManager", "mood_") == "jazzy" then
+        listboxwindow:setItemSelectState(3,true)
     else
         listboxwindow:setItemSelectState(0,true)
 …
     if listboxwindow:isItemSelected(1) then
         orxonox.config("MoodManager", "mood_", "dnb")
+    elseif listboxwindow:isItemSelected(2) then
+        orxonox.config("MoodManager", "mood_", "eightbit")
+    elseif listboxwindow:isItemSelected(3) then
+        orxonox.config("MoodManager", "mood_", "jazzy")
     else
         orxonox.config("MoodManager", "mood_", "default")

code/branches/kicklib2/src/CMakeLists.txt

-                      r8283
+                      r8284
 # Set the search paths for include files
 INCLUDE_DIRECTORIES(
+  # OrxonoxConfig.h
+  ${CMAKE_CURRENT_BINARY_DIR}
+  # All includes in "externals" should be prefixed with the path
+  # relative to "external" to avoid conflicts
+  ${CMAKE_CURRENT_SOURCE_DIR}/external
+  # Include directories needed even if only included by Orxonox
+  ${CMAKE_CURRENT_SOURCE_DIR}/external/bullet
+  ${CMAKE_CURRENT_SOURCE_DIR}/external/ois
   # External
   ${OGRE_INCLUDE_DIR}
   ${CEGUI_INCLUDE_DIR}
+  ${CEGUI_TOLUA_INCLUDE_DIR}
   #${ENET_INCLUDE_DIR}
   ${Boost_INCLUDE_DIRS}
 …
   ${DIRECTX_INCLUDE_DIR}
   ${ZLIB_INCLUDE_DIR}
+)
+  # All includes in "externals" should be prefixed with the path
+  # relative to "external" to avoid conflicts
+  ${CMAKE_CURRENT_SOURCE_DIR}/external
+  # Include directories needed even if only included by Orxonox
+  ${CMAKE_CURRENT_SOURCE_DIR}/external/bullet
+  ${CMAKE_CURRENT_SOURCE_DIR}/external/ois
+  # OrxonoxConfig.h
+  ${CMAKE_CURRENT_BINARY_DIR}
+)
+IF(CEGUI_OLD_VERSION)
+  INCLUDE_DIRECTORIES(${CEGUILUA_INCLUDE_DIR})
+ENDIF()
 IF (DBGHELP_FOUND)
 …
     SET(MSVC_PLATFORM "Win32")
   ENDIF()
+  STRING(REGEX REPLACE "^Visual Studio ([0-9][0-9]?) .*$" "\\1"
+         VISUAL_STUDIO_VERSION_SIMPLE "${CMAKE_GENERATOR}")
+  CONFIGURE_FILE("${CMAKE_CURRENT_SOURCE_DIR}/orxonox-main.vcproj.user.in" "${CMAKE_CURRENT_BINARY_DIR}/orxonox-main.vcproj.user")
+  IF(MSVC10)
+    CONFIGURE_FILE("${CMAKE_CURRENT_SOURCE_DIR}/orxonox-main.vcxproj.user.in" "${CMAKE_CURRENT_BINARY_DIR}/orxonox-main.vcxproj.user")
+  ELSE()
+    STRING(REGEX REPLACE "^Visual Studio ([0-9][0-9]?).*$" "\\1"
+           VISUAL_STUDIO_VERSION_SIMPLE "${CMAKE_GENERATOR}")
+    CONFIGURE_FILE("${CMAKE_CURRENT_SOURCE_DIR}/orxonox-main.vcproj.user.in" "${CMAKE_CURRENT_BINARY_DIR}/orxonox-main.vcproj.user")
+  ENDIF()
 ENDIF(MSVC)

code/branches/kicklib2/src/OrxonoxConfig.cmake

-                      r7818
+                      r8284
 # Global switch to disable Precompiled Header Files
+IF(PCH_COMPILER_SUPPORT)
+# Note: PCH temporarily disabled on Mac because of severe problems
+IF(PCH_COMPILER_SUPPORT AND NOT APPLE)
   OPTION(PCH_ENABLE "Global PCH switch" TRUE)
 ENDIF()
 …
 ENDIF()
-# 32/64 bit system check
-IF(CMAKE_SIZEOF_VOID_P EQUAL 8)
-  SET(ORXONOX_ARCH_64 TRUE)
-ELSE()
-  SET(ORXONOX_ARCH_32 TRUE)
-ENDIF()
 # Platforms
 SET(ORXONOX_PLATFORM_WINDOWS ${WIN32})
 …
 ENDIF(MSVC)
 # Check iso646.h include (literal operators)
+# Check some non standard system includes
 INCLUDE(CheckIncludeFileCXX)
 CHECK_INCLUDE_FILE_CXX(iso646.h HAVE_ISO646_H)
+CHECK_INCLUDE_FILE_CXX(stdint.h HAVE_STDINT_H)
+# Part of a woraround for OS X warnings. See OrxonoxConfig.h.in
+SET(ORX_HAVE_STDINT_H ${HAVE_STDINT_H})
 IF(MSVC)
 …
 SET(ORXONOX_CONFIG_FILES
-  ${CMAKE_CURRENT_BINARY_DIR}/OrxonoxConfig.h
   ${CMAKE_CURRENT_SOURCE_DIR}/OrxonoxConfig.h.in
-  ${CMAKE_CURRENT_BINARY_DIR}/SpecialConfig.h
   ${CMAKE_CURRENT_SOURCE_DIR}/SpecialConfig.h.in
+)
+SET(ORXONOX_CONFIG_FILES_GENERATED
+  ${CMAKE_CURRENT_BINARY_DIR}/OrxonoxConfig.h
+  ${CMAKE_CURRENT_BINARY_DIR}/SpecialConfig.h
+)

code/branches/kicklib2/src/OrxonoxConfig.h.in

-                      r7818
+                      r8284
 // Architecture
+#cmakedefine ORXONOX_ARCH_32
+#cmakedefine ORXONOX_ARCH_64
+#if defined(__x86_64__) || defined(_M_X64) || defined(__powerpc64__) || defined(__alpha__) || defined(__ia64__) || defined(__s390__) || defined(__s390x__)
+#   define ORXONOX_ARCH_64
+#else
+#   define ORXONOX_ARCH_32
+#endif
 // See if we can use __forceinline or if we need to use __inline instead
 #cmakedefine HAVE_FORCEINLINE
 #ifndef FORCEINLINE
+#ifndef ORX_FORCEINLINE
 #  ifdef HAVE_FORCEINLINE
 #    define FORCEINLINE __forceinline
+#    define ORX_FORCEINLINE __forceinline
 #  else
 #    define FORCEINLINE __inline
+#    define ORX_FORCEINLINE __inline
 #  endif
 #endif
 …
 #endif
+#cmakedefine HAVE_STDINT_H
+#ifdef HAVE_STDINT_H
+// On OS X some headers already define HAVE_STDINT_H and that spits out
+// some warnings. Therefore we use this macro.
+// Note: This requires some extra code in OrxonoxConfig.cmake
+#cmakedefine ORX_HAVE_STDINT_H
+#ifdef ORX_HAVE_STDINT_H
 #  include <stdint.h>
 #elif defined(ORXONOX_COMPILER_MSVC)

code/branches/kicklib2/src/SpecialConfig.h.in

-                      r8283
+                      r8284
     // OGRE PLUGINS
+    // Apple has trouble finding OGRE plugins because of its install-name convention
+    // Adopting the executable_path structure for later use in app bundles
 #ifdef NDEBUG
     const char ogrePlugins[] = "@OGRE_PLUGINS_RELEASE@";
 #  ifdef DEPENDENCY_PACKAGE_ENABLE
+    const char ogrePluginsDirectory[] = ".";
+#    ifdef ORXONOX_PLATFORM_APPLE
+       const char ogrePluginsDirectory[] = "@executable_path/../Plugins";
+#    else
+       const char ogrePluginsDirectory[] = ".";
+#    endif
 #  else
     const char ogrePluginsDirectory[] = "@OGRE_PLUGINS_FOLDER_RELEASE@";
 …
     const char ogrePlugins[] = "@OGRE_PLUGINS_DEBUG@";
 #  ifdef DEPENDENCY_PACKAGE_ENABLE
+    const char ogrePluginsDirectory[] = ".";
+#    ifdef ORXONOX_PLATFORM_APPLE
+       const char ogrePluginsDirectory[] = "@OGRE_PLUGINS_FOLDER_DEBUG@";
+#    else
+       const char ogrePluginsDirectory[] = ".";
+#    endif
 #  else
     const char ogrePluginsDirectory[] = "@OGRE_PLUGINS_FOLDER_DEBUG@";

code/branches/kicklib2/src/external/bullet/Bullet-C-Api.h

r5781	r8284
148	148	extern void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation);
149	149	extern void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient);
	150	extern void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
150	151
151	152	typedef struct plRayCastResult {

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h

-                      r5781
+                      r8284
         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
+        virtual void    aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
         void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
 …
+                        {
                                 rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
+                        }
+                }
+        }
+}
+template <typename BP_FP_INT_TYPE>
+void    btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
+{
+        if (m_raycastAccelerator)
+        {
+                m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback);
+        } else
+        {
+                //choose axis?
+                BP_FP_INT_TYPE axis = 0;
+                //for each proxy
+                for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
+                {
+                        if (m_pEdges[axis][i].IsMax())
+                        {
+                                Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
+                                if (TestAabbAgainstAabb2(aabbMin,aabbMax,handle->m_aabbMin,handle->m_aabbMax))
+                                {
+                                        callback.process(handle);
+                                }
+                        }
+                }

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

-                      r5781
+                      r8284
+struct  btBroadphaseRayCallback
+struct  btBroadphaseAabbCallback
+{
+        virtual ~btBroadphaseAabbCallback() {}
+        virtual bool    process(const btBroadphaseProxy* proxy) = 0;
+};
+struct  btBroadphaseRayCallback : public btBroadphaseAabbCallback
+{
         ///added some cached data to accelerate ray-AABB tests
 …
         virtual ~btBroadphaseRayCallback() {}
-        virtual bool    process(const btBroadphaseProxy* proxy) = 0;
 };
 …
         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0;
+        virtual void    aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0;
         ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
         virtual void    calculateOverlappingPairs(btDispatcher* dispatcher)=0;
 …
         ///reset broadphase internal structures, to ensure determinism/reproducability
         virtual void resetPool(btDispatcher* dispatcher) {};
+        virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; };
         virtual void    printStats() = 0;

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

-                      r5781
+                      r8284
         MINKOWSKI_SUM_SHAPE_PROXYTYPE,
         MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
+        BOX_2D_SHAPE_PROXYTYPE,
+        CONVEX_2D_SHAPE_PROXYTYPE,
         CUSTOM_CONVEX_SHAPE_TYPE,
 //concave shapes
 …
+        }
+        static SIMD_FORCE_INLINE bool   isNonMoving(int proxyType)
+        {
+                return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE));
+        }
         static SIMD_FORCE_INLINE bool   isConcave(int proxyType)
+        {
 …
                 return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
+        }
+        static SIMD_FORCE_INLINE bool   isSoftBody(int proxyType)
+        {
+                return (proxyType == SOFTBODY_SHAPE_PROXYTYPE);
+        }
         static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
+        {
                 return (proxyType == STATIC_PLANE_PROXYTYPE);
+        }
+        static SIMD_FORCE_INLINE bool isConvex2d(int proxyType)
+        {
+                return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
+        }
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp

r5781	r8284
62	62	{
63	63	getmaxdepth(node->childs[0],depth+1,maxdepth);
64		getmaxdepth(node->childs[0],depth+1,maxdepth);
	64	getmaxdepth(node->childs[1],depth+1,maxdepth);
65	65	} else maxdepth=btMax(maxdepth,depth);
66	66	}
…	…
239	239	for(int i=0,ni=leaves.size();i<ni;++i)
240	240	{
241		if(dot(axis,leaves[i]->volume.Center()-org)<0)
	241	if(btDot(axis,leaves[i]->volume.Center()-org)<0)
242	242	left.push_back(leaves[i]);
243	243	else
…	…
320	320	for(int j=0;j<3;++j)
321	321	{
322		++splitcount[j][dot(x,axis[j])>0?1:0];
	322	++splitcount[j][btDot(x,axis[j])>0?1:0];
323	323	}
324	324	}

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvt.h

-                      r5781
+                      r8284
 // Template implementation of ICollide
 #ifdef WIN32
+#ifdef _WIN32
 #if (defined (_MSC_VER) && _MSC_VER >= 1400)
 #define DBVT_USE_TEMPLATE               1
 …
 //SSE gives errors on a MSVC 7.1
 #ifdef BT_USE_SSE
+#if defined (BT_USE_SSE) && defined (_WIN32)
 #define DBVT_SELECT_IMPL                DBVT_IMPL_SSE
 #define DBVT_MERGE_IMPL                 DBVT_IMPL_SSE
 …
 #if DBVT_USE_MEMMOVE
 #ifndef __CELLOS_LV2__
+#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
 #include <memory.h>
 #endif
 …
                 pi=btVector3(mi.x(),mi.y(),mi.z());break;
+        }
         if((dot(n,px)+o)<0)             return(-1);
         if((dot(n,pi)+o)>=0)    return(+1);
+        if((btDot(n,px)+o)<0)           return(-1);
+        if((btDot(n,pi)+o)>=0)  return(+1);
         return(0);
+}
 …
                 b[(signs>>1)&1]->y(),
                 b[(signs>>2)&1]->z());
         return(dot(p,v));
+        return(btDot(p,v));
+}
 …
                                                                 DBVT_IPOLICY) const
+{
+        (void) rayTo;
         DBVT_CHECKTYPE
         if(root)
 …
+                {
                         const btDbvtNode*       node=stack[--depth];
                         bounds[0] = node->volume.Mins()+aabbMin;
                         bounds[1] = node->volume.Maxs()+aabbMax;
+                        bounds[0] = node->volume.Mins()-aabbMax;
+                        bounds[1] = node->volume.Maxs()-aabbMin;
                         btScalar tmin=1.f,lambda_min=0.f;
                         unsigned int result1=false;
 …
                         rayDir.normalize ();
                         ///what about division by zero? --> just set rayDirection[i] to INF/1e30
+                        ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
                         btVector3 rayDirectionInverse;
                         rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
                         rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
                         rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+                        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+                        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+                        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
                         unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 ///btDbvtBroadphase implementation by Nathanael Presson
 …
         m_needcleanup           =       true;
         m_releasepaircache      =       (paircache!=0)?false:true;
         m_prediction            =       1/(btScalar)2;
+        m_prediction            =       0;
         m_stageCurrent          =       0;
         m_fixedleft                     =       0;
 …
+}
+struct  BroadphaseAabbTester : btDbvt::ICollide
+{
+        btBroadphaseAabbCallback& m_aabbCallback;
+        BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
+                :m_aabbCallback(orgCallback)
+        {
+        }
+        void                                    Process(const btDbvtNode* leaf)
+        {
+                btDbvtProxy*    proxy=(btDbvtProxy*)leaf->data;
+                m_aabbCallback.process(proxy);
+        }
+};
+void    btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback)
+{
+        BroadphaseAabbTester callback(aabbCallback);
+        const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax);
+                //process all children, that overlap with  the given AABB bounds
+        m_sets[0].collideTV(m_sets[0].m_root,bounds,callback);
+        m_sets[1].collideTV(m_sets[1].m_root,bounds,callback);
+}
 //
 void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
 …
+                }
+        }
+}
+//
+void                                                    btDbvtBroadphase::setAabbForceUpdate(           btBroadphaseProxy* absproxy,
+                                                                                                                  const btVector3& aabbMin,
+                                                                                                                  const btVector3& aabbMax,
+                                                                                                                  btDispatcher* /*dispatcher*/)
+{
+        btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
+        ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+        bool    docollide=false;
+        if(proxy->stage==STAGECOUNT)
+        {/* fixed -> dynamic set        */
+                m_sets[1].remove(proxy->leaf);
+                proxy->leaf=m_sets[0].insert(aabb,proxy);
+                docollide=true;
+        }
+        else
+        {/* dynamic set                         */
+                ++m_updates_call;
+                /* Teleporting                  */
+                m_sets[0].update(proxy->leaf,aabb);
+                ++m_updates_done;
+                docollide=true;
+        }
+        listremove(proxy,m_stageRoots[proxy->stage]);
+        proxy->m_aabbMin = aabbMin;
+        proxy->m_aabbMax = aabbMax;
+        proxy->stage    =       m_stageCurrent;
+        listappend(proxy,m_stageRoots[m_stageCurrent]);
+        if(docollide)
+        {
+                m_needcleanup=true;
+                if(!m_deferedcollide)
+                {
+                        btDbvtTreeCollider      collider(this);
+                        m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
+                        m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
+                }
+        }
+}
 …
                 m_deferedcollide        =       false;
                 m_needcleanup           =       true;
-                m_prediction            =       1/(btScalar)2;
                 m_stageCurrent          =       0;
                 m_fixedleft                     =       0;

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 ///btDbvtBroadphase implementation by Nathanael Presson
 #ifndef BT_DBVT_BROADPHASE_H
 …
         void                                                    collide(btDispatcher* dispatcher);
         void                                                    optimize();
+        /* btBroadphaseInterface Implementation */
+        /* btBroadphaseInterface Implementation */
         btBroadphaseProxy*                              createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+        void                                                    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+        void                                                    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+        virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
+        virtual void                                    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+        virtual void                                    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+        virtual void                                    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
+        virtual void                                    aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
+        virtual void    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+        void                                                    calculateOverlappingPairs(btDispatcher* dispatcher);
+        btOverlappingPairCache*                 getOverlappingPairCache();
+        const btOverlappingPairCache*   getOverlappingPairCache() const;
+        void                                                    getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+        void                                                    printStats();
+        static void                                             benchmark(btBroadphaseInterface*);
+        virtual void                                    getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+        virtual void                                    calculateOverlappingPairs(btDispatcher* dispatcher);
+        virtual btOverlappingPairCache* getOverlappingPairCache();
+        virtual const btOverlappingPairCache*   getOverlappingPairCache() const;
+        virtual void                                    getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+        virtual void                                    printStats();
-        void    performDeferredRemoval(btDispatcher* dispatcher);
         ///reset broadphase internal structures, to ensure determinism/reproducability
         virtual void resetPool(btDispatcher* dispatcher);
+        void    performDeferredRemoval(btDispatcher* dispatcher);
+        void    setVelocityPrediction(btScalar prediction)
+        {
+                m_prediction = prediction;
+        }
+        btScalar getVelocityPrediction() const
+        {
+                return m_prediction;
+        }
+        ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
+        ///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase.
+        ///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
+        ///http://code.google.com/p/bullet/issues/detail?id=223
+        void                                                    setAabbForceUpdate(             btBroadphaseProxy* absproxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* /*dispatcher*/);
+        static void                                             benchmark(btBroadphaseInterface*);
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h

-                      r5781
+                      r8284
                 m_useEpa(true),
                 m_allowedCcdPenetration(btScalar(0.04)),
                 m_useConvexConservativeDistanceUtil(true),
+                m_useConvexConservativeDistanceUtil(false),
                 m_convexConservativeDistanceThreshold(0.0f),
+                m_convexMaxDistanceUseCPT(false),
                 m_stackAllocator(0)
+        {
 …
         bool            m_useConvexConservativeDistanceUtil;
         btScalar        m_convexConservativeDistanceThreshold;
+        bool            m_convexMaxDistanceUseCPT;
         btStackAlloc*   m_stackAllocator;
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

r5781	r8284
134	134	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
135	135	{
136		aabbMin.setValue(-~~1e30f,-1e30f,-1e30f~~);
137		aabbMax.setValue(~~1e30f,1e30f,1e30f~~);
	136	aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
	137	aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
138	138	}
139	139

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

-                      r5781
+                      r8284
         int count = m_overlappingPairArray.size();
         int oldCapacity = m_overlappingPairArray.capacity();
         void* mem = &m_overlappingPairArray.expand();
+        void* mem = &m_overlappingPairArray.expandNonInitializing();
         //this is where we add an actual pair, so also call the 'ghost'
 …
                 return 0;
         void* mem = &m_overlappingPairArray.expand();
+        void* mem = &m_overlappingPairArray.expandNonInitializing();
         btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

r5781	r8284
458	458	virtual void sortOverlappingPairs(btDispatcher* dispatcher)
459	459	{
	460	(void) dispatcher;
460	461	}
461	462

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

-                      r5781
+                      r8284
 #include "LinearMath/btAabbUtil2.h"
 #include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btSerializer.h"
 #define RAYAABB2
 …
 btVector3 color[4]=
+{
         btVector3(255,0,0),
         btVector3(0,255,0),
         btVector3(0,0,255),
         btVector3(0,255,255)
+        btVector3(1,0,0),
+        btVector3(0,1,0),
+        btVector3(0,0,1),
+        btVector3(0,1,1)
 };
 #endif //DEBUG_PATCH_COLORS
 …
         ///what about division by zero? --> just set rayDirection[i] to 1.0
         btVector3 rayDirectionInverse;
         rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
         rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
         rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
         unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
 #endif
 …
                 bounds[1] = rootNode->m_aabbMaxOrg;
                 /* Add box cast extents */
                 bounds[0] += aabbMin;
                 bounds[1] += aabbMax;
+                bounds[0] -= aabbMax;
+                bounds[1] -= aabbMin;
                 aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
 …
         lambda_max = rayDirection.dot(rayTarget-raySource);
         ///what about division by zero? --> just set rayDirection[i] to 1.0
         rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0];
         rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1];
         rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2];
+        rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0];
+        rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1];
+        rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[2];
         unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0};
 #endif
 …
                         bounds[1] = unQuantize(rootNode->m_quantizedAabbMax);
                         /* Add box cast extents */
                         bounds[0] += aabbMin;
                         bounds[1] += aabbMax;
+                        bounds[0] -= aabbMax;
+                        bounds[1] -= aabbMin;
                         btVector3 normal;
 #if 0
 …
+}
 unsigned btQuantizedBvh::calculateSerializeBufferSize()
+unsigned btQuantizedBvh::calculateSerializeBufferSize() const
+{
         unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding();
 …
+}
 bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
+bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const
+{
         btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
 …
+}
+void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData)
+{
+        m_bvhAabbMax.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMax);
+        m_bvhAabbMin.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMin);
+        m_bvhQuantization.deSerializeFloat(quantizedBvhFloatData.m_bvhQuantization);
+        m_curNodeIndex = quantizedBvhFloatData.m_curNodeIndex;
+        m_useQuantization = quantizedBvhFloatData.m_useQuantization!=0;
+        {
+                int numElem = quantizedBvhFloatData.m_numContiguousLeafNodes;
+                m_contiguousNodes.resize(numElem);
+                if (numElem)
+                {
+                        btOptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr;
+                        for (int i=0;i<numElem;i++,memPtr++)
+                        {
+                                m_contiguousNodes[i].m_aabbMaxOrg.deSerializeFloat(memPtr->m_aabbMaxOrg);
+                                m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg);
+                                m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex;
+                                m_contiguousNodes[i].m_subPart = memPtr->m_subPart;
+                                m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex;
+                        }
+                }
+        }
+        {
+                int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes;
+                m_quantizedContiguousNodes.resize(numElem);
+                if (numElem)
+                {
+                        btQuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr;
+                        for (int i=0;i<numElem;i++,memPtr++)
+                        {
+                                m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex;
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+                        }
+                }
+        }
+        m_traversalMode = btTraversalMode(quantizedBvhFloatData.m_traversalMode);
+        {
+                int numElem = quantizedBvhFloatData.m_numSubtreeHeaders;
+                m_SubtreeHeaders.resize(numElem);
+                if (numElem)
+                {
+                        btBvhSubtreeInfoData* memPtr = quantizedBvhFloatData.m_subTreeInfoPtr;
+                        for (int i=0;i<numElem;i++,memPtr++)
+                        {
+                                m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ;
+                                m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+                                m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+                                m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+                                m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+                                m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+                                m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex;
+                                m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize;
+                        }
+                }
+        }
+}
+void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData)
+{
+        m_bvhAabbMax.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMax);
+        m_bvhAabbMin.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMin);
+        m_bvhQuantization.deSerializeDouble(quantizedBvhDoubleData.m_bvhQuantization);
+        m_curNodeIndex = quantizedBvhDoubleData.m_curNodeIndex;
+        m_useQuantization = quantizedBvhDoubleData.m_useQuantization!=0;
+        {
+                int numElem = quantizedBvhDoubleData.m_numContiguousLeafNodes;
+                m_contiguousNodes.resize(numElem);
+                if (numElem)
+                {
+                        btOptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr;
+                        for (int i=0;i<numElem;i++,memPtr++)
+                        {
+                                m_contiguousNodes[i].m_aabbMaxOrg.deSerializeDouble(memPtr->m_aabbMaxOrg);
+                                m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg);
+                                m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex;
+                                m_contiguousNodes[i].m_subPart = memPtr->m_subPart;
+                                m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex;
+                        }
+                }
+        }
+        {
+                int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes;
+                m_quantizedContiguousNodes.resize(numElem);
+                if (numElem)
+                {
+                        btQuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr;
+                        for (int i=0;i<numElem;i++,memPtr++)
+                        {
+                                m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex;
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+                                m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+                        }
+                }
+        }
+        m_traversalMode = btTraversalMode(quantizedBvhDoubleData.m_traversalMode);
+        {
+                int numElem = quantizedBvhDoubleData.m_numSubtreeHeaders;
+                m_SubtreeHeaders.resize(numElem);
+                if (numElem)
+                {
+                        btBvhSubtreeInfoData* memPtr = quantizedBvhDoubleData.m_subTreeInfoPtr;
+                        for (int i=0;i<numElem;i++,memPtr++)
+                        {
+                                m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ;
+                                m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+                                m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+                                m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+                                m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+                                m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+                                m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex;
+                                m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize;
+                        }
+                }
+        }
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btQuantizedBvhData* quantizedData = (btQuantizedBvhData*)dataBuffer;
+        m_bvhAabbMax.serialize(quantizedData->m_bvhAabbMax);
+        m_bvhAabbMin.serialize(quantizedData->m_bvhAabbMin);
+        m_bvhQuantization.serialize(quantizedData->m_bvhQuantization);
+        quantizedData->m_curNodeIndex = m_curNodeIndex;
+        quantizedData->m_useQuantization = m_useQuantization;
+        quantizedData->m_numContiguousLeafNodes = m_contiguousNodes.size();
+        quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*) (m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0);
+        if (quantizedData->m_contiguousNodesPtr)
+        {
+                int sz = sizeof(btOptimizedBvhNodeData);
+                int numElem = m_contiguousNodes.size();
+                btChunk* chunk = serializer->allocate(sz,numElem);
+                btOptimizedBvhNodeData* memPtr = (btOptimizedBvhNodeData*)chunk->m_oldPtr;
+                for (int i=0;i<numElem;i++,memPtr++)
+                {
+                        m_contiguousNodes[i].m_aabbMaxOrg.serialize(memPtr->m_aabbMaxOrg);
+                        m_contiguousNodes[i].m_aabbMinOrg.serialize(memPtr->m_aabbMinOrg);
+                        memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex;
+                        memPtr->m_subPart = m_contiguousNodes[i].m_subPart;
+                        memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex;
+                }
+                serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]);
+        }
+        quantizedData->m_numQuantizedContiguousNodes = m_quantizedContiguousNodes.size();
+//      printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes);
+        quantizedData->m_quantizedContiguousNodesPtr =(btQuantizedBvhNodeData*) (m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0);
+        if (quantizedData->m_quantizedContiguousNodesPtr)
+        {
+                int sz = sizeof(btQuantizedBvhNodeData);
+                int numElem = m_quantizedContiguousNodes.size();
+                btChunk* chunk = serializer->allocate(sz,numElem);
+                btQuantizedBvhNodeData* memPtr = (btQuantizedBvhNodeData*)chunk->m_oldPtr;
+                for (int i=0;i<numElem;i++,memPtr++)
+                {
+                        memPtr->m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex;
+                        memPtr->m_quantizedAabbMax[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[0];
+                        memPtr->m_quantizedAabbMax[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[1];
+                        memPtr->m_quantizedAabbMax[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[2];
+                        memPtr->m_quantizedAabbMin[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[0];
+                        memPtr->m_quantizedAabbMin[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[1];
+                        memPtr->m_quantizedAabbMin[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[2];
+                }
+                serializer->finalizeChunk(chunk,"btQuantizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_quantizedContiguousNodes[0]);
+        }
+        quantizedData->m_traversalMode = int(m_traversalMode);
+        quantizedData->m_numSubtreeHeaders = m_SubtreeHeaders.size();
+        quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*) (m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0);
+        if (quantizedData->m_subTreeInfoPtr)
+        {
+                int sz = sizeof(btBvhSubtreeInfoData);
+                int numElem = m_SubtreeHeaders.size();
+                btChunk* chunk = serializer->allocate(sz,numElem);
+                btBvhSubtreeInfoData* memPtr = (btBvhSubtreeInfoData*)chunk->m_oldPtr;
+                for (int i=0;i<numElem;i++,memPtr++)
+                {
+                        memPtr->m_quantizedAabbMax[0] = m_SubtreeHeaders[i].m_quantizedAabbMax[0];
+                        memPtr->m_quantizedAabbMax[1] = m_SubtreeHeaders[i].m_quantizedAabbMax[1];
+                        memPtr->m_quantizedAabbMax[2] = m_SubtreeHeaders[i].m_quantizedAabbMax[2];
+                        memPtr->m_quantizedAabbMin[0] = m_SubtreeHeaders[i].m_quantizedAabbMin[0];
+                        memPtr->m_quantizedAabbMin[1] = m_SubtreeHeaders[i].m_quantizedAabbMin[1];
+                        memPtr->m_quantizedAabbMin[2] = m_SubtreeHeaders[i].m_quantizedAabbMin[2];
+                        memPtr->m_rootNodeIndex = m_SubtreeHeaders[i].m_rootNodeIndex;
+                        memPtr->m_subtreeSize = m_SubtreeHeaders[i].m_subtreeSize;
+                }
+                serializer->finalizeChunk(chunk,"btBvhSubtreeInfoData",BT_ARRAY_CODE,(void*)&m_SubtreeHeaders[0]);
+        }
+        return btQuantizedBvhDataName;
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.h

-                      r5781
+                      r8284
 #define QUANTIZED_BVH_H
+class btSerializer;
 //#define DEBUG_CHECK_DEQUANTIZATION 1
 #ifdef DEBUG_CHECK_DEQUANTIZATION
 …
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedAllocator.h"
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btQuantizedBvhData btQuantizedBvhDoubleData
+#define btOptimizedBvhNodeData btOptimizedBvhNodeDoubleData
+#define btQuantizedBvhDataName "btQuantizedBvhDoubleData"
+#else
+#define btQuantizedBvhData btQuantizedBvhFloatData
+#define btOptimizedBvhNodeData btOptimizedBvhNodeFloatData
+#define btQuantizedBvhDataName "btQuantizedBvhFloatData"
+#endif
 …
         //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
         int m_subtreeHeaderCount;
+        mutable int m_subtreeHeaderCount;
 …
+        }
+////////////////////////////////////////////////////////////////////
         /////Calculate space needed to store BVH for serialization
         unsigned calculateSerializeBufferSize();
+        unsigned calculateSerializeBufferSize() const;
         /// Data buffer MUST be 16 byte aligned
         virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian);
+        virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
         ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
 …
         static unsigned int getAlignmentSerializationPadding();
+//////////////////////////////////////////////////////////////////////
+        virtual int     calculateSerializeBufferSizeNew() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
+        virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData);
+        virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData);
+////////////////////////////////////////////////////////////////////
         SIMD_FORCE_INLINE bool isQuantized()
 …
+struct  btBvhSubtreeInfoData
+{
+        int                     m_rootNodeIndex;
+        int                     m_subtreeSize;
+        unsigned short m_quantizedAabbMin[3];
+        unsigned short m_quantizedAabbMax[3];
+};
+struct btOptimizedBvhNodeFloatData
+{
+        btVector3FloatData      m_aabbMinOrg;
+        btVector3FloatData      m_aabbMaxOrg;
+        int     m_escapeIndex;
+        int     m_subPart;
+        int     m_triangleIndex;
+        char m_pad[4];
+};
+struct btOptimizedBvhNodeDoubleData
+{
+        btVector3DoubleData     m_aabbMinOrg;
+        btVector3DoubleData     m_aabbMaxOrg;
+        int     m_escapeIndex;
+        int     m_subPart;
+        int     m_triangleIndex;
+        char    m_pad[4];
+};
+struct btQuantizedBvhNodeData
+{
+        unsigned short m_quantizedAabbMin[3];
+        unsigned short m_quantizedAabbMax[3];
+        int     m_escapeIndexOrTriangleIndex;
+};
+struct  btQuantizedBvhFloatData
+{
+        btVector3FloatData                      m_bvhAabbMin;
+        btVector3FloatData                      m_bvhAabbMax;
+        btVector3FloatData                      m_bvhQuantization;
+        int                                     m_curNodeIndex;
+        int                                     m_useQuantization;
+        int                                     m_numContiguousLeafNodes;
+        int                                     m_numQuantizedContiguousNodes;
+        btOptimizedBvhNodeFloatData     *m_contiguousNodesPtr;
+        btQuantizedBvhNodeData          *m_quantizedContiguousNodesPtr;
+        btBvhSubtreeInfoData    *m_subTreeInfoPtr;
+        int                                     m_traversalMode;
+        int                                     m_numSubtreeHeaders;
+};
+struct  btQuantizedBvhDoubleData
+{
+        btVector3DoubleData                     m_bvhAabbMin;
+        btVector3DoubleData                     m_bvhAabbMax;
+        btVector3DoubleData                     m_bvhQuantization;
+        int                                                     m_curNodeIndex;
+        int                                                     m_useQuantization;
+        int                                                     m_numContiguousLeafNodes;
+        int                                                     m_numQuantizedContiguousNodes;
+        btOptimizedBvhNodeDoubleData    *m_contiguousNodesPtr;
+        btQuantizedBvhNodeData                  *m_quantizedContiguousNodesPtr;
+        int                                                     m_traversalMode;
+        int                                                     m_numSubtreeHeaders;
+        btBvhSubtreeInfoData            *m_subTreeInfoPtr;
+};
+SIMD_FORCE_INLINE       int     btQuantizedBvh::calculateSerializeBufferSizeNew() const
+{
+        return sizeof(btQuantizedBvhData);
+}
 #endif //QUANTIZED_BVH_H

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

-                      r5781
+                      r8284
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAabbUtil2.h"
 #include <new>
 …
+                }
                 rayCallback.process(proxy);
+        }
+}
+void    btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
+{
+        for (int i=0; i <= m_LastHandleIndex; i++)
+        {
+                btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
+                if(!proxy->m_clientObject)
+                {
+                        continue;
+                }
+                if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
+                {
+                        callback.process(proxy);
+                }
+        }
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

-                      r5781
+                      r8284
         virtual void    rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
+        virtual void    aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
         btOverlappingPairCache* getOverlappingPairCache()
 …
         virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+        {
                 aabbMin.setValue(-1e30f,-1e30f,-1e30f);
                 aabbMax.setValue(1e30f,1e30f,1e30f);
+                aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+                aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+        }

code/branches/kicklib2/src/external/bullet/BulletCollision/CMakeLists.txt

-                      r5929
+                      r8284
         BroadphaseCollision/btBroadphaseProxy.cpp
         BroadphaseCollision/btCollisionAlgorithm.cpp
+        BroadphaseCollision/btDbvt.cpp
+        BroadphaseCollision/btDbvtBroadphase.cpp
         BroadphaseCollision/btDispatcher.cpp
-        BroadphaseCollision/btDbvtBroadphase.cpp
-        BroadphaseCollision/btDbvt.cpp
         BroadphaseCollision/btMultiSapBroadphase.cpp
         BroadphaseCollision/btOverlappingPairCache.cpp
 …
         CollisionDispatch/btActivatingCollisionAlgorithm.cpp
+        CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
+        CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
+        CollisionDispatch/btBoxBoxDetector.cpp
         CollisionDispatch/btCollisionDispatcher.cpp
         CollisionDispatch/btCollisionObject.cpp
 …
         CollisionDispatch/btCompoundCollisionAlgorithm.cpp
         CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+        CollisionDispatch/btConvexConvexAlgorithm.cpp
+        CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
+        CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
         CollisionDispatch/btDefaultCollisionConfiguration.cpp
+        CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+        CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
+        CollisionDispatch/btBoxBoxDetector.cpp
+        CollisionDispatch/btEmptyCollisionAlgorithm.cpp
         CollisionDispatch/btGhostObject.cpp
+        CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+        CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
+        CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
+        CollisionDispatch/btConvexConvexAlgorithm.cpp
+        CollisionDispatch/btEmptyCollisionAlgorithm.cpp
+        CollisionDispatch/btInternalEdgeUtility.cpp
+        CollisionDispatch/btInternalEdgeUtility.h
         CollisionDispatch/btManifoldResult.cpp
         CollisionDispatch/btSimulationIslandManager.cpp
+        CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+        CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+        CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
         CollisionDispatch/btUnionFind.cpp
         CollisionDispatch/SphereTriangleDetector.cpp
         CollisionShapes/btBoxShape.cpp
+        CollisionShapes/btBox2dShape.cpp
         CollisionShapes/btBvhTriangleMeshShape.cpp
         CollisionShapes/btCapsuleShape.cpp
 …
         CollisionShapes/btConeShape.cpp
         CollisionShapes/btConvexHullShape.cpp
+        CollisionShapes/btConvexInternalShape.cpp
         CollisionShapes/btConvexPointCloudShape.cpp
         CollisionShapes/btConvexShape.cpp
         CollisionShapes/btConvexInternalShape.cpp
+        CollisionShapes/btConvex2dShape.cpp
         CollisionShapes/btConvexTriangleMeshShape.cpp
         CollisionShapes/btCylinderShape.cpp
 …
         CollisionShapes/btPolyhedralConvexShape.cpp
         CollisionShapes/btScaledBvhTriangleMeshShape.cpp
         CollisionShapes/btTetrahedronShape.cpp
+        CollisionShapes/btShapeHull.cpp
         CollisionShapes/btSphereShape.cpp
-        CollisionShapes/btShapeHull.cpp
         CollisionShapes/btStaticPlaneShape.cpp
         CollisionShapes/btStridingMeshInterface.cpp
+        CollisionShapes/btTetrahedronShape.cpp
+        CollisionShapes/btTriangleBuffer.cpp
         CollisionShapes/btTriangleCallback.cpp
-        CollisionShapes/btTriangleBuffer.cpp
         CollisionShapes/btTriangleIndexVertexArray.cpp
         CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
 …
         CollisionShapes/btUniformScalingShape.cpp
-        Gimpact/btContactProcessing.cpp
-        Gimpact/btGImpactShape.cpp
-        Gimpact/btGImpactBvh.cpp
-        Gimpact/btGenericPoolAllocator.cpp
-        Gimpact/btGImpactCollisionAlgorithm.cpp
-        Gimpact/btTriangleShapeEx.cpp
-        Gimpact/btGImpactQuantizedBvh.cpp
         NarrowPhaseCollision/btContinuousConvexCollision.cpp
+        NarrowPhaseCollision/btConvexCast.cpp
+        NarrowPhaseCollision/btGjkConvexCast.cpp
         NarrowPhaseCollision/btGjkEpa2.cpp
         NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
-        NarrowPhaseCollision/btConvexCast.cpp
-        NarrowPhaseCollision/btGjkConvexCast.cpp
         NarrowPhaseCollision/btGjkPairDetector.cpp
         NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
 …
 COMPILATION_END
-COMPILATION_BEGIN BulletGImpactCompilation.cpp
-        Gimpact/gim_contact.cpp
-        Gimpact/gim_memory.cpp
-        Gimpact/gim_tri_collision.cpp
-        Gimpact/gim_box_set.cpp
-COMPILATION_END
         # Headers
         BroadphaseCollision/btAxisSweep3.h
 …
         BroadphaseCollision/btBroadphaseProxy.h
         BroadphaseCollision/btCollisionAlgorithm.h
+        BroadphaseCollision/btDbvt.h
+        BroadphaseCollision/btDbvtBroadphase.h
         BroadphaseCollision/btDispatcher.h
-        BroadphaseCollision/btDbvtBroadphase.h
-        BroadphaseCollision/btDbvt.h
         BroadphaseCollision/btMultiSapBroadphase.h
         BroadphaseCollision/btOverlappingPairCache.h
 …
         CollisionDispatch/btActivatingCollisionAlgorithm.h
+        CollisionDispatch/btBoxBoxCollisionAlgorithm.h
+        CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
+        CollisionDispatch/btBoxBoxDetector.h
         CollisionDispatch/btCollisionConfiguration.h
         CollisionDispatch/btCollisionCreateFunc.h
 …
         CollisionDispatch/btCompoundCollisionAlgorithm.h
         CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+        CollisionDispatch/btConvexConvexAlgorithm.h
+        CollisionDispatch/btConvex2dConvex2dAlgorithm.h
+        CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
         CollisionDispatch/btDefaultCollisionConfiguration.h
+        CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+        CollisionDispatch/btBoxBoxCollisionAlgorithm.h
+        CollisionDispatch/btBoxBoxDetector.h
+        CollisionDispatch/btEmptyCollisionAlgorithm.h
         CollisionDispatch/btGhostObject.h
-        CollisionDispatch/btSphereBoxCollisionAlgorithm.h
-        CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
-        CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
-        CollisionDispatch/btConvexConvexAlgorithm.h
-        CollisionDispatch/btEmptyCollisionAlgorithm.h
         CollisionDispatch/btManifoldResult.h
         CollisionDispatch/btSimulationIslandManager.h
+        CollisionDispatch/btSphereBoxCollisionAlgorithm.h
+        CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+        CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
         CollisionDispatch/btUnionFind.h
         CollisionDispatch/SphereTriangleDetector.h
         CollisionShapes/btBoxShape.h
+        CollisionShapes/btBox2dShape.h
         CollisionShapes/btBvhTriangleMeshShape.h
         CollisionShapes/btCapsuleShape.h
         CollisionShapes/btCollisionMargin
+        CollisionShapes/btCollisionMargin.h
         CollisionShapes/btCollisionShape.h
         CollisionShapes/btCompoundShape.h
 …
         CollisionShapes/btConeShape.h
         CollisionShapes/btConvexHullShape.h
+        CollisionShapes/btConvexInternalShape.h
         CollisionShapes/btConvexPointCloudShape.h
         CollisionShapes/btConvexShape.h
         CollisionShapes/btConvexInternalShape.h
+        CollisionShapes/btConvex2dShape.h
         CollisionShapes/btConvexTriangleMeshShape.h
         CollisionShapes/btCylinderShape.h
         CollisionShapes/btEmptyShape.h
         CollisionShapes/btHeightfieldTerrainShape.h
+        CollisionShapes/btMaterial.h
         CollisionShapes/btMinkowskiSumShape.h
-        CollisionShapes/btMaterial.h
         CollisionShapes/btMultimaterialTriangleMeshShape.h
         CollisionShapes/btMultiSphereShape.h
 …
         CollisionShapes/btPolyhedralConvexShape.h
         CollisionShapes/btScaledBvhTriangleMeshShape.h
         CollisionShapes/btTetrahedronShape.h
+        CollisionShapes/btShapeHull.h
         CollisionShapes/btSphereShape.h
-        CollisionShapes/btShapeHull.h
         CollisionShapes/btStaticPlaneShape.h
         CollisionShapes/btStridingMeshInterface.h
+        CollisionShapes/btTetrahedronShape.h
+        CollisionShapes/btTriangleBuffer.h
         CollisionShapes/btTriangleCallback.h
-        CollisionShapes/btTriangleBuffer.h
         CollisionShapes/btTriangleIndexVertexArray.h
         CollisionShapes/btTriangleIndexVertexMaterialArray.h
+        CollisionShapes/btTriangleInfoMap.h
         CollisionShapes/btTriangleMesh.h
         CollisionShapes/btTriangleMeshShape.h
+        CollisionShapes/btTriangleShape.h
         CollisionShapes/btUniformScalingShape.h
-        Gimpact/btGImpactShape.h
-        Gimpact/gim_contact.h
-        Gimpact/btGImpactBvh.h
-        Gimpact/btGenericPoolAllocator.h
-        Gimpact/gim_memory.h
-        Gimpact/btGImpactCollisionAlgorithm.h
-        Gimpact/btTriangleShapeEx.h
-        Gimpact/gim_tri_collision.h
-        Gimpact/btGImpactQuantizedBvh.h
-        Gimpact/gim_box_set.h
         NarrowPhaseCollision/btContinuousConvexCollision.h

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

r5781	r8284
38	38	btScalar timeOfImpact = btScalar(1.);
39	39	btScalar depth = btScalar(0.);
40		// output.m_distance = btScalar(~~1e30~~);
	40	// output.m_distance = btScalar(BT_LARGE_FLOAT);
41	41	//move sphere into triangle space
42	42	btTransform sphereInTr = transformB.inverseTimes(transformA);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h

-                      r5781
+                      r8284
         virtual ~SphereTriangleDetector() {};
+        bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold);
 private:
         bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold);
         bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
         bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp

r5781	r8284
62	62
63	63	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
64		input.m_maximumDistanceSquared = ~~1e30f~~;
	64	input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
65	65	input.m_transformA = body0->getWorldTransform();
66	66	input.m_transformB = body1->getWorldTransform();

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp

-                      r5781
+                      r8284
 /*
  * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
 …
         } else
+        {
                 a=1e30f;
+                a=BT_LARGE_FLOAT;
+        }
     cx = a*(cx + q*(p[n*2-2]+p[0]));
 …
+{
   const btScalar fudge_factor = btScalar(1.05);
   btVector3 p,pp,normalC;
+  btVector3 p,pp,normalC(0.f,0.f,0.f);
   const btScalar *normalR = 0;
   btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
 …
 #define TST(expr1,expr2,n1,n2,n3,cc) \
   s2 = btFabs(expr1) - (expr2); \
   if (s2 > 0) return 0; \
+  if (s2 > SIMD_EPSILON) return 0; \
   l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
   if (l > 0) { \
+  if (l > SIMD_EPSILON) { \
     s2 /= l; \
     if (s2*fudge_factor > s) { \
 …
     } \
+  }
+  btScalar fudge2 (1.0e-5f);
+  Q11 += fudge2;
+  Q12 += fudge2;
+  Q13 += fudge2;
+  Q21 += fudge2;
+  Q22 += fudge2;
+  Q23 += fudge2;
+  Q31 += fudge2;
+  Q32 += fudge2;
+  Q33 += fudge2;
   // separating axis = u1 x (v1,v2,v3)
 …
 #else
                 output.addContactPoint(-normal,pb,-*depth);
 #endif //
                 *return_code = code;
 …
   if (cnum <= maxc) {
+          if (code<4)
+          {
     // we have less contacts than we need, so we use them all
+    for (j=0; j < cnum; j++) {
+                //AddContactPoint...
+                //dContactGeom *con = CONTACT(contact,skip*j);
+      //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
+      //con->depth = dep[j];
+    for (j=0; j < cnum; j++)
+        {
                 btVector3 pointInWorld;
                 for (i=0; i<3; i++)
 …
+    }
+          } else
+          {
+                  // we have less contacts than we need, so we use them all
+                for (j=0; j < cnum; j++)
+                {
+                        btVector3 pointInWorld;
+                        for (i=0; i<3; i++)
+                                pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
+                                //pointInWorld[i] = point[j*3+i] + pa[i];
+                        output.addContactPoint(-normal,pointInWorld,-dep[j]);
+                }
+          }
+  }
   else {
 …
                 for (i=0; i<3; i++)
                         posInWorld[i] = point[iret[j]*3+i] + pa[i];
+                output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+                if (code<4)
+           {
+                        output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+                } else
+                {
+                        output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
+                }
+    }
     cnum = maxc;

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

-                      r5781
+                      r8284
 btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration):
+        m_count(0),
+        m_useIslands(true),
+        m_staticWarningReported(false),
+m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
         m_collisionConfiguration(collisionConfiguration)
+{
 …
         btCollisionObject* body1 = (btCollisionObject*)b1;
+        //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold'
+        //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
+        btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold());
+        //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
+        btScalar contactBreakingThreshold =  (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ?
+                btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
+                : gContactBreakingThreshold ;
         btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
 …
 #ifdef BT_DEBUG
         if (!m_staticWarningReported)
+        if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED))
+        {
                 //broadphase filtering already deals with this
 …
                         (body1->isStaticObject() || body1->isKinematicObject()))
+                {
                         m_staticWarningReported = true;
+                        m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED;
                         printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+                }

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

-                      r5781
+                      r8284
 class btCollisionDispatcher : public btDispatcher
+{
         int m_count;
+        int             m_dispatcherFlags;
         btAlignedObjectArray<btPersistentManifold*>     m_manifoldsPtr;
-        bool m_useIslands;
-        bool    m_staticWarningReported;
         btManifoldResult        m_defaultManifoldResult;
 …
         btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
         btCollisionConfiguration*       m_collisionConfiguration;
 …
 public:
+        enum DispatcherFlags
+        {
+                CD_STATIC_STATIC_REPORTED = 1,
+                CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2
+        };
+        int     getDispatcherFlags() const
+        {
+                return m_dispatcherFlags;
+        }
+        void    setDispatcherFlags(int flags)
+        {
+        (void) flags;
+                m_dispatcherFlags = 0;
+        }
         ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp

-                      r5781
+                      r8284
 #include "btCollisionObject.h"
+#include "LinearMath/btSerializer.h"
 btCollisionObject::btCollisionObject()
         :       m_anisotropicFriction(1.f,1.f,1.f),
         m_hasAnisotropicFriction(false),
         m_contactProcessingThreshold(1e30f),
+        m_contactProcessingThreshold(BT_LARGE_FLOAT),
                 m_broadphaseHandle(0),
                 m_collisionShape(0),
+                m_extensionPointer(0),
                 m_rootCollisionShape(0),
                 m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
 …
                 m_friction(btScalar(0.5)),
                 m_restitution(btScalar(0.)),
+                m_internalType(CO_COLLISION_OBJECT),
                 m_userObjectPointer(0),
-                m_internalType(CO_COLLISION_OBJECT),
                 m_hitFraction(btScalar(1.)),
                 m_ccdSweptSphereRadius(btScalar(0.)),
 …
                 m_checkCollideWith(false)
+{
+        m_worldTransform.setIdentity();
+}
 …
+}
+const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer;
+        m_worldTransform.serialize(dataOut->m_worldTransform);
+        m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform);
+        m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity);
+        m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity);
+        m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction);
+        dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction;
+        dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold;
+        dataOut->m_broadphaseHandle = 0;
+        dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape);
+        dataOut->m_rootCollisionShape = 0;//@todo
+        dataOut->m_collisionFlags = m_collisionFlags;
+        dataOut->m_islandTag1 = m_islandTag1;
+        dataOut->m_companionId = m_companionId;
+        dataOut->m_activationState1 = m_activationState1;
+        dataOut->m_activationState1 = m_activationState1;
+        dataOut->m_deactivationTime = m_deactivationTime;
+        dataOut->m_friction = m_friction;
+        dataOut->m_restitution = m_restitution;
+        dataOut->m_internalType = m_internalType;
+        char* name = (char*) serializer->findNameForPointer(this);
+        dataOut->m_name = (char*)serializer->getUniquePointer(name);
+        if (dataOut->m_name)
+        {
+                serializer->serializeName(name);
+        }
+        dataOut->m_hitFraction = m_hitFraction;
+        dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
+        dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
+        dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
+        dataOut->m_checkCollideWith = m_checkCollideWith;
+        return btCollisionObjectDataName;
+}
+void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const
+{
+        int len = calculateSerializeBufferSize();
+        btChunk* chunk = serializer->allocate(len,1);
+        const char* structType = serialize(chunk->m_oldPtr, serializer);
+        serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this);
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h

-                      r5781
+                      r8284
 struct  btBroadphaseProxy;
 class   btCollisionShape;
+struct btCollisionShapeData;
 #include "LinearMath/btMotionState.h"
 #include "LinearMath/btAlignedAllocator.h"
 #include "LinearMath/btAlignedObjectArray.h"
 typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btCollisionObjectData btCollisionObjectDoubleData
+#define btCollisionObjectDataName "btCollisionObjectDoubleData"
+#else
+#define btCollisionObjectData btCollisionObjectFloatData
+#define btCollisionObjectDataName "btCollisionObjectFloatData"
+#endif
 …
         btVector3       m_interpolationAngularVelocity;
         btVector3               m_anisotropicFriction;
         bool                            m_hasAnisotropicFriction;
         btScalar                m_contactProcessingThreshold;
+        btVector3       m_anisotropicFriction;
+        int                     m_hasAnisotropicFriction;
+        btScalar        m_contactProcessingThreshold;
         btBroadphaseProxy*              m_broadphaseHandle;
         btCollisionShape*               m_collisionShape;
+        ///m_extensionPointer is used by some internal low-level Bullet extensions.
+        void*                                   m_extensionPointer;
         ///m_rootCollisionShape is temporarily used to store the original collision shape
 …
         btScalar                m_restitution;
-        ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
-        void*                   m_userObjectPointer;
         ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
         ///do not assign your own m_internalType unless you write a new dynamics object class.
         int                             m_internalType;
+        ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+        void*                   m_userObjectPointer;
         ///time of impact calculation
         btScalar                m_hitFraction;
 …
         /// If some object should have elaborate collision filtering by sub-classes
+        bool                    m_checkCollideWith;
+        char    m_pad[7];
+        int                     m_checkCollideWith;
         virtual bool    checkCollideWithOverride(btCollisionObject* /* co */)
 …
                 CF_NO_CONTACT_RESPONSE = 4,
                 CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
+                CF_CHARACTER_OBJECT = 16
+                CF_CHARACTER_OBJECT = 16,
+                CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
+                CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
         };
 …
+        {
                 CO_COLLISION_OBJECT =1,
                 CO_RIGID_BODY,
+                CO_RIGID_BODY=2,
                 ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
                 ///It is useful for collision sensors, explosion objects, character controller etc.
+                CO_GHOST_OBJECT,
+                CO_SOFT_BODY,
+                CO_HF_FLUID
+                CO_GHOST_OBJECT=4,
+                CO_SOFT_BODY=8,
+                CO_HF_FLUID=16,
+                CO_USER_TYPE=32
         };
 …
         bool    hasAnisotropicFriction() const
+        {
                 return m_hasAnisotropicFriction;
+                return m_hasAnisotropicFriction!=0;
+        }
 …
+        }
+        ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions.
+        ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+        void*           internalGetExtensionPointer() const
+        {
+                return m_extensionPointer;
+        }
+        ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions
+        ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+        void    internalSetExtensionPointer(void* pointer)
+        {
+                m_extensionPointer = pointer;
+        }
         SIMD_FORCE_INLINE       int     getActivationState() const { return m_activationState1;}
 …
         void    setCcdMotionThreshold(btScalar ccdMotionThreshold)
+        {
                 m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold;
+                m_ccdMotionThreshold = ccdMotionThreshold;
+        }
 …
                 return true;
+        }
+        virtual int     calculateSerializeBufferSize()  const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, class btSerializer* serializer) const;
+        virtual void serializeSingleObject(class btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCollisionObjectDoubleData
+{
+        void                                    *m_broadphaseHandle;
+        void                                    *m_collisionShape;
+        btCollisionShapeData    *m_rootCollisionShape;
+        char                                    *m_name;
+        btTransformDoubleData   m_worldTransform;
+        btTransformDoubleData   m_interpolationWorldTransform;
+        btVector3DoubleData             m_interpolationLinearVelocity;
+        btVector3DoubleData             m_interpolationAngularVelocity;
+        btVector3DoubleData             m_anisotropicFriction;
+        double                                  m_contactProcessingThreshold;
+        double                                  m_deactivationTime;
+        double                                  m_friction;
+        double                                  m_restitution;
+        double                                  m_hitFraction;
+        double                                  m_ccdSweptSphereRadius;
+        double                                  m_ccdMotionThreshold;
+        int                                             m_hasAnisotropicFriction;
+        int                                             m_collisionFlags;
+        int                                             m_islandTag1;
+        int                                             m_companionId;
+        int                                             m_activationState1;
+        int                                             m_internalType;
+        int                                             m_checkCollideWith;
+        char    m_padding[4];
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCollisionObjectFloatData
+{
+        void                                    *m_broadphaseHandle;
+        void                                    *m_collisionShape;
+        btCollisionShapeData    *m_rootCollisionShape;
+        char                                    *m_name;
+        btTransformFloatData    m_worldTransform;
+        btTransformFloatData    m_interpolationWorldTransform;
+        btVector3FloatData              m_interpolationLinearVelocity;
+        btVector3FloatData              m_interpolationAngularVelocity;
+        btVector3FloatData              m_anisotropicFriction;
+        float                                   m_contactProcessingThreshold;
+        float                                   m_deactivationTime;
+        float                                   m_friction;
+        float                                   m_restitution;
+        float                                   m_hitFraction;
+        float                                   m_ccdSweptSphereRadius;
+        float                                   m_ccdMotionThreshold;
+        int                                             m_hasAnisotropicFriction;
+        int                                             m_collisionFlags;
+        int                                             m_islandTag1;
+        int                                             m_companionId;
+        int                                             m_activationState1;
+        int                                             m_internalType;
+        int                                             m_checkCollideWith;
+};
+SIMD_FORCE_INLINE       int     btCollisionObject::calculateSerializeBufferSize() const
+{
+        return sizeof(btCollisionObjectData);
+}
 #endif //COLLISION_OBJECT_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

-                      r5781
+                      r8284
 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 #include "LinearMath/btAabbUtil2.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btSerializer.h"
 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
 …
+///for debug drawing
+//for debug rendering
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
 :m_dispatcher1(dispatcher),
 m_broadphasePairCache(pairCache),
+m_debugDrawer(0)
+m_debugDrawer(0),
+m_forceUpdateAllAabbs(true)
+{
         m_stackAlloc = collisionConfiguration->getStackAllocator();
 …
+{
+        btAssert(collisionObject);
         //check that the object isn't already added
                 btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
                 m_collisionObjects.push_back(collisionObject);
                 //calculate new AABB
                 btTransform trans = collisionObject->getWorldTransform();
                 btVector3       minAabb;
                 btVector3       maxAabb;
                 collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
                 int type = collisionObject->getCollisionShape()->getShapeType();
                 collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
                         minAabb,
                         maxAabb,
                         type,
                         collisionObject,
                         collisionFilterGroup,
                         collisionFilterMask,
                         m_dispatcher1,0
                         ))      ;
+        btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+        m_collisionObjects.push_back(collisionObject);
+        //calculate new AABB
+        btTransform trans = collisionObject->getWorldTransform();
+        btVector3       minAabb;
+        btVector3       maxAabb;
+        collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+        int type = collisionObject->getCollisionShape()->getShapeType();
+        collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+                minAabb,
+                maxAabb,
+                type,
+                collisionObject,
+                collisionFilterGroup,
+                collisionFilterMask,
+                m_dispatcher1,0
+                ))      ;
 …
                 //only update aabb of active objects
                 if (colObj->isActive())
+                if (m_forceUpdateAllAabbs || colObj->isActive())
+                {
                         updateSingleAabb(colObj);
 …
 void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
                                           btCollisionObject* collisionObject,
                                           const btCollisionShape* collisionShape,
                                           const btTransform& colObjWorldTransform,
                                           RayResultCallback& resultCallback)
+                                                                                btCollisionObject* collisionObject,
+                                                                                const btCollisionShape* collisionShape,
+                                                                                const btTransform& colObjWorldTransform,
+                                                                                RayResultCallback& resultCallback)
+{
         btSphereShape pointShape(btScalar(0.0));
 …
         if (collisionShape->isConvex())
+        {
 //              BT_PROFILE("rayTestConvex");
+                //              BT_PROFILE("rayTestConvex");
                 btConvexCast::CastResult castResult;
                 castResult.m_fraction = resultCallback.m_closestHitFraction;
 …
                                         btCollisionWorld::LocalRayResult localRayResult
+                                                (
                                                         collisionObject,
 ,
                                                         castResult.m_normal,
                                                         castResult.m_fraction
+                                                collisionObject,
+,
+                                                castResult.m_normal,
+                                                castResult.m_fraction
                                                 );
 …
                 if (collisionShape->isConcave())
+                {
 //                      BT_PROFILE("rayTestConcave");
+                        //                      BT_PROFILE("rayTestConcave");
                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+                        {
 …
                                         btTriangleMeshShape*    m_triangleMesh;
+                                        btTransform m_colObjWorldTransform;
                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
+                  //@BP Mod
+                                                btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+                                                        m_resultCallback(resultCallback),
+                                                        m_collisionObject(collisionObject),
+                                                        m_triangleMesh(triangleMesh)
+                                                {
+                                                }
+                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh,const btTransform& colObjWorldTransform):
+                                        //@BP Mod
+                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh),
+                                                m_colObjWorldTransform(colObjWorldTransform)
+                                        {
+                                        }
 …
                                                 shapeInfo.m_triangleIndex = triangleIndex;
+                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
                                                 btCollisionWorld::LocalRayResult rayResult
                                                 (m_collisionObject,
+                                                        (m_collisionObject,
                                                         &shapeInfo,
                                                         hitNormalLocal,
+                                                        hitNormalWorld,
                                                         hitFraction);
                                                 bool    normalInWorldSpace = false;
+                                                bool    normalInWorldSpace = true;
                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+                                        }
 …
                                 };
                                 BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
+                                BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
                                 triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
 …
                                         btConcaveShape* m_triangleMesh;
+                                        btTransform m_colObjWorldTransform;
                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
+                  //@BP Mod
+                  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+                                                        m_resultCallback(resultCallback),
+                                                        m_collisionObject(collisionObject),
+                                                        m_triangleMesh(triangleMesh)
+                                                {
+                                                }
+                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
+                                        //@BP Mod
+                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh),
+                                                m_colObjWorldTransform(colObjWorldTransform)
+                                        {
+                                        }
 …
                                                 shapeInfo.m_triangleIndex = triangleIndex;
+                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
                                                 btCollisionWorld::LocalRayResult rayResult
                                                 (m_collisionObject,
+                                                        (m_collisionObject,
                                                         &shapeInfo,
                                                         hitNormalLocal,
+                                                        hitNormalWorld,
                                                         hitFraction);
                                                 bool    normalInWorldSpace = false;
+                                                bool    normalInWorldSpace = true;
                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+                                        }
 …
                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
+                                BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 …
+                        }
                 } else {
 //                      BT_PROFILE("rayTestCompound");
+                        //                      BT_PROFILE("rayTestCompound");
                         ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
                         if (collisionShape->isCompound())
 …
                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
                                         collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+                    struct LocalInfoAdder2 : public RayResultCallback {
+                                                RayResultCallback* m_userCallback;
+                                                int m_i;
+                        LocalInfoAdder2 (int i, RayResultCallback *user)
+                                                        : m_userCallback(user),
+                                                        m_i(i)
+                                                {
+                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                }
+                                                virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
+                            {
+                                    btCollisionWorld::LocalShapeInfo    shapeInfo;
+                                    shapeInfo.m_shapePart = -1;
+                                    shapeInfo.m_triangleIndex = m_i;
+                                    if (r.m_localShapeInfo == NULL)
+                                        r.m_localShapeInfo = &shapeInfo;
+                                                                        const btScalar result = m_userCallback->addSingleResult(r, b);
+                                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                                        return result;
+                            }
+                    };
+                    LocalInfoAdder2 my_cb(i, &resultCallback);
                                         rayTestSingle(rayFromTrans,rayToTrans,
                                                 collisionObject,
                                                 childCollisionShape,
                                                 childWorldTrans,
                                                 resultCallback);
+                                                my_cb);
                                         // restore
                                         collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
 …
 void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
                                           btCollisionObject* collisionObject,
                                           const btCollisionShape* collisionShape,
                                           const btTransform& colObjWorldTransform,
                                           ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+                                                                                        btCollisionObject* collisionObject,
+                                                                                        const btCollisionShape* collisionShape,
+                                                                                        const btTransform& colObjWorldTransform,
+                                                                                        ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+{
         if (collisionShape->isConvex())
 …
                 btVoronoiSimplexSolver  simplexSolver;
                 btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
                 btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
                 //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
 …
                 btConvexCast* castPtr = &convexCaster1;
                 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+                {
 …
                                         castResult.m_normal.normalize();
                                         btCollisionWorld::LocalConvexResult localConvexResult
+                                                                (
                                                                         collisionObject,
 ,
                                                                         castResult.m_normal,
                                                                         castResult.m_hitPoint,
                                                                         castResult.m_fraction
                                                                 );
+                                                (
+                                                collisionObject,
+,
+                                                castResult.m_normal,
+                                                castResult.m_hitPoint,
+                                                castResult.m_fraction
+                                                );
                                         bool normalInWorldSpace = true;
 …
                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
                                                 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
                                                         m_resultCallback(resultCallback),
                                                         m_collisionObject(collisionObject),
                                                         m_triangleMesh(triangleMesh)
+                                                {
+                                                }
+                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh)
+                                        {
+                                        }
 …
                                                         btCollisionWorld::LocalConvexResult convexResult
                                                         (m_collisionObject,
+                                                                (m_collisionObject,
                                                                 &shapeInfo,
                                                                 hitNormalLocal,
 …
                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*      triangleMesh, const btTransform& triangleToWorld):
                                                 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
                                                         m_resultCallback(resultCallback),
                                                         m_collisionObject(collisionObject),
                                                         m_triangleMesh(triangleMesh)
+                                                {
+                                                }
+                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh)
+                                        {
+                                        }
 …
                                                         btCollisionWorld::LocalConvexResult convexResult
                                                         (m_collisionObject,
+                                                                (m_collisionObject,
                                                                 &shapeInfo,
                                                                 hitNormalLocal,
 …
                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
                                         collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+                    struct      LocalInfoAdder : public ConvexResultCallback {
+                            ConvexResultCallback* m_userCallback;
+                                                        int m_i;
+                            LocalInfoAdder (int i, ConvexResultCallback *user)
+                                                                : m_userCallback(user), m_i(i)
+                                                        {
+                                                                m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                        }
+                            virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&      r,      bool b)
+                            {
+                                    btCollisionWorld::LocalShapeInfo    shapeInfo;
+                                    shapeInfo.m_shapePart = -1;
+                                    shapeInfo.m_triangleIndex = m_i;
+                                    if (r.m_localShapeInfo == NULL)
+                                        r.m_localShapeInfo = &shapeInfo;
+                                                                        const btScalar result = m_userCallback->addSingleResult(r, b);
+                                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                                        return result;
+                            }
+                    };
+                    LocalInfoAdder my_cb(i, &resultCallback);
                                         objectQuerySingle(castShape, convexFromTrans,convexToTrans,
                                                 collisionObject,
                                                 childCollisionShape,
                                                 childWorldTrans,
                                                 resultCallback, allowedPenetration);
+                                                my_cb, allowedPenetration);
                                         // restore
                                         collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
 …
         btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
         :m_rayFromWorld(rayFromWorld),
         m_rayToWorld(rayToWorld),
         m_world(world),
         m_resultCallback(resultCallback)
+                :m_rayFromWorld(rayFromWorld),
+                m_rayToWorld(rayToWorld),
+                m_world(world),
+                m_resultCallback(resultCallback)
+        {
                 m_rayFromTrans.setIdentity();
 …
                 rayDir.normalize ();
                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 …
+        }
         virtual bool    process(const btBroadphaseProxy* proxy)
 …
                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
                                         collisionObject,
                                                 collisionObject->getCollisionShape(),
                                                 collisionObject->getWorldTransform(),
                                                 m_resultCallback);
+                                        collisionObject->getCollisionShape(),
+                                        collisionObject->getWorldTransform(),
+                                        m_resultCallback);
+                        }
+                }
 …
 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+{
         BT_PROFILE("rayTest");
+        //BT_PROFILE("rayTest");
         /// use the broadphase to accelerate the search for objects, based on their aabb
         /// and for each object with ray-aabb overlap, perform an exact ray test
 …
                 btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
                 btVector3 rayDir = unnormalizedRayDir.normalized();
                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 …
                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                         m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
                                         collisionObject,
                                                 collisionObject->getCollisionShape(),
                                                 collisionObject->getWorldTransform(),
                                                 m_resultCallback,
                                                 m_allowedCcdPenetration);
+                }
+                                collisionObject,
+                                collisionObject->getCollisionShape(),
+                                collisionObject->getWorldTransform(),
+                                m_resultCallback,
+                                m_allowedCcdPenetration);
+                }
                 return true;
+        }
 …
         /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
         btTransform     convexFromTrans,convexToTrans;
 …
                                 objectQuerySingle(castShape, convexFromTrans,convexToTrans,
                                         collisionObject,
                                                 collisionObject->getCollisionShape(),
                                                 collisionObject->getWorldTransform(),
                                                 resultCallback,
                                                 allowedCcdPenetration);
+                                        collisionObject->getCollisionShape(),
+                                        collisionObject->getWorldTransform(),
+                                        resultCallback,
+                                        allowedCcdPenetration);
+                        }
+                }
 …
 #endif //USE_BRUTEFORCE_RAYBROADPHASE
+}
+struct btBridgedManifoldResult : public btManifoldResult
+{
+        btCollisionWorld::ContactResultCallback&        m_resultCallback;
+        btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
+                :btManifoldResult(obj0,obj1),
+                m_resultCallback(resultCallback)
+        {
+        }
+        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+        {
+                bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+                btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+                btVector3 localA;
+                btVector3 localB;
+                if (isSwapped)
+                {
+                        localA = m_rootTransB.invXform(pointA );
+                        localB = m_rootTransA.invXform(pointInWorld);
+                } else
+                {
+                        localA = m_rootTransA.invXform(pointA );
+                        localB = m_rootTransB.invXform(pointInWorld);
+                }
+                btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+                newPt.m_positionWorldOnA = pointA;
+                newPt.m_positionWorldOnB = pointInWorld;
+           //BP mod, store contact triangles.
+                if (isSwapped)
+                {
+                        newPt.m_partId0 = m_partId1;
+                        newPt.m_partId1 = m_partId0;
+                        newPt.m_index0  = m_index1;
+                        newPt.m_index1  = m_index0;
+                } else
+                {
+                        newPt.m_partId0 = m_partId0;
+                        newPt.m_partId1 = m_partId1;
+                        newPt.m_index0  = m_index0;
+                        newPt.m_index1  = m_index1;
+                }
+                //experimental feature info, for per-triangle material etc.
+                btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
+                btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
+                m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
+        }
+};
+struct btSingleContactCallback : public btBroadphaseAabbCallback
+{
+        btCollisionObject* m_collisionObject;
+        btCollisionWorld*       m_world;
+        btCollisionWorld::ContactResultCallback&        m_resultCallback;
+        btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
+                :m_collisionObject(collisionObject),
+                m_world(world),
+                m_resultCallback(resultCallback)
+        {
+        }
+        virtual bool    process(const btBroadphaseProxy* proxy)
+        {
+                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
+                if (collisionObject == m_collisionObject)
+                        return true;
+                //only perform raycast if filterMask matches
+                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+                {
+                        btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
+                        if (algorithm)
+                        {
+                                btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
+                                //discrete collision detection query
+                                algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
+                                algorithm->~btCollisionAlgorithm();
+                                m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
+                        }
+                }
+                return true;
+        }
+};
+///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
+///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
+void    btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
+{
+        btVector3 aabbMin,aabbMax;
+        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
+        btSingleContactCallback contactCB(colObj,this,resultCallback);
+        m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
+}
+///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
+///it reports one or more contact points (including the one with deepest penetration)
+void    btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
+{
+        btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
+        if (algorithm)
+        {
+                btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
+                //discrete collision detection query
+                algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
+                algorithm->~btCollisionAlgorithm();
+                getDispatcher()->freeCollisionAlgorithm(algorithm);
+        }
+}
+class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
+{
+        btIDebugDraw*   m_debugDrawer;
+        btVector3       m_color;
+        btTransform     m_worldTrans;
+public:
+        DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
+          m_debugDrawer(debugDrawer),
+                  m_color(color),
+                  m_worldTrans(worldTrans)
+          {
+          }
+          virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+          {
+                  processTriangle(triangle,partId,triangleIndex);
+          }
+          virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
+          {
+                  (void)partId;
+                  (void)triangleIndex;
+                  btVector3 wv0,wv1,wv2;
+                  wv0 = m_worldTrans*triangle[0];
+                  wv1 = m_worldTrans*triangle[1];
+                  wv2 = m_worldTrans*triangle[2];
+                  btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
+                  btVector3 normal = (wv1-wv0).cross(wv2-wv0);
+                  normal.normalize();
+                  btVector3 normalColor(1,1,0);
+                  m_debugDrawer->drawLine(center,center+normal,normalColor);
+                  m_debugDrawer->drawLine(wv0,wv1,m_color);
+                  m_debugDrawer->drawLine(wv1,wv2,m_color);
+                  m_debugDrawer->drawLine(wv2,wv0,m_color);
+          }
+};
+void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
+        // Draw a small simplex at the center of the object
+        getDebugDrawer()->drawTransform(worldTransform,1);
+        if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+        {
+                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
+                for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+                {
+                        btTransform childTrans = compoundShape->getChildTransform(i);
+                        const btCollisionShape* colShape = compoundShape->getChildShape(i);
+                        debugDrawObject(worldTransform*childTrans,colShape,color);
+                }
+        } else
+        {
+                switch (shape->getShapeType())
+                {
+                case BOX_SHAPE_PROXYTYPE:
+                        {
+                                const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
+                                btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
+                                getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
+                                break;
+                        }
+                case SPHERE_SHAPE_PROXYTYPE:
+                        {
+                                const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
+                                btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
+                                getDebugDrawer()->drawSphere(radius, worldTransform, color);
+                                break;
+                        }
+                case MULTI_SPHERE_SHAPE_PROXYTYPE:
+                        {
+                                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+                                btTransform childTransform;
+                                childTransform.setIdentity();
+                                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+                                {
+                                        childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
+                                        getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
+                                }
+                                break;
+                        }
+                case CAPSULE_SHAPE_PROXYTYPE:
+                        {
+                                const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+                                btScalar radius = capsuleShape->getRadius();
+                                btScalar halfHeight = capsuleShape->getHalfHeight();
+                                int upAxis = capsuleShape->getUpAxis();
+                                btVector3 capStart(0.f,0.f,0.f);
+                                capStart[upAxis] = -halfHeight;
+                                btVector3 capEnd(0.f,0.f,0.f);
+                                capEnd[upAxis] = halfHeight;
+                                // Draw the ends
+                                {
+                                        btTransform childTransform = worldTransform;
+                                        childTransform.getOrigin() = worldTransform * capStart;
+                                        getDebugDrawer()->drawSphere(radius, childTransform, color);
+                                }
+                                {
+                                        btTransform childTransform = worldTransform;
+                                        childTransform.getOrigin() = worldTransform * capEnd;
+                                        getDebugDrawer()->drawSphere(radius, childTransform, color);
+                                }
+                                // Draw some additional lines
+                                btVector3 start = worldTransform.getOrigin();
+                                capStart[(upAxis+1)%3] = radius;
+                                capEnd[(upAxis+1)%3] = radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+                                capStart[(upAxis+1)%3] = -radius;
+                                capEnd[(upAxis+1)%3] = -radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+                                capStart[(upAxis+1)%3] = 0.f;
+                                capEnd[(upAxis+1)%3] = 0.f;
+                                capStart[(upAxis+2)%3] = radius;
+                                capEnd[(upAxis+2)%3] = radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+                                capStart[(upAxis+2)%3] = -radius;
+                                capEnd[(upAxis+2)%3] = -radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+                                break;
+                        }
+                case CONE_SHAPE_PROXYTYPE:
+                        {
+                                const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+                                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
+                                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
+                                btVector3 start = worldTransform.getOrigin();
+                                int upAxis= coneShape->getConeUpIndex();
+                                btVector3       offsetHeight(0,0,0);
+                                offsetHeight[upAxis] = height * btScalar(0.5);
+                                btVector3       offsetRadius(0,0,0);
+                                offsetRadius[(upAxis+1)%3] = radius;
+                                btVector3       offset2Radius(0,0,0);
+                                offset2Radius[(upAxis+2)%3] = radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+                                // Drawing the base of the cone
+                                btVector3 yaxis(0,0,0);
+                                yaxis[upAxis] = btScalar(1.0);
+                                btVector3 xaxis(0,0,0);
+                                xaxis[(upAxis+1)%3] = btScalar(1.0);
+                                getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
+                break;
+                        }
+                case CYLINDER_SHAPE_PROXYTYPE:
+                        {
+                                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+                                int upAxis = cylinder->getUpAxis();
+                                btScalar radius = cylinder->getRadius();
+                                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
+                                btVector3 start = worldTransform.getOrigin();
+                                btVector3       offsetHeight(0,0,0);
+                                offsetHeight[upAxis] = halfHeight;
+                                btVector3       offsetRadius(0,0,0);
+                                offsetRadius[(upAxis+1)%3] = radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+                                // Drawing top and bottom caps of the cylinder
+                                btVector3 yaxis(0,0,0);
+                                yaxis[upAxis] = btScalar(1.0);
+                                btVector3 xaxis(0,0,0);
+                                xaxis[(upAxis+1)%3] = btScalar(1.0);
+                                getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+                                getDebugDrawer()->drawArc(start+worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+                                break;
+                        }
+                case STATIC_PLANE_PROXYTYPE:
+                        {
+                                const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+                                btScalar planeConst = staticPlaneShape->getPlaneConstant();
+                                const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+                                btVector3 planeOrigin = planeNormal * planeConst;
+                                btVector3 vec0,vec1;
+                                btPlaneSpace1(planeNormal,vec0,vec1);
+                                btScalar vecLen = 100.f;
+                                btVector3 pt0 = planeOrigin + vec0*vecLen;
+                                btVector3 pt1 = planeOrigin - vec0*vecLen;
+                                btVector3 pt2 = planeOrigin + vec1*vecLen;
+                                btVector3 pt3 = planeOrigin - vec1*vecLen;
+                                getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
+                                getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+                                break;
+                        }
+                default:
+                        {
+                                if (shape->isConcave())
+                                {
+                                        btConcaveShape* concaveMesh = (btConcaveShape*) shape;
+                                        ///@todo pass camera, for some culling? no -> we are not a graphics lib
+                                        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                                        btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+                                        concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+                                }
+                                if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+                                {
+                                        btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
+                                        //todo: pass camera for some culling
+                                        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                                        btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+                                        //DebugDrawcallback drawCallback;
+                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+                                        convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+                                }
+                                /// for polyhedral shapes
+                                if (shape->isPolyhedral())
+                                {
+                                        btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+                                        int i;
+                                        for (i=0;i<polyshape->getNumEdges();i++)
+                                        {
+                                                btVector3 a,b;
+                                                polyshape->getEdge(i,a,b);
+                                                btVector3 wa = worldTransform * a;
+                                                btVector3 wb = worldTransform * b;
+                                                getDebugDrawer()->drawLine(wa,wb,color);
+                                        }
+                                }
+                        }
+                }
+        }
+}
+void    btCollisionWorld::debugDrawWorld()
+{
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+        {
+                int numManifolds = getDispatcher()->getNumManifolds();
+                btVector3 color(0,0,0);
+                for (int i=0;i<numManifolds;i++)
+                {
+                        btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+                        //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+                        //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+                        int numContacts = contactManifold->getNumContacts();
+                        for (int j=0;j<numContacts;j++)
+                        {
+                                btManifoldPoint& cp = contactManifold->getContactPoint(j);
+                                getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+                        }
+                }
+        }
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
+        {
+                int i;
+                for (  i=0;i<m_collisionObjects.size();i++)
+                {
+                        btCollisionObject* colObj = m_collisionObjects[i];
+                        if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
+                        {
+                                if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+                                {
+                                        btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
+                                        switch(colObj->getActivationState())
+                                        {
+                                        case  ACTIVE_TAG:
+                                                color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
+                                        case ISLAND_SLEEPING:
+                                                color =  btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
+                                        case WANTS_DEACTIVATION:
+                                                color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
+                                        case DISABLE_DEACTIVATION:
+                                                color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
+                                        case DISABLE_SIMULATION:
+                                                color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
+                                        default:
+                                                {
+                                                        color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
+                                                }
+                                        };
+                                        debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+                                }
+                                if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+                                {
+                                        btVector3 minAabb,maxAabb;
+                                        btVector3 colorvec(1,0,0);
+                                        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+                                        m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+                                }
+                        }
+                }
+        }
+}
+void    btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
+{
+        int i;
+        //serialize all collision objects
+        for (i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
+                {
+                        colObj->serializeSingleObject(serializer);
+                }
+        }
+        ///keep track of shapes already serialized
+        btHashMap<btHashPtr,btCollisionShape*>  serializedShapes;
+        for (i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btCollisionShape* shape = colObj->getCollisionShape();
+                if (!serializedShapes.find(shape))
+                {
+                        serializedShapes.insert(shape,shape);
+                        shape->serializeSingleShape(serializer);
+                }
+        }
+}
+void    btCollisionWorld::serialize(btSerializer* serializer)
+{
+        serializer->startSerialization();
+        serializeCollisionObjects(serializer);
+        serializer->finishSerialization();
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h

-                      r5781
+                      r8284
  * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
+ *
+ * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution.
  * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
  * Please visit http://www.bulletphysics.com
 …
  * @subsection step1 Step 1: Download
  * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
+ *
  * @subsection step2 Step 2: Building
+ * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8.
+ * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version).
+ *
+ * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet.
+ * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam.
+ * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files.
+ * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam
+ * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org
+ * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles.
+ * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles.
+ * You can also use cmake in the command-line. Here are some examples for various platforms:
+ * cmake . -G "Visual Studio 9 2008"
+ * cmake . -G Xcode
+ * cmake . -G "Unix Makefiles"
+ * Although cmake is recommended, you can also use autotools for UNIX: ./autogen.sh ./configure to create a Makefile and then run make.
+ *
  * @subsection step3 Step 3: Testing demos
 …
+ *
  * @section copyright Copyright
+ * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon
+ * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky,
+ * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt.
+ * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf
+ *
  */
 …
 class btConvexShape;
 class btBroadphaseInterface;
+class btSerializer;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
 …
+{
 protected:
         btAlignedObjectArray<btCollisionObject*>        m_collisionObjects;
         btDispatcher*   m_dispatcher1;
 …
         btIDebugDraw*   m_debugDrawer;
+        ///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs
+        ///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB)
+        bool m_forceUpdateAllAabbs;
+        void    serializeCollisionObjects(btSerializer* serializer);
 public:
 …
         virtual void    updateAabbs();
         virtual void    setDebugDrawer(btIDebugDraw*    debugDrawer)
 …
                 return m_debugDrawer;
+        }
+        virtual void    debugDrawWorld();
+        virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 …
                 int     m_shapePart;
                 int     m_triangleIndex;
                 //const btCollisionShape*       m_shapeTemp;
 …
                 btVector3       m_hitNormalWorld;
                 btVector3       m_hitPointWorld;
                 virtual btScalar        addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
 …
                         //caller already does the filter on the m_closestHitFraction
                         btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
                         m_closestHitFraction = rayResult.m_hitFraction;
 …
         };
+        struct  AllHitsRayResultCallback : public RayResultCallback
+        {
+                AllHitsRayResultCallback(const btVector3&       rayFromWorld,const btVector3&   rayToWorld)
+                :m_rayFromWorld(rayFromWorld),
+                m_rayToWorld(rayToWorld)
+                {
+                }
+                btAlignedObjectArray<btCollisionObject*>                m_collisionObjects;
+                btVector3       m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+                btVector3       m_rayToWorld;
+                btAlignedObjectArray<btVector3> m_hitNormalWorld;
+                btAlignedObjectArray<btVector3> m_hitPointWorld;
+                btAlignedObjectArray<btScalar> m_hitFractions;
+                virtual btScalar        addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+                {
+                        m_collisionObject = rayResult.m_collisionObject;
+                        m_collisionObjects.push_back(rayResult.m_collisionObject);
+                        btVector3 hitNormalWorld;
+                        if (normalInWorldSpace)
+                        {
+                                hitNormalWorld = rayResult.m_hitNormalLocal;
+                        } else
+                        {
+                                ///need to transform normal into worldspace
+                                hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+                        }
+                        m_hitNormalWorld.push_back(hitNormalWorld);
+                        btVector3 hitPointWorld;
+                        hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+                        m_hitPointWorld.push_back(hitPointWorld);
+                        m_hitFractions.push_back(rayResult.m_hitFraction);
+                        return m_closestHitFraction;
+                }
+        };
         struct LocalConvexResult
 …
                 short int       m_collisionFilterGroup;
                 short int       m_collisionFilterMask;
                 ConvexResultCallback()
 …
+                {
+                }
                 bool    hasHit() const
 …
                         return (m_closestHitFraction < btScalar(1.));
+                }
 …
                 btVector3       m_hitPointWorld;
                 btCollisionObject*      m_hitCollisionObject;
                 virtual btScalar        addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
 …
 //caller already does the filter on the m_closestHitFraction
                         btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
                         m_closestHitFraction = convexResult.m_hitFraction;
 …
         };
+        ///ContactResultCallback is used to report contact points
+        struct  ContactResultCallback
+        {
+                short int       m_collisionFilterGroup;
+                short int       m_collisionFilterMask;
+                ContactResultCallback()
+                        :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+                        m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+                {
+                }
+                virtual ~ContactResultCallback()
+                {
+                }
+                virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+                {
+                        bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+                        collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+                        return collides;
+                }
+                virtual btScalar        addSingleResult(btManifoldPoint& cp,    const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) = 0;
+        };
         int     getNumCollisionObjects() const
+        {
 …
         /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
         /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
         void    rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
         // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
         // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
+        virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
+        /// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
+        /// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
         void    convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback,  btScalar allowedCcdPenetration = btScalar(0.)) const;
+        ///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback.
+        ///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
+        void    contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
+        ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
+        ///it reports one or more contact points (including the one with deepest penetration)
+        void    contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
 …
                                           ConvexResultCallback& resultCallback, btScalar        allowedPenetration);
         void    addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+        virtual void    addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
         btCollisionObjectArray& getCollisionObjectArray()
 …
         void    removeCollisionObject(btCollisionObject* collisionObject);
+        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
         virtual void    performDiscreteCollisionDetection();
 …
                 return m_dispatchInfo;
+        }
+        bool    getForceUpdateAllAabbs() const
+        {
+                return m_forceUpdateAllAabbs;
+        }
+        void setForceUpdateAllAabbs( bool forceUpdateAllAabbs)
+        {
+                m_forceUpdateAllAabbs = forceUpdateAllAabbs;
+        }
+        ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo)
+        virtual void    serialize(btSerializer* serializer);
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

-                      r5781
+                      r8284
         void    ProcessChildShape(btCollisionShape* childShape,int index)
+        {
+                btAssert(index>=0);
                 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+                btAssert(index<compoundShape->getNumChildShapes());
 …
                         if (!m_childCollisionAlgorithms[index])
                                 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
+                        ///detect swapping case
+                        if (m_resultOut->getBody0Internal() == m_compoundColObj)
+                        {
+                                m_resultOut->setShapeIdentifiersA(-1,index);
+                        } else
+                        {
+                                m_resultOut->setShapeIdentifiersB(-1,index);
+                        }
                         m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
 …
                 int i;
                 btManifoldArray manifoldArray;
+        btCollisionShape* childShape = 0;
+        btTransform     orgTrans;
+        btTransform     orgInterpolationTrans;
+        btTransform     newChildWorldTrans;
+        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
                 for (i=0;i<numChildren;i++)
+                {
                         if (m_childCollisionAlgorithms[i])
+                        {
                                 btCollisionShape* childShape = compoundShape->getChildShape(i);
+                                childShape = compoundShape->getChildShape(i);
                         //if not longer overlapping, remove the algorithm
                                 btTransform     orgTrans = colObj->getWorldTransform();
                                 btTransform     orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+                orgTrans = colObj->getWorldTransform();
+                orgInterpolationTrans = colObj->getInterpolationWorldTransform();
                                 const btTransform& childTrans = compoundShape->getChildTransform(i);
                                 btTransform     newChildWorldTrans = orgTrans*childTrans ;
+                newChildWorldTrans = orgTrans*childTrans ;
                                 //perform an AABB check first
-                                btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
                                 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
                                 otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
 …
                                         m_childCollisionAlgorithms[i] = 0;
+                                }
+                        }
+                }
+                }
+        }
+}
 …
         int numChildren = m_childCollisionAlgorithms.size();
         int i;
+    btTransform orgTrans;
+    btScalar frac;
         for (i=0;i<numChildren;i++)
+        {
 …
                 //backup
                 btTransform     orgTrans = colObj->getWorldTransform();
+        orgTrans = colObj->getWorldTransform();
                 const btTransform& childTrans = compoundShape->getChildTransform(i);
 …
                 btCollisionShape* tmpShape = colObj->getCollisionShape();
                 colObj->internalSetTemporaryCollisionShape( childShape );
                 btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
+        frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
                 if (frac<hitFraction)
+                {

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

-                      r5781
+                      r8284
         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
+        {
                 btVector3 color(255,255,0);
+                btVector3 color(1,1,0);
                 btTransform& tr = ob->getWorldTransform();
                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
 …
                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
+                ///this should use the btDispatcher, so the actual registered algorithm is used
+                //              btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
+                m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
+        //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
+//              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+                if (m_resultOut->getBody0Internal() == m_triBody)
+                {
+                        m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
+                }
+                else
+                {
+                        m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+                }
                 colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
                 colAlgo->~btCollisionAlgorithm();

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

-                      r5781
+                      r8284
 */
+///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
+///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
+///with reproduction case
+//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
 #include "btConvexConvexAlgorithm.h"
 …
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 …
+///////////
+static SIMD_FORCE_INLINE void segmentsClosestPoints(
+        btVector3& ptsVector,
+        btVector3& offsetA,
+        btVector3& offsetB,
+        btScalar& tA, btScalar& tB,
+        const btVector3& translation,
+        const btVector3& dirA, btScalar hlenA,
+        const btVector3& dirB, btScalar hlenB )
+{
+        // compute the parameters of the closest points on each line segment
+        btScalar dirA_dot_dirB = btDot(dirA,dirB);
+        btScalar dirA_dot_trans = btDot(dirA,translation);
+        btScalar dirB_dot_trans = btDot(dirB,translation);
+        btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+        if ( denom == 0.0f ) {
+                tA = 0.0f;
+        } else {
+                tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+                if ( tA < -hlenA )
+                        tA = -hlenA;
+                else if ( tA > hlenA )
+                        tA = hlenA;
+        }
+        tB = tA * dirA_dot_dirB - dirB_dot_trans;
+        if ( tB < -hlenB ) {
+                tB = -hlenB;
+                tA = tB * dirA_dot_dirB + dirA_dot_trans;
+                if ( tA < -hlenA )
+                        tA = -hlenA;
+                else if ( tA > hlenA )
+                        tA = hlenA;
+        } else if ( tB > hlenB ) {
+                tB = hlenB;
+                tA = tB * dirA_dot_dirB + dirA_dot_trans;
+                if ( tA < -hlenA )
+                        tA = -hlenA;
+                else if ( tA > hlenA )
+                        tA = hlenA;
+        }
+        // compute the closest points relative to segment centers.
+        offsetA = dirA * tA;
+        offsetB = dirB * tB;
+        ptsVector = translation - offsetA + offsetB;
+}
+static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
+        btVector3& normalOnB,
+        btVector3& pointOnB,
+        btScalar capsuleLengthA,
+        btScalar        capsuleRadiusA,
+        btScalar capsuleLengthB,
+        btScalar        capsuleRadiusB,
+        int capsuleAxisA,
+        int capsuleAxisB,
+        const btTransform& transformA,
+        const btTransform& transformB,
+        btScalar distanceThreshold )
+{
+        btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
+        btVector3 translationA = transformA.getOrigin();
+        btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
+        btVector3 translationB = transformB.getOrigin();
+        // translation between centers
+        btVector3 translation = translationB - translationA;
+        // compute the closest points of the capsule line segments
+        btVector3 ptsVector;           // the vector between the closest points
+        btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
+        btScalar tA, tB;              // parameters on line segment
+        segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
+                                                   directionA, capsuleLengthA, directionB, capsuleLengthB );
+        btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
+        if ( distance > distanceThreshold )
+                return distance;
+        btScalar lenSqr = ptsVector.length2();
+        if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
+        {
+                //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
+                btVector3 q;
+                btPlaneSpace1(directionA,normalOnB,q);
+        } else
+        {
+                // compute the contact normal
+                normalOnB = ptsVector*-btRecipSqrt(lenSqr);
+        }
+        pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
+        return distance;
+}
+//////////
 …
 m_lowLevelOfDetail(false),
 #ifdef USE_SEPDISTANCE_UTIL2
 ,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
+m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
 #endif
 …
                 m_transformA(transformA),
                 m_transformB(transformB),
+                m_unPerturbedTransform(unPerturbedTransform),
                 m_perturbA(perturbA),
-                m_unPerturbedTransform(unPerturbedTransform),
                 m_debugDrawer(debugDrawer)
+        {
 …
 extern btScalar gContactBreakingThreshold;
 //
 // Convex-Convex collision algorithm
 …
         btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+        btVector3  normalOnB;
+                btVector3  pointOnBWorld;
+#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+        if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
+        {
+                btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
+                btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
+                btVector3 localScalingA = capsuleA->getLocalScaling();
+                btVector3 localScalingB = capsuleB->getLocalScaling();
+                btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+                btScalar dist = capsuleCapsuleDistance(normalOnB,       pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
+                        capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
+                        body0->getWorldTransform(),body1->getWorldTransform(),threshold);
+                if (dist<threshold)
+                {
+                        btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
+                        resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
+                }
+                resultOut->refreshContactPoints();
+                return;
+        }
+#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 #ifdef USE_SEPDISTANCE_UTIL2
+        m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
+        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+        {
+                m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
+        }
         if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
 #endif //USE_SEPDISTANCE_UTIL2
 …
         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+        {
                 input.m_maximumDistanceSquared = 1e30f;
+                input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
         } else
 #endif //USE_SEPDISTANCE_UTIL2
+        {
+                input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+                if (dispatchInfo.m_convexMaxDistanceUseCPT)
+                {
+                        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
+                } else
+                {
+                        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+                }
                 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+        }
 …
         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+        btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
+        //now perturbe directions to get multiple contact points
+        btVector3 v0,v1;
+        btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+        btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+#ifdef USE_SEPDISTANCE_UTIL2
+        btScalar sepDist = 0.f;
+        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+        {
+                sepDist = gjkPairDetector.getCachedSeparatingDistance();
+                if (sepDist>SIMD_EPSILON)
+                {
+                        sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
+                        //now perturbe directions to get multiple contact points
+                }
+        }
+#endif //USE_SEPDISTANCE_UTIL2
         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
         if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
+        if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
+        {
                 int i;
+                btVector3 v0,v1;
+                btVector3 sepNormalWorldSpace;
+                sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+                btPlaneSpace1(sepNormalWorldSpace,v0,v1);
                 bool perturbeA = true;
 …
                 for ( i=0;i<m_numPerturbationIterations;i++)
+                {
+                        if (v0.length2()>SIMD_EPSILON)
+                        {
                         btQuaternion perturbeRot(v0,perturbeAngle);
                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
 …
                         btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
                         gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
+                        }
+                }
+        }
 …
 #ifdef USE_SEPDISTANCE_UTIL2
         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+        if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
+        {
                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

-                      r5781
+                      r8284
 ///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util
 ///for certain pairs that have a small size ratio
+///#define USE_SEPDISTANCE_UTIL2 1
+//#define USE_SEPDISTANCE_UTIL2 1
 ///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

-                      r5781
+                      r8284
         btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
     bool hasCollision = false;
         const btVector3& planeNormal = planeShape->getPlaneNormal();
         const btScalar& planeConstant = planeShape->getPlaneConstant();
+        //const btScalar& planeConstant = planeShape->getPlaneConstant();
         //first perform a collision query with the non-perturbated collision objects

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

r5781	r8284
61	61
62	62	CreateFunc()
63		: m_numPerturbationIterations(3),
64		m_minimumPointsPerturbationThreshold(3)
	63	: m_numPerturbationIterations(1),
	64	m_minimumPointsPerturbationThreshold(1)
65	65	{
66	66	}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

-                      r5781
+                      r8284
         void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
         m_simplexSolver = new (mem)btVoronoiSimplexSolver();
+#define USE_EPA 1
+#ifdef USE_EPA
         mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
         m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
 #else
         mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
         m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+#endif//USE_EPA
+        if (constructionInfo.m_useEpaPenetrationAlgorithm)
+        {
+                mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
+                m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
+        }else
+        {
+                mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
+                m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+        }
         //default CreationFunctions, filling the m_doubleDispatch table
         mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
 …
         int sl = sizeof(btConvexSeparatingDistanceUtil);
         sl = sizeof(btGjkPairDetector);
+        int     collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2);
+        int     collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
+        collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
         collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

-                      r5781
+                      r8284
         int                                     m_defaultMaxPersistentManifoldPoolSize;
         int                                     m_defaultMaxCollisionAlgorithmPoolSize;
+        int                                     m_customCollisionAlgorithmMaxElementSize;
         int                                     m_defaultStackAllocatorSize;
+        int                                     m_useEpaPenetrationAlgorithm;
         btDefaultCollisionConstructionInfo()
 …
                 m_defaultMaxPersistentManifoldPoolSize(4096),
                 m_defaultMaxCollisionAlgorithmPoolSize(4096),
+                m_defaultStackAllocatorSize(0)
+                m_customCollisionAlgorithmMaxElementSize(0),
+                m_defaultStackAllocatorSize(0),
+                m_useEpaPenetrationAlgorithm(true)
+        {
+        }
 …
+        }
+        virtual btVoronoiSimplexSolver* getSimplexSolver()
+        {
+                return m_simplexSolver;
+        }
         virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btGhostObject.h

r5781	r8284
161	161	}
162	162
163		virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* ~~proxy0,btDispatcher* dispatcher~~)
	163	virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /proxy0/,btDispatcher* /dispatcher/)
164	164	{
165	165	btAssert(0);
…	…
173	173
174	174	#endif
	175

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp

-                      r5781
+                      r8284
                 m_body0(body0),
                 m_body1(body1)
+#ifdef DEBUG_PART_INDEX
+                ,m_partId0(-1),
+        m_partId1(-1),
+        m_index0(-1),
+        m_index1(-1)
+#endif //DEBUG_PART_INDEX
+{
         m_rootTransA = body0->getWorldTransform();
 …
    //BP mod, store contact triangles.
+   newPt.m_partId0 = m_partId0;
+   newPt.m_partId1 = m_partId1;
+   newPt.m_index0  = m_index0;
+   newPt.m_index1  = m_index1;
+        if (isSwapped)
+        {
+                newPt.m_partId0 = m_partId1;
+                newPt.m_partId1 = m_partId0;
+                newPt.m_index0  = m_index1;
+                newPt.m_index1  = m_index0;
+        } else
+        {
+                newPt.m_partId0 = m_partId0;
+                newPt.m_partId1 = m_partId1;
+                newPt.m_index0  = m_index0;
+                newPt.m_index1  = m_index1;
+        }
         //printf("depth=%f\n",depth);
         ///@todo, check this for any side effects
 …
                 btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
                 btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
                 (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
+                (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
+        }

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h

-                      r5781
+                      r8284
 extern ContactAddedCallback             gContactAddedCallback;
+//#define DEBUG_PART_INDEX 1
 …
 class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
+{
+protected:
         btPersistentManifold* m_manifoldPtr;
 …
         btManifoldResult()
+#ifdef DEBUG_PART_INDEX
+                :
+        m_partId0(-1),
+        m_partId1(-1),
+        m_index0(-1),
+        m_index1(-1)
+#endif //DEBUG_PART_INDEX
+        {
+        }
 …
+        }
         virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+        virtual void setShapeIdentifiersA(int partId0,int index0)
+        {
+                        m_partId0=partId0;
+                        m_partId1=partId1;
+                        m_index0=index0;
+                        m_index1=index1;
+                m_partId0=partId0;
+                m_index0=index0;
+        }
+        virtual void setShapeIdentifiersB(      int partId1,int index1)
+        {
+                m_partId1=partId1;
+                m_index1=index1;
+        }
 …
+        }
+        const btCollisionObject* getBody0Internal() const
+        {
+                return m_body0;
+        }
+        const btCollisionObject* getBody1Internal() const
+        {
+                return m_body1;
+        }
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 …
+        {
+                btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
+                const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
+                btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
+                for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
+                {
+                        btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
+                for (int i=0;i<numOverlappingPairs;i++)
+                {
                         const btBroadphasePair& collisionPair = pairPtr[i];
                         btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
 …
+}
+#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+        // put the index into m_controllers into m_tag
+        int index = 0;
+        {
+                int i;
+                for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+                {
+                        btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
+                        //Adding filtering here
+                        if (!collisionObject->isStaticOrKinematicObject())
+                        {
+                                collisionObject->setIslandTag(index++);
+                        }
+                        collisionObject->setCompanionId(-1);
+                        collisionObject->setHitFraction(btScalar(1.));
+                }
+        }
+        // do the union find
+        initUnionFind( index );
+        findUnions(dispatcher,colWorld);
+}
+void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+        // put the islandId ('find' value) into m_tag
+        {
+                int index = 0;
+                int i;
+                for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+                {
+                        btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+                        if (!collisionObject->isStaticOrKinematicObject())
+                        {
+                                collisionObject->setIslandTag( m_unionFind.find(index) );
+                                //Set the correct object offset in Collision Object Array
+                                m_unionFind.getElement(index).m_sz = i;
+                                collisionObject->setCompanionId(-1);
+                                index++;
+                        } else
+                        {
+                                collisionObject->setIslandTag(-1);
+                                collisionObject->setCompanionId(-2);
+                        }
+                }
+        }
+}
+#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
         initUnionFind( int (colWorld->getCollisionObjectArray().size()));
         // put the index into m_controllers into m_tag
+        {
                 int index = 0;
                 int i;
 …
                         collisionObject->setHitFraction(btScalar(1.));
                         index++;
+                }
+        }
         // do the union find
         findUnions(dispatcher,colWorld);
+}
+}
 void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
 …
         // put the islandId ('find' value) into m_tag
+        {
                 int index = 0;
                 int i;
 …
+        }
+}
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 inline  int     getIslandId(const btPersistentManifold* lhs)

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

r5781	r8284
60	60
61	61	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
62		input.m_maximumDistanceSquared = btScalar(~~1e30~~);///@todo: tighter bounds
	62	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
63	63	input.m_transformA = sphereObj->getWorldTransform();
64	64	input.m_transformB = triObj->getWorldTransform();

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp

r5781	r8284
71	71	{
72	72	m_elements[i].m_id = find(i);
	73	#ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION
73	74	m_elements[i].m_sz = i;
	75	#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
74	76	}
75	77
…	…
79	81
80	82	}
81

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionDispatch/btUnionFind.h

-                      r5781
+                      r8284
 #include "LinearMath/btAlignedObjectArray.h"
+        #define USE_PATH_COMPRESSION 1
+#define USE_PATH_COMPRESSION 1
+///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406
+#define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1
 struct  btElement
 …
                 #ifdef USE_PATH_COMPRESSION
+                                //
+                                m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;
+                #endif //
+                                const btElement* elementPtr = &m_elements[m_elements[x].m_id];
+                                m_elements[x].m_id = elementPtr->m_id;
+                                x = elementPtr->m_id;
+                #else//
                                 x = m_elements[x].m_id;
+                #endif
                                 //btAssert(x < m_N);
                                 //btAssert(x >= 0);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btBoxShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btBoxShape.h"

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btBoxShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         const btVector3& getHalfExtentsWithoutMargin() const
+        {
                 return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+                return m_implicitShapeDimensions;//scaling is included, margin is not
+        }
 …
 };
 #endif //OBB_BOX_MINKOWSKI_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
 #include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+#include "LinearMath/btSerializer.h"
 ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
 …
 :btTriangleMeshShape(meshInterface),
 m_bvh(0),
+m_triangleInfoMap(0),
 m_useQuantizedAabbCompression(useQuantizedAabbCompression),
 m_ownsBvh(false)
 …
 #ifndef DISABLE_BVH
-        btVector3 bvhAabbMin,bvhAabbMax;
-        if(meshInterface->hasPremadeAabb())
+        {
-                meshInterface->getPremadeAabb(&bvhAabbMin, &bvhAabbMax);
+        }
-        else
+        {
-                meshInterface->calculateAabbBruteForce(bvhAabbMin,bvhAabbMax);
+        }
         if (buildBvh)
+        {
+                void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+                m_bvh = new (mem) btOptimizedBvh();
+                m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax);
+                m_ownsBvh = true;
+                buildOptimizedBvh();
+        }
 …
 :btTriangleMeshShape(meshInterface),
 m_bvh(0),
+m_triangleInfoMap(0),
 m_useQuantizedAabbCompression(useQuantizedAabbCompression),
 m_ownsBvh(false)
 …
+   {
       btTriangleMeshShape::setLocalScaling(scaling);
+      if (m_ownsBvh)
+      {
+         m_bvh->~btOptimizedBvh();
+         btAlignedFree(m_bvh);
+      }
+      ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
+      void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+      m_bvh = new(mem) btOptimizedBvh();
+      //rebuild the bvh...
+      m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax);
+      m_ownsBvh = true;
+          buildOptimizedBvh();
+   }
+}
+void   btBvhTriangleMeshShape::buildOptimizedBvh()
+{
+        if (m_ownsBvh)
+        {
+                m_bvh->~btOptimizedBvh();
+                btAlignedFree(m_bvh);
+        }
+        ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
+        void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+        m_bvh = new(mem) btOptimizedBvh();
+        //rebuild the bvh...
+        m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax);
+        m_ownsBvh = true;
+}
 …
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer;
+        btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer);
+        m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer);
+        trimeshData->m_collisionMargin = float(m_collisionMargin);
+        if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH))
+        {
+                void* chunk = serializer->findPointer(m_bvh);
+                if (chunk)
+                {
+#ifdef BT_USE_DOUBLE_PRECISION
+                        trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk;
+                        trimeshData->m_quantizedFloatBvh = 0;
+#else
+                        trimeshData->m_quantizedFloatBvh  = (btQuantizedBvhData*)chunk;
+                        trimeshData->m_quantizedDoubleBvh= 0;
+#endif //BT_USE_DOUBLE_PRECISION
+                } else
+                {
+#ifdef BT_USE_DOUBLE_PRECISION
+                        trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
+                        trimeshData->m_quantizedFloatBvh = 0;
+#else
+                        trimeshData->m_quantizedFloatBvh  = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
+                        trimeshData->m_quantizedDoubleBvh= 0;
+#endif //BT_USE_DOUBLE_PRECISION
+                        int sz = m_bvh->calculateSerializeBufferSizeNew();
+                        btChunk* chunk = serializer->allocate(sz,1);
+                        const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
+                        serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh);
+                }
+        } else
+        {
+                trimeshData->m_quantizedFloatBvh = 0;
+                trimeshData->m_quantizedDoubleBvh = 0;
+        }
+        if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP))
+        {
+                void* chunk = serializer->findPointer(m_triangleInfoMap);
+                if (chunk)
+                {
+                        trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk;
+                } else
+                {
+                        trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap);
+                        int sz = m_triangleInfoMap->calculateSerializeBufferSize();
+                        btChunk* chunk = serializer->allocate(sz,1);
+                        const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
+                        serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap);
+                }
+        } else
+        {
+                trimeshData->m_triangleInfoMap = 0;
+        }
+        return "btTriangleMeshShapeData";
+}
+void    btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const
+{
+        if (m_bvh)
+        {
+                int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place
+                btChunk* chunk = serializer->allocate(len,1);
+                const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
+                serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh);
+        }
+}
+void    btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const
+{
+        if (m_triangleInfoMap)
+        {
+                int len = m_triangleInfoMap->calculateSerializeBufferSize();
+                btChunk* chunk = serializer->allocate(len,1);
+                const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
+                serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap);
+        }
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "btOptimizedBvh.h"
 #include "LinearMath/btAlignedAllocator.h"
+#include "btTriangleInfoMap.h"
 ///The btBvhTriangleMeshShape is a static-triangle mesh shape with several optimizations, such as bounding volume hierarchy and cache friendly traversal for PlayStation 3 Cell SPU. It is recommended to enable useQuantizedAabbCompression for better memory usage.
 …
         btOptimizedBvh* m_bvh;
+        btTriangleInfoMap*      m_triangleInfoMap;
         bool m_useQuantizedAabbCompression;
         bool m_ownsBvh;
 …
         BT_DECLARE_ALIGNED_ALLOCATOR();
         btBvhTriangleMeshShape() : btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;};
+        btBvhTriangleMeshShape() : btTriangleMeshShape(0),m_bvh(0),m_triangleInfoMap(0),m_ownsBvh(false) {m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;};
         btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
 …
+        }
+        void    setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1));
         void    setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1));
+        void    buildOptimizedBvh();
         bool    usesQuantizedAabbCompression() const
 …
                 return  m_useQuantizedAabbCompression;
+        }
+        void    setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap)
+        {
+                m_triangleInfoMap = triangleInfoMap;
+        }
+        const btTriangleInfoMap*        getTriangleInfoMap() const
+        {
+                return m_triangleInfoMap;
+        }
+        btTriangleInfoMap*      getTriangleInfoMap()
+        {
+                return m_triangleInfoMap;
+        }
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
+        virtual void    serializeSingleBvh(btSerializer* serializer) const;
+        virtual void    serializeSingleTriangleInfoMap(btSerializer* serializer) const;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btTriangleMeshShapeData
+{
+        btCollisionShapeData    m_collisionShapeData;
+        btStridingMeshInterfaceData m_meshInterface;
+        btQuantizedBvhFloatData         *m_quantizedFloatBvh;
+        btQuantizedBvhDoubleData        *m_quantizedDoubleBvh;
+        btTriangleInfoMapData   *m_triangleInfoMap;
+        float   m_collisionMargin;
+        char m_pad3[4];
+};
+SIMD_FORCE_INLINE       int     btBvhTriangleMeshShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btTriangleMeshShapeData);
+}
+;
 #endif //BVH_TRIANGLE_MESH_SHAPE_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCapsuleShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         btVector3 supVec(0,0,0);
         btScalar maxDot(btScalar(-1e30));
+        btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
         btVector3 vec = vec0;
 …
         for (int j=0;j<numVectors;j++)
+        {
                 btScalar maxDot(btScalar(-1e30));
+                btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
                 const btVector3& vec = vectors[j];

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+        virtual void setMargin(btScalar collisionMargin)
+        {
+                //correct the m_implicitShapeDimensions for the margin
+                btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+                btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+                btConvexInternalShape::setMargin(collisionMargin);
+                btVector3 newMargin(getMargin(),getMargin(),getMargin());
+                m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+        }
         virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+        {
 …
                 return m_implicitShapeDimensions[m_upAxis];
+        }
+        virtual void    setLocalScaling(const btVector3& scaling)
+        {
+                btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+                btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+                btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+                btConvexInternalShape::setLocalScaling(scaling);
+                m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+        }
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 …
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCapsuleShapeData
+{
+        btConvexInternalShapeData       m_convexInternalShapeData;
+        int     m_upAxis;
+        char    m_padding[4];
+};
+SIMD_FORCE_INLINE       int     btCapsuleShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btCapsuleShapeData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer;
+        btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+        shapeData->m_upAxis = m_upAxis;
+        return "btCapsuleShapeData";
+}
 #endif //BT_CAPSULE_SHAPE_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCollisionMargin.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
+btScalar gContactThresholdFactor=btScalar(0.02);
+#include "LinearMath/btSerializer.h"
 /*
 …
+}
+btScalar        btCollisionShape::getContactBreakingThreshold() const
+btScalar        btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const
+{
         return getAngularMotionDisc() * gContactThresholdFactor;
+        return getAngularMotionDisc() * defaultContactThreshold;
+}
 btScalar        btCollisionShape::getAngularMotionDisc() const
+{
 …
         temporalAabbMax += angularMotion3d;
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer;
+        char* name = (char*) serializer->findNameForPointer(this);
+        shapeData->m_name = (char*)serializer->getUniquePointer(name);
+        if (shapeData->m_name)
+        {
+                serializer->serializeName(name);
+        }
+        shapeData->m_shapeType = m_shapeType;
+        //shapeData->m_padding//??
+        return "btCollisionShapeData";
+}
+void    btCollisionShape::serializeSingleShape(btSerializer* serializer) const
+{
+        int len = calculateSerializeBufferSize();
+        btChunk* chunk = serializer->allocate(len,1);
+        const char* structType = serialize(chunk->m_oldPtr, serializer);
+        serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this);
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCollisionShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "LinearMath/btMatrix3x3.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
+class btSerializer;
 ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects.
 …
         virtual btScalar        getAngularMotionDisc() const;
         virtual btScalar        getContactBreakingThreshold() const;
+        virtual btScalar        getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const;
 …
         void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const;
+#ifndef __SPU__
         SIMD_FORCE_INLINE bool  isPolyhedral() const
 …
+        }
+        SIMD_FORCE_INLINE bool  isConvex2d() const
+        {
+                return btBroadphaseProxy::isConvex2d(getShapeType());
+        }
         SIMD_FORCE_INLINE bool  isConvex() const
+        {
                 return btBroadphaseProxy::isConvex(getShapeType());
+        }
+        SIMD_FORCE_INLINE bool  isNonMoving() const
+        {
+                return btBroadphaseProxy::isNonMoving(getShapeType());
+        }
         SIMD_FORCE_INLINE bool  isConcave() const
 …
+        }
+        SIMD_FORCE_INLINE bool  isSoftBody() const
+        {
+                return btBroadphaseProxy::isSoftBody(getShapeType());
+        }
         ///isInfinite is used to catch simulation error (aabb check)
         SIMD_FORCE_INLINE bool isInfinite() const
 …
+        }
+#ifndef __SPU__
         virtual void    setLocalScaling(const btVector3& scaling) =0;
         virtual const btVector3& getLocalScaling() const =0;
 …
+        }
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
+        virtual void    serializeSingleShape(btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCollisionShapeData
+{
+        char    *m_name;
+        int             m_shapeType;
+        char    m_padding[4];
+};
+SIMD_FORCE_INLINE       int     btCollisionShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btCollisionShapeData);
+}
 #endif //COLLISION_SHAPE_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "btCollisionShape.h"
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btSerializer.h"
 btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
+: m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)),
+m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)),
+: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
+m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
+m_dynamicAabbTree(0),
+m_updateRevision(1),
 m_collisionMargin(btScalar(0.)),
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
+m_dynamicAabbTree(0),
+m_updateRevision(1)
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+{
         m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
 …
         //m_childShapes.push_back(shape);
         btCompoundShapeChild child;
+        child.m_node = 0;
         child.m_transform = localTransform;
         child.m_childShape = shape;
 …
                 m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
                 ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
                 int index = m_children.size()-1;
+                //int index = m_children.size()-1;
                 m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
+        }
 …
+        }
         m_children.swap(childShapeIndex,m_children.size()-1);
+    if (m_dynamicAabbTree)
+                m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
         m_children.pop_back();
 …
                 if(m_children[i].m_childShape == shape)
+                {
+                        m_children.swap(i,m_children.size()-1);
+                        m_children.pop_back();
+                        //remove it from the m_dynamicAabbTree too
+                        //@todo: this leads to problems due to caching in the btCompoundCollisionAlgorithm
+                        //so effectively, removeChildShape is broken at the moment
+                        //m_dynamicAabbTree->remove(m_aabbProxies[i]);
+                        //m_aabbProxies.swap(i,m_children.size()-1);
+                        //m_aabbProxies.pop_back();
+                        removeChildShapeByIndex(i);
+                }
+        }
 …
         // Brute force, it iterates over all the shapes left.
         m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
         m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+        m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+        m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
         //extend the local aabbMin/aabbMax
 …
         for (k = 0; k < n; k++)
+        {
+                btAssert(masses[k]>0);
                 center += m_children[k].m_transform.getOrigin() * masses[k];
                 totalMass += masses[k];
+        }
+        btAssert(totalMass>0);
         center /= totalMass;
         principal.setOrigin(center);
 …
+void btCompoundShape::setLocalScaling(const btVector3& scaling)
+{
+        for(int i = 0; i < m_children.size(); i++)
+        {
+                btTransform childTrans = getChildTransform(i);
+                btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
+//              childScale = childScale * (childTrans.getBasis() * scaling);
+                childScale = childScale * scaling / m_localScaling;
+                m_children[i].m_childShape->setLocalScaling(childScale);
+                childTrans.setOrigin((childTrans.getOrigin())*scaling);
+                updateChildTransform(i, childTrans);
+                recalculateLocalAabb();
+        }
+        m_localScaling = scaling;
+}
+void btCompoundShape::createAabbTreeFromChildren()
+{
+    if ( !m_dynamicAabbTree )
+    {
+        void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+        m_dynamicAabbTree = new(mem) btDbvt();
+        btAssert(mem==m_dynamicAabbTree);
+        for ( int index = 0; index < m_children.size(); index++ )
+        {
+            btCompoundShapeChild &child = m_children[index];
+            //extend the local aabbMin/aabbMax
+            btVector3 localAabbMin,localAabbMax;
+            child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
+            const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+            child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+        }
+    }
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
+        btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
+        shapeData->m_collisionMargin = float(m_collisionMargin);
+        shapeData->m_numChildShapes = m_children.size();
+        shapeData->m_childShapePtr = 0;
+        if (shapeData->m_numChildShapes)
+        {
+                btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
+                btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
+                shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
+                for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
+                {
+                        memPtr->m_childMargin = float(m_children[i].m_childMargin);
+                        memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
+                        //don't serialize shapes that already have been serialized
+                        if (!serializer->findPointer(m_children[i].m_childShape))
+                        {
+                                btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
+                                const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
+                                serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
+                        }
+                        memPtr->m_childShapeType = m_children[i].m_childShapeType;
+                        m_children[i].m_transform.serializeFloat(memPtr->m_transform);
+                }
+                serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
+        }
+        return "btCompoundShapeData";
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCompoundShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         int                                                             m_updateRevision;
+        btScalar        m_collisionMargin;
+protected:
+        btVector3       m_localScaling;
 public:
         BT_DECLARE_ALIGNED_ALLOCATOR();
 …
         virtual void recalculateLocalAabb();
+        virtual void    setLocalScaling(const btVector3& scaling)
+        {
+                m_localScaling = scaling;
+        }
+        virtual void    setLocalScaling(const btVector3& scaling);
         virtual const btVector3& getLocalScaling() const
+        {
 …
+        }
-        //this is optional, but should make collision queries faster, by culling non-overlapping nodes
-        void    createAabbTreeFromChildren();
         btDbvt*                                                 getDynamicAabbTree()
 …
                 return m_dynamicAabbTree;
+        }
+        void createAabbTreeFromChildren();
         ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
 …
+        }
+private:
+        btScalar        m_collisionMargin;
+protected:
+        btVector3       m_localScaling;
+};
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btCompoundShapeChildData
+{
+        btTransformFloatData    m_transform;
+        btCollisionShapeData    *m_childShape;
+        int                                             m_childShapeType;
+        float                                   m_childMargin;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCompoundShapeData
+{
+        btCollisionShapeData            m_collisionShapeData;
+        btCompoundShapeChildData        *m_childShapePtr;
+        int                                                     m_numChildShapes;
+        float   m_collisionMargin;
+};
+SIMD_FORCE_INLINE       int     btCompoundShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btCompoundShapeData);
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConcaveShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConcaveShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConeShape.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConeShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btConvexHullShape.h"
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 #include "LinearMath/btQuaternion.h"
+btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexShape ()
+#include "LinearMath/btSerializer.h"
+btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape ()
+{
         m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
         m_unscaledPoints.resize(numPoints);
         unsigned char* pointsBaseAddress = (unsigned char*)points;
+        unsigned char* pointsAddress = (unsigned char*)points;
         for (int i=0;i<numPoints;i++)
+        {
+                btVector3* point = (btVector3*)(pointsBaseAddress + i*stride);
+                m_unscaledPoints[i] = point[0];
+                btScalar* point = (btScalar*)pointsAddress;
+                m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]);
+                pointsAddress += stride;
+        }
 …
+}
 btVector3       btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3       btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
         btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+        btScalar newDot,maxDot = btScalar(-1e30);
+        btVector3 vec = vec0;
+        btScalar lenSqr = vec.length2();
+        if (lenSqr < btScalar(0.0001))
+        {
+                vec.setValue(1,0,0);
+        } else
+        {
+                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+                vec *= rlen;
+        }
+        btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
         for (int i=0;i<m_unscaledPoints.size();i++)
 …
                 for (int i=0;i<numVectors;i++)
+                {
                         supportVerticesOut[i][3] = btScalar(-1e30);
+                        supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+                }
+        }
 …
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        //int szc = sizeof(btConvexHullShapeData);
+        btConvexHullShapeData* shapeData = (btConvexHullShapeData*) dataBuffer;
+        btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
+        int numElem = m_unscaledPoints.size();
+        shapeData->m_numUnscaledPoints = numElem;
+#ifdef BT_USE_DOUBLE_PRECISION
+        shapeData->m_unscaledPointsFloatPtr = 0;
+        shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]):  0;
+#else
+        shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]):  0;
+        shapeData->m_unscaledPointsDoublePtr = 0;
+#endif
+        if (numElem)
+        {
+                int sz = sizeof(btVector3Data);
+        //      int sz2 = sizeof(btVector3DoubleData);
+        //      int sz3 = sizeof(btVector3FloatData);
+                btChunk* chunk = serializer->allocate(sz,numElem);
+                btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr;
+                for (int i=0;i<numElem;i++,memPtr++)
+                {
+                        m_unscaledPoints[i].serialize(*memPtr);
+                }
+                serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]);
+        }
+        return "btConvexHullShapeData";
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "LinearMath/btAlignedObjectArray.h"
 ///The btConvexHullShape implements an implicit convex hull of an array of vertices.
 ///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex.
 ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape
+ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape
+{
         btAlignedObjectArray<btVector3> m_unscaledPoints;
 …
         virtual void    setLocalScaling(const btVector3& scaling);
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btConvexHullShapeData
+{
+        btConvexInternalShapeData       m_convexInternalShapeData;
+        btVector3FloatData      *m_unscaledPointsFloatPtr;
+        btVector3DoubleData     *m_unscaledPointsDoublePtr;
+        int             m_numUnscaledPoints;
+        char m_padding3[4];
+};
+SIMD_FORCE_INLINE       int     btConvexHullShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btConvexHullShapeData);
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 void    btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const
+{
+#ifndef __SPU__
+        //use localGetSupportingVertexWithoutMargin?
         btScalar margin = getMargin();
         for (int i=0;i<3;i++)
 …
                 minAabb[i] = tmp[i]-margin;
+        }
+#endif
+}
 …
+btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape()
+        :       btConvexInternalShape(),
+m_localAabbMin(1,1,1),
+m_localAabbMax(-1,-1,-1),
+m_isLocalAabbValid(false)
+{
+}
+void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+        getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+}
+void    btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling)
+{
+        btConvexInternalShape::setLocalScaling(scaling);
+        recalcLocalAabb();
+}
+void    btConvexInternalAabbCachingShape::recalcLocalAabb()
+{
+        m_isLocalAabbValid = true;
+        #if 1
+        static const btVector3 _directions[] =
+        {
+                btVector3( 1.,  0.,  0.),
+                btVector3( 0.,  1.,  0.),
+                btVector3( 0.,  0.,  1.),
+                btVector3( -1., 0.,  0.),
+                btVector3( 0., -1.,  0.),
+                btVector3( 0.,  0., -1.)
+        };
+        btVector3 _supporting[] =
+        {
+                btVector3( 0., 0., 0.),
+                btVector3( 0., 0., 0.),
+                btVector3( 0., 0., 0.),
+                btVector3( 0., 0., 0.),
+                btVector3( 0., 0., 0.),
+                btVector3( 0., 0., 0.)
+        };
+        batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
+        for ( int i = 0; i < 3; ++i )
+        {
+                m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
+                m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
+        }
+        #else
+        for (int i=0;i<3;i++)
+        {
+                btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+                vec[i] = btScalar(1.);
+                btVector3 tmp = localGetSupportingVertex(vec);
+                m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+                vec[i] = btScalar(-1.);
+                tmp = localGetSupportingVertex(vec);
+                m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+        }
+        #endif
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.h

-                      r5781
+                      r8284
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+. This notice may not be removed or altered from any source distribution.
+*/
 #ifndef BT_CONVEX_INTERNAL_SHAPE_H
 …
 #include "btConvexShape.h"
+#include "LinearMath/btAabbUtil2.h"
 ///The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
 …
+        {
                 return m_implicitShapeDimensions;
+        }
+        ///warning: use setImplicitShapeDimensions with care
+        ///changing a collision shape while the body is in the world is not recommended,
+        ///it is best to remove the body from the world, then make the change, and re-add it
+        ///alternatively flush the contact points, see documentation for 'cleanProxyFromPairs'
+        void    setImplicitShapeDimensions(const btVector3& dimensions)
+        {
+                m_implicitShapeDimensions = dimensions;
+        }
 …
+        }
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btConvexInternalShapeData
+{
+        btCollisionShapeData    m_collisionShapeData;
+        btVector3FloatData      m_localScaling;
+        btVector3FloatData      m_implicitShapeDimensions;
+        float                   m_collisionMargin;
+        int     m_padding;
+};
+SIMD_FORCE_INLINE       int     btConvexInternalShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btConvexInternalShapeData);
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer;
+        btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
+        m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions);
+        m_localScaling.serializeFloat(shapeData->m_localScaling);
+        shapeData->m_collisionMargin = float(m_collisionMargin);
+        return "btConvexInternalShapeData";
+}
+///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations
+class btConvexInternalAabbCachingShape : public btConvexInternalShape
+{
+        btVector3       m_localAabbMin;
+        btVector3       m_localAabbMax;
+        bool            m_isLocalAabbValid;
+protected:
+        btConvexInternalAabbCachingShape();
+        void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
+        {
+                m_isLocalAabbValid = true;
+                m_localAabbMin = aabbMin;
+                m_localAabbMax = aabbMax;
+        }
+        inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
+        {
+                btAssert(m_isLocalAabbValid);
+                aabbMin = m_localAabbMin;
+                aabbMax = m_localAabbMax;
+        }
+        inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
+        {
+                //lazy evaluation of local aabb
+                btAssert(m_isLocalAabbValid);
+                btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
+        }
+public:
+        virtual void    setLocalScaling(const btVector3& scaling);
+        virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+        void    recalcLocalAabb();
+};
 #endif //BT_CONVEX_INTERNAL_SHAPE_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btConvexPointCloudShape.h"
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 …
+{
         btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
         btScalar newDot,maxDot = btScalar(-1e30);
+        btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
         btVector3 vec = vec0;
 …
                 for (int i=0;i<numVectors;i++)
+                {
                         supportVerticesOut[i][3] = btScalar(-1e30);
+                        supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+                }
+        }

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 ///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
 ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape
+ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape
+{
         btVector3* m_unscaledPoints;
 …
 public:
         BT_DECLARE_ALIGNED_ALLOCATOR();
+        btConvexPointCloudShape()
+        {
+                m_localScaling.setValue(1.f,1.f,1.f);
+                m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
+                m_unscaledPoints = 0;
+                m_numPoints = 0;
+        }
         btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true)
 …
+        }
         void setPoints (btVector3* points, int numPoints, bool computeAabb = true)
+        void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f))
+        {
                 m_unscaledPoints = points;
                 m_numPoints = numPoints;
+                m_localScaling = localScaling;
                 if (computeAabb)

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "btConvexPointCloudShape.h"
+///not supported on IBM SDK, until we fix the alignment of btVector3
+#if defined (__CELLOS_LV2__) && defined (__SPU__)
+#include <spu_intrinsics.h>
+static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
+{
+    vec_float4 result;
+    result = spu_mul( vec0, vec1 );
+    result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
+    return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
+}
+#endif //__SPU__
 btConvexShape::btConvexShape ()
+{
 …
+static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints, const btVector3& localScaling)
+{
+        btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+        btScalar newDot,maxDot = btScalar(-1e30);
+        btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+        btVector3 vec = vec0;
+        btScalar lenSqr = vec.length2();
+        if (lenSqr < btScalar(0.0001))
+        {
+                vec.setValue(1,0,0);
+        } else {
+                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+                vec *= rlen;
+        }
+static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
+{
+        btVector3 vec = localDirOrg * localScaling;
+#if defined (__CELLOS_LV2__) && defined (__SPU__)
+        btVector3 localDir = vec;
+        vec_float4 v_distMax = {-FLT_MAX,0,0,0};
+        vec_int4 v_idxMax = {-999,0,0,0};
+        int v=0;
+        int numverts = numPoints;
+        for(;v<(int)numverts-4;v+=4) {
+                vec_float4 p0 = vec_dot3(points[v  ].get128(),localDir.get128());
+                vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128());
+                vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128());
+                vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128());
+                const vec_int4 i0 = {v  ,0,0,0};
+                const vec_int4 i1 = {v+1,0,0,0};
+                const vec_int4 i2 = {v+2,0,0,0};
+                const vec_int4 i3 = {v+3,0,0,0};
+                vec_uint4  retGt01 = spu_cmpgt(p0,p1);
+                vec_float4 pmax01 = spu_sel(p1,p0,retGt01);
+                vec_int4   imax01 = spu_sel(i1,i0,retGt01);
+                vec_uint4  retGt23 = spu_cmpgt(p2,p3);
+                vec_float4 pmax23 = spu_sel(p3,p2,retGt23);
+                vec_int4   imax23 = spu_sel(i3,i2,retGt23);
+                vec_uint4  retGt0123 = spu_cmpgt(pmax01,pmax23);
+                vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123);
+                vec_int4   imax0123 = spu_sel(imax23,imax01,retGt0123);
+                vec_uint4  retGtMax = spu_cmpgt(v_distMax,pmax0123);
+                v_distMax = spu_sel(pmax0123,v_distMax,retGtMax);
+                v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax);
+        }
+        for(;v<(int)numverts;v++) {
+                vec_float4 p = vec_dot3(points[v].get128(),localDir.get128());
+                const vec_int4 i = {v,0,0,0};
+                vec_uint4  retGtMax = spu_cmpgt(v_distMax,p);
+                v_distMax = spu_sel(p,v_distMax,retGtMax);
+                v_idxMax = spu_sel(i,v_idxMax,retGtMax);
+        }
+        int ptIndex = spu_extract(v_idxMax,0);
+        const btVector3& supVec= points[ptIndex] * localScaling;
+        return supVec;
+#else
+        btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
+        int ptIndex = -1;
         for (int i=0;i<numPoints;i++)
+        {
+                btVector3 vtx = points[i] * localScaling;
+                newDot = vec.dot(vtx);
+                newDot = vec.dot(points[i]);
                 if (newDot > maxDot)
+                {
                         maxDot = newDot;
+                        supVec = vtx;
+                }
+        }
+        return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
+                        ptIndex = i;
+                }
+        }
+        btAssert(ptIndex >= 0);
+        btVector3 supVec = points[ptIndex] * localScaling;
+        return supVec;
+#endif //__SPU__
+}
 …
                 btVector3 supVec(0,0,0);
                 btScalar maxDot(btScalar(-1e30));
+                btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
                 btVector3 vec = vec0;
 …
         return btScalar(0.0f);
+}
+#ifndef __SPU__
 void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+{
 …
         case CONVEX_HULL_SHAPE_PROXYTYPE:
+        {
                 btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
+                btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
                 btScalar margin = convexHullShape->getMarginNonVirtual();
                 convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
 …
         btAssert (0);
+}
+#endif //__SPU__

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btConvexTriangleMeshShape.h"
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 …
 btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb)
 : btPolyhedralConvexShape(), m_stridingMesh(meshInterface)
+: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
+{
         m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
 …
         LocalSupportVertexCallback(const btVector3& supportVecLocal)
                 : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)),
                 m_maxDot(btScalar(-1e30)),
+                m_maxDot(btScalar(-BT_LARGE_FLOAT)),
                 m_supportVecLocal(supportVecLocal)
+        {
 …
         LocalSupportVertexCallback      supportCallback(vec);
         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
         m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
         supVec = supportCallback.GetSupportVertexLocal();
 …
                 for (int i=0;i<numVectors;i++)
+                {
                         supportVerticesOut[i][3] = btScalar(-1e30);
+                        supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+                }
+        }
 …
                 const btVector3& vec = vectors[j];
                 LocalSupportVertexCallback      supportCallback(vec);
                 btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+                btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
                 m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
                 supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
 …
    CenterCallback centerCallback;
    btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+   btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
    m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
    btVector3 center = centerCallback.getCenter();

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h

-                      r5781
+                      r8284
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+. This notice may not be removed or altered from any source distribution.
+*/
 #ifndef CONVEX_TRIANGLEMESH_SHAPE_H
 #define CONVEX_TRIANGLEMESH_SHAPE_H
 …
 /// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape.
 /// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead.
 class btConvexTriangleMeshShape : public btPolyhedralConvexShape
+class btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape
+{

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCylinderShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btCylinderShape.h"
 btCylinderShape::btCylinderShape (const btVector3& halfExtents)
 :btBoxShape(halfExtents),
+:btConvexInternalShape(),
 m_upAxis(1)
+{
+        btVector3 margin(getMargin(),getMargin(),getMargin());
+        m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
         m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
-        recalcLocalAabb();
+}
 …
+{
         m_upAxis = 0;
+        recalcLocalAabb();
+}
 …
+{
         m_upAxis = 2;
+        recalcLocalAabb();
+}
 void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+        //skip the box 'getAabb'
+        btPolyhedralConvexShape::getAabb(t,aabbMin,aabbMax);
+        btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+}
+void    btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+        //approximation of box shape, todo: implement cylinder shape inertia before people notice ;-)
+        btVector3 halfExtents = getHalfExtentsWithMargin();
+        btScalar lx=btScalar(2.)*(halfExtents.x());
+        btScalar ly=btScalar(2.)*(halfExtents.y());
+        btScalar lz=btScalar(2.)*(halfExtents.z());
+        inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
+                                        mass/(btScalar(12.0)) * (lx*lx + lz*lz),
+                                        mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btCylinderShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 /// The btCylinderShape class implements a cylinder shape primitive, centered around the origin. Its central axis aligned with the Y axis. btCylinderShapeX is aligned with the X axis and btCylinderShapeZ around the Z axis.
 class btCylinderShape : public btBoxShape
+class btCylinderShape : public btConvexInternalShape
+{
 …
 public:
+        btVector3 getHalfExtentsWithMargin() const
+        {
+                btVector3 halfExtents = getHalfExtentsWithoutMargin();
+                btVector3 margin(getMargin(),getMargin(),getMargin());
+                halfExtents += margin;
+                return halfExtents;
+        }
+        const btVector3& getHalfExtentsWithoutMargin() const
+        {
+                return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+        }
         btCylinderShape (const btVector3& halfExtents);
-        ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
         void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+        virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const;
         virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+        virtual void setMargin(btScalar collisionMargin)
+        {
+                //correct the m_implicitShapeDimensions for the margin
+                btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+                btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+                btConvexInternalShape::setMargin(collisionMargin);
+                btVector3 newMargin(getMargin(),getMargin(),getMargin());
+                m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+        }
         virtual btVector3       localGetSupportingVertex(const btVector3& vec) const
 …
+        }
+        virtual void    setLocalScaling(const btVector3& scaling)
+        {
+                btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+                btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+                btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+                btConvexInternalShape::setLocalScaling(scaling);
+                m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+        }
         //debugging
         virtual const char*     getName()const
 …
+        }
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 …
         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-        virtual int     getUpAxis() const
+        {
-                return 2;
+        }
                 //debugging
         virtual const char*     getName()const
 …
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCylinderShapeData
+{
+        btConvexInternalShapeData       m_convexInternalShapeData;
+        int     m_upAxis;
+        char    m_padding[4];
+};
+SIMD_FORCE_INLINE       int     btCylinderShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btCylinderShapeData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer;
+        btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+        shapeData->m_upAxis = m_upAxis;
+        return "btCylinderShapeData";
+}
 #endif //CYLINDER_MINKOWSKI_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btEmptyShape.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btEmptyShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btMaterial.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #endif // MATERIAL_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btMinkowskiSumShape.h"

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 */
 #include "btMultiSphereShape.h"
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 #include "LinearMath/btQuaternion.h"
+#include "LinearMath/btSerializer.h"
 btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres)
 :btConvexInternalShape (), m_inertiaHalfExtents(inertiaHalfExtents)
+btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
+:btConvexInternalAabbCachingShape ()
+{
         m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
         btScalar startMargin = btScalar(1e30);
+        //btScalar startMargin = btScalar(BT_LARGE_FLOAT);
+        m_numSpheres = numSpheres;
+        for (int i=0;i<m_numSpheres;i++)
+        m_localPositionArray.resize(numSpheres);
+        m_radiArray.resize(numSpheres);
+        for (int i=0;i<numSpheres;i++)
+        {
+                m_localPositions[i] = positions[i];
+                m_radi[i] = radi[i];
+                if (radi[i] < startMargin)
+                        startMargin = radi[i];
+                m_localPositionArray[i] = positions[i];
+                m_radiArray[i] = radi[i];
+        }
+        setMargin(startMargin);
+        recalcLocalAabb();
+}
 …
         btVector3 supVec(0,0,0);
         btScalar maxDot(btScalar(-1e30));
+        btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 …
         btScalar newDot;
+        const btVector3* pos = &m_localPositions[0];
+        const btScalar* rad = &m_radi[0];
+        const btVector3* pos = &m_localPositionArray[0];
+        const btScalar* rad = &m_radiArray[0];
+        int numSpheres = m_localPositionArray.size();
         for (i=0;i<m_numSpheres;i++)
+        for (i=0;i<numSpheres;i++)
+        {
                 vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
 …
         for (int j=0;j<numVectors;j++)
+        {
                 btScalar maxDot(btScalar(-1e30));
+                btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
                 const btVector3& vec = vectors[j];
 …
                 btScalar newDot;
                 const btVector3* pos = &m_localPositions[0];
                 const btScalar* rad = &m_radi[0];
                 for (int i=0;i<m_numSpheres;i++)
+                const btVector3* pos = &m_localPositionArray[0];
+                const btScalar* rad = &m_radiArray[0];
+                int numSpheres = m_localPositionArray.size();
+                for (int i=0;i<numSpheres;i++)
+                {
                         vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
 …
         //as an approximation, take the inertia of the box that bounds the spheres
         btTransform ident;
         ident.setIdentity();
 //      btVector3 aabbMin,aabbMax;
+        btVector3 localAabbMin,localAabbMax;
+        getCachedLocalAabb(localAabbMin,localAabbMax);
+        btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
+//      getAabb(ident,aabbMin,aabbMax);
+        btScalar lx=btScalar(2.)*(halfExtents.x());
+        btScalar ly=btScalar(2.)*(halfExtents.y());
+        btScalar lz=btScalar(2.)*(halfExtents.z());
+        btVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* btScalar(0.5);
+        btScalar margin = CONVEX_DISTANCE_MARGIN;
+        btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
+        btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
+        btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
+        const btScalar x2 = lx*lx;
+        const btScalar y2 = ly*ly;
+        const btScalar z2 = lz*lz;
+        const btScalar scaledmass = mass * btScalar(.08333333);
+        inertia[0] = scaledmass * (y2+z2);
+        inertia[1] = scaledmass * (x2+z2);
+        inertia[2] = scaledmass * (x2+y2);
+        inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
+                                        mass/(btScalar(12.0)) * (lx*lx + lz*lz),
+                                        mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
+        btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
+        int numElem = m_localPositionArray.size();
+        shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]):  0;
+        shapeData->m_localPositionArraySize = numElem;
+        if (numElem)
+        {
+                btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem);
+                btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
+                for (int i=0;i<numElem;i++,memPtr++)
+                {
+                        m_localPositionArray[i].serializeFloat(memPtr->m_pos);
+                        memPtr->m_radius = float(m_radiArray[i]);
+                }
+                serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
+        }
+        return "btMultiSphereShapeData";
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "btConvexInternalShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#define MAX_NUM_SPHERES 5
+///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes.
+///It is possible to animate the spheres for deformation.
+class btMultiSphereShape : public btConvexInternalShape
+{
+        btVector3 m_localPositions[MAX_NUM_SPHERES];
+        btScalar  m_radi[MAX_NUM_SPHERES];
+        btVector3       m_inertiaHalfExtents;
+        int m_numSpheres;
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAabbUtil2.h"
+///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes.
+///It is possible to animate the spheres for deformation, but call 'recalcLocalAabb' after changing any sphere position/radius
+class btMultiSphereShape : public btConvexInternalAabbCachingShape
+{
+        btAlignedObjectArray<btVector3> m_localPositionArray;
+        btAlignedObjectArray<btScalar>  m_radiArray;
 public:
         btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres);
+        btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres);
         ///CollisionShape Interface
 …
         int     getSphereCount() const
+        {
                 return m_numSpheres;
+                return m_localPositionArray.size();
+        }
         const btVector3&        getSpherePosition(int index) const
+        {
                 return m_localPositions[index];
+                return m_localPositionArray[index];
+        }
         btScalar        getSphereRadius(int index) const
+        {
                 return m_radi[index];
+                return m_radiArray[index];
+        }
 …
+        }
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+struct  btPositionAndRadius
+{
+        btVector3FloatData      m_pos;
+        float           m_radius;
+};
+struct  btMultiSphereShapeData
+{
+        btConvexInternalShapeData       m_convexInternalShapeData;
+        btPositionAndRadius     *m_localPositionArrayPtr;
+        int                             m_localPositionArraySize;
+        char    m_padding[4];
+};
+SIMD_FORCE_INLINE       int     btMultiSphereShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btMultiSphereShapeData);
+}
 #endif //MULTI_SPHERE_MINKOWSKI_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~8 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
             m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
-            btVector3 m_triangle[3];
             const unsigned char *vertexbase;
             int numverts;
 …
             m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
-            btVector3 m_triangle[3];
             const unsigned char *vertexbase;
             int numverts;

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 */
 #include "btOptimizedBvh.h"
 #include "btStridingMeshInterface.h"
 …
                         btOptimizedBvhNode node;
                         btVector3       aabbMin,aabbMax;
                         aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
                         aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+                        aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                        aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
                         aabbMin.setMin(triangle[0]);
                         aabbMax.setMax(triangle[0]);
 …
                         btQuantizedBvhNode node;
                         btVector3       aabbMin,aabbMax;
                         aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
                         aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+                        aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                        aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
                         aabbMin.setMin(triangle[0]);
                         aabbMax.setMax(triangle[0]);
 …
                 NodeTriangleCallback    callback(m_leafNodes);
                 btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
                 btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+                btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+                btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
                 triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
 …
                                 aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
                                 aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+                                aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                                aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
                                 aabbMin.setMin(triangleVerts[0]);
                                 aabbMax.setMax(triangleVerts[0]);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
+///Contains contributions from Disney Studio's
 #ifndef OPTIMIZED_BVH_H
 …
         /// Data buffer MUST be 16 byte aligned
         virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian)
+        virtual bool serializeInPlace(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const
+        {
                 return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
+m_localAabbMin(1,1,1),
+m_localAabbMax(-1,-1,-1),
+m_isLocalAabbValid(false),
+m_optionalHull(0)
+btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape()
+{
 …
 btVector3       btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+        btVector3 supVec(0,0,0);
+#ifndef __SPU__
         int i;
+        btVector3 supVec(0,0,0);
+        btScalar maxDot(btScalar(-1e30));
+        btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
         btVector3 vec = vec0;
 …
+        }
+#endif //__SPU__
         return supVec;
+}
+}
 void    btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+#ifndef __SPU__
         int i;
 …
         for (i=0;i<numVectors;i++)
+        {
                 supportVerticesOut[i][3] = btScalar(-1e30);
+                supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+        }
 …
+                }
+        }
+#endif //__SPU__
+}
 …
 void    btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+#ifndef __SPU__
         //not yet, return box inertia
 …
         inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+#endif //__SPU__
+}
+void btPolyhedralConvexShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+        getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+}
+void    btPolyhedralConvexShape::setLocalScaling(const btVector3& scaling)
+void    btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
+{
         btConvexInternalShape::setLocalScaling(scaling);
 …
+}
+void    btPolyhedralConvexShape::recalcLocalAabb()
+btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
+:btPolyhedralConvexShape(),
+m_localAabbMin(1,1,1),
+m_localAabbMax(-1,-1,-1),
+m_isLocalAabbValid(false)
+{
+}
+void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+        getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+}
+void    btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
+{
         m_isLocalAabbValid = true;
 …
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "LinearMath/btMatrix3x3.h"
-#include "LinearMath/btAabbUtil2.h"
 #include "btConvexInternalShape.h"
 …
 protected:
+        btVector3       m_localAabbMin;
+        btVector3       m_localAabbMax;
+        bool            m_isLocalAabbValid;
 public:
 …
         virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
         virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+        virtual int     getNumVertices() const = 0 ;
+        virtual int getNumEdges() const = 0;
+        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0;
+        virtual void getVertex(int i,btVector3& vtx) const = 0;
+        virtual int     getNumPlanes() const = 0;
+        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0;
+//      virtual int getIndex(int i) const = 0 ;
+        virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0;
+};
+///The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape
+class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape
+{
+        btVector3       m_localAabbMin;
+        btVector3       m_localAabbMax;
+        bool            m_isLocalAabbValid;
+protected:
         void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
 …
+        }
+public:
+        btPolyhedralConvexAabbCachingShape();
         inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
+        {
 …
+        }
+        virtual void    setLocalScaling(const btVector3& scaling);
         virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-        virtual void    setLocalScaling(const btVector3& scaling);
         void    recalcLocalAabb();
-        virtual int     getNumVertices() const = 0 ;
-        virtual int getNumEdges() const = 0;
-        virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0;
-        virtual void getVertex(int i,btVector3& vtx) const = 0;
-        virtual int     getNumPlanes() const = 0;
-        virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0;
-//      virtual int getIndex(int i) const = 0 ;
-        virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0;
-        /// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
-        class   Hull*   m_optionalHull;
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btScaledBvhTriangleMeshShape.h"

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~8 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp

-                      r5781
+                      r8284
 /*
-btbtShapeHull implemented by John McCutchan.
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
 Permission is granted to anyone to use this software for any purpose,
 including commercial applications, and to alter it and redistribute it freely,
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 …
 */
+//btShapeHull was implemented by John McCutchan.
 #include "btShapeHull.h"
 #include "LinearMath/btConvexHull.h"
 #define NUM_UNITSPHERE_POINTS 42
-static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
+{
-        btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
-        btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
-        btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
-        btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
-        btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
-        btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
-        btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
-        btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
-        btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
-        btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
-        btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
-        btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
-        btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
-        btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
-        btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
-        btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
-        btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
-        btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
-        btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
-        btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
-        btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
-        btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
-        btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
-        btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
-        btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-        btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
-        btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-        btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
-        btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
-        btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-        btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
-        btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-        btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
-        btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
-        btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
-        btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
-        btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
-        btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
-        btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
-        btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
-        btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
-        btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
-};
 btShapeHull::btShapeHull (const btConvexShape* shape)
 …
                                 btVector3 norm;
                                 m_shape->getPreferredPenetrationDirection(i,norm);
                                 btUnitSpherePoints[numSampleDirections] = norm;
+                                getUnitSpherePoints()[numSampleDirections] = norm;
                                 numSampleDirections++;
+                        }
 …
         for (i = 0; i < numSampleDirections; i++)
+        {
                 supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]);
+                supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]);
+        }
 …
+}
+btVector3* btShapeHull::getUnitSpherePoints()
+{
+        static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
+        {
+                btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+                btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+                btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+                btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+                btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+                btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+                btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+                btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+                btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+                btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+                btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+                btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+                btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+                btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+                btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+                btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+                btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+                btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+                btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+                btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+                btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+                btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+                btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+                btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+                btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+                btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+                btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+                btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+                btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+                btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+                btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+                btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+                btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+                btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+                btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+                btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+                btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+                btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+                btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+                btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+                btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+                btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+        };
+        return sUnitSpherePoints;
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btShapeHull.h

-                      r5781
+                      r8284
 /*
-btShapeHull implemented by John McCutchan.
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
 Permission is granted to anyone to use this software for any purpose,
 including commercial applications, and to alter it and redistribute it freely,
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 …
 . This notice may not be removed or altered from any source distribution.
 */
+///btShapeHull implemented by John McCutchan.
 #ifndef _SHAPE_HULL_H
 …
 class btShapeHull
+{
+protected:
+        btAlignedObjectArray<btVector3> m_vertices;
+        btAlignedObjectArray<unsigned int> m_indices;
+        unsigned int m_numIndices;
+        const btConvexShape* m_shape;
+        static btVector3* getUnitSpherePoints();
 public:
         btShapeHull (const btConvexShape* shape);
 …
                 return &m_indices[0];
+        }
-protected:
-        btAlignedObjectArray<btVector3> m_vertices;
-        btAlignedObjectArray<unsigned int> m_indices;
-        unsigned int m_numIndices;
-        const btConvexShape* m_shape;
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btSphereShape.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btSphereShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #ifndef SPHERE_MINKOWSKI_H
 #define SPHERE_MINKOWSKI_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         (void)t;
         /*
         btVector3 infvec (btScalar(1e30),btScalar(1e30),btScalar(1e30));
+        btVector3 infvec (btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
         btVector3 center = m_planeNormal*m_planeConstant;
 …
         */
         aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
         aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+        aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+        aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 ///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane.
 class btStaticPlaneShape : public btConcaveShape
+ATTRIBUTE_ALIGNED16(class) btStaticPlaneShape : public btConcaveShape
+{
 protected:
 …
         virtual const char*     getName()const {return "STATICPLANE";}
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btStaticPlaneShapeData
+{
+        btCollisionShapeData    m_collisionShapeData;
+        btVector3FloatData      m_localScaling;
+        btVector3FloatData      m_planeNormal;
+        float                   m_planeConstant;
+        char    m_pad[4];
+};
+SIMD_FORCE_INLINE       int     btStaticPlaneShape::calculateSerializeBufferSize() const
+{
+        return sizeof(btStaticPlaneShapeData);
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*) dataBuffer;
+        btCollisionShape::serialize(&planeData->m_collisionShapeData,serializer);
+        m_localScaling.serializeFloat(planeData->m_localScaling);
+        m_planeNormal.serializeFloat(planeData->m_planeNormal);
+        planeData->m_planeConstant = float(m_planeConstant);
+        return "btStaticPlaneShapeData";
+}
 #endif //STATIC_PLANE_SHAPE_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "btStridingMeshInterface.h"
+#include "LinearMath/btSerializer.h"
 btStridingMeshInterface::~btStridingMeshInterface()
 …
         int gfxindex;
         btVector3 triangle[3];
-        btScalar* graphicsbase;
         btVector3 meshScaling = getScaling();
 …
                 numtotalphysicsverts+=numtriangles*3; //upper bound
+                switch (gfxindextype)
+                ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build
+                ///so disable this feature by default
+                ///see patch http://code.google.com/p/bullet/issues/detail?id=213
+                switch (type)
+                {
+                case PHY_INTEGER:
+                case PHY_FLOAT:
+                 {
+                         float* graphicsbase;
+                         switch (gfxindextype)
+                         {
+                         case PHY_INTEGER:
+                                 {
+                                         for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                         {
+                                                 unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
+                                                 graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
+                                                 triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+                                                 graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
+                                                 triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),    graphicsbase[2]*meshScaling.getZ());
+                                                 graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
+                                                 triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),    graphicsbase[2]*meshScaling.getZ());
+                                                 callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+                                         }
+                                         break;
+                                 }
+                         case PHY_SHORT:
+                                 {
+                                         for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                         {
+                                                 unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+                                                 graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
+                                                 triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+                                                 graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
+                                                 triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),    graphicsbase[2]*meshScaling.getZ());
+                                                 graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
+                                                 triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),    graphicsbase[2]*meshScaling.getZ());
+                                                 callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+                                         }
+                                         break;
+                                 }
+                         default:
+                                 btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+                         }
+                         break;
+                 }
+                case PHY_DOUBLE:
+                        {
+                                for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                {
+                                        unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
+                                        graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride);
+                                        triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+                                        graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride);
+                                        triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),     graphicsbase[2]*meshScaling.getZ());
+                                        graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride);
+                                        triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),     graphicsbase[2]*meshScaling.getZ());
+                                        callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+                                }
+                                break;
+                        }
+                case PHY_SHORT:
+                        {
+                                for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                {
+                                        unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+                                        graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride);
+                                        triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+                                        graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride);
+                                        triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),     graphicsbase[2]*meshScaling.getZ());
+                                        graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride);
+                                        triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),     graphicsbase[2]*meshScaling.getZ());
+                                        callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+                                double* graphicsbase;
+                                switch (gfxindextype)
+                                {
+                                case PHY_INTEGER:
+                                        {
+                                                for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                                {
+                                                        unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
+                                                        graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
+                                                        triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ());
+                                                        graphicsbase = (double*)(vertexbase+tri_indices[1]*stride);
+                                                        triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
+                                                        graphicsbase = (double*)(vertexbase+tri_indices[2]*stride);
+                                                        triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
+                                                        callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+                                                }
+                                                break;
+                                        }
+                                case PHY_SHORT:
+                                        {
+                                                for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                                {
+                                                        unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+                                                        graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
+                                                        triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ());
+                                                        graphicsbase = (double*)(vertexbase+tri_indices[1]*stride);
+                                                        triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
+                                                        graphicsbase = (double*)(vertexbase+tri_indices[2]*stride);
+                                                        triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
+                                                        callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+                                                }
+                                                break;
+                                        }
+                                default:
+                                        btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+                                }
                                 break;
+                        }
                 default:
                         btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+                        btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
+                }
 …
                 AabbCalculationCallback()
+                {
                         m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
                         m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+                        m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                        m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+                }
 …
         };
                 //first calculate the total aabb for all triangles
+        //first calculate the total aabb for all triangles
         AabbCalculationCallback aabbCallback;
         aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
         aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+        aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+        aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
         InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);
 …
         aabbMax = aabbCallback.m_aabbMax;
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer;
+        trimeshData->m_numMeshParts = getNumSubParts();
+        //void* uniquePtr = 0;
+        trimeshData->m_meshPartsPtr = 0;
+        if (trimeshData->m_numMeshParts)
+        {
+                btChunk* chunk = serializer->allocate(sizeof(btMeshPartData),trimeshData->m_numMeshParts);
+                btMeshPartData* memPtr = (btMeshPartData*)chunk->m_oldPtr;
+                trimeshData->m_meshPartsPtr = (btMeshPartData *)serializer->getUniquePointer(memPtr);
+        //      int numtotalphysicsverts = 0;
+                int part,graphicssubparts = getNumSubParts();
+                const unsigned char * vertexbase;
+                const unsigned char * indexbase;
+                int indexstride;
+                PHY_ScalarType type;
+                PHY_ScalarType gfxindextype;
+                int stride,numverts,numtriangles;
+                int gfxindex;
+        //      btVector3 triangle[3];
+                btVector3 meshScaling = getScaling();
+                ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
+                for (part=0;part<graphicssubparts ;part++,memPtr++)
+                {
+                        getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
+                        memPtr->m_numTriangles = numtriangles;//indices = 3*numtriangles
+                        memPtr->m_numVertices = numverts;
+                        memPtr->m_indices16 = 0;
+                        memPtr->m_indices32 = 0;
+                        memPtr->m_3indices16 = 0;
+                        memPtr->m_vertices3f = 0;
+                        memPtr->m_vertices3d = 0;
+                        switch (gfxindextype)
+                        {
+                        case PHY_INTEGER:
+                                {
+                                        int numindices = numtriangles*3;
+                                        if (numindices)
+                                        {
+                                                btChunk* chunk = serializer->allocate(sizeof(btIntIndexData),numindices);
+                                                btIntIndexData* tmpIndices = (btIntIndexData*)chunk->m_oldPtr;
+                                                memPtr->m_indices32 = (btIntIndexData*)serializer->getUniquePointer(tmpIndices);
+                                                for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                                {
+                                                        unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
+                                                        tmpIndices[gfxindex*3].m_value = tri_indices[0];
+                                                        tmpIndices[gfxindex*3+1].m_value = tri_indices[1];
+                                                        tmpIndices[gfxindex*3+2].m_value = tri_indices[2];
+                                                }
+                                                serializer->finalizeChunk(chunk,"btIntIndexData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+                                        }
+                                        break;
+                                }
+                        case PHY_SHORT:
+                                {
+                                        if (numtriangles)
+                                        {
+                                                btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData),numtriangles);
+                                                btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr;
+                                                memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices);
+                                                for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+                                                {
+                                                        unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+                                                        tmpIndices[gfxindex].m_values[0] = tri_indices[0];
+                                                        tmpIndices[gfxindex].m_values[1] = tri_indices[1];
+                                                        tmpIndices[gfxindex].m_values[2] = tri_indices[2];
+                                                }
+                                                serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+                                        }
+                                        break;
+                                }
+                        default:
+                                {
+                                        btAssert(0);
+                                        //unknown index type
+                                }
+                        }
+                        switch (type)
+                        {
+                        case PHY_FLOAT:
+                         {
+                                 float* graphicsbase;
+                                 if (numverts)
+                                 {
+                                         btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData),numverts);
+                                         btVector3FloatData* tmpVertices = (btVector3FloatData*) chunk->m_oldPtr;
+                                         memPtr->m_vertices3f = (btVector3FloatData *)serializer->getUniquePointer(tmpVertices);
+                                         for (int i=0;i<numverts;i++)
+                                         {
+                                                 graphicsbase = (float*)(vertexbase+i*stride);
+                                                 tmpVertices[i].m_floats[0] = graphicsbase[0];
+                                                 tmpVertices[i].m_floats[1] = graphicsbase[1];
+                                                 tmpVertices[i].m_floats[2] = graphicsbase[2];
+                                         }
+                                         serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+                                 }
+                                 break;
+                                }
+                        case PHY_DOUBLE:
+                                {
+                                        if (numverts)
+                                        {
+                                                btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData),numverts);
+                                                btVector3DoubleData* tmpVertices = (btVector3DoubleData*) chunk->m_oldPtr;
+                                                memPtr->m_vertices3d = (btVector3DoubleData *) serializer->getUniquePointer(tmpVertices);
+                                                for (int i=0;i<numverts;i++)
+                                         {
+                                                 double* graphicsbase = (double*)(vertexbase+i*stride);//for now convert to float, might leave it at double
+                                                 tmpVertices[i].m_floats[0] = graphicsbase[0];
+                                                 tmpVertices[i].m_floats[1] = graphicsbase[1];
+                                                 tmpVertices[i].m_floats[2] = graphicsbase[2];
+                                         }
+                                                serializer->finalizeChunk(chunk,"btVector3DoubleData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+                                        }
+                                        break;
+                                }
+                        default:
+                                btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
+                        }
+                        unLockReadOnlyVertexBase(part);
+                }
+                serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr);
+        }
+        m_scaling.serializeFloat(trimeshData->m_scaling);
+        return "btStridingMeshInterfaceData";
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "btTriangleCallback.h"
 #include "btConcaveShape.h"
 …
+                }
+                virtual int     calculateSerializeBufferSize() const;
+                ///fills the dataBuffer and returns the struct name (and 0 on failure)
+                virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+struct  btIntIndexData
+{
+        int     m_value;
+};
+struct  btShortIntIndexData
+{
+        short m_value;
+        char m_pad[2];
+};
+struct  btShortIntIndexTripletData
+{
+        short   m_values[3];
+        char    m_pad[2];
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btMeshPartData
+{
+        btVector3FloatData                      *m_vertices3f;
+        btVector3DoubleData                     *m_vertices3d;
+        btIntIndexData                          *m_indices32;
+        btShortIntIndexTripletData      *m_3indices16;
+        btShortIntIndexData                     *m_indices16;//backwards compatibility
+        int                     m_numTriangles;//length of m_indices = m_numTriangles
+        int                     m_numVertices;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btStridingMeshInterfaceData
+{
+        btMeshPartData  *m_meshPartsPtr;
+        btVector3FloatData      m_scaling;
+        int     m_numMeshParts;
+        char m_padding[4];
+};
+SIMD_FORCE_INLINE       int     btStridingMeshInterface::calculateSerializeBufferSize() const
+{
+        return sizeof(btStridingMeshInterfaceData);
+}
 #endif //STRIDING_MESHINTERFACE_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btTetrahedronShape.h"
 #include "LinearMath/btMatrix3x3.h"
 btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexShape (),
 m_numVertices(0)
+{
         m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+}
 btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexShape (),
 m_numVertices(0)
+{
         m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
         addVertex(pt0);
+}
 btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexShape (),
+btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (),
+m_numVertices(0)
+{
+        m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+}
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape (),
+m_numVertices(0)
+{
+        m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+        addVertex(pt0);
+}
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexAabbCachingShape (),
 m_numVertices(0)
+{
 …
+}
 btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexShape (),
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexAabbCachingShape (),
 m_numVertices(0)
+{
 …
+}
 btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexShape (),
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexAabbCachingShape (),
 m_numVertices(0)
+{
 …
+void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+#if 1
+        btPolyhedralConvexAabbCachingShape::getAabb(t,aabbMin,aabbMax);
+#else
+        aabbMin.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+        aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+        //just transform the vertices in worldspace, and take their AABB
+        for (int i=0;i<m_numVertices;i++)
+        {
+                btVector3 worldVertex = t(m_vertices[i]);
+                aabbMin.setMin(worldVertex);
+                aabbMax.setMax(worldVertex);
+        }
+#endif
+}
 …
+{
         m_vertices[m_numVertices++] = pt;
         recalcLocalAabb();
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 ///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead.
 class btBU_Simplex1to4 : public btPolyhedralConvexShape
+class btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape
+{
 protected:
 …
+        }
+        virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
         void addVertex(const btVector3& pt);

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleCallback.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleCallback.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
 Permission is granted to anyone to use this software for any purpose,
 including commercial applications, and to alter it and redistribute it freely,
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 …
         numverts = mesh.m_numVertices;
         (*vertexbase) = (unsigned char *) mesh.m_vertexBase;
+        #ifdef BT_USE_DOUBLE_PRECISION
+        type = PHY_DOUBLE;
+        #else
+        type = PHY_FLOAT;
+        #endif
+   type = mesh.m_vertexType;
         vertexStride = mesh.m_vertexStride;
 …
         numverts = mesh.m_numVertices;
         (*vertexbase) = (const unsigned char *)mesh.m_vertexBase;
+        #ifdef BT_USE_DOUBLE_PRECISION
+        type = PHY_DOUBLE;
+        #else
+        type = PHY_FLOAT;
+        #endif
+   type = mesh.m_vertexType;
         vertexStride = mesh.m_vertexStride;

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         BT_DECLARE_ALIGNED_ALLOCATOR();
+        int                     m_numTriangles;
+        const unsigned char *           m_triangleIndexBase;
+        int                     m_triangleIndexStride;
+        int                     m_numVertices;
+        const unsigned char *           m_vertexBase;
+        int                     m_vertexStride;
+        // The index type is set when adding an indexed mesh to the
+        // btTriangleIndexVertexArray, do not set it manually
+        PHY_ScalarType          m_indexType;
+        int                     pad;
+   int                     m_numTriangles;
+   const unsigned char *   m_triangleIndexBase;
+   int                     m_triangleIndexStride;
+   int                     m_numVertices;
+   const unsigned char *   m_vertexBase;
+   int                     m_vertexStride;
+   // The index type is set when adding an indexed mesh to the
+   // btTriangleIndexVertexArray, do not set it manually
+   PHY_ScalarType m_indexType;
+   // The vertex type has a default type similar to Bullet's precision mode (float or double)
+   // but can be set manually if you for example run Bullet with double precision but have
+   // mesh data in single precision..
+   PHY_ScalarType m_vertexType;
+   btIndexedMesh()
+           :m_indexType(PHY_INTEGER),
+#ifdef BT_USE_DOUBLE_PRECISION
+      m_vertexType(PHY_DOUBLE)
+#else // BT_USE_DOUBLE_PRECISION
+      m_vertexType(PHY_FLOAT)
+#endif // BT_USE_DOUBLE_PRECISION
+      {
+      }
+}
+;

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
 Permission is granted to anyone to use this software for any purpose,
 including commercial applications, and to alter it and redistribute it freely,
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #include "btTriangleMesh.h"

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleMesh.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #ifndef TRIANGLE_MESH_H
 …
                 btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true);
-                int             findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices);
-                void    addIndex(int index);
                 bool    getUse32bitIndices() const
+                {
 …
                 virtual void    preallocateIndices(int numindices){(void) numindices;}
+                ///findOrAddVertex is an internal method, use addTriangle instead
+                int             findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices);
+                ///addIndex is an internal method, use addTriangle instead
+                void    addIndex(int index);
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans)
                 : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30))
+                : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-BT_LARGE_FLOAT))
+        {
 …
         SupportVertexCallback supportCallback(vec,ident);
         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
         processAllTriangles(&supportCallback,-aabbMax,aabbMax);
 …
         return supportVertex;
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
         virtual const char*     getName()const {return "TRIANGLEMESH";}
 };
 #endif //TRIANGLE_MESH_SHAPE_H

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btTriangleShape.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "btBoxShape.h"
 class btTriangleShape : public btPolyhedralConvexShape
+ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape
+{
 …
+        {
                 return 3;
+        }
+        btVector3& getVertexPtr(int index)
+        {
+                return m_vertices1[index];
+        }
 …
+        }
+        btTriangleShape() : btPolyhedralConvexShape ()
+    {
+                m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+        }
         btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape ()

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.cpp

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~7 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.h

r5781	r8284
1	1	/*
2	2	Bullet Continuous Collision Detection and Physics Library
3		Copyright (c) 2003-200~~6 Erwin Coumans http://continuousphysics.com/Bullet/~~
	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4	4
5	5	This software is provided 'as-is', without any express or implied warranty.

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp

-                      r5781
+                      r8284
+        {
                 btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+                btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
                 btGjkPairDetector::ClosestPointInput input;
 …
                 while (dist > radius)
+                {
+                        if (result.m_debugDrawer)
+                        {
+                                result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
+                        }
                         numIter++;
                         if (numIter > maxIter)
 …
                         btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
                         relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+                        if (result.m_debugDrawer)
+                        {
+                                result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
+                        }
                         result.DebugDraw( lambda );
 …
                                 return false;
+                        }
+                }
 …
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h

r5781	r8284
42	42
43	43	CastResult()
44		:m_fraction(btScalar(~~1e30~~)),
	44	:m_fraction(btScalar(BT_LARGE_FLOAT)),
45	45	m_debugDrawer(0),
46	46	m_allowedPenetration(btScalar(0))

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h

-                      r5781
+                      r8284
 #ifndef CONVEX_PENETRATION_DEPTH_H
 #define CONVEX_PENETRATION_DEPTH_H
+#ifndef __CONVEX_PENETRATION_DEPTH_H
+#define __CONVEX_PENETRATION_DEPTH_H
 class btStackAlloc;

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h

-                      r5781
+                      r8284
                 virtual ~Result(){}
+                ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner
+                virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)=0;
+                ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner
+                virtual void setShapeIdentifiersA(int partId0,int index0)=0;
+                virtual void setShapeIdentifiersB(int partId1,int index1)=0;
                 virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0;
         };
 …
+        {
                 ClosestPointInput()
                         :m_maximumDistanceSquared(btScalar(1e30)),
+                        :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)),
                         m_stackAlloc(0)
+                {
 …
                 btScalar        m_distance; //negative means penetration !
                 btStorageResult() : m_distance(btScalar(1e30))
+                btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
+                {

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

-                      r5781
+                      r8284
                 btMatrix3x3                             m_toshape1;
                 btTransform                             m_toshape0;
+#ifdef __SPU__
+                bool                                    m_enableMargin;
+#else
                 btVector3                               (btConvexShape::*Ls)(const btVector3&) const;
+#endif//__SPU__
+                MinkowskiDiff()
+                {
+                }
+#ifdef __SPU__
+                        void                                    EnableMargin(bool enable)
+                {
+                        m_enableMargin = enable;
+                }
+                inline btVector3                Support0(const btVector3& d) const
+                {
+                        if (m_enableMargin)
+                        {
+                                return m_shapes[0]->localGetSupportVertexNonVirtual(d);
+                        } else
+                        {
+                                return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
+                        }
+                }
+                inline btVector3                Support1(const btVector3& d) const
+                {
+                        if (m_enableMargin)
+                        {
+                                return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
+                        } else
+                        {
+                                return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
+                        }
+                }
+#else
                 void                                    EnableMargin(bool enable)
+                {
 …
                         return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
+                }
+#endif //__SPU__
                 inline btVector3                Support(const btVector3& d) const
+                {
 …
+                                        }
                                         /* Check for termination                                */
                                         const btScalar  omega=dot(m_ray,w)/rl;
+                                        const btScalar  omega=btDot(m_ray,w)/rl;
                                         alpha=btMax(omega,alpha);
                                         if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
 …
                                 case    eStatus::Valid:         m_distance=m_ray.length();break;
                                 case    eStatus::Inside:        m_distance=0;break;
+                                default:
+                                        {
+                                        }
+                                }
                                 return(m_status);
 …
                                                         btVector3               axis=btVector3(0,0,0);
                                                         axis[i]=1;
                                                         const btVector3 p=cross(d,axis);
+                                                        const btVector3 p=btCross(d,axis);
                                                         if(p.length2()>0)
+                                                        {
 …
                                 case    3:
+                                        {
                                                 const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+                                                const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
                                                         m_simplex->c[2]->w-m_simplex->c[0]->w);
                                                 if(n.length2()>0)
 …
                                 if(l>GJK_SIMPLEX2_EPS)
+                                {
                                         const btScalar  t(l>0?-dot(a,d)/l:0);
+                                        const btScalar  t(l>0?-btDot(a,d)/l:0);
                                         if(t>=1)                { w[0]=0;w[1]=1;m=2;return(b.length2()); }
                                         else if(t<=0)   { w[0]=1;w[1]=0;m=1;return(a.length2()); }
 …
                                 const btVector3*        vt[]={&a,&b,&c};
                                 const btVector3         dl[]={a-b,b-c,c-a};
                                 const btVector3         n=cross(dl[0],dl[1]);
+                                const btVector3         n=btCross(dl[0],dl[1]);
                                 const btScalar          l=n.length2();
                                 if(l>GJK_SIMPLEX3_EPS)
+                                {
                                         btScalar        mindist=-1;
                                         btScalar        subw[2];
                                         U                       subm;
+                                        btScalar        subw[2]={0.f,0.f};
+                                        U                       subm(0);
                                         for(U i=0;i<3;++i)
+                                        {
                                                 if(dot(*vt[i],cross(dl[i],n))>0)
+                                                if(btDot(*vt[i],btCross(dl[i],n))>0)
+                                                {
                                                         const U                 j=imd3[i];
 …
                                         if(mindist<0)
+                                        {
                                                 const btScalar  d=dot(a,n);
+                                                const btScalar  d=btDot(a,n);
                                                 const btScalar  s=btSqrt(l);
                                                 const btVector3 p=n*(d/l);
                                                 mindist =       p.length2();
                                                 m               =       7;
                                                 w[0]    =       (cross(dl[1],b-p)).length()/s;
                                                 w[1]    =       (cross(dl[2],c-p)).length()/s;
+                                                w[0]    =       (btCross(dl[1],b-p)).length()/s;
+                                                w[1]    =       (btCross(dl[2],c-p)).length()/s;
                                                 w[2]    =       1-(w[0]+w[1]);
+                                        }
 …
                                 const btVector3         dl[]={a-d,b-d,c-d};
                                 const btScalar          vl=det(dl[0],dl[1],dl[2]);
                                 const bool                      ng=(vl*dot(a,cross(b-c,a-b)))<=0;
+                                const bool                      ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
                                 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+                                {
                                         btScalar        mindist=-1;
                                         btScalar        subw[3];
                                         U                       subm;
+                                        btScalar        subw[3]={0.f,0.f,0.f};
+                                        U                       subm(0);
                                         for(U i=0;i<3;++i)
+                                        {
                                                 const U                 j=imd3[i];
                                                 const btScalar  s=vl*dot(d,cross(dl[i],dl[j]));
+                                                const btScalar  s=vl*btDot(d,btCross(dl[i],dl[j]));
                                                 if(s>0)
+                                                {
 …
                                                                 best->pass      =       (U1)(++pass);
                                                                 gjk.getsupport(best->n,*w);
                                                                 const btScalar  wdist=dot(best->n,w->w)-best->d;
+                                                                const btScalar  wdist=btDot(best->n,w->w)-best->d;
                                                                 if(wdist>EPA_ACCURACY)
+                                                                {
 …
                                                 m_result.c[1]   =       outer.c[1];
                                                 m_result.c[2]   =       outer.c[2];
                                                 m_result.p[0]   =       cross(  outer.c[1]->w-projection,
+                                                m_result.p[0]   =       btCross(        outer.c[1]->w-projection,
                                                         outer.c[2]->w-projection).length();
                                                 m_result.p[1]   =       cross(  outer.c[2]->w-projection,
+                                                m_result.p[1]   =       btCross(        outer.c[2]->w-projection,
                                                         outer.c[0]->w-projection).length();
                                                 m_result.p[2]   =       cross(  outer.c[0]->w-projection,
+                                                m_result.p[2]   =       btCross(        outer.c[0]->w-projection,
                                                         outer.c[1]->w-projection).length();
                                                 const btScalar  sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
 …
                                         face->c[1]      =       b;
                                         face->c[2]      =       c;
                                         face->n         =       cross(b->w-a->w,c->w-a->w);
+                                        face->n         =       btCross(b->w-a->w,c->w-a->w);
                                         const btScalar  l=face->n.length();
                                         const bool              v=l>EPA_ACCURACY;
                                         face->p         =       btMin(btMin(
                                                 dot(a->w,cross(face->n,a->w-b->w)),
                                                 dot(b->w,cross(face->n,b->w-c->w))),
                                                 dot(c->w,cross(face->n,c->w-a->w)))     /
+                                                btDot(a->w,btCross(face->n,a->w-b->w)),
+                                                btDot(b->w,btCross(face->n,b->w-c->w))),
+                                                btDot(c->w,btCross(face->n,c->w-a->w))) /
                                                 (v?l:1);
                                         face->p         =       face->p>=-EPA_INSIDE_EPS?0:face->p;
                                         if(v)
+                                        {
                                                 face->d         =       dot(a->w,face->n)/l;
+                                                face->d         =       btDot(a->w,face->n)/l;
                                                 face->n         /=      l;
                                                 if(forced||(face->d>=-EPA_PLANE_EPS))
 …
+                                {
                                         const U e1=i1m3[e];
                                         if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+                                        if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+                                        {
                                                 sFace*  nf=newface(f->c[e1],f->c[e],w,false);
 …
                 results.status=sResults::GJK_Failed;
                 break;
+                default:
+                                        {
+                                        }
+        }
         return(false);
+}
+#ifndef __SPU__
 //
 btScalar        btGjkEpaSolver2::SignedDistance(const btVector3& position,
 …
                 return(true);
+}
+#endif //__SPU__
 /* Symbols cleanup              */

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h

-                      r5781
+                      r8284
                                                         sResults& results,
                                                         bool usemargins=true);
+#ifndef __SPU__
 static btScalar SignedDistance( const btVector3& position,
                                                                 btScalar margin,
 …
                                                                 const btVector3& guess,
                                                                 sResults& results);
+#endif //__SPU__
 };

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp

-                      r5781
+                      r8284
         (void)simplexSolver;
         const btScalar                          radialmargin(btScalar(0.));
+//      const btScalar                          radialmargin(btScalar(0.));
         btVector3       guessVector(transformA.getOrigin()-transformB.getOrigin());
         btGjkEpaSolver2::sResults       results;
         if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
                                                                 pConvexB,transformB,
 …
                 wWitnessOnA = results.witnesses[0];
                 wWitnessOnB = results.witnesses[1];
+                v = results.normal;
                 return true;
+                } else
+        {
+                if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
+                {
+                        wWitnessOnA = results.witnesses[0];
+                        wWitnessOnB = results.witnesses[1];
+                        v = results.normal;
+                        return false;
+                }
+        }
         return false;

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h

-                      r5781
+                      r8284
         public :
+                btGjkEpaPenetrationDepthSolver()
+                {
+                }
                 bool                    calcPenDepth( btSimplexSolverInterface& simplexSolver,
                                                                           const btConvexShape* pConvexA, const btConvexShape* pConvexB,

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

-                      r5781
+                      r8284
 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*  penetrationDepthSolver)
 :m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
 m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
 m_minkowskiA(objectA),
 m_minkowskiB(objectB),
+m_shapeTypeA(objectA->getShapeType()),
+m_shapeTypeB(objectB->getShapeType()),
+m_marginA(objectA->getMargin()),
+m_marginB(objectB->getMargin()),
 m_ignoreMargin(false),
 m_lastUsedMethod(-1),
 …
+{
+}
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*        penetrationDepthSolver)
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_shapeTypeA(shapeTypeA),
+m_shapeTypeB(shapeTypeB),
+m_marginA(marginA),
+m_marginB(marginB),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+void    btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+        (void)swapResults;
+        getClosestPointsNonVirtual(input,output,debugDraw);
+}
+#ifdef __SPU__
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#else
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#endif
+{
         m_cachedSeparatingDistance = 0.f;
 …
         localTransB.getOrigin() -= positionOffset;
+#ifdef __SPU__
+        btScalar marginA = m_minkowskiA->getMarginNonVirtual();
+        btScalar marginB = m_minkowskiB->getMarginNonVirtual();
+#else
+        btScalar marginA = m_minkowskiA->getMargin();
+        btScalar marginB = m_minkowskiB->getMargin();
+#ifdef TEST_NON_VIRTUAL
+        btScalar marginAv = m_minkowskiA->getMarginNonVirtual();
+        btScalar marginBv = m_minkowskiB->getMarginNonVirtual();
+        btAssert(marginA == marginAv);
+        btAssert(marginB == marginBv);
+#endif //TEST_NON_VIRTUAL
+#endif
+        bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
+        btScalar marginA = m_marginA;
+        btScalar marginB = m_marginB;
         gNumGjkChecks++;
 …
+        {
                 btScalar squaredDistance = SIMD_INFINITY;
+                btScalar squaredDistance = BT_LARGE_FLOAT;
                 btScalar delta = btScalar(0.);
 …
                         btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+#if 1
+                        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+                        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+//                      btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
+//                      btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
+#else
 #ifdef __SPU__
                         btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 …
 #endif //
 #endif //__SPU__
+#endif
                         btVector3  pWorld = localTransA(pInA);
 …
 #endif
+                        if (check2d)
+                        {
+                                pWorld[2] = 0.f;
+                                qWorld[2] = 0.f;
+                        }
                         btVector3 w     = pWorld - qWorld;
                         delta = m_cachedSeparatingAxis.dot(w);
 …
                         if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
+                        {
+                                m_degenerateSimplex = 10;
                                 checkSimplex=true;
                                 //checkPenetration = false;
 …
+                                {
                                         m_degenerateSimplex = 2;
+                                } else
+                                {
+                                        m_degenerateSimplex = 11;
+                                }
                                 checkSimplex = true;
 …
                 spu_printf("addVertex 2\n");
 #endif
+                        btVector3 newCachedSeparatingAxis;
                         //calculate the closest point to the origin (update vector v)
                         if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+                        if (!m_simplexSolver->closest(newCachedSeparatingAxis))
+                        {
                                 m_degenerateSimplex = 3;
 …
+                        }
                         if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
+                        if(newCachedSeparatingAxis.length2()<REL_ERROR2)
+            {
+                                m_cachedSeparatingAxis = newCachedSeparatingAxis;
                 m_degenerateSimplex = 6;
                 checkSimplex = true;
 …
                         btScalar previousSquaredDistance = squaredDistance;
+                        squaredDistance = m_cachedSeparatingAxis.length2();
+                        squaredDistance = newCachedSeparatingAxis.length2();
+#if 0
+///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
+                        if (squaredDistance>previousSquaredDistance)
+                        {
+                                m_degenerateSimplex = 7;
+                                squaredDistance = previousSquaredDistance;
+                checkSimplex = false;
+                break;
+                        }
+#endif //
+                        m_cachedSeparatingAxis = newCachedSeparatingAxis;
                         //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 …
                                 m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
                                 checkSimplex = true;
+                                m_degenerateSimplex = 12;
                                 break;
+                        }
 …
                                 //do we need this backup_closest here ?
                                 m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+                                m_degenerateSimplex = 13;
                                 break;
+                        }
 …
                         m_simplexSolver->compute_points(pointOnA, pointOnB);
                         normalInB = pointOnA-pointOnB;
+                        btScalar lenSqr = m_cachedSeparatingAxis.length2();
+                        btScalar lenSqr =m_cachedSeparatingAxis.length2();
                         //valid normal
                         if (lenSqr < 0.0001)
 …
+                {
                         //penetration case
                         //if there is no way to handle penetrations, bail out
                         if (m_penetrationDepthSolver)
 …
                                 gNumDeepPenetrationChecks++;
+                                m_cachedSeparatingAxis.setZero();
                                 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
 …
                                         );
                                 if (isValid2)
+                                {
                                         btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
                                         btScalar lenSqr = tmpNormalInB.length2();
+                                        if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+                                        {
+                                                tmpNormalInB = m_cachedSeparatingAxis;
+                                                lenSqr = m_cachedSeparatingAxis.length2();
+                                        }
                                         if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+                                        {
 …
                                                 } else
+                                                {
+                                                        m_lastUsedMethod = 8;
+                                                }
                                         } else
+                                        {
+                                                //isValid = false;
+                                                m_lastUsedMethod = 4;
+                                                m_lastUsedMethod = 9;
+                                        }
                                 } else
+                                {
+                                        m_lastUsedMethod = 5;
+                                        ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+                                        ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+                                        ///reports a valid positive distance. Use the results of the second GJK instead of failing.
+                                        ///thanks to Jacob.Langford for the reproduction case
+                                        ///http://code.google.com/p/bullet/issues/detail?id=250
+                                        if (m_cachedSeparatingAxis.length2() > btScalar(0.))
+                                        {
+                                                btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
+                                                //only replace valid distances when the distance is less
+                                                if (!isValid || (distance2 < distance))
+                                                {
+                                                        distance = distance2;
+                                                        pointOnA = tmpPointOnA;
+                                                        pointOnB = tmpPointOnB;
+                                                        pointOnA -= m_cachedSeparatingAxis * marginA ;
+                                                        pointOnB += m_cachedSeparatingAxis * marginB ;
+                                                        normalInB = m_cachedSeparatingAxis;
+                                                        normalInB.normalize();
+                                                        isValid = true;
+                                                        m_lastUsedMethod = 6;
+                                                } else
+                                                {
+                                                        m_lastUsedMethod = 5;
+                                                }
+                                        }
+                                }
+                        }
+                }
+        }
+        if (isValid)
+        if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
+        {
+#ifdef __SPU__
+                //spu_printf("distance\n");
+#endif //__CELLOS_LV2__
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+                spu_printf("output 1\n");
+#endif
+#if 0
+///some debugging
+//              if (check2d)
+                {
+                        printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
+                        printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
+                }
+#endif
                 m_cachedSeparatingAxis = normalInB;
                 m_cachedSeparatingDistance = distance;
 …
                         distance);
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-                spu_printf("output 2\n");
-#endif
-                //printf("gjk add:%f",distance);
+        }

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h

-                      r5781
+                      r8284
         const btConvexShape* m_minkowskiA;
         const btConvexShape* m_minkowskiB;
+        int     m_shapeTypeA;
+        int m_shapeTypeB;
+        btScalar        m_marginA;
+        btScalar        m_marginB;
         bool            m_ignoreMargin;
         btScalar        m_cachedSeparatingDistance;
 …
         btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*     penetrationDepthSolver);
+        btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*   penetrationDepthSolver);
         virtual ~btGjkPairDetector() {};
         virtual void    getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+        void    getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
         void setMinkowskiA(btConvexShape* minkA)

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

-                      r5781
+                      r8284
 #include "LinearMath/btTransformUtil.h"
+// Don't change following order of parameters
+ATTRIBUTE_ALIGNED16(struct) PfxConstraintRow {
+        btScalar mNormal[3];
+        btScalar mRhs;
+        btScalar mJacDiagInv;
+        btScalar mLowerLimit;
+        btScalar mUpperLimit;
+        btScalar mAccumImpulse;
+};
 …
                                 m_appliedImpulseLateral1(0.f),
                                 m_appliedImpulseLateral2(0.f),
+                                m_contactMotion1(0.f),
+                                m_contactMotion2(0.f),
+                                m_contactCFM1(0.f),
+                                m_contactCFM2(0.f),
                                 m_lifeTime(0)
+                        {
 …
                                         m_appliedImpulseLateral1(0.f),
                                         m_appliedImpulseLateral2(0.f),
+                                        m_contactMotion1(0.f),
+                                        m_contactMotion2(0.f),
+                                        m_contactCFM1(0.f),
+                                        m_contactCFM2(0.f),
                                         m_lifeTime(0)
+                        {
+                                mConstraintRow[0].mAccumImpulse = 0.f;
+                                mConstraintRow[1].mAccumImpulse = 0.f;
+                                mConstraintRow[2].mAccumImpulse = 0.f;
+                        }
 …
                         btScalar                m_appliedImpulseLateral1;
                         btScalar                m_appliedImpulseLateral2;
+                        btScalar                m_contactMotion1;
+                        btScalar                m_contactMotion2;
+                        btScalar                m_contactCFM1;
+                        btScalar                m_contactCFM2;
                         int                             m_lifeTime;//lifetime of the contactpoint in frames
                         btVector3               m_lateralFrictionDir1;
                         btVector3               m_lateralFrictionDir2;
+                        PfxConstraintRow mConstraintRow[3];
                         btScalar getDistance() const

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp

-                      r5781
+                      r8284
 #define NUM_UNITSPHERE_POINTS 42
-static btVector3        sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
+{
-btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
-btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
-btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
-btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
-btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
-btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
-btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
-btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
-btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
-btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
-btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
-btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
-btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
-btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
-btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
-btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
-btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
-btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
-btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
-btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
-btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
-btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
-btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
-btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
-btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
-btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
-btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
-btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
-btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
-btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
-btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
-btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
-btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
-btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
-btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
-btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
-btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
-btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
-};
 …
         (void)v;
+        bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
         struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
 …
                 bool    m_hasResult;
                 virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+                virtual void setShapeIdentifiersA(int partId0,int index0)
+                {
                         (void)partId0;
                         (void)index0;
+                }
+                virtual void setShapeIdentifiersB(int partId1,int index1)
+                {
                         (void)partId1;
                         (void)index1;
 …
         //just take fixed number of orientation, and sample the penetration depth in that direction
         btScalar minProj = btScalar(1e30);
+        btScalar minProj = btScalar(BT_LARGE_FLOAT);
         btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
         btVector3 minA,minB;
 …
         for (i=0;i<numSampleDirections;i++)
+        {
                 const btVector3& norm = sPenetrationDirections[i];
+                btVector3 norm = getPenetrationDirections()[i];
                 seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
                 seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
 …
                                 convexA->getPreferredPenetrationDirection(i,norm);
                                 norm  = transA.getBasis() * norm;
                                 sPenetrationDirections[numSampleDirections] = norm;
+                                getPenetrationDirections()[numSampleDirections] = norm;
                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 …
                                 convexB->getPreferredPenetrationDirection(i,norm);
                                 norm  = transB.getBasis() * norm;
                                 sPenetrationDirections[numSampleDirections] = norm;
+                                getPenetrationDirections()[numSampleDirections] = norm;
                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 …
         convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
         convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
 …
         for (i=0;i<numSampleDirections;i++)
+        {
+                const btVector3& norm = sPenetrationDirections[i];
+                seperatingAxisInA = seperatingAxisInABatch[i];
+                seperatingAxisInB = seperatingAxisInBBatch[i];
+                pInA = supportVerticesABatch[i];
+                qInB = supportVerticesBBatch[i];
+                pWorld = transA(pInA);
+                qWorld = transB(qInB);
+                w       = qWorld - pWorld;
+                btScalar delta = norm.dot(w);
+                //find smallest delta
+                if (delta < minProj)
+                {
+                        minProj = delta;
+                        minNorm = norm;
+                        minA = pWorld;
+                        minB = qWorld;
+                btVector3 norm = getPenetrationDirections()[i];
+                if (check2d)
+                {
+                        norm[2] = 0.f;
+                }
+                if (norm.length2()>0.01)
+                {
+                        seperatingAxisInA = seperatingAxisInABatch[i];
+                        seperatingAxisInB = seperatingAxisInBBatch[i];
+                        pInA = supportVerticesABatch[i];
+                        qInB = supportVerticesBBatch[i];
+                        pWorld = transA(pInA);
+                        qWorld = transB(qInB);
+                        if (check2d)
+                        {
+                                pWorld[2] = 0.f;
+                                qWorld[2] = 0.f;
+                        }
+                        w       = qWorld - pWorld;
+                        btScalar delta = norm.dot(w);
+                        //find smallest delta
+                        if (delta < minProj)
+                        {
+                                minProj = delta;
+                                minNorm = norm;
+                                minA = pWorld;
+                                minB = qWorld;
+                        }
+                }
+        }
 …
                                 convexA->getPreferredPenetrationDirection(i,norm);
                                 norm  = transA.getBasis() * norm;
                                 sPenetrationDirections[numSampleDirections] = norm;
+                                getPenetrationDirections()[numSampleDirections] = norm;
                                 numSampleDirections++;
+                        }
 …
                                 convexB->getPreferredPenetrationDirection(i,norm);
                                 norm  = transB.getBasis() * norm;
                                 sPenetrationDirections[numSampleDirections] = norm;
+                                getPenetrationDirections()[numSampleDirections] = norm;
                                 numSampleDirections++;
+                        }
 …
         for (int i=0;i<numSampleDirections;i++)
+        {
                 const btVector3& norm = sPenetrationDirections[i];
+                const btVector3& norm = getPenetrationDirections()[i];
                 seperatingAxisInA = (-norm)* transA.getBasis();
                 seperatingAxisInB = norm* transB.getBasis();
 …
                 return false;
+        minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
+        btScalar extraSeparation = 0.5f;///scale dependent
+        minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
 …
         input.m_transformA = displacedTrans;
         input.m_transformB = transB;
         input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
+        input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
         btIntermediateResult res;
+        gjkdet.setCachedSeperatingAxis(-minNorm);
         gjkdet.getClosestPoints(input,res,debugDraw);
 …
         btScalar penetration_relaxation= btScalar(1.);
         minNorm*=penetration_relaxation;
         if (res.m_hasResult)
 …
                 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
                 pb = res.m_pointInWorld;
+                v = minNorm;
 #ifdef DEBUG_DRAW
 …
+}
+btVector3*      btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
+{
+        static btVector3        sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
+        {
+        btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+        btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+        btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+        btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+        btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+        btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+        btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+        btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+        btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+        btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+        btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+        btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+        btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+        btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+        btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+        btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+        btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+        btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+        btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+        btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+        btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+        btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+        btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+        btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+        btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+        btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+        btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+        btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+        btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+        btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+        btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+        btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+        btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+        btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+        btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+        btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+        btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+        btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+        btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+        btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+        btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+        btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+        };
+        return sPenetrationDirections;
+}

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h

-                      r5781
+                      r8284
 class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+protected:
+        static btVector3*       getPenetrationDirections();
 public:

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

-                      r5781
+                      r8284
 btPersistentManifold::btPersistentManifold()
+:m_body0(0),
+:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
+m_body0(0),
 m_body1(0),
 m_cachedPoints (0),

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

-                      r5781
+                      r8284
 typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
 extern ContactDestroyedCallback gContactDestroyedCallback;
+extern ContactProcessedCallback gContactProcessedCallback;
+enum btContactManifoldTypes
+{
+        BT_PERSISTENT_MANIFOLD_TYPE = 1,
+        MAX_CONTACT_MANIFOLD_TYPE
+};
 #define MANIFOLD_CACHE_SIZE 4
 …
 ///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
 ///note that some pairs of objects might have more then one contact manifold.
+ATTRIBUTE_ALIGNED16( class) btPersistentManifold
+ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
+//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject
+{
 …
         void* m_body0;
         void* m_body1;
         int     m_cachedPoints;
 …
         BT_DECLARE_ALIGNED_ALLOCATOR();
+        int     m_companionIdA;
+        int     m_companionIdB;
         int m_index1a;
 …
         btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
+                : m_body0(body0),m_body1(body1),m_cachedPoints(0),
+                : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
+        m_body0(body0),m_body1(body1),m_cachedPoints(0),
                 m_contactBreakingThreshold(contactBreakingThreshold),
                 m_contactProcessingThreshold(contactProcessingThreshold)
+        {
+        }
 …
                         //get rid of duplicated userPersistentData pointer
                         m_pointCache[lastUsedIndex].m_userPersistentData = 0;
+                        m_pointCache[lastUsedIndex].mConstraintRow[0].mAccumImpulse = 0.f;
+                        m_pointCache[lastUsedIndex].mConstraintRow[1].mAccumImpulse = 0.f;
+                        m_pointCache[lastUsedIndex].mConstraintRow[2].mAccumImpulse = 0.f;
                         m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
                         m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
 …
 #ifdef MAINTAIN_PERSISTENCY
                 int     lifeTime = m_pointCache[insertIndex].getLifeTime();
+                btScalar        appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+                btScalar        appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
+                btScalar        appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
+                btScalar        appliedImpulse = m_pointCache[insertIndex].mConstraintRow[0].mAccumImpulse;
+                btScalar        appliedLateralImpulse1 = m_pointCache[insertIndex].mConstraintRow[1].mAccumImpulse;
+                btScalar        appliedLateralImpulse2 = m_pointCache[insertIndex].mConstraintRow[2].mAccumImpulse;
+//              bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
                 btAssert(lifeTime>=0);
                 void* cache = m_pointCache[insertIndex].m_userPersistentData;
 …
                 m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
+                m_pointCache[insertIndex].mConstraintRow[0].mAccumImpulse =  appliedImpulse;
+                m_pointCache[insertIndex].mConstraintRow[1].mAccumImpulse = appliedLateralImpulse1;
+                m_pointCache[insertIndex].mConstraintRow[2].mAccumImpulse = appliedLateralImpulse2;
                 m_pointCache[insertIndex].m_lifeTime = lifeTime;
 #else

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h

r5781	r8284
32	32
33	33	btPointCollector ()
34		: m_distance(btScalar(~~1e30~~)),m_hasResult(false)
	34	: m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false)
35	35	{
36	36	}
37	37
38		virtual void setShapeIdentifiers~~(int partId0,int index0, int partId1,int index1~~)
	38	virtual void setShapeIdentifiersA(int partId0,int index0)
39	39	{
40	40	(void)partId0;
41	41	(void)index0;
	42
	43	}
	44	virtual void setShapeIdentifiersB(int partId1,int index1)
	45	{
42	46	(void)partId1;
43	47	(void)index1;
44		~~//??~~
45	48	}
46	49

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

r5781	r8284
115	115	}
116	116	}
117		m_simplexSolver->addVertex( w, supVertexA , supVertexB);
	117	///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
	118	if (!m_simplexSolver->inSimplex(w))
	119	m_simplexSolver->addVertex( w, supVertexA , supVertexB);
	120
118	121	if (m_simplexSolver->closest(v))
119	122	{

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp

-                      r5781
+                      r8284
         m_numVertices = 0;
         m_needsUpdate = true;
         m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+        m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
         m_cachedBC.reset();
+}
 …
         for (i=0;i<numverts;i++)
+        {
+#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
+                if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
+#else
                 if (m_simplexVectorW[i] == w)
+#endif
                         found = true;
+        }

code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h

-                      r5781
+                      r8284
 #define VORONOI_SIMPLEX_MAX_VERTS 5
+///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure
+#define BT_USE_EQUAL_VERTEX_THRESHOLD
+#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
 struct btUsageBitfield{
 …
         btVector3       m_cachedV;
         btVector3       m_lastW;
+        btScalar        m_equalVertexThreshold;
         bool            m_cachedValidClosest;
         btSubSimplexClosestResult m_cachedBC;
 …
 public:
+        btVoronoiSimplexSolver()
+                :  m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
+        {
+        }
          void reset();
          void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
+         void   setEqualVertexThreshold(btScalar threshold)
+         {
+                 m_equalVertexThreshold = threshold;
+         }
+         btScalar       getEqualVertexThreshold() const
+         {
+                 return m_equalVertexThreshold;
+         }
          bool closest(btVector3& v);

code/branches/kicklib2/src/external/bullet/BulletDynamics/CMakeLists.txt

-                      r5929
+                      r8284
 COMPILATION_BEGIN BulletDynamicsCompilation.cpp
+        Character/btKinematicCharacterController.cpp
+        ConstraintSolver/btConeTwistConstraint.cpp
         ConstraintSolver/btContactConstraint.cpp
-        ConstraintSolver/btConeTwistConstraint.cpp
         ConstraintSolver/btGeneric6DofConstraint.cpp
+        ConstraintSolver/btGeneric6DofSpringConstraint.cpp
+        ConstraintSolver/btHinge2Constraint.cpp
         ConstraintSolver/btHingeConstraint.cpp
         ConstraintSolver/btPoint2PointConstraint.cpp
 …
         ConstraintSolver/btSolve2LinearConstraint.cpp
         ConstraintSolver/btTypedConstraint.cpp
+        ConstraintSolver/btUniversalConstraint.cpp
+        Dynamics/btContinuousDynamicsWorld.cpp
+        Dynamics/btDiscreteDynamicsWorld.cpp
+        Dynamics/btRigidBody.cpp
+        Dynamics/btSimpleDynamicsWorld.cpp
         Dynamics/Bullet-C-API.cpp
-        Dynamics/btDiscreteDynamicsWorld.cpp
-        Dynamics/btSimpleDynamicsWorld.cpp
-        Dynamics/btRigidBody.cpp
         Vehicle/btRaycastVehicle.cpp
         Vehicle/btWheelInfo.cpp
-        Character/btKinematicCharacterController.cpp
 COMPILATION_END
         # Headers
+        ConstraintSolver/btConeTwistConstraint.h
         ConstraintSolver/btConstraintSolver.h
         ConstraintSolver/btContactConstraint.h
         ConstraintSolver/btContactSolverInfo.h
-        ConstraintSolver/btConeTwistConstraint.h
         ConstraintSolver/btGeneric6DofConstraint.h
+        ConstraintSolver/btGeneric6DofSpringConstraint.h
+        ConstraintSolver/btHinge2Constraint.h
         ConstraintSolver/btHingeConstraint.h
         ConstraintSolver/btJacobianEntry.h
 …
         ConstraintSolver/btSolverConstraint.h
         ConstraintSolver/btTypedConstraint.h
+        ConstraintSolver/btUniversalConstraint.h
         Dynamics/btActionInterface.h
         Dynamics/btContinuousDynamicsWorld.h

code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btCharacterControllerInterface.h

r5781	r8284
31	31
32	32	virtual void setWalkDirection(const btVector3& walkDirection) = 0;
	33	virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
33	34	virtual void reset () = 0;
34	35	virtual void warp (const btVector3& origin) = 0;

code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

-                      r5781
+                      r8284
 . This notice may not be removed or altered from any source distribution.
 */
 #include "LinearMath/btIDebugDraw.h"
 …
 #include "btKinematicCharacterController.h"
+static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+// static helper method
+static btVector3
+getNormalizedVector(const btVector3& v)
+{
+        btVector3 n = v.normalized();
+        if (n.length() < SIMD_EPSILON) {
+                n.setValue(0, 0, 0);
+        }
+        return n;
+}
 ///@todo Interact with dynamic objects,
 …
+{
 public:
+        btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+        {
+                m_me = me;
+        btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
+        : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+        , m_me(me)
+        , m_up(up)
+        , m_minSlopeDot(minSlopeDot)
+        {
+        }
 …
+        {
                 if (convexResult.m_hitCollisionObject == m_me)
+                        return 1.0;
+                        return btScalar(1.0);
+                btVector3 hitNormalWorld;
+                if (normalInWorldSpace)
+                {
+                        hitNormalWorld = convexResult.m_hitNormalLocal;
+                } else
+                {
+                        ///need to transform normal into worldspace
+                        hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+                }
+                btScalar dotUp = m_up.dot(hitNormalWorld);
+                if (dotUp < m_minSlopeDot) {
+                        return btScalar(1.0);
+                }
                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
 …
 protected:
         btCollisionObject* m_me;
+        const btVector3 m_up;
+        btScalar m_minSlopeDot;
 };
 …
+{
         m_upAxis = upAxis;
         m_addedMargin = 0.02f;
+        m_addedMargin = 0.02;
         m_walkDirection.setValue(0,0,0);
         m_useGhostObjectSweepTest = true;
 …
         m_turnAngle = btScalar(0.0);
         m_convexShape=convexShape;
+        m_useWalkDirection = true;      // use walk direction by default, legacy behavior
+        m_velocityTimeInterval = 0.0;
+        m_verticalVelocity = 0.0;
+        m_verticalOffset = 0.0;
+        m_gravity = 9.8 * 3 ; // 3G acceleration.
+        m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
+        m_jumpSpeed = 10.0; // ?
+        m_wasOnGround = false;
+        m_wasJumping = false;
+        setMaxSlope(btRadians(45.0));
+}
 …
                                 const btManifoldPoint&pt = manifold->getContactPoint(p);
+                                if (pt.getDistance() < 0.0)
+                                btScalar dist = pt.getDistance();
+                                if (dist < 0.0)
+                                {
                                         if (pt.getDistance() < maxPen)
+                                        if (dist < maxPen)
+                                        {
                                                 maxPen = pt.getDistance();
+                                                maxPen = dist;
                                                 m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+                                        }
                                         m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2);
+                                        m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
                                         penetration = true;
                                 } else {
                                         //printf("touching %f\n", pt.getDistance());
+                                        //printf("touching %f\n", dist);
+                                }
+                        }
 …
         // phase 1: up
         btTransform start, end;
         m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
+        m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
         start.setIdentity ();
 …
         /* FIXME: Handle penetration properly */
         start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
+        start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
         end.setOrigin (m_targetPosition);
         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+        btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 …
         if (callback.hasHit())
+        {
+                // we moved up only a fraction of the step height
+                m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                // Only modify the position if the hit was a slope and not a wall or ceiling.
+                if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+                {
+                        // we moved up only a fraction of the step height
+                        m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+                        m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                }
+                m_verticalVelocity = 0.0;
+                m_verticalOffset = 0.0;
         } else {
                 m_currentStepOffset = m_stepHeight;
 …
 void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
+{
+        btVector3 originalDir = walkMove.normalized();
+        if (walkMove.length() < SIMD_EPSILON)
+        {
+                originalDir.setValue(0.f,0.f,0.f);
+        }
+//      printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
+        // printf("m_normalizedDirection=%f,%f,%f\n",
+        //      m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
         // phase 2: forward and strafe
         btTransform start, end;
         m_targetPosition = m_currentPosition + walkMove;
         start.setIdentity ();
         end.setIdentity ();
 …
         if (m_touchingContact)
+        {
+                if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
+                if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
+                {
                         updateTargetPositionBasedOnCollision (m_touchingNormal);
+                }
+        }
 …
                 start.setOrigin (m_currentPosition);
                 end.setOrigin (m_targetPosition);
+                btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+                btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
+                btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
                 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
                 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 …
+                {
                         // we moved only a fraction
+                        btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+                        if (hitDistance<0.f)
+                        {
+//                              printf("neg dist?\n");
+                        }
+                        /* If the distance is farther than the collision margin, move */
+                        if (hitDistance > m_addedMargin)
+                        {
+//                              printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
+                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                        }
+                        btScalar hitDistance;
+                        hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+//                      m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
                         updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
 …
                                 currentDir.normalize();
                                 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
                                 if (currentDir.dot(originalDir) <= btScalar(0.0))
+                                if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
+                                {
                                         break;
 …
                                 break;
+                        }
                 } else {
                         // we moved whole way
 …
         // phase 3: down
+        btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
+        btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
+        m_targetPosition -= (step_drop + gravity_drop);
+        /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
+        btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+        btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
+        btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
+        m_targetPosition -= (step_drop + gravity_drop);*/
+        btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+        if(downVelocity > 0.0 && downVelocity < m_stepHeight
+                && (m_wasOnGround || !m_wasJumping))
+        {
+                downVelocity = m_stepHeight;
+        }
+        btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+        m_targetPosition -= step_drop;
         start.setIdentity ();
 …
         end.setOrigin (m_targetPosition);
         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+        btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 …
                 // we dropped a fraction of the height -> hit floor
                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                m_verticalVelocity = 0.0;
+                m_verticalOffset = 0.0;
+                m_wasJumping = false;
         } else {
                 // we dropped the full height
 …
+}
+void btKinematicCharacterController::setWalkDirection
+(
+const btVector3& walkDirection
+)
+{
+        m_useWalkDirection = true;
+        m_walkDirection = walkDirection;
+        m_normalizedDirection = getNormalizedVector(m_walkDirection);
+}
+void btKinematicCharacterController::setVelocityForTimeInterval
+(
+const btVector3& velocity,
+btScalar timeInterval
+)
+{
+//      printf("setVelocity!\n");
+//      printf("  interval: %f\n", timeInterval);
+//      printf("  velocity: (%f, %f, %f)\n",
+//               velocity.x(), velocity.y(), velocity.z());
+        m_useWalkDirection = false;
+        m_walkDirection = velocity;
+        m_normalizedDirection = getNormalizedVector(m_walkDirection);
+        m_velocityTimeInterval = timeInterval;
+}
 void btKinematicCharacterController::reset ()
+{
 …
                 if (numPenetrationLoops > 4)
+                {
 //                      printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+                        //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
                         break;
+                }
 …
+}
+#include <stdio.h>
 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
+{
+//      printf("playerStep(): ");
+//      printf("  dt = %f", dt);
+        // quick check...
+        if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
+//              printf("\n");
+                return;         // no motion
+        }
+        m_wasOnGround = onGround();
+        // Update fall velocity.
+        m_verticalVelocity -= m_gravity * dt;
+        if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+        {
+                m_verticalVelocity = m_jumpSpeed;
+        }
+        if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+        {
+                m_verticalVelocity = -btFabs(m_fallSpeed);
+        }
+        m_verticalOffset = m_verticalVelocity * dt;
         btTransform xform;
         xform = m_ghostObject->getWorldTransform ();
 …
         stepUp (collisionWorld);
+        stepForwardAndStrafe (collisionWorld, m_walkDirection);
+        if (m_useWalkDirection) {
+                stepForwardAndStrafe (collisionWorld, m_walkDirection);
+        } else {
+                //printf("  time: %f", m_velocityTimeInterval);
+                // still have some time left for moving!
+                btScalar dtMoving =
+                        (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
+                m_velocityTimeInterval -= dt;
+                // how far will we move while we are moving?
+                btVector3 move = m_walkDirection * dtMoving;
+                //printf("  dtMoving: %f", dtMoving);
+                // okay, step
+                stepForwardAndStrafe(collisionWorld, move);
+        }
         stepDown (collisionWorld, dt);
+        // printf("\n");
         xform.setOrigin (m_currentPosition);
 …
         if (!canJump())
                 return;
+        m_verticalVelocity = m_jumpSpeed;
+        m_wasJumping = true;
 #if 0
 …
+}
+void btKinematicCharacterController::setGravity(btScalar gravity)
+{
+        m_gravity = gravity;
+}
+btScalar btKinematicCharacterController::getGravity() const
+{
+        return m_gravity;
+}
+void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
+{
+        m_maxSlopeRadians = slopeRadians;
+        m_maxSlopeCosine = btCos(slopeRadians);
+}
+btScalar btKinematicCharacterController::getMaxSlope() const
+{
+        return m_maxSlopeRadians;
+}
 bool btKinematicCharacterController::onGround () const
+{
+        return true;
+}
+void    btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}
+        return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+}
+btVector3* btKinematicCharacterController::getUpAxisDirections()
+{
+        static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+        return sUpAxisDirection;
+}
+void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.h

-                      r5781
+                      r8284
 */
 #ifndef KINEMATIC_CHARACTER_CONTROLLER_H
 #define KINEMATIC_CHARACTER_CONTROLLER_H
 …
 #include "btCharacterControllerInterface.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 class btCollisionShape;
 …
+{
 protected:
         btScalar m_halfHeight;
 …
         btConvexShape*  m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+        btScalar m_verticalVelocity;
+        btScalar m_verticalOffset;
         btScalar m_fallSpeed;
         btScalar m_jumpSpeed;
         btScalar m_maxJumpHeight;
+        btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
+        btScalar m_maxSlopeCosine;  // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
+        btScalar m_gravity;
         btScalar m_turnAngle;
 …
         ///this is the desired walk direction, set by the user
         btVector3       m_walkDirection;
+        btVector3       m_normalizedDirection;
         //some internal variables
 …
         btVector3 m_touchingNormal;
+        bool  m_wasOnGround;
+        bool  m_wasJumping;
         bool    m_useGhostObjectSweepTest;
+        bool    m_useWalkDirection;
+        btScalar        m_velocityTimeInterval;
+        int m_upAxis;
         int m_upAxis;
+        static btVector3* getUpAxisDirections();
         btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
         btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
 …
+        }
+        virtual void    setWalkDirection(const btVector3& walkDirection)
+        {
+                m_walkDirection = walkDirection;
+        }
+        /// This should probably be called setPositionIncrementPerSimulatorStep.
+        /// This is neither a direction nor a velocity, but the amount to
+        ///     increment the position each simulation iteration, regardless
+        ///     of dt.
+        /// This call will reset any velocity set by setVelocityForTimeInterval().
+        virtual void    setWalkDirection(const btVector3& walkDirection);
+        /// Caller provides a velocity with which the character should move for
+        ///     the given time period.  After the time period, velocity is reset
+        ///     to zero.
+        /// This call will reset any walk direction set by setWalkDirection().
+        /// Negative time intervals will result in no motion.
+        virtual void setVelocityForTimeInterval(const btVector3& velocity,
+                                btScalar timeInterval);
         void reset ();
 …
         void setMaxJumpHeight (btScalar maxJumpHeight);
         bool canJump () const;
         void jump ();
+        void setGravity(btScalar gravity);
+        btScalar getGravity() const;
+        /// The max slope determines the maximum angle that the controller can walk up.
+        /// The slope angle is measured in radians.
+        void setMaxSlope(btScalar slopeRadians);
+        btScalar getMaxSlope() const;
         btPairCachingGhostObject* getGhostObject();

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

-                      r5781
+                      r8284
 #include <new>
+//-----------------------------------------------------------------------------
+//#define CONETWIST_USE_OBSOLETE_SOLVER true
 #define CONETWIST_USE_OBSOLETE_SOLVER false
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
+//-----------------------------------------------------------------------------
+btConeTwistConstraint::btConeTwistConstraint()
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+}
+SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
+{
+        btVector3 vec = axis * invInertiaWorld;
+        return axis.dot(vec);
+}
 …
         m_maxMotorImpulse = btScalar(-1);
         setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
+        setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
         m_damping = btScalar(0.01);
         m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+}
+//-----------------------------------------------------------------------------
+        m_flags = 0;
+        m_linCFM = btScalar(0.f);
+        m_linERP = btScalar(0.7f);
+        m_angCFM = btScalar(0.f);
+}
 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 …
                 info->m_numConstraintRows = 3;
                 info->nub = 3;
                 calcAngleInfo2();
+                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
                 if(m_solveSwingLimit)
+                {
 …
+                }
+        }
+} // btConeTwistConstraint::getInfo1()
+}
+void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+        //always reserve 6 rows: object transform is not available on SPU
+        info->m_numConstraintRows = 6;
+        info->nub = 0;
+}
-//-----------------------------------------------------------------------------
 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+}
+void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
+        calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
         btAssert(!m_useSolveConstraintObsolete);
-        //retrieve matrices
-        btTransform body0_trans;
-        body0_trans = m_rbA.getCenterOfMassTransform();
-    btTransform body1_trans;
-        body1_trans = m_rbB.getCenterOfMassTransform();
     // set jacobian
     info->m_J1linearAxis[0] = 1;
     info->m_J1linearAxis[info->rowskip+1] = 1;
     info->m_J1linearAxis[2*info->rowskip+2] = 1;
         btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
+        btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
 …
                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
         btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+        btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 …
+        }
     // set right hand side
+    btScalar k = info->fps * info->erp;
+        btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
+    btScalar k = info->fps * linERP;
     int j;
         for (j=0; j<3; j++)
+    {
         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
                 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
                 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+                if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
+                {
+                        info->cfm[j*info->rowskip] = m_linCFM;
+                }
+    }
         int row = 3;
 …
                 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+                {
                         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+                        btTransform trA = transA*m_rbAFrame;
                         btVector3 p = trA.getBasis().getColumn(1);
                         btVector3 q = trA.getBasis().getColumn(2);
 …
                         info->m_constraintError[srow] = k * m_swingCorrection;
+                        info->cfm[srow] = 0.0f;
+                        if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+                        {
+                                info->cfm[srow] = m_angCFM;
+                        }
                         // m_swingCorrection is always positive or 0
                         info->m_lowerLimit[srow] = 0;
 …
                 btScalar k = info->fps * m_biasFactor;
                 info->m_constraintError[srow] = k * m_twistCorrection;
+                info->cfm[srow] = 0.0f;
+                if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+                {
+                        info->cfm[srow] = m_angCFM;
+                }
                 if(m_twistSpan > 0.0f)
+                {
 …
+}
+//-----------------------------------------------------------------------------
 void    btConeTwistConstraint::buildJacobian()
 …
                 m_accTwistLimitImpulse = btScalar(0.);
                 m_accSwingLimitImpulse = btScalar(0.);
+                m_accMotorImpulse = btVector3(0.,0.,0.);
                 if (!m_angularOnly)
 …
+                }
+                calcAngleInfo2();
+        }
+}
+//-----------------------------------------------------------------------------
+void    btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+        }
+}
+void    btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar   timeStep)
+{
+        #ifndef __SPU__
         if (m_useSolveConstraintObsolete)
+        {
 …
                         btVector3 vel1;
                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
                         btVector3 vel2;
                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
                         btVector3 vel = vel1 - vel2;
 …
                                 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
                                 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
                                 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
                                 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+                                bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+                                bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+                        }
 …
                         btTransform trACur = m_rbA.getCenterOfMassTransform();
                         btTransform trBCur = m_rbB.getCenterOfMassTransform();
                         btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
                         btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+                        btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
+                        btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
                         btTransform trAPred; trAPred.setIdentity();
                         btVector3 zerovec(0,0,0);
 …
                                 btVector3 impulseAxis =  impulse / impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                        }
+                }
                 else // no motor: do a little damping
+                {
                         const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
                         const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                        }
+                }
+                else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
+                {
+                        btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
+                        btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
                         btVector3 relVel = angVelB - angVelA;
                         if (relVel.length2() > SIMD_EPSILON)
 …
                                 btScalar  impulseMag  = impulse.length();
                                 btVector3 impulseAxis = impulse / impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                        }
+                }
 …
                         ///solve angular part
                         btVector3 angVelA;
                         bodyA.getAngularVelocity(angVelA);
+                        bodyA.internalGetAngularVelocity(angVelA);
                         btVector3 angVelB;
                         bodyB.getAngularVelocity(angVelB);
+                        bodyB.internalGetAngularVelocity(angVelB);
                         // solve swing limit
 …
                                 btVector3 noTwistSwingAxis = impulse / impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+                        }
 …
                                 btVector3 impulse = m_twistAxis * impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+                        }
+                }
+        }
+}
+//-----------------------------------------------------------------------------
+#else
+btAssert(0);
+#endif //__SPU__
+}
 void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
 …
+}
+//-----------------------------------------------------------------------------
+#ifndef __SPU__
 void btConeTwistConstraint::calcAngleInfo()
+{
 …
+                }
+        }
 } // btConeTwistConstraint::calcAngleInfo()
+}
+#endif //__SPU__
 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+//-----------------------------------------------------------------------------
 void btConeTwistConstraint::calcAngleInfo2()
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
         m_swingCorrection = btScalar(0.);
 …
         m_solveTwistLimit = false;
         m_solveSwingLimit = false;
+        // compute rotation of A wrt B (in constraint space)
+        if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
+        {       // it is assumed that setMotorTarget() was alredy called
+                // and motor target m_qTarget is within constraint limits
+                // TODO : split rotation to pure swing and pure twist
+                // compute desired transforms in world
+                btTransform trPose(m_qTarget);
+                btTransform trA = transA * m_rbAFrame;
+                btTransform trB = transB * m_rbBFrame;
+                btTransform trDeltaAB = trB * trPose * trA.inverse();
+                btQuaternion qDeltaAB = trDeltaAB.getRotation();
+                btVector3 swingAxis =   btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+                m_swingAxis = swingAxis;
+                m_swingAxis.normalize();
+                m_swingCorrection = qDeltaAB.getAngle();
+                if(!btFuzzyZero(m_swingCorrection))
+                {
+                        m_solveSwingLimit = true;
+                }
+                return;
+        }
+        {
                 // compute rotation of A wrt B (in constraint space)
                 btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
                 btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
+                btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
+                btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
                 btQuaternion qAB = qB.inverse() * qA;
                 // split rotation into cone and twist
                 // (all this is done from B's perspective. Maybe I should be averaging axes...)
 …
                                 m_kSwing =  btScalar(1.) /
                                         (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
                                          getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+                                        (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
+                                         computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
+                        }
+                }
 …
                         // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
                         // anyway, we have either hinge or fixed joint
                         btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
                         btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
                         btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
                         btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
+                        btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+                        btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+                        btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
+                        btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
                         btVector3 target;
                         btScalar x = ivB.dot(ivA);
 …
                                 m_kTwist = btScalar(1.) /
                                         (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
                                          getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+                                        (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
+                                         computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
+                        }
 …
+                }
+        }
 } // btConeTwistConstraint::calcAngleInfo2()
+}
 …
+}
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
+void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
+{
+        switch(num)
+        {
+                case BT_CONSTRAINT_ERP :
+                case BT_CONSTRAINT_STOP_ERP :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                m_linERP = value;
+                                m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
+                        }
+                        else
+                        {
+                                m_biasFactor = value;
+                        }
+                        break;
+                case BT_CONSTRAINT_CFM :
+                case BT_CONSTRAINT_STOP_CFM :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                m_linCFM = value;
+                                m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
+                        }
+                        else
+                        {
+                                m_angCFM = value;
+                                m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
+                        }
+                        break;
+                default:
+                        btAssertConstrParams(0);
+                        break;
+        }
+}
+///return the local value of parameter
+btScalar btConeTwistConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal = 0;
+        switch(num)
+        {
+                case BT_CONSTRAINT_ERP :
+                case BT_CONSTRAINT_STOP_ERP :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
+                                retVal = m_linERP;
+                        }
+                        else if((axis >= 3) && (axis < 6))
+                        {
+                                retVal = m_biasFactor;
+                        }
+                        else
+                        {
+                                btAssertConstrParams(0);
+                        }
+                        break;
+                case BT_CONSTRAINT_CFM :
+                case BT_CONSTRAINT_STOP_CFM :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
+                                retVal = m_linCFM;
+                        }
+                        else if((axis >= 3) && (axis < 6))
+                        {
+                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
+                                retVal = m_angCFM;
+                        }
+                        else
+                        {
+                                btAssertConstrParams(0);
+                        }
+                        break;
+                default :
+                        btAssertConstrParams(0);
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

-                      r5781
+                      r8284
+/*
+Overview:
+btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
+It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
+It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
+Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
+(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
+In the contraint's frame of reference:
+twist is along the x-axis,
+and swing 1 and 2 are along the z and y axes respectively.
+*/
 #ifndef CONETWISTCONSTRAINT_H
 #define CONETWISTCONSTRAINT_H
 …
 class btRigidBody;
+enum btConeTwistFlags
+{
+        BT_CONETWIST_FLAGS_LIN_CFM = 1,
+        BT_CONETWIST_FLAGS_LIN_ERP = 2,
+        BT_CONETWIST_FLAGS_ANG_CFM = 4
+};
 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
 …
         btVector3        m_accMotorImpulse;
+        // parameters
+        int                     m_flags;
+        btScalar        m_linCFM;
+        btScalar        m_linERP;
+        btScalar        m_angCFM;
+protected:
+        void init();
+        void computeConeLimitInfo(const btQuaternion& qCone, // in
+                btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+        void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+                btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+        void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
 public:
 …
         btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
-        btConeTwistConstraint();
         virtual void    buildJacobian();
         virtual void getInfo1 (btConstraintInfo1* info);
+        void    getInfo1NonVirtual(btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+        virtual void    solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar  timeStep);
         void    updateRHS(btScalar      timeStep);
 …
+        }
+        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+        // setLimit(), a few notes:
+        // _softness:
+        //              0->1, recommend ~0.8->1.
+        //              describes % of limits where movement is free.
+        //              beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
+        // _biasFactor:
+        //              0->1?, recommend 0.3 +/-0.3 or so.
+        //              strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
+        // __relaxationFactor:
+        //              0->1, recommend to stay near 1.
+        //              the lower the value, the less the constraint will fight velocities which violate the angular limits.
+        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+        {
                 m_swingSpan1 = _swingSpan1;
 …
         void calcAngleInfo();
         void calcAngleInfo2();
+        void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
         inline btScalar getSwingSpan1()
 …
         btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
+protected:
+        void init();
+        void computeConeLimitInfo(const btQuaternion& qCone, // in
+                btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+        void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+                btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+        void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+        ///If no axis is provided, it uses the default axis for this constraint.
+        virtual void setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btConeTwistConstraintData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame;
+        btTransformFloatData m_rbBFrame;
+        //limits
+        float   m_swingSpan1;
+        float   m_swingSpan2;
+        float   m_twistSpan;
+        float   m_limitSoftness;
+        float   m_biasFactor;
+        float   m_relaxationFactor;
+        float   m_damping;
+        char m_pad[4];
+};
+SIMD_FORCE_INLINE int   btConeTwistConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btConeTwistConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char*   btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
+        btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
+        m_rbAFrame.serializeFloat(cone->m_rbAFrame);
+        m_rbBFrame.serializeFloat(cone->m_rbBFrame);
+        cone->m_swingSpan1 = float(m_swingSpan1);
+        cone->m_swingSpan2 = float(m_swingSpan2);
+        cone->m_twistSpan = float(m_twistSpan);
+        cone->m_limitSoftness = float(m_limitSoftness);
+        cone->m_biasFactor = float(m_biasFactor);
+        cone->m_relaxationFactor = float(m_relaxationFactor);
+        cone->m_damping = float(m_damping);
+        return "btConeTwistConstraintData";
+}
 #endif //CONETWISTCONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

-                      r5781
+                      r8284
 . This notice may not be removed or altered from any source distribution.
 */
+#include "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
+        m_contactManifold(*contactManifold)
+{
+}
+btContactConstraint::~btContactConstraint()
+{
+}
+void    btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+{
+        m_contactManifold = *contactManifold;
+}
+void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+{
+}
+void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+{
+}
+void    btContactConstraint::buildJacobian()
+{
+}
 …
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollision(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
-        const btVector3& normal = contactPoint.m_normalWorldOnB;
-        //constant over all iterations
-        btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
-        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-        btVector3 vel = vel1 - vel2;
-        btScalar rel_vel;
-        rel_vel = normal.dot(vel);
-        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-        // btScalar damping = solverInfo.m_damping ;
-        btScalar Kerp = solverInfo.m_erp;
-        btScalar Kcor = Kerp *Kfps;
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar distance = cpd->m_penetration;
-        btScalar positionalError = Kcor *-distance;
-        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-        btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-        btScalar sum = oldNormalImpulse + normalImpulse;
-        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-        if (body1.getInvMass())
+        {
-                body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+        }
-        if (body2.getInvMass())
+        {
-                body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+        }
-#else //USE_INTERNAL_APPLY_IMPULSE
-        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-        return normalImpulse;
+}
-btScalar resolveSingleFriction(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        (void)solverInfo;
-        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar combinedFriction = cpd->m_friction;
-        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-        if (cpd->m_appliedImpulse>btScalar(0.))
-        //friction
+        {
-                //apply friction in the 2 tangential directions
-                // 1st tangent
-                btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                btVector3 vel = vel1 - vel2;
-                btScalar j1,j2;
+                {
-                        btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        j1 = -vrel * cpd->m_jacDiagABInvTangent0;
-                        btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
-                        cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
-                        btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
-                        btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
-                        j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
+                }
+                {
-                        // 2nd tangent
-                        btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        j2 = -vrel * cpd->m_jacDiagABInvTangent1;
-                        btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
-                        cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
-                        btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
-                        btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
-                        j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
+                }
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-        if (body1.getInvMass())
+        {
-                body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
-                body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
+        }
-        if (body2.getInvMass())
+        {
-                body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
-                body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);
+        }
-#else //USE_INTERNAL_APPLY_IMPULSE
-        body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
-        body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
+        }
-        return cpd->m_appliedImpulse;
+}
-btScalar resolveSingleFrictionOriginal(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo);
-btScalar resolveSingleFrictionOriginal(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        (void)solverInfo;
-        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar combinedFriction = cpd->m_friction;
-        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-        //if (contactPoint.m_appliedImpulse>btScalar(0.))
-        //friction
+        {
-                //apply friction in the 2 tangential directions
+                {
-                        // 1st tangent
-                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                        btVector3 vel = vel1 - vel2;
-                        btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
-                        btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
-                        btSetMin(total, limit);
-                        btSetMax(total, -limit);
-                        j = total - cpd->m_accumulatedTangentImpulse0;
-                        cpd->m_accumulatedTangentImpulse0 = total;
-                        body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
-                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
+                }
+                {
-                        // 2nd tangent
-                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                        btVector3 vel = vel1 - vel2;
-                        btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
-                        btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
-                        btSetMin(total, limit);
-                        btSetMax(total, -limit);
-                        j = total - cpd->m_accumulatedTangentImpulse1;
-                        cpd->m_accumulatedTangentImpulse1 = total;
-                        body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
-                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
+                }
+        }
-        return cpd->m_appliedImpulse;
+}
-//velocity + friction
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollisionCombined(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-        const btVector3& normal = contactPoint.m_normalWorldOnB;
-        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-        btVector3 vel = vel1 - vel2;
-        btScalar rel_vel;
-        rel_vel = normal.dot(vel);
-        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-        //btScalar damping = solverInfo.m_damping ;
-        btScalar Kerp = solverInfo.m_erp;
-        btScalar Kcor = Kerp *Kfps;
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar distance = cpd->m_penetration;
-        btScalar positionalError = Kcor *-distance;
-        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-        btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-        btScalar sum = oldNormalImpulse + normalImpulse;
-        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-        if (body1.getInvMass())
+        {
-                body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+        }
-        if (body2.getInvMass())
+        {
-                body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+        }
-#else //USE_INTERNAL_APPLY_IMPULSE
-        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
+        {
-                //friction
-                btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                btVector3 vel = vel1 - vel2;
-                rel_vel = normal.dot(vel);
-                btVector3 lat_vel = vel - normal * rel_vel;
-                btScalar lat_rel_vel = lat_vel.length();
-                btScalar combinedFriction = cpd->m_friction;
-                if (cpd->m_appliedImpulse > 0)
-                if (lat_rel_vel > SIMD_EPSILON)
+                {
-                        lat_vel /= lat_rel_vel;
-                        btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
-                        btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
-                        btScalar friction_impulse = lat_rel_vel /
-                                (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-                        btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
-                        btSetMin(friction_impulse, normal_impulse);
-                        btSetMax(friction_impulse, -normal_impulse);
-                        body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
-                        body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+                }
+        }
-        return normalImpulse;
+}
-btScalar resolveSingleFrictionEmpty(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo);
-btScalar resolveSingleFrictionEmpty(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        (void)contactPoint;
-        (void)body1;
-        (void)body2;
-        (void)solverInfo;
-        return btScalar(0.);
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h

-                      r5781
+                      r8284
 #define CONTACT_CONSTRAINT_H
+///@todo: make into a proper class working with the iterative constraint solver
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
+struct btContactSolverInfo;
+class btManifoldPoint;
+///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
+ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
+{
+protected:
+enum {
+        DEFAULT_CONTACT_SOLVER_TYPE=0,
+        CONTACT_SOLVER_TYPE1,
+        CONTACT_SOLVER_TYPE2,
+        USER_CONTACT_SOLVER_TYPE1,
+        MAX_CONTACT_SOLVER_TYPES
+        btPersistentManifold m_contactManifold;
+public:
+        btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
+        void    setContactManifold(btPersistentManifold* contactManifold);
+        btPersistentManifold* getContactManifold()
+        {
+                return &m_contactManifold;
+        }
+        const btPersistentManifold* getContactManifold() const
+        {
+                return &m_contactManifold;
+        }
+        virtual ~btContactConstraint();
+        virtual void getInfo1 (btConstraintInfo1* info);
+        virtual void getInfo2 (btConstraintInfo2* info);
+        ///obsolete methods
+        virtual void    buildJacobian();
 };
+typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
+                                                                         btRigidBody& body2,
+                                                                         class btManifoldPoint& contactPoint,
+                                                                         const btContactSolverInfo& info);
+///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
+struct btConstraintPersistentData
+{
+        inline btConstraintPersistentData()
+        :m_appliedImpulse(btScalar(0.)),
+        m_prevAppliedImpulse(btScalar(0.)),
+        m_accumulatedTangentImpulse0(btScalar(0.)),
+        m_accumulatedTangentImpulse1(btScalar(0.)),
+        m_jacDiagABInv(btScalar(0.)),
+        m_persistentLifeTime(0),
+        m_restitution(btScalar(0.)),
+        m_friction(btScalar(0.)),
+        m_penetration(btScalar(0.)),
+        m_contactSolverFunc(0),
+        m_frictionSolverFunc(0)
+        {
+        }
+                                /// total applied impulse during most recent frame
+                        btScalar        m_appliedImpulse;
+                        btScalar        m_prevAppliedImpulse;
+                        btScalar        m_accumulatedTangentImpulse0;
+                        btScalar        m_accumulatedTangentImpulse1;
+                        btScalar        m_jacDiagABInv;
+                        btScalar        m_jacDiagABInvTangent0;
+                        btScalar        m_jacDiagABInvTangent1;
+                        int             m_persistentLifeTime;
+                        btScalar        m_restitution;
+                        btScalar        m_friction;
+                        btScalar        m_penetration;
+                        btVector3       m_frictionWorldTangential0;
+                        btVector3       m_frictionWorldTangential1;
+                        btVector3       m_frictionAngularComponent0A;
+                        btVector3       m_frictionAngularComponent0B;
+                        btVector3       m_frictionAngularComponent1A;
+                        btVector3       m_frictionAngularComponent1B;
+                        //some data doesn't need to be persistent over frames: todo: clean/reuse this
+                        btVector3       m_angularComponentA;
+                        btVector3       m_angularComponentB;
+                        ContactSolverFunc       m_contactSolverFunc;
+                        ContactSolverFunc       m_frictionSolverFunc;
+};
+///bilateral constraint between two dynamic objects
+///positive distance = separation, negative distance = penetration
+///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
                       btRigidBody& body2, const btVector3& pos2,
 …
-///contact constraint resolution:
-///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
-///positive distance = separation, negative distance = penetration
-btScalar resolveSingleCollision(
-        btRigidBody& body1,
-        btRigidBody& body2,
-                btManifoldPoint& contactPoint,
-                 const btContactSolverInfo& info);
-btScalar resolveSingleFriction(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo
-                );
-btScalar resolveSingleCollisionCombined(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo
-                );
 #endif //CONTACT_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

-                      r5781
+                      r8284
         btScalar        m_tau;
         btScalar        m_damping;
+        btScalar        m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
         btScalar        m_friction;
         btScalar        m_timeStep;
 …
         int                     m_solverMode;
         int     m_restingContactRestitutionThreshold;
+        int                     m_minimumSolverBatchSize;
 …
                 m_linearSlop = btScalar(0.0);
                 m_warmstartingFactor=btScalar(0.85);
                 m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
+                m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
                 m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
+                m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
+        }
 };

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

-                      r5781
+                      r8284
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
 #include <new>
 #define D6_USE_OBSOLETE_METHOD false
+//-----------------------------------------------------------------------------
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE),
+m_useLinearReferenceFrameA(true),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+}
+//-----------------------------------------------------------------------------
+#define D6_USE_FRAME_OFFSET true
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
 …
 , m_frameInB(frameInB),
 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+m_flags(0),
 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+}
+//-----------------------------------------------------------------------------
+        calculateTransforms();
+}
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
+        : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
+                m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameB),
+                m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+                m_flags(0),
+                m_useSolveConstraintObsolete(false)
+{
+        ///not providing rigidbody A means implicitly using worldspace for body A
+        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+        calculateTransforms();
+}
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
+//-----------------------------------------------------------------------------
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
 …
+}
+//-----------------------------------------------------------------------------
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
 …
                 return 0;
+        }
         if (test_value < m_loLimit)
+        {
 …
+}
+//-----------------------------------------------------------------------------
 btScalar btRotationalLimitMotor::solveAngularLimits(
         btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
         btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
+        btRigidBody * body0, btRigidBody * body1 )
+{
         if (needApplyTorques()==false) return 0.0f;
 …
         if (m_currentLimit!=0)
+        {
                 target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+                target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
                 maxMotorForce = m_maxLimitForce;
+        }
 …
         btVector3 angVelA;
         bodyA.getAngularVelocity(angVelA);
+        body0->internalGetAngularVelocity(angVelA);
         btVector3 angVelB;
         bodyB.getAngularVelocity(angVelB);
+        body1->internalGetAngularVelocity(angVelB);
         btVector3 vel_diff;
 …
         // sort with accumulated impulses
         btScalar        lo = btScalar(-1e30);
         btScalar        hi = btScalar(1e30);
+        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
+        btScalar        hi = btScalar(BT_LARGE_FLOAT);
         btScalar oldaccumImpulse = m_accumulatedImpulse;
 …
         //body1->applyTorqueImpulse(-motorImp);
         bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
         bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
+        body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
+        body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
 …
         m_currentLimitError[limitIndex] = btScalar(0.f);
         return 0;
 } // btTranslationalLimitMotor::testLimitValue()
+//-----------------------------------------------------------------------------
+}
 btScalar btTranslationalLimitMotor::solveLinearAxis(
         btScalar timeStep,
         btScalar jacDiagABInv,
         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
 …
         btVector3 vel1;
         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+        body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
         btVector3 vel2;
         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+        body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
         btVector3 vel = vel1 - vel2;
 …
         //positional error (zeroth order error)
         btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
         btScalar        lo = btScalar(-1e30);
         btScalar        hi = btScalar(1e30);
+        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
+        btScalar        hi = btScalar(BT_LARGE_FLOAT);
         btScalar minLimit = m_lowerLimit[limit_index];
 …
         btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
         btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
         bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
         bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+        body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+        body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
 …
+}
-//-----------------------------------------------------------------------------
 void btGeneric6DofConstraint::calculateTransforms()
+{
+        m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+        m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+        m_calculatedTransformA = transA * m_frameInA;
+        m_calculatedTransformB = transB * m_frameInB;
         calculateLinearInfo();
         calculateAngleInfo();
+}
+//-----------------------------------------------------------------------------
+        if(m_useOffsetForConstraintFrame)
+        {       //  get weight factors depending on masses
+                btScalar miA = getRigidBodyA().getInvMass();
+                btScalar miB = getRigidBodyB().getInvMass();
+                m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+                btScalar miS = miA + miB;
+                if(miS > btScalar(0.f))
+                {
+                        m_factA = miB / miS;
+                }
+                else
+                {
+                        m_factA = btScalar(0.5f);
+                }
+                m_factB = btScalar(1.0f) - m_factA;
+        }
+}
 void btGeneric6DofConstraint::buildLinearJacobian(
 …
+}
+//-----------------------------------------------------------------------------
 void btGeneric6DofConstraint::buildAngularJacobian(
 …
+}
+//-----------------------------------------------------------------------------
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
+{
         btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+        angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+        m_angularLimits[axis_index].m_currentPosition = angle;
         //test limits
         m_angularLimits[axis_index].testLimitValue(angle);
 …
+}
+//-----------------------------------------------------------------------------
 void btGeneric6DofConstraint::buildJacobian()
+{
+#ifndef __SPU__
         if (m_useSolveConstraintObsolete)
+        {
 …
+                }
                 //calculates transform
                 calculateTransforms();
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
 …
+        }
+}
+//-----------------------------------------------------------------------------
+#endif //__SPU__
+}
 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 …
+        {
                 //prepare constraint
                 calculateTransforms();
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 info->m_numConstraintRows = 0;
                 info->nub = 6;
 …
+}
+//-----------------------------------------------------------------------------
+void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        } else
+        {
+                //pre-allocate all 6
+                info->m_numConstraintRows = 6;
+                info->nub = 0;
+        }
+}
 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+{
         btAssert(!m_useSolveConstraintObsolete);
+        int row = setLinearLimits(info);
+        setAngularLimits(info, row);
+}
+//-----------------------------------------------------------------------------
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
+{
+        btGeneric6DofConstraint * d6constraint = this;
+        int row = 0;
+        const btTransform& transA = m_rbA.getCenterOfMassTransform();
+        const btTransform& transB = m_rbB.getCenterOfMassTransform();
+        const btVector3& linVelA = m_rbA.getLinearVelocity();
+        const btVector3& linVelB = m_rbB.getLinearVelocity();
+        const btVector3& angVelA = m_rbA.getAngularVelocity();
+        const btVector3& angVelB = m_rbB.getAngularVelocity();
+        if(m_useOffsetForConstraintFrame)
+        { // for stability better to solve angular limits first
+                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+        else
+        { // leave old version for compatibility
+                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+}
+void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+        //prepare constraint
+        calculateTransforms(transA,transB);
+        int i;
+        for (i=0;i<3 ;i++ )
+        {
+                testAngularLimitMotor(i);
+        }
+        if(m_useOffsetForConstraintFrame)
+        { // for stability better to solve angular limits first
+                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+        else
+        { // leave old version for compatibility
+                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+}
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+//      int row = 0;
         //solve linear limits
         btRotationalLimitMotor limot;
 …
                         limot.m_bounce = btScalar(0.f);
                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+                        limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
                         limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
                         limot.m_damping  = m_linearLimits.m_damping;
                         limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
-                        limot.m_ERP  = m_linearLimits.m_restitution;
                         limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
                         limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
 …
                         limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
                         btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
+                        row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
+                        int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
+                        limot.m_normalCFM       = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+                        limot.m_stopCFM         = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+                        limot.m_stopERP         = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+                        if(m_useOffsetForConstraintFrame)
+                        {
+                                int indx1 = (i + 1) % 3;
+                                int indx2 = (i + 2) % 3;
+                                int rotAllowed = 1; // rotations around orthos to current axis
+                                if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+                                {
+                                        rotAllowed = 0;
+                                }
+                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+                        }
+                        else
+                        {
+                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+                        }
+                }
+        }
 …
+}
+//-----------------------------------------------------------------------------
 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
         btGeneric6DofConstraint * d6constraint = this;
 …
+                {
                         btVector3 axis = d6constraint->getAxis(i);
+                        row += get_limit_motor_info2(
+                                d6constraint->getRotationalLimitMotor(i),
+                                &m_rbA,
+                                &m_rbB,
+                                info,row,axis,1);
+                        int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
+                        if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
+                        {
+                                m_angularLimits[i].m_normalCFM = info->cfm[0];
+                        }
+                        if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
+                        {
+                                m_angularLimits[i].m_stopCFM = info->cfm[0];
+                        }
+                        if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
+                        {
+                                m_angularLimits[i].m_stopERP = info->erp;
+                        }
+                        row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
+                                                                                                transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+                }
+        }
 …
+}
+//-----------------------------------------------------------------------------
+void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar  timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                m_timeStep = timeStep;
+                //calculateTransforms();
+                int i;
+                // linear
+                btVector3 pointInA = m_calculatedTransformA.getOrigin();
+                btVector3 pointInB = m_calculatedTransformB.getOrigin();
+                btScalar jacDiagABInv;
+                btVector3 linear_axis;
+                for (i=0;i<3;i++)
+                {
+                        if (m_linearLimits.isLimited(i))
+                        {
+                                jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+                                if (m_useLinearReferenceFrameA)
+                                        linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+                                else
+                                        linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+                                m_linearLimits.solveLinearAxis(
+                                        m_timeStep,
+                                        jacDiagABInv,
+                                        m_rbA,bodyA,pointInA,
+                                        m_rbB,bodyB,pointInB,
+                                        i,linear_axis, m_AnchorPos);
+                        }
+                }
+                // angular
+                btVector3 angular_axis;
+                btScalar angularJacDiagABInv;
+                for (i=0;i<3;i++)
+                {
+                        if (m_angularLimits[i].needApplyTorques())
+                        {
+                                // get axis
+                                angular_axis = getAxis(i);
+                                angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+                                m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
+                        }
+                }
+        }
+}
+//-----------------------------------------------------------------------------
 void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
 …
+}
+//-----------------------------------------------------------------------------
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 …
+}
+//-----------------------------------------------------------------------------
+btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
+{
+        return m_calculatedAxisAngleDiff[axis_index];
+}
+//-----------------------------------------------------------------------------
+btScalar        btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+        return m_calculatedLinearDiff[axisIndex];
+}
+btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
+{
+        return m_calculatedAxisAngleDiff[axisIndex];
+}
 void btGeneric6DofConstraint::calcAnchorPos(void)
 …
         m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
         return;
 } // btGeneric6DofConstraint::calcAnchorPos()
+//-----------------------------------------------------------------------------
+}
 void btGeneric6DofConstraint::calculateLinearInfo()
 …
         for(int i = 0; i < 3; i++)
+        {
+                m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+        }
 } // btGeneric6DofConstraint::calculateLinearInfo()
+//-----------------------------------------------------------------------------
+}
 int btGeneric6DofConstraint::get_limit_motor_info2(
         btRotationalLimitMotor * limot,
         btRigidBody * body0, btRigidBody * body1,
         btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
+        const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+        btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
+{
     int srow = row * info->rowskip;
 …
             J2[srow+2] = -ax1[2];
+        }
         if((!rotational) && limit)
+        if((!rotational))
+        {
+                        btVector3 ltd;  // Linear Torque Decoupling vector
+                        btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
+                        ltd = c.cross(ax1);
+            info->m_J1angularAxis[srow+0] = ltd[0];
+            info->m_J1angularAxis[srow+1] = ltd[1];
+            info->m_J1angularAxis[srow+2] = ltd[2];
+                        c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
+                        ltd = -c.cross(ax1);
+                        info->m_J2angularAxis[srow+0] = ltd[0];
+            info->m_J2angularAxis[srow+1] = ltd[1];
+            info->m_J2angularAxis[srow+2] = ltd[2];
+                        if (m_useOffsetForConstraintFrame)
+                        {
+                                btVector3 tmpA, tmpB, relA, relB;
+                                // get vector from bodyB to frameB in WCS
+                                relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+                                // get its projection to constraint axis
+                                btVector3 projB = ax1 * relB.dot(ax1);
+                                // get vector directed from bodyB to constraint axis (and orthogonal to it)
+                                btVector3 orthoB = relB - projB;
+                                // same for bodyA
+                                relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+                                btVector3 projA = ax1 * relA.dot(ax1);
+                                btVector3 orthoA = relA - projA;
+                                // get desired offset between frames A and B along constraint axis
+                                btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
+                                // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
+                                btVector3 totalDist = projA + ax1 * desiredOffs - projB;
+                                // get offset vectors relA and relB
+                                relA = orthoA + totalDist * m_factA;
+                                relB = orthoB - totalDist * m_factB;
+                                tmpA = relA.cross(ax1);
+                                tmpB = relB.cross(ax1);
+                                if(m_hasStaticBody && (!rotAllowed))
+                                {
+                                        tmpA *= m_factA;
+                                        tmpB *= m_factB;
+                                }
+                                int i;
+                                for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
+                                for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+                        } else
+                        {
+                                btVector3 ltd;  // Linear Torque Decoupling vector
+                                btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
+                                ltd = c.cross(ax1);
+                                info->m_J1angularAxis[srow+0] = ltd[0];
+                                info->m_J1angularAxis[srow+1] = ltd[1];
+                                info->m_J1angularAxis[srow+2] = ltd[2];
+                                c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+                                ltd = -c.cross(ax1);
+                                info->m_J2angularAxis[srow+0] = ltd[0];
+                                info->m_J2angularAxis[srow+1] = ltd[1];
+                                info->m_J2angularAxis[srow+2] = ltd[2];
+                        }
+        }
         // if we're limited low and high simultaneously, the joint motor is
 …
         if (powered)
+        {
             info->cfm[srow] = 0.0f;
+                        info->cfm[srow] = limot->m_normalCFM;
             if(!limit)
+            {
+                info->m_constraintError[srow] += limot->m_targetVelocity;
+                                btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+                                btScalar mot_fact = getMotorFactor(     limot->m_currentPosition,
+                                                                                                        limot->m_loLimit,
+                                                                                                        limot->m_hiLimit,
+                                                                                                        tag_vel,
+                                                                                                        info->fps * limot->m_stopERP);
+                                info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
 …
         if(limit)
+        {
             btScalar k = info->fps * limot->m_ERP;
+            btScalar k = info->fps * limot->m_stopERP;
                         if(!rotational)
+                        {
 …
                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+                        }
             info->cfm[srow] = 0.0f;
+                        info->cfm[srow] = limot->m_stopCFM;
             if (limot->m_loLimit == limot->m_hiLimit)
             {   // limited low and high simultaneously
 …
                     if (rotational)
+                    {
+                        vel = body0->getAngularVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getAngularVelocity().dot(ax1);
+                        vel = angVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+//                        if (body1)
+                            vel -= angVelB.dot(ax1);
+                    }
                     else
+                    {
+                        vel = body0->getLinearVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getLinearVelocity().dot(ax1);
+                        vel = linVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+//                        if (body1)
+                            vel -= linVelB.dot(ax1);
+                    }
                     // only apply bounce if the velocity is incoming, and if the
 …
+}
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+        ///If no axis is provided, it uses the default axis for this constraint.
+void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
+{
+        if((axis >= 0) && (axis < 3))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                m_linearLimits.m_stopERP[axis] = value;
+                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                m_linearLimits.m_stopCFM[axis] = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                m_linearLimits.m_normalCFM[axis] = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else if((axis >=3) && (axis < 6))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                m_angularLimits[axis - 3].m_stopERP = value;
+                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                m_angularLimits[axis - 3].m_stopCFM = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                m_angularLimits[axis - 3].m_normalCFM = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+}
+        ///return the local value of parameter
+btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal = 0;
+        if((axis >= 0) && (axis < 3))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_linearLimits.m_stopERP[axis];
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_linearLimits.m_stopCFM[axis];
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_linearLimits.m_normalCFM[axis];
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else if((axis >=3) && (axis < 6))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_angularLimits[axis - 3].m_stopERP;
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_angularLimits[axis - 3].m_stopCFM;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_angularLimits[axis - 3].m_normalCFM;
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

-                      r5781
+                      r8284
 . This notice may not be removed or altered from any source distribution.
 */
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
 /*
 -09-09
 …
     btScalar m_damping;//!< Damping.
     btScalar m_limitSoftness;//! Relaxation factor
+    btScalar m_ERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_normalCFM;//!< Constraint force mixing factor
+    btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
     btScalar m_bounce;//!< restitution factor
     bool m_enableMotor;
 …
     //!@{
     btScalar m_currentLimitError;//!  How much is violated this limit
+    btScalar m_currentPosition;     //!  current value of angle
     int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
     btScalar m_accumulatedImpulse;
 …
         m_maxMotorForce = 0.1f;
         m_maxLimitForce = 300.0f;
+        m_loLimit = -SIMD_INFINITY;
+        m_hiLimit = SIMD_INFINITY;
+        m_ERP = 0.5f;
+        m_loLimit = 1.0f;
+        m_hiLimit = -1.0f;
+                m_normalCFM = 0.f;
+                m_stopERP = 0.2f;
+                m_stopCFM = 0.f;
         m_bounce = 0.0f;
         m_damping = 1.0f;
 …
         m_loLimit = limot.m_loLimit;
         m_hiLimit = limot.m_hiLimit;
+        m_ERP = limot.m_ERP;
+                m_normalCFM = limot.m_normalCFM;
+                m_stopERP = limot.m_stopERP;
+                m_stopCFM =     limot.m_stopCFM;
         m_bounce = limot.m_bounce;
         m_currentLimit = limot.m_currentLimit;
 …
         //! apply the correction impulses for two bodies
     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
 };
 …
     btScalar    m_damping;//!< Damping for linear limit
     btScalar    m_restitution;//! Bounce parameter for linear limit
+        btVector3       m_normalCFM;//!< Constraint force mixing factor
+    btVector3   m_stopERP;//!< Error tolerance factor when joint is at limit
+        btVector3       m_stopCFM;//!< Constraint force mixing factor when joint is at limit
     //!@}
         bool            m_enableMotor[3];
 …
     btVector3   m_maxMotorForce;//!< max force on motor
     btVector3   m_currentLimitError;//!  How much is violated this limit
+    btVector3   m_currentLinearDiff;//!  Current relative offset of constraint frames
     int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 …
         m_upperLimit.setValue(0.f,0.f,0.f);
         m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+                m_normalCFM.setValue(0.f, 0.f, 0.f);
+                m_stopERP.setValue(0.2f, 0.2f, 0.2f);
+                m_stopCFM.setValue(0.f, 0.f, 0.f);
         m_limitSoftness = 0.7f;
 …
         m_damping = other.m_damping;
         m_restitution = other.m_restitution;
+                m_normalCFM = other.m_normalCFM;
+                m_stopERP = other.m_stopERP;
+                m_stopCFM = other.m_stopCFM;
                 for(int i=0; i < 3; i++)
+                {
 …
         btScalar timeStep,
         btScalar jacDiagABInv,
         btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
         btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
 …
 };
+enum bt6DofFlags
+{
+        BT_6DOF_FLAGS_CFM_NORM = 1,
+        BT_6DOF_FLAGS_CFM_STOP = 2,
+        BT_6DOF_FLAGS_ERP_STOP = 4
+};
+#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 …
 <li> Angulars limits have these possible ranges:
 <table border=1 >
+<tr
+<tr>
         <td><b>AXIS</b></td>
         <td><b>MIN ANGLE</b></td>
         <td><b>MAX ANGLE</b></td>
+</tr><tr>
         <td>X</td>
+                <td>-PI</td>
+                <td>PI</td>
+        <td>-PI</td>
+        <td>PI</td>
+</tr><tr>
         <td>Y</td>
+                <td>-PI/2</td>
+                <td>PI/2</td>
+        <td>-PI/2</td>
+        <td>PI/2</td>
+</tr><tr>
         <td>Z</td>
                 <td>-PI/2</td>
                 <td>PI/2</td>
+        <td>-PI</td>
+        <td>PI</td>
 </tr>
 </table>
 …
     btVector3 m_calculatedAxis[3];
     btVector3 m_calculatedLinearDiff;
+        btScalar        m_factA;
+        btScalar        m_factB;
+        bool            m_hasStaticBody;
         btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
     bool        m_useLinearReferenceFrameA;
+        bool    m_useOffsetForConstraintFrame;
+        int             m_flags;
     //!@}
 …
         int setAngularLimits(btConstraintInfo2 *info, int row_offset);
         int setLinearLimits(btConstraintInfo2 *info);
+        int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+        int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
     void buildLinearJacobian(
 …
     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+    btGeneric6DofConstraint();
+    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
         //! Calcs global transform of the offsets
         /*!
 …
         \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
         */
+    void calculateTransforms();
+    void calculateTransforms(const btTransform& transA,const btTransform& transB);
+        void calculateTransforms();
         //! Gets the global transform of the offset for body A
 …
         virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual (btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+    virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
     void        updateRHS(btScalar      timeStep);
 …
     //! Get the relative Euler angle
     /*!
         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
+        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
         */
     btScalar getAngle(int axis_index) const;
+        //! Get the relative position of the constraint pivot
+    /*!
+        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+        */
+        btScalar getRelativePivotPosition(int axis_index) const;
         //! Test angular limit.
         /*!
         Calculates angular correction and returns true if limit needs to be corrected.
         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
+        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
         */
     bool testAngularLimitMotor(int axis_index);
 …
     void        setAngularLowerLimit(const btVector3& angularLower)
+    {
+        m_angularLimits[0].m_loLimit = angularLower.getX();
+        m_angularLimits[1].m_loLimit = angularLower.getY();
+        m_angularLimits[2].m_loLimit = angularLower.getZ();
+                for(int i = 0; i < 3; i++)
+                        m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
+    }
     void        setAngularUpperLimit(const btVector3& angularUpper)
+    {
+        m_angularLimits[0].m_hiLimit = angularUpper.getX();
+        m_angularLimits[1].m_hiLimit = angularUpper.getY();
+        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
+                for(int i = 0; i < 3; i++)
+                        m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
+    }
 …
         else
+        {
+                        lo = btNormalizeAngle(lo);
+                        hi = btNormalizeAngle(hi);
                 m_angularLimits[axis-3].m_loLimit = lo;
                 m_angularLimits[axis-3].m_hiLimit = hi;
 …
+    }
-    const btRigidBody& getRigidBodyA() const
+    {
-        return m_rbA;
+    }
-    const btRigidBody& getRigidBodyB() const
+    {
-        return m_rbB;
+    }
         virtual void calcAnchorPos(void); // overridable
         int get_limit_motor_info2(      btRotationalLimitMotor * limot,
+                                                                btRigidBody * body0, btRigidBody * body1,
+                                                                btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+                                                                const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+                                                                btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
+        // access for UseFrameOffset
+        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+        ///If no axis is provided, it uses the default axis for this constraint.
+        virtual void setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btGeneric6DofConstraintData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformFloatData m_rbBFrame;
+        btVector3FloatData      m_linearUpperLimit;
+        btVector3FloatData      m_linearLowerLimit;
+        btVector3FloatData      m_angularUpperLimit;
+        btVector3FloatData      m_angularLowerLimit;
+        int     m_useLinearReferenceFrameA;
+        int m_useOffsetForConstraintFrame;
+};
+SIMD_FORCE_INLINE       int     btGeneric6DofConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btGeneric6DofConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
+        btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+        m_frameInA.serializeFloat(dof->m_rbAFrame);
+        m_frameInB.serializeFloat(dof->m_rbBFrame);
+        int i;
+        for (i=0;i<3;i++)
+        {
+                dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
+                dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
+                dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
+                dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
+        }
+        dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
+        dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
+        return "btGeneric6DofConstraintData";
+}
 #endif //GENERIC_6DOF_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

-                      r5781
+                      r8284
 #include "btSolverBody.h"
+//-----------------------------------------------------------------------------
+//#define HINGE_USE_OBSOLETE_SOLVER false
 #define HINGE_USE_OBSOLETE_SOLVER false
+//-----------------------------------------------------------------------------
+btHingeConstraint::btHingeConstraint()
+: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(false)
+{
+        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+//-----------------------------------------------------------------------------
+#define HINGE_USE_FRAME_OFFSET true
+#ifndef __SPU__
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
                                                                          btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
+                                                                         const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
                                                                          :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
                                                                          m_angularOnly(false),
                                                                          m_enableAngularMotor(false),
                                                                          m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+                                                                         m_useReferenceFrameA(useReferenceFrameA)
+                                                                         m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+                                                                         m_useReferenceFrameA(useReferenceFrameA),
+                                                                         m_flags(0)
+{
         m_rbAFrame.getOrigin() = pivotInA;
 …
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
+{
 …
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
 …
 m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
+{
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 …
 m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
+{
         ///not providing rigidbody B means implicitly using worldspace for body B
 …
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 void    btHingeConstraint::buildJacobian()
 …
+        {
                 m_appliedImpulse = btScalar(0.);
+                m_accMotorImpulse = btScalar(0.);
                 if (!m_angularOnly)
 …
                 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
-                getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
                 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
                 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
 …
                         // test angular limit
                         testLimit();
+                        testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 //Compute K = J*W*J' for hinge axis
 …
+}
+//-----------------------------------------------------------------------------
+#endif //__SPU__
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 …
                 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
                 info->nub = 1;
+                //always add the row, to avoid computation (data is not available yet)
                 //prepare constraint
                 testLimit();
+                testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 if(getSolveLimit() || getEnableAngularMotor())
+                {
 …
                         info->nub--;
+                }
+        }
+} // btHingeConstraint::getInfo1 ()
+//-----------------------------------------------------------------------------
+        }
+}
+void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        }
+        else
+        {
+                //always add the 'limit' row, to avoid computation (data is not available yet)
+                info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
+                info->nub = 0;
+        }
+}
 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        if(m_useOffsetForConstraintFrame)
+        {
+                getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+        }
+        else
+        {
+                getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+        }
+}
+void    btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+        ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
+        testLimit(transA,transB);
+        getInfo2Internal(info,transA,transB,angVelA,angVelB);
+}
+void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
         btAssert(!m_useSolveConstraintObsolete);
         int i, s = info->rowskip;
+        int i, skip = info->rowskip;
         // transforms in world space
         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
         btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+        btTransform trA = transA*m_rbAFrame;
+        btTransform trB = transB*m_rbBFrame;
         // pivot point
         btVector3 pivotAInW = trA.getOrigin();
         btVector3 pivotBInW = trB.getOrigin();
+#if 0
+        if (0)
+        {
+                for (i=0;i<6;i++)
+                {
+                        info->m_J1linearAxis[i*skip]=0;
+                        info->m_J1linearAxis[i*skip+1]=0;
+                        info->m_J1linearAxis[i*skip+2]=0;
+                        info->m_J1angularAxis[i*skip]=0;
+                        info->m_J1angularAxis[i*skip+1]=0;
+                        info->m_J1angularAxis[i*skip+2]=0;
+                        info->m_J2angularAxis[i*skip]=0;
+                        info->m_J2angularAxis[i*skip+1]=0;
+                        info->m_J2angularAxis[i*skip+2]=0;
+                        info->m_constraintError[i*skip]=0.f;
+                }
+        }
+#endif //#if 0
         // linear (all fixed)
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[s + 1] = 1;
+    info->m_J1linearAxis[2 * s + 2] = 1;
+        btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
+        if (!m_angularOnly)
+        {
+                info->m_J1linearAxis[0] = 1;
+                info->m_J1linearAxis[skip + 1] = 1;
+                info->m_J1linearAxis[2 * skip + 2] = 1;
+        }
+        btVector3 a1 = pivotAInW - transA.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
+                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
                 btVector3 a1neg = -a1;
                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
         btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+        btVector3 a2 = pivotBInW - transB.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
+                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
         // linear RHS
     btScalar k = info->fps * info->erp;
+        for(i = 0; i < 3; i++)
+    {
+        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
+    }
+        if (!m_angularOnly)
+        {
+                for(i = 0; i < 3; i++)
+                {
+                        info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
+                }
+        }
         // make rotations around X and Y equal
         // the hinge axis should be the only unconstrained
 …
+                }
                 info->m_constraintError[srow] = btScalar(0.0f);
+                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
                 if(powered)
+                {
+            info->cfm[srow] = btScalar(0.0);
+                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+                        {
+                                info->cfm[srow] = m_normalCFM;
+                        }
+                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
 …
                 if(limit)
+                {
                         k = info->fps * info->erp;
+                        k = info->fps * currERP;
                         info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0);
+                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+                        {
+                                info->cfm[srow] = m_stopCFM;
+                        }
                         if(lostop == histop)
+                        {
 …
                         if(bounce > btScalar(0.0))
+                        {
                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
+                                btScalar vel = angVelA.dot(ax1);
+                                vel -= angVelB.dot(ax1);
                                 // only apply bounce if the velocity is incoming, and if the
                                 // resulting c[] exceeds what we already have.
 …
+}
+//-----------------------------------------------------------------------------
+void    btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar     timeStep)
+{
+        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+        if (m_useSolveConstraintObsolete)
+        {
+                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+                btScalar tau = btScalar(0.3);
+                //linear part
+                if (!m_angularOnly)
+                {
+                        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+                        btVector3 vel1,vel2;
+                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        btVector3 vel = vel1 - vel2;
+                        for (int i=0;i<3;i++)
+                        {
+                                const btVector3& normal = m_jac[i].m_linearJointAxis;
+                                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+                                btScalar rel_vel;
+                                rel_vel = normal.dot(vel);
+                                //positional error (zeroth order error)
+                                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+                                btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+                                m_appliedImpulse += impulse;
+                                btVector3 impulse_vector = normal * impulse;
+                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+                                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+                                bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+                                bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+                        }
+                }
+                {
+                        ///solve angular part
+                        // get axes in world space
+                        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+                        btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+                        btVector3 angVelA;
+                        bodyA.getAngularVelocity(angVelA);
+                        btVector3 angVelB;
+                        bodyB.getAngularVelocity(angVelB);
+                        btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+                        btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+                        btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+                        btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+                        btVector3 velrelOrthog = angAorthog-angBorthog;
+                        {
+                                //solve orthogonal angular velocity correction
+                                btScalar relaxation = btScalar(1.);
+                                btScalar len = velrelOrthog.length();
+                                if (len > btScalar(0.00001))
+                                {
+                                        btVector3 normal = velrelOrthog.normalized();
+                                        btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+                                                getRigidBodyB().computeAngularImpulseDenominator(normal);
+                                        // scale for mass and relaxation
+                                        //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
+                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
+                                }
+                                //solve angular positional correction
+                                btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
+                                btScalar len2 = angularError.length();
+                                if (len2>btScalar(0.00001))
+                                {
+                                        btVector3 normal2 = angularError.normalized();
+                                        btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+                                                        getRigidBodyB().computeAngularImpulseDenominator(normal2);
+                                        //angularError *= (btScalar(1.)/denom2) * relaxation;
+                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
+                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
+                                }
+                                // solve limit
+                                if (m_solveLimit)
+                                {
+                                        btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
+                                        btScalar impulseMag = amplitude * m_kHinge;
+                                        // Clamp the accumulated impulse
+                                        btScalar temp = m_accLimitImpulse;
+                                        m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
+                                        impulseMag = m_accLimitImpulse - temp;
+                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
+                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
+                                }
+                        }
+                        //apply motor
+                        if (m_enableAngularMotor)
+                        {
+                                //todo: add limits too
+                                btVector3 angularLimit(0,0,0);
+                                btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+                                btScalar projRelVel = velrel.dot(axisA);
+                                btScalar desiredMotorVel = m_motorTargetVelocity;
+                                btScalar motor_relvel = desiredMotorVel - projRelVel;
+                                btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+                                //todo: should clip against accumulated impulse
+                                btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+                                clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+                                btVector3 motorImp = clippedMotorImpulse * axisA;
+                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
+                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
+                        }
+                }
+        }
+}
+//-----------------------------------------------------------------------------
 void    btHingeConstraint::updateRHS(btScalar   timeStep)
 …
+}
-//-----------------------------------------------------------------------------
 btScalar btHingeConstraint::getHingeAngle()
+{
+        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+        btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+        return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
+{
+        const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+        const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+        const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
+//      btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+        btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
         return m_referenceSign * angle;
+}
+//-----------------------------------------------------------------------------
+#if 0
 void btHingeConstraint::testLimit()
+{
 …
+        }
         return;
+} // btHingeConstraint::testLimit()
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+}
+#else
+void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
+{
+        // Compute limit information
+        m_hingeAngle = getHingeAngle(transA,transB);
+        m_correction = btScalar(0.);
+        m_limitSign = btScalar(0.);
+        m_solveLimit = false;
+        if (m_lowerLimit <= m_upperLimit)
+        {
+                m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
+                if (m_hingeAngle <= m_lowerLimit)
+                {
+                        m_correction = (m_lowerLimit - m_hingeAngle);
+                        m_limitSign = 1.0f;
+                        m_solveLimit = true;
+                }
+                else if (m_hingeAngle >= m_upperLimit)
+                {
+                        m_correction = m_upperLimit - m_hingeAngle;
+                        m_limitSign = -1.0f;
+                        m_solveLimit = true;
+                }
+        }
+        return;
+}
+#endif
+static btVector3 vHinge(0, 0, btScalar(1));
+void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
+{
+        // convert target from body to constraint space
+        btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
+        qConstraint.normalize();
+        // extract "pure" hinge component
+        btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+        btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
+        btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
+        qHinge.normalize();
+        // compute angular target, clamped to limits
+        btScalar targetAngle = qHinge.getAngle();
+        if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+        {
+                qHinge = operator-(qHinge);
+                targetAngle = qHinge.getAngle();
+        }
+        if (qHinge.getZ() < 0)
+                targetAngle = -targetAngle;
+        setMotorTarget(targetAngle, dt);
+}
+void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
+{
+        if (m_lowerLimit < m_upperLimit)
+        {
+                if (targetAngle < m_lowerLimit)
+                        targetAngle = m_lowerLimit;
+                else if (targetAngle > m_upperLimit)
+                        targetAngle = m_upperLimit;
+        }
+        // compute angular velocity
+        btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+        btScalar dAngle = targetAngle - curAngle;
+        m_motorTargetVelocity = dAngle / dt;
+}
+void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+        int i, s = info->rowskip;
+        // transforms in world space
+        btTransform trA = transA*m_rbAFrame;
+        btTransform trB = transB*m_rbBFrame;
+        // pivot point
+        btVector3 pivotAInW = trA.getOrigin();
+        btVector3 pivotBInW = trB.getOrigin();
+#if 1
+        // difference between frames in WCS
+        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+        // now get weight factors depending on masses
+        btScalar miA = getRigidBodyA().getInvMass();
+        btScalar miB = getRigidBodyB().getInvMass();
+        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+        btScalar miS = miA + miB;
+        btScalar factA, factB;
+        if(miS > btScalar(0.f))
+        {
+                factA = miB / miS;
+        }
+        else
+        {
+                factA = btScalar(0.5f);
+        }
+        factB = btScalar(1.0f) - factA;
+        // get the desired direction of hinge axis
+        // as weighted sum of Z-orthos of frameA and frameB in WCS
+        btVector3 ax1A = trA.getBasis().getColumn(2);
+        btVector3 ax1B = trB.getBasis().getColumn(2);
+        btVector3 ax1 = ax1A * factA + ax1B * factB;
+        ax1.normalize();
+        // fill first 3 rows
+        // we want: velA + wA x relA == velB + wB x relB
+        btTransform bodyA_trans = transA;
+        btTransform bodyB_trans = transB;
+        int s0 = 0;
+        int s1 = s;
+        int s2 = s * 2;
+        int nrow = 2; // last filled row
+        btVector3 tmpA, tmpB, relA, relB, p, q;
+        // get vector from bodyB to frameB in WCS
+        relB = trB.getOrigin() - bodyB_trans.getOrigin();
+        // get its projection to hinge axis
+        btVector3 projB = ax1 * relB.dot(ax1);
+        // get vector directed from bodyB to hinge axis (and orthogonal to it)
+        btVector3 orthoB = relB - projB;
+        // same for bodyA
+        relA = trA.getOrigin() - bodyA_trans.getOrigin();
+        btVector3 projA = ax1 * relA.dot(ax1);
+        btVector3 orthoA = relA - projA;
+        btVector3 totalDist = projA - projB;
+        // get offset vectors relA and relB
+        relA = orthoA + totalDist * factA;
+        relB = orthoB - totalDist * factB;
+        // now choose average ortho to hinge axis
+        p = orthoB * factA + orthoA * factB;
+        btScalar len2 = p.length2();
+        if(len2 > SIMD_EPSILON)
+        {
+                p /= btSqrt(len2);
+        }
+        else
+        {
+                p = trA.getBasis().getColumn(1);
+        }
+        // make one more ortho
+        q = ax1.cross(p);
+        // fill three rows
+        tmpA = relA.cross(p);
+        tmpB = relB.cross(p);
+    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
+        tmpA = relA.cross(q);
+        tmpB = relB.cross(q);
+        if(hasStaticBody && getSolveLimit())
+        { // to make constraint between static and dynamic objects more rigid
+                // remove wA (or wB) from equation if angular limit is hit
+                tmpB *= factB;
+                tmpA *= factA;
+        }
+        for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
+        tmpA = relA.cross(ax1);
+        tmpB = relB.cross(ax1);
+        if(hasStaticBody)
+        { // to make constraint between static and dynamic objects more rigid
+                // remove wA (or wB) from equation
+                tmpB *= factB;
+                tmpA *= factA;
+        }
+        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+        btScalar k = info->fps * info->erp;
+        if (!m_angularOnly)
+        {
+                for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
+        // compute three elements of right hand side
+                btScalar rhs = k * p.dot(ofs);
+                info->m_constraintError[s0] = rhs;
+                rhs = k * q.dot(ofs);
+                info->m_constraintError[s1] = rhs;
+                rhs = k * ax1.dot(ofs);
+                info->m_constraintError[s2] = rhs;
+        }
+        // the hinge axis should be the only unconstrained
+        // rotational axis, the angular velocity of the two bodies perpendicular to
+        // the hinge axis should be equal. thus the constraint equations are
+        //    p*w1 - p*w2 = 0
+        //    q*w1 - q*w2 = 0
+        // where p and q are unit vectors normal to the hinge axis, and w1 and w2
+        // are the angular velocity vectors of the two bodies.
+        int s3 = 3 * s;
+        int s4 = 4 * s;
+        info->m_J1angularAxis[s3 + 0] = p[0];
+        info->m_J1angularAxis[s3 + 1] = p[1];
+        info->m_J1angularAxis[s3 + 2] = p[2];
+        info->m_J1angularAxis[s4 + 0] = q[0];
+        info->m_J1angularAxis[s4 + 1] = q[1];
+        info->m_J1angularAxis[s4 + 2] = q[2];
+        info->m_J2angularAxis[s3 + 0] = -p[0];
+        info->m_J2angularAxis[s3 + 1] = -p[1];
+        info->m_J2angularAxis[s3 + 2] = -p[2];
+        info->m_J2angularAxis[s4 + 0] = -q[0];
+        info->m_J2angularAxis[s4 + 1] = -q[1];
+        info->m_J2angularAxis[s4 + 2] = -q[2];
+        // compute the right hand side of the constraint equation. set relative
+        // body velocities along p and q to bring the hinge back into alignment.
+        // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
+        // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
+        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
+        // along u to cover angle erp*theta in one step :
+        //   |angular_velocity| = angle/time = erp*theta / stepsize
+        //                      = (erp*fps) * theta
+        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+        // ...as ax1 and ax2 are unit length. if theta is smallish,
+        // theta ~= sin(theta), so
+        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+        // ax1 x ax2 is in the plane space of ax1, so we project the angular
+        // velocity to p and q to find the right hand side.
+        k = info->fps * info->erp;
+        btVector3 u = ax1A.cross(ax1B);
+        info->m_constraintError[s3] = k * u.dot(p);
+        info->m_constraintError[s4] = k * u.dot(q);
+#endif
+        // check angular limits
+        nrow = 4; // last filled row
+        int srow;
+        btScalar limit_err = btScalar(0.0);
+        int limit = 0;
+        if(getSolveLimit())
+        {
+                limit_err = m_correction * m_referenceSign;
+                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+        }
+        // if the hinge has joint limits or motor, add in the extra row
+        int powered = 0;
+        if(getEnableAngularMotor())
+        {
+                powered = 1;
+        }
+        if(limit || powered)
+        {
+                nrow++;
+                srow = nrow * info->rowskip;
+                info->m_J1angularAxis[srow+0] = ax1[0];
+                info->m_J1angularAxis[srow+1] = ax1[1];
+                info->m_J1angularAxis[srow+2] = ax1[2];
+                info->m_J2angularAxis[srow+0] = -ax1[0];
+                info->m_J2angularAxis[srow+1] = -ax1[1];
+                info->m_J2angularAxis[srow+2] = -ax1[2];
+                btScalar lostop = getLowerLimit();
+                btScalar histop = getUpperLimit();
+                if(limit && (lostop == histop))
+                {  // the joint motor is ineffective
+                        powered = 0;
+                }
+                info->m_constraintError[srow] = btScalar(0.0f);
+                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
+                if(powered)
+                {
+                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+                        {
+                                info->cfm[srow] = m_normalCFM;
+                        }
+                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
+                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
+                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+                        info->m_upperLimit[srow] =   m_maxMotorImpulse;
+                }
+                if(limit)
+                {
+                        k = info->fps * currERP;
+                        info->m_constraintError[srow] += k * limit_err;
+                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+                        {
+                                info->cfm[srow] = m_stopCFM;
+                        }
+                        if(lostop == histop)
+                        {
+                                // limited low and high simultaneously
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        else if(limit == 1)
+                        { // low limit
+                                info->m_lowerLimit[srow] = 0;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        else
+                        { // high limit
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = 0;
+                        }
+                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+                        btScalar bounce = m_relaxationFactor;
+                        if(bounce > btScalar(0.0))
+                        {
+                                btScalar vel = angVelA.dot(ax1);
+                                vel -= angVelB.dot(ax1);
+                                // only apply bounce if the velocity is incoming, and if the
+                                // resulting c[] exceeds what we already have.
+                                if(limit == 1)
+                                {       // low limit
+                                        if(vel < 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if(newc > info->m_constraintError[srow])
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                                else
+                                {       // high limit - all those computations are reversed
+                                        if(vel > 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if(newc < info->m_constraintError[srow])
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                        }
+                        info->m_constraintError[srow] *= m_biasFactor;
+                } // if(limit)
+        } // if angular limit or powered
+}
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
+void btHingeConstraint::setParam(int num, btScalar value, int axis)
+{
+        if((axis == -1) || (axis == 5))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                m_stopERP = value;
+                                m_flags |= BT_HINGE_FLAGS_ERP_STOP;
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                m_stopCFM = value;
+                                m_flags |= BT_HINGE_FLAGS_CFM_STOP;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                m_normalCFM = value;
+                                m_flags |= BT_HINGE_FLAGS_CFM_NORM;
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+}
+///return the local value of parameter
+btScalar btHingeConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal = 0;
+        if((axis == -1) || (axis == 5))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
+                                retVal = m_stopERP;
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
+                                retVal = m_stopCFM;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
+                                retVal = m_normalCFM;
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

-                      r5781
+                      r8284
 class btRigidBody;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btHingeConstraintData   btHingeConstraintDoubleData
+#define btHingeConstraintDataName       "btHingeConstraintDoubleData"
+#else
+#define btHingeConstraintData   btHingeConstraintFloatData
+#define btHingeConstraintDataName       "btHingeConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+enum btHingeFlags
+{
+        BT_HINGE_FLAGS_CFM_STOP = 1,
+        BT_HINGE_FLAGS_ERP_STOP = 2,
+        BT_HINGE_FLAGS_CFM_NORM = 4
+};
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
 class btHingeConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
+{
 #ifdef IN_PARALLELL_SOLVER
 …
         bool            m_solveLimit;
         bool            m_useSolveConstraintObsolete;
+        bool            m_useOffsetForConstraintFrame;
         bool            m_useReferenceFrameA;
+        btScalar        m_accMotorImpulse;
+        int                     m_flags;
+        btScalar        m_normalCFM;
+        btScalar        m_stopCFM;
+        btScalar        m_stopERP;
 public:
         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
+        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
+        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 …
         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
-        btHingeConstraint();
         virtual void    buildJacobian();
 …
         virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual(btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+        void    getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+        void    getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
         void    updateRHS(btScalar      timeStep);
 …
+        }
+        // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
+        // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
+        //       maintain a given angular target.
+        void enableMotor(bool enableMotor)      { m_enableAngularMotor = enableMotor; }
+        void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
+        void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
+        void setMotorTarget(btScalar targetAngle, btScalar dt);
         void    setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+        {
                 m_lowerLimit = low;
                 m_upperLimit = high;
+                m_lowerLimit = btNormalizeAngle(low);
+                m_upperLimit = btNormalizeAngle(high);
                 m_limitSoftness =  _softness;
 …
+        }
+        void    setAxis(btVector3& axisInA)
+        {
+                btVector3 rbAxisA1, rbAxisA2;
+                btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
+                btVector3 pivotInA = m_rbAFrame.getOrigin();
+//              m_rbAFrame.getOrigin() = pivotInA;
+                m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+                                                                                rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+                                                                                rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+                btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
+                btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+                btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+                btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+                m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA);
+                m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+                                                                                rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+                                                                                rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+        }
         btScalar        getLowerLimit() const
+        {
 …
         btScalar getHingeAngle();
+        void testLimit();
+        const btTransform& getAFrame() { return m_rbAFrame; };
+        const btTransform& getBFrame() { return m_rbBFrame; };
+        btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
+        void testLimit(const btTransform& transA,const btTransform& transB);
+        const btTransform& getAFrame() const { return m_rbAFrame; };
+        const btTransform& getBFrame() const { return m_rbBFrame; };
+        btTransform& getAFrame() { return m_rbAFrame; };
+        btTransform& getBFrame() { return m_rbBFrame; };
         inline int getSolveLimit()
 …
                 return m_maxMotorImpulse;
+        }
+        // access for UseFrameOffset
+        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+        ///If no axis is provided, it uses the default axis for this constraint.
+        virtual void    setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btHingeConstraintDoubleData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformDoubleData m_rbBFrame;
+        int                     m_useReferenceFrameA;
+        int                     m_angularOnly;
+        int                     m_enableAngularMotor;
+        float   m_motorTargetVelocity;
+        float   m_maxMotorImpulse;
+        float   m_lowerLimit;
+        float   m_upperLimit;
+        float   m_limitSoftness;
+        float   m_biasFactor;
+        float   m_relaxationFactor;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btHingeConstraintFloatData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformFloatData m_rbBFrame;
+        int                     m_useReferenceFrameA;
+        int                     m_angularOnly;
+        int                     m_enableAngularMotor;
+        float   m_motorTargetVelocity;
+        float   m_maxMotorImpulse;
+        float   m_lowerLimit;
+        float   m_upperLimit;
+        float   m_limitSoftness;
+        float   m_biasFactor;
+        float   m_relaxationFactor;
+};
+SIMD_FORCE_INLINE       int     btHingeConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btHingeConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
+        btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
+        m_rbAFrame.serialize(hingeData->m_rbAFrame);
+        m_rbBFrame.serialize(hingeData->m_rbBFrame);
+        hingeData->m_angularOnly = m_angularOnly;
+        hingeData->m_enableAngularMotor = m_enableAngularMotor;
+        hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
+        hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
+        hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
+        hingeData->m_lowerLimit = float(m_lowerLimit);
+        hingeData->m_upperLimit = float(m_upperLimit);
+        hingeData->m_limitSoftness = float(m_limitSoftness);
+        hingeData->m_biasFactor = float(m_biasFactor);
+        hingeData->m_relaxationFactor = float(m_relaxationFactor);
+        return btHingeConstraintDataName;
+}
 #endif //HINGECONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h

r5781	r8284
29	29	/// it can be used in combination with a constraint solver
30	30	/// Can be used to relate the effect of an impulse to the constraint error
31		~~class~~ btJacobianEntry
	31	ATTRIBUTE_ALIGNED16(class) btJacobianEntry
32	32	{
33	33	public:

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

-                      r5781
+                      r8284
+btPoint2PointConstraint::btPoint2PointConstraint()
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(false)
+{
+}
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
+{
 …
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
+{
 …
 void    btPoint2PointConstraint::buildJacobian()
+{
         ///we need it for both methods
+        {
 …
+        }
+}
+}
 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+        getInfo1NonVirtual(info);
+}
+void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
         if (m_useSolveConstraintObsolete)
 …
+}
 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+{
         btAssert(!m_useSolveConstraintObsolete);
          //retrieve matrices
-        btTransform body0_trans;
-        body0_trans = m_rbA.getCenterOfMassTransform();
-    btTransform body1_trans;
-        body1_trans = m_rbB.getCenterOfMassTransform();
         // anchor points in global coordinates with respect to body PORs.
 …
     // set jacobian
     info->m_J1linearAxis[0] = 1;
     info->m_J1linearAxis[info->rowskip+1] = 1;
     info->m_J1linearAxis[2*info->rowskip+2] = 1;
+        info->m_J1linearAxis[info->rowskip+1] = 1;
+        info->m_J1linearAxis[2*info->rowskip+2] = 1;
         btVector3 a1 = body0_trans.getBasis()*getPivotInA();
 …
     // set right hand side
+    btScalar k = info->fps * info->erp;
+        btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
+    btScalar k = info->fps * currERP;
     int j;
         for (j=0; j<3; j++)
+    {
         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
                 //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+        if(m_flags & BT_P2P_FLAGS_CFM)
+        {
+                for (j=0; j<3; j++)
+                {
+                        info->cfm[j*info->rowskip] = m_cfm;
+                }
+        }
         btScalar impulseClamp = m_setting.m_impulseClamp;//
 …
+                }
+        }
+}
+void    btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar       timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+                btVector3 normal(0,0,0);
+        //      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+        //      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+                for (int i=0;i<3;i++)
+                {
+                        normal[i] = 1;
+                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+                        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+                        //this jacobian entry could be re-used for all iterations
+                        btVector3 vel1,vel2;
+                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        btVector3 vel = vel1 - vel2;
+                        btScalar rel_vel;
+                        rel_vel = normal.dot(vel);
+                /*
+                        //velocity error (first order error)
+                        btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+                                                                                                                        m_rbB.getLinearVelocity(),angvelB);
+                */
+                        //positional error (zeroth order error)
+                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+                        btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+                        btScalar impulseClamp = m_setting.m_impulseClamp;
+                        const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
+                        if (sum < -impulseClamp)
+                        {
+                                deltaImpulse = -impulseClamp-m_appliedImpulse;
+                                m_appliedImpulse = -impulseClamp;
+                        }
+                        else if (sum > impulseClamp)
+                        {
+                                deltaImpulse = impulseClamp-m_appliedImpulse;
+                                m_appliedImpulse = impulseClamp;
+                        }
+                        else
+                        {
+                                m_appliedImpulse = sum;
+                        }
+                        btVector3 impulse_vector = normal * deltaImpulse;
+                        btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+                        btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+                        bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
+                        bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
+                        normal[i] = 0;
+                }
+        }
+}
+        info->m_damping = m_setting.m_damping;
+}
 void    btPoint2PointConstraint::updateRHS(btScalar     timeStep)
 …
+}
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
+void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
+{
+        if(axis != -1)
+        {
+                btAssertConstrParams(0);
+        }
+        else
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_ERP :
+                        case BT_CONSTRAINT_STOP_ERP :
+                                m_erp = value;
+                                m_flags |= BT_P2P_FLAGS_ERP;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                        case BT_CONSTRAINT_STOP_CFM :
+                                m_cfm = value;
+                                m_flags |= BT_P2P_FLAGS_CFM;
+                                break;
+                        default:
+                                btAssertConstrParams(0);
+                }
+        }
+}
+///return the local value of parameter
+btScalar btPoint2PointConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal(SIMD_INFINITY);
+        if(axis != -1)
+        {
+                btAssertConstrParams(0);
+        }
+        else
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_ERP :
+                        case BT_CONSTRAINT_STOP_ERP :
+                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
+                                retVal = m_erp;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                        case BT_CONSTRAINT_STOP_CFM :
+                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
+                                retVal = m_cfm;
+                                break;
+                        default:
+                                btAssertConstrParams(0);
+                }
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

-                      r5781
+                      r8284
 class btRigidBody;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btPoint2PointConstraintData     btPoint2PointConstraintDoubleData
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
+#else
+#define btPoint2PointConstraintData     btPoint2PointConstraintFloatData
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
 struct  btConstraintSetting
+{
 …
 };
+enum btPoint2PointFlags
+{
+        BT_P2P_FLAGS_ERP = 1,
+        BT_P2P_FLAGS_CFM = 2
+};
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
 class btPoint2PointConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
+{
 #ifdef IN_PARALLELL_SOLVER
 …
         btVector3       m_pivotInB;
+        int                     m_flags;
+        btScalar        m_erp;
+        btScalar        m_cfm;
 public:
 …
         btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
-        btPoint2PointConstraint();
         virtual void    buildJacobian();
 …
         virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual (btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
         void    updateRHS(btScalar      timeStep);
 …
+        }
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+        ///If no axis is provided, it uses the default axis for this constraint.
+        virtual void    setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btPoint2PointConstraintFloatData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btVector3FloatData      m_pivotInA;
+        btVector3FloatData      m_pivotInB;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btPoint2PointConstraintDoubleData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btVector3DoubleData     m_pivotInA;
+        btVector3DoubleData     m_pivotInB;
+};
+SIMD_FORCE_INLINE       int     btPoint2PointConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btPoint2PointConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
+        btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
+        m_pivotInA.serialize(p2pData->m_pivotInA);
+        m_pivotInB.serialize(p2pData->m_pivotInB);
+        return btPoint2PointConstraintDataName;
+}
 #endif //POINT2POINTCONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

-                      r5781
+                      r8284
 #include <string.h> //for memset
+int             gNumSplitImpulseRecoveries = 0;
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 :m_btSeed2(0)
 …
 // Project Gauss Seidel or the equivalent Sequential Impulse
 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
 #ifdef USE_SIMD
 …
         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 …
         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
         __m128 impulseMagnitude = deltaImpulse;
         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+        body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 #else
         resolveSingleConstraintRowGeneric(body1,body2,c);
 …
 // Project Gauss Seidel or the equivalent Sequential Impulse
  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
         const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
+        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+//      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
 …
                 c.m_appliedImpulse = sum;
+        }
+        if (body1.m_invMass)
+                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+        if (body2.m_invMass)
+                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+                body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+                body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+}
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
 #ifdef USE_SIMD
 …
         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 …
         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
         __m128 impulseMagnitude = deltaImpulse;
         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+        body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 #else
         resolveSingleConstraintRowLowerLimit(body1,body2,c);
 …
 // Project Gauss Seidel or the equivalent Sequential Impulse
  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
 …
                 c.m_appliedImpulse = sum;
+        }
+        if (body1.m_invMass)
+                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+        if (body2.m_invMass)
+                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+        body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+        body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+}
+void    btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& c)
+{
+                if (c.m_rhsPenetration)
+        {
+                        gNumSplitImpulseRecoveries++;
+                        btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
+                        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetPushVelocity())  + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
+                        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
+                        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
+                        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
+                        const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
+                        if (sum < c.m_lowerLimit)
+                        {
+                                deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
+                                c.m_appliedPushImpulse = c.m_lowerLimit;
+                        }
+                        else
+                        {
+                                c.m_appliedPushImpulse = sum;
+                        }
+                        body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+                        body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+        }
+}
+ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+        if (!c.m_rhsPenetration)
+                return;
+        gNumSplitImpulseRecoveries++;
+        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
+        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+        __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+        btSimdScalar resultLowerLess,resultUpperLess;
+        resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+        resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+        __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
+        __m128 impulseMagnitude = deltaImpulse;
+        body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+        resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+#endif
+}
 …
+#if 0
 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+{
         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
+        solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+        solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+        solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+        solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+        solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+        solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
         if (rb)
+        {
                 solverBody->m_invMass = rb->getInvMass();
+                solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
                 solverBody->m_originalBody = rb;
                 solverBody->m_angularFactor = rb->getAngularFactor();
         } else
+        {
                 solverBody->m_invMass = 0.f;
+                solverBody->internalGetInvMass().setValue(0,0,0);
                 solverBody->m_originalBody = 0;
+                solverBody->m_angularFactor = 1.f;
+        }
+}
+int             gNumSplitImpulseRecoveries = 0;
+                solverBody->m_angularFactor.setValue(1,1,1);
+        }
+}
+#endif
 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
 …
+btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
+void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+{
 …
         btRigidBody* body1=btRigidBody::upcast(colObj1);
-        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
-        memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
         solverConstraint.m_contactNormal = normalAxis;
+        solverConstraint.m_solverBodyIdA = solverBodyIdA;
+        solverConstraint.m_solverBodyIdB = solverBodyIdB;
+        solverConstraint.m_frictionIndex = frictionIndex;
+        solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
+        solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
         solverConstraint.m_friction = cp.m_combinedFriction;
 …
         solverConstraint.m_appliedImpulse = 0.f;
         //      solverConstraint.m_appliedPushImpulse = 0.f;
+        solverConstraint.m_appliedPushImpulse = 0.f;
+        {
 …
                 rel_vel = vel1Dotn+vel2Dotn;
                 btScalar positionalError = 0.f;
                 btSimdScalar velocityError =  - rel_vel;
+//              btScalar positionalError = 0.f;
+                btSimdScalar velocityError =  desiredVelocity - rel_vel;
                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
                 solverConstraint.m_rhs = velocityImpulse;
                 solverConstraint.m_cfm = 0.f;
+                solverConstraint.m_cfm = cfmSlip;
                 solverConstraint.m_lowerLimit = 0;
                 solverConstraint.m_upperLimit = 1e10f;
+        }
+}
+btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+{
+        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+        solverConstraint.m_frictionIndex = frictionIndex;
+        setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2,
+                                                        colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
         return solverConstraint;
+}
 …
 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+{
+#if 0
         int solverBodyIdA = -1;
 …
+        }
         return solverBodyIdA;
+#endif
+        return 0;
+}
 #include <stdio.h>
+void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+        btCollisionObject* colObj0=0,*colObj1=0;
+        colObj0 = (btCollisionObject*)manifold->getBody0();
+        colObj1 = (btCollisionObject*)manifold->getBody1();
+        int solverBodyIdA=-1;
+        int solverBodyIdB=-1;
+        if (manifold->getNumContacts())
+        {
+                solverBodyIdA = getOrInitSolverBody(*colObj0);
+                solverBodyIdB = getOrInitSolverBody(*colObj1);
+        }
+        ///avoid collision response between two static objects
+        if (!solverBodyIdA && !solverBodyIdB)
+                return;
+        btVector3 rel_pos1;
+        btVector3 rel_pos2;
+        btScalar relaxation;
+        for (int j=0;j<manifold->getNumContacts();j++)
+        {
+                btManifoldPoint& cp = manifold->getContactPoint(j);
+                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+                {
+void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
+                                                                                                                                 btCollisionObject* colObj0, btCollisionObject* colObj1,
+                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+                                                                                                                                 btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
+                                                                                                                                 btVector3& rel_pos1, btVector3& rel_pos2)
+{
+                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
                         const btVector3& pos1 = cp.getPositionWorldOnA();
                         const btVector3& pos2 = cp.getPositionWorldOnB();
+//                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+//                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
                         rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
                         rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
                         relaxation = 1.f;
+                        btScalar rel_vel;
+                        btVector3 vel;
+                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
+                        {
+                                btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
+                                btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                                btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+                                solverConstraint.m_solverBodyIdA = solverBodyIdA;
+                                solverConstraint.m_solverBodyIdB = solverBodyIdB;
+                                solverConstraint.m_originalContactPoint = &cp;
+                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+                                solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+                                solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+                        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+                        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+                                {
 #ifdef COMPUTE_IMPULSE_DENOM
 …
+                                btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+                                btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
                                 vel  = vel1 - vel2;
                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
+                        btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+                        btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+                        vel  = vel1 - vel2;
+                        rel_vel = cp.m_normalWorldOnB.dot(vel);
                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
 …
                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
                                         if (rb0)
                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+                                                rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
                                         if (rb1)
                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+                                                rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
                                 } else
+                                {
 …
+                                }
                                 //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
+                                solverConstraint.m_appliedPushImpulse = 0.f;
+                                {
 …
                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+                                        if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+                                        {
+                                                //combine position and velocity into rhs
+                                                solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+                                                solverConstraint.m_rhsPenetration = 0.f;
+                                        } else
+                                        {
+                                                //split position and velocity into rhs and m_rhsPenetration
+                                                solverConstraint.m_rhs = velocityImpulse;
+                                                solverConstraint.m_rhsPenetration = penetrationImpulse;
+                                        }
                                         solverConstraint.m_cfm = 0.f;
                                         solverConstraint.m_lowerLimit = 0;
 …
+                                /////setup the friction constraints
+                                if (1)
+                                {
+                                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+                                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+                                        {
+                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+                                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+                                                {
+                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        {
+                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+                                                                cp.m_lateralFrictionDir2.normalize();//??
+                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        }
+                                                        cp.m_lateralFrictionInitialized = true;
+                                                } else
+                                                {
+                                                        //re-calculate friction direction every frame, todo: check if this is really needed
+                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        {
+                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        }
+                                                        cp.m_lateralFrictionInitialized = true;
+                                                }
+                                        } else
+                                        {
+                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        }
+}
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
+                                                                                                                                                btRigidBody* rb0, btRigidBody* rb1,
+                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
+{
                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+                                        {
 …
                                                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
                                                                 if (rb0)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+                                                                        rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
                                                                 if (rb1)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+                                                                        rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
                                                         } else
+                                                        {
 …
                                                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
                                                                 if (rb0)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+                                                                        rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
                                                                 if (rb1)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+                                                                        rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
                                                         } else
+                                                        {
 …
+                                                }
+                                        }
+}
+void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+        btCollisionObject* colObj0=0,*colObj1=0;
+        colObj0 = (btCollisionObject*)manifold->getBody0();
+        colObj1 = (btCollisionObject*)manifold->getBody1();
+        btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
+        btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
+        ///avoid collision response between two static objects
+        if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
+                return;
+        for (int j=0;j<manifold->getNumContacts();j++)
+        {
+                btManifoldPoint& cp = manifold->getContactPoint(j);
+                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+                {
+                        btVector3 rel_pos1;
+                        btVector3 rel_pos2;
+                        btScalar relaxation;
+                        btScalar rel_vel;
+                        btVector3 vel;
+                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
+                        btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
+                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+                        solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
+                        solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
+                        solverConstraint.m_originalContactPoint = &cp;
+                        setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+//                      const btVector3& pos1 = cp.getPositionWorldOnA();
+//                      const btVector3& pos2 = cp.getPositionWorldOnB();
+                        /////setup the friction constraints
+                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+                        {
+                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+                                {
+                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                        {
+                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+                                                cp.m_lateralFrictionDir2.normalize();//??
+                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        }
+                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        cp.m_lateralFrictionInitialized = true;
+                                } else
+                                {
+                                        //re-calculate friction direction every frame, todo: check if this is really needed
+                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                        {
+                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        }
+                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        cp.m_lateralFrictionInitialized = true;
+                                }
+                        }
+                }
+        }
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+                        } else
+                        {
+                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
+                        }
+                        setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
+                }
+        }
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
         BT_PROFILE("solveGroupCacheFriendlySetup");
 …
+        }
+        if (infoGlobal.m_splitImpulse)
+        {
+                for (int i = 0; i < numBodies; i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                        {
+                                body->internalGetDeltaLinearVelocity().setZero();
+                                body->internalGetDeltaAngularVelocity().setZero();
+                                body->internalGetPushVelocity().setZero();
+                                body->internalGetTurnVelocity().setZero();
+                        }
+                }
+        }
+        else
+        {
+                for (int i = 0; i < numBodies; i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                        {
+                                body->internalGetDeltaLinearVelocity().setZero();
+                                body->internalGetDeltaAngularVelocity().setZero();
+                        }
+                }
+        }
         if (1)
+        {
 …
+                }
+        }
-        btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-        initSolverBody(&fixedBody,0);
         //btRigidBody* rb0=0,*rb1=0;
 …
                         int totalNumRows = 0;
                         int i;
+                        m_tmpConstraintSizesPool.resize(numConstraints);
                         //calculate the total number of contraint rows
                         for (i=0;i<numConstraints;i++)
+                        {
+                                btTypedConstraint::btConstraintInfo1 info1;
+                                btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
                                 constraints[i]->getInfo1(&info1);
                                 totalNumRows += info1.m_numConstraintRows;
 …
                         m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
+                        btTypedConstraint::btConstraintInfo1 info1;
+                        info1.m_numConstraintRows = 0;
                         ///setup the btSolverConstraints
                         int currentRow = 0;
                         for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
+                        for (i=0;i<numConstraints;i++)
+                        {
+                                constraints[i]->getInfo1(&info1);
+                                const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
                                 if (info1.m_numConstraintRows)
+                                {
 …
                                         btRigidBody& rbB = constraint->getRigidBodyB();
+                                        int solverBodyIdA = getOrInitSolverBody(rbA);
+                                        int solverBodyIdB = getOrInitSolverBody(rbB);
+                                        btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+                                        btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
                                         int j;
                                         for ( j=0;j<info1.m_numConstraintRows;j++)
 …
                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+                                                currentConstraintRow[j].m_solverBodyA = &rbA;
+                                                currentConstraintRow[j].m_solverBodyB = &rbB;
+                                        }
                                         bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
                                         bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
                                         bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
                                         bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+                                        rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+                                        rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+                                        rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+                                        rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
 …
                                         btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
+                                        currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+                                        info2.m_damping = infoGlobal.m_damping;
                                         info2.cfm = &currentConstraintRow->m_cfm;
                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+                                        info2.m_numIterations = infoGlobal.m_numIterations;
                                         constraints[i]->getInfo2(&info2);
 …
+                                        {
                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
+                                                solverConstraint.m_originalContactPoint = constraint;
+                                                {
 …
                                                         btScalar restitution = 0.f;
                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
+                                                        btScalar        velocityError = restitution - rel_vel * info2.m_damping;
                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 …
+                                        }
+                                }
+                                currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
+                        }
+                }
 …
                         int i;
                         btPersistentManifold* manifold = 0;
                         btCollisionObject* colObj0=0,*colObj1=0;
+//                      btCollisionObject* colObj0=0,*colObj1=0;
 …
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
+        BT_PROFILE("solveGroupCacheFriendlyIterations");
+btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+        int j;
+        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+        {
+                if ((iteration & 7) == 0) {
+                        for (j=0; j<numConstraintPool; ++j) {
+                                int tmp = m_orderTmpConstraintPool[j];
+                                int swapi = btRandInt2(j+1);
+                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+                                m_orderTmpConstraintPool[swapi] = tmp;
+                        }
+                        for (j=0; j<numFrictionPool; ++j) {
+                                int tmp = m_orderFrictionConstraintPool[j];
+                                int swapi = btRandInt2(j+1);
+                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+                                m_orderFrictionConstraintPool[swapi] = tmp;
+                        }
+                }
+        }
+        if (infoGlobal.m_solverMode & SOLVER_SIMD)
+        {
+                ///solve all joint constraints, using SIMD, if available
+                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                {
+                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                        resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+                }
+                for (j=0;j<numConstraints;j++)
+                {
+                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+                }
+                ///solve all contact constraints using SIMD, if available
+                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                for (j=0;j<numPoolConstraints;j++)
+                {
+                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                        resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                }
+                ///solve all friction constraints, using SIMD, if available
+                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                for (j=0;j<numFrictionPoolConstraints;j++)
+                {
+                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                        if (totalImpulse>btScalar(0))
+                        {
+                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,     *solveManifold.m_solverBodyB,solveManifold);
+                        }
+                }
+        } else
+        {
+                ///solve all joint constraints
+                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                {
+                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                        resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+                }
+                for (j=0;j<numConstraints;j++)
+                {
+                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+                }
+                ///solve all contact constraints
+                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                for (j=0;j<numPoolConstraints;j++)
+                {
+                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                        resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                }
+                ///solve all friction constraints
+                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                for (j=0;j<numFrictionPoolConstraints;j++)
+                {
+                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                        if (totalImpulse>btScalar(0))
+                        {
+                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                        }
+                }
+        }
+        return 0.f;
+}
+void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+        int iteration;
+        if (infoGlobal.m_splitImpulse)
+        {
+                if (infoGlobal.m_solverMode & SOLVER_SIMD)
+                {
+                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+                        {
+                                {
+                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                        int j;
+                                        for (j=0;j<numPoolConstraints;j++)
+                                        {
+                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                                resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                                        }
+                                }
+                        }
+                }
+                else
+                {
+                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+                        {
+                                {
+                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                        int j;
+                                        for (j=0;j<numPoolConstraints;j++)
+                                        {
+                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                                resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                                        }
+                                }
+                        }
+                }
+        }
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+        BT_PROFILE("solveGroupCacheFriendlyIterations");
         //should traverse the contacts random order...
         int iteration;
 …
                 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+                {
+                        int j;
+                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+                        {
+                                if ((iteration & 7) == 0) {
+                                        for (j=0; j<numConstraintPool; ++j) {
+                                                int tmp = m_orderTmpConstraintPool[j];
+                                                int swapi = btRandInt2(j+1);
+                                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+                                                m_orderTmpConstraintPool[swapi] = tmp;
+                                        }
+                                        for (j=0; j<numFrictionPool; ++j) {
+                                                int tmp = m_orderFrictionConstraintPool[j];
+                                                int swapi = btRandInt2(j+1);
+                                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+                                                m_orderFrictionConstraintPool[swapi] = tmp;
+                                        }
+                                }
+                        }
+                        if (infoGlobal.m_solverMode & SOLVER_SIMD)
+                        {
+                                ///solve all joint constraints, using SIMD, if available
+                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                                {
+                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                                        resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+                                }
+                                for (j=0;j<numConstraints;j++)
+                                {
+                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+                                }
+                                ///solve all contact constraints using SIMD, if available
+                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                for (j=0;j<numPoolConstraints;j++)
+                                {
+                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                        resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                }
+                                ///solve all friction constraints, using SIMD, if available
+                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                                for (j=0;j<numFrictionPoolConstraints;j++)
+                                {
+                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                                        if (totalImpulse>btScalar(0))
+                                        {
+                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                                resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],       m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                        }
+                                }
+                        } else
+                        {
+                                ///solve all joint constraints
+                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                                {
+                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                                        resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+                                }
+                                for (j=0;j<numConstraints;j++)
+                                {
+                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+                                }
+                                ///solve all contact constraints
+                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                for (j=0;j<numPoolConstraints;j++)
+                                {
+                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                        resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                }
+                                ///solve all friction constraints
+                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                                for (j=0;j<numFrictionPoolConstraints;j++)
+                                {
+                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                                        if (totalImpulse>btScalar(0))
+                                        {
+                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                                resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                        }
+                                }
+                        }
+                }
+                        solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+                }
+                solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+        }
         return 0.f;
+}
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+        BT_PROFILE("solveGroup");
+        //we only implement SOLVER_CACHE_FRIENDLY now
+        //you need to provide at least some bodies
+        btAssert(bodies);
+        btAssert(numBodies);
+        int i;
+        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
         int j;
+        int i,j;
         for (j=0;j<numPoolConstraints;j++)
 …
+        }
+        numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
+        for (j=0;j<numPoolConstraints;j++)
+        {
+                const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
+                btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
+                btScalar sum = constr->internalGetAppliedImpulse();
+                sum += solverConstr.m_appliedImpulse;
+                constr->internalSetAppliedImpulse(sum);
+        }
         if (infoGlobal.m_splitImpulse)
+        {
+                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+                {
+                        m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
+                for ( i=0;i<numBodies;i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                                body->internalWritebackVelocity(infoGlobal.m_timeStep);
+                }
         } else
+        {
+                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+                {
+                        m_tmpSolverBodyPool[i].writebackVelocity();
+                }
+        }
+        m_tmpSolverBodyPool.resize(0);
+                for ( i=0;i<numBodies;i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                                body->internalWritebackVelocity();
+                }
+        }
         m_tmpSolverContactConstraintPool.resize(0);
         m_tmpSolverNonContactConstraintPool.resize(0);
 …
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+        BT_PROFILE("solveGroup");
+        //you need to provide at least some bodies
+        btAssert(bodies);
+        btAssert(numBodies);
+        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        return 0.f;
+}
 void    btSequentialImpulseConstraintSolver::reset()
 …
+}
+btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
+{
+        static btRigidBody s_fixed(0, 0,0);
+        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+        return s_fixed;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

-                      r5781
+                      r8284
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 …
 protected:
-        btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
         btConstraintArray                       m_tmpSolverContactConstraintPool;
         btConstraintArray                       m_tmpSolverNonContactConstraintPool;
 …
         btAlignedObjectArray<int>       m_orderTmpConstraintPool;
         btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
+        btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
+        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
+        void setupFrictionConstraint(   btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB,
+                                                                        btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+                                                                        btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
+                                                                        btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+        void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp,
+                                                                const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
+                                                                btVector3& rel_pos1, btVector3& rel_pos2);
+        void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1,
+                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
         ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
         unsigned long   m_btSeed2;
         void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+//      void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
         btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
         void    convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+        void    resolveSplitPenetrationSIMD(
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& contactConstraint);
         void    resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& contactConstraint);
         //internal method
         int     getOrInitSolverBody(btCollisionObject& body);
         void    resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
         void    resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
         void    resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
         void    resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
+protected:
+        static btRigidBody& getFixedBody();
+        virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
 public:
 …
         virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
-        btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-        btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
         ///clear internal cached data and reset random seed
         virtual void    reset();

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

-                      r5781
+                      r8284
 */
+//-----------------------------------------------------------------------------
 #include "btSliderConstraint.h"
 …
 #include <new>
+//-----------------------------------------------------------------------------
+#define USE_OFFSET_FOR_CONSTANT_FRAME true
 void btSliderConstraint::initParams()
 …
         m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingDirLin = btScalar(0.);
+        m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingDirAng = btScalar(0.);
+        m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
         m_poweredLinMotor = false;
 …
         m_accumulatedAngMotorImpulse = btScalar(0.0);
+} // btSliderConstraint::initParams()
+//-----------------------------------------------------------------------------
+btSliderConstraint::btSliderConstraint()
+        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
+                m_useLinearReferenceFrameA(true),
+                m_useSolveConstraintObsolete(false)
+//              m_useSolveConstraintObsolete(true)
+        m_flags = 0;
+        m_flags = 0;
+        m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
+        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
+                m_useSolveConstraintObsolete(false),
+                m_frameInA(frameInA),
+        m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
         initParams();
+} // btSliderConstraint::btSliderConstraint()
+//-----------------------------------------------------------------------------
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
+        , m_frameInA(frameInA)
+        , m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+                m_useSolveConstraintObsolete(false)
+//              m_useSolveConstraintObsolete(true)
+{
+}
+btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
+                m_useSolveConstraintObsolete(false),
+                m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+        ///not providing rigidbody A means implicitly using worldspace for body A
+        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+//      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
         initParams();
 } // btSliderConstraint::btSliderConstraint()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::buildJacobian()
+{
+        if (!m_useSolveConstraintObsolete)
+        {
+                return;
+        }
         if(m_useLinearReferenceFrameA)
+        {
                 buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
+}
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        }
         else
+        {
+                buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
+        }
+} // btSliderConstraint::buildJacobian()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+{
+        //calculate transforms
+    m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
+    m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
+                info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+                info->nub = 2;
+                //prepare constraint
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+                testAngLimits();
+                testLinLimits();
+                if(getSolveLinLimit() || getPoweredLinMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd linear as well
+                        info->nub--;
+                }
+                if(getSolveAngLimit() || getPoweredAngMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd angular as well
+                        info->nub--;
+                }
+        }
+}
+void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+        info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
+        info->nub = 0;
+}
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+}
+void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+        {
+                m_calculatedTransformA = transA * m_frameInA;
+                m_calculatedTransformB = transB * m_frameInB;
+        }
+        else
+        {
+                m_calculatedTransformA = transB * m_frameInB;
+                m_calculatedTransformB = transA * m_frameInA;
+        }
         m_realPivotAInW = m_calculatedTransformA.getOrigin();
         m_realPivotBInW = m_calculatedTransformB.getOrigin();
         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+        m_delta = m_realPivotBInW - m_realPivotAInW;
+        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+        {
+                m_delta = m_realPivotBInW - m_realPivotAInW;
+        }
+        else
+        {
+                m_delta = m_realPivotAInW - m_realPivotBInW;
+        }
         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
-        m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
-        m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
     btVector3 normalWorld;
     int i;
 …
+    {
                 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-                new (&m_jacLin[i]) btJacobianEntry(
-                        rbA.getCenterOfMassTransform().getBasis().transpose(),
-                        rbB.getCenterOfMassTransform().getBasis().transpose(),
-                        m_relPosA,
-                        m_relPosB,
-                        normalWorld,
-                        rbA.getInvInertiaDiagLocal(),
-                        rbA.getInvMass(),
-                        rbB.getInvInertiaDiagLocal(),
-                        rbB.getInvMass()
-                        );
-                m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
                 m_depth[i] = m_delta.dot(normalWorld);
+    }
+        testLinLimits();
+    // angular part
+    for(i = 0; i < 3; i++)
+    {
+                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+                new (&m_jacAng[i])      btJacobianEntry(
+                        normalWorld,
+            rbA.getCenterOfMassTransform().getBasis().transpose(),
+            rbB.getCenterOfMassTransform().getBasis().transpose(),
+            rbA.getInvInertiaDiagLocal(),
+            rbB.getInvInertiaDiagLocal()
+                        );
+        }
+        testAngLimits();
+        btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
+        m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
+        // clear accumulator for motors
+        m_accumulatedLinMotorImpulse = btScalar(0.0);
+        m_accumulatedAngMotorImpulse = btScalar(0.0);
+} // btSliderConstraint::buildJacobianInt()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+}
+void btSliderConstraint::testLinLimits(void)
+{
+        m_solveLinLim = false;
+        m_linPos = m_depth[0];
+        if(m_lowerLinLimit <= m_upperLinLimit)
+        {
+                if(m_depth[0] > m_upperLinLimit)
+                {
+                        m_depth[0] -= m_upperLinLimit;
+                        m_solveLinLim = true;
+                }
+                else if(m_depth[0] < m_lowerLinLimit)
+                {
+                        m_depth[0] -= m_lowerLinLimit;
+                        m_solveLinLim = true;
+                }
+                else
+                {
+                        m_depth[0] = btScalar(0.);
+                }
+        }
         else
+        {
+                info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+                info->nub = 2;
+                //prepare constraint
+                calculateTransforms();
+                testLinLimits();
+                if(getSolveLinLimit() || getPoweredLinMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd linear as well
+                        info->nub--;
+                }
+                testAngLimits();
+                if(getSolveAngLimit() || getPoweredAngMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd angular as well
+                        info->nub--;
+                }
+        }
+} // btSliderConstraint::getInfo1()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+                m_depth[0] = btScalar(0.);
+        }
+}
+void btSliderConstraint::testAngLimits(void)
+{
+        m_angDepth = btScalar(0.);
+        m_solveAngLim = false;
+        if(m_lowerAngLimit <= m_upperAngLimit)
+        {
+                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+//              btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+                btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
+                rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
+                m_angPos = rot;
+                if(rot < m_lowerAngLimit)
+                {
+                        m_angDepth = rot - m_lowerAngLimit;
+                        m_solveAngLim = true;
+                }
+                else if(rot > m_upperAngLimit)
+                {
+                        m_angDepth = rot - m_upperAngLimit;
+                        m_solveAngLim = true;
+                }
+        }
+}
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+        btVector3 ancorInA;
+        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+        return ancorInA;
+}
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+        btVector3 ancorInB;
+        ancorInB = m_frameInB.getOrigin();
+        return ancorInB;
+}
+void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
+{
+        const btTransform& trA = getCalculatedTransformA();
+        const btTransform& trB = getCalculatedTransformB();
         btAssert(!m_useSolveConstraintObsolete);
         int i, s = info->rowskip;
+        const btTransform& trA = getCalculatedTransformA();
+        const btTransform& trB = getCalculatedTransformB();
         btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
+        // make rotations around Y and Z equal
+        // difference between frames in WCS
+        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+        // now get weight factors depending on masses
+        btScalar miA = rbAinvMass;
+        btScalar miB = rbBinvMass;
+        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+        btScalar miS = miA + miB;
+        btScalar factA, factB;
+        if(miS > btScalar(0.f))
+        {
+                factA = miB / miS;
+        }
+        else
+        {
+                factA = btScalar(0.5f);
+        }
+        factB = btScalar(1.0f) - factA;
+        btVector3 ax1, p, q;
+        btVector3 ax1A = trA.getBasis().getColumn(0);
+        btVector3 ax1B = trB.getBasis().getColumn(0);
+        if(m_useOffsetForConstraintFrame)
+        {
+                // get the desired direction of slider axis
+                // as weighted sum of X-orthos of frameA and frameB in WCS
+                ax1 = ax1A * factA + ax1B * factB;
+                ax1.normalize();
+                // construct two orthos to slider axis
+                btPlaneSpace1 (ax1, p, q);
+        }
+        else
+        { // old way - use frameA
+                ax1 = trA.getBasis().getColumn(0);
+                // get 2 orthos to slider axis (Y, Z)
+                p = trA.getBasis().getColumn(1);
+                q = trA.getBasis().getColumn(2);
+        }
+        // make rotations around these orthos equal
         // the slider axis should be the only unconstrained
         // rotational axis, the angular velocity of the two bodies perpendicular to
 …
         // where p and q are unit vectors normal to the slider axis, and w1 and w2
         // are the angular velocity vectors of the two bodies.
-        // get slider axis (X)
-        btVector3 ax1 = trA.getBasis().getColumn(0);
-        // get 2 orthos to slider axis (Y, Z)
-        btVector3 p = trA.getBasis().getColumn(1);
-        btVector3 q = trA.getBasis().getColumn(2);
-        // set the two slider rows
         info->m_J1angularAxis[0] = p[0];
         info->m_J1angularAxis[1] = p[1];
 …
         // compute the right hand side of the constraint equation. set relative
         // body velocities along p and q to bring the slider back into alignment.
         // if ax1,ax2 are the unit length slider axes as computed from body1 and
         // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+        // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
+        // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
         // along u to cover angle erp*theta in one step :
 …
         // ax1 x ax2 is in the plane space of ax1, so we project the angular
         // velocity to p and q to find the right hand side.
+        btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+    btVector3 ax2 = trB.getBasis().getColumn(0);
+        btVector3 u = ax1.cross(ax2);
+//      btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+        btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
+        btScalar k = info->fps * currERP;
+        btVector3 u = ax1A.cross(ax1B);
         info->m_constraintError[0] = k * u.dot(p);
         info->m_constraintError[s] = k * u.dot(q);
+        // pull out pos and R for both bodies. also get the connection
+        // vector c = pos2-pos1.
+        // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
+        // result in three equations, so we project along the planespace vectors
+        // so that sliding along the slider axis is disregarded. for symmetry we
+        // also consider rotation around center of mass of two bodies (factA and factB).
+        btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
+        btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
+        int s2 = 2 * s, s3 = 3 * s;
+        btVector3 c;
+        btScalar miA = m_rbA.getInvMass();
+        btScalar miB = m_rbB.getInvMass();
+        btScalar miS = miA + miB;
+        btScalar factA, factB;
+        if(miS > btScalar(0.f))
+        {
+                factA = miB / miS;
+        }
+        else
+        {
+                factA = btScalar(0.5f);
+        }
+        if(factA > 0.99f) factA = 0.99f;
+        if(factA < 0.01f) factA = 0.01f;
+        factB = btScalar(1.0f) - factA;
+        c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+        btVector3 tmp = c.cross(p);
+        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+        for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+        tmp = c.cross(q);
+        for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+        for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+        for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+        for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+        // compute two elements of right hand side. we want to align the offset
+        // point (in body 2's frame) with the center of body 1.
+        btVector3 ofs; // offset point in global coordinates
+        ofs = trB.getOrigin() - trA.getOrigin();
+        k = info->fps * info->erp * getSoftnessOrthoLin();
+        info->m_constraintError[s2] = k * p.dot(ofs);
+        info->m_constraintError[s3] = k * q.dot(ofs);
+        int nrow = 3; // last filled row
+        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
+        {
+                info->cfm[0] = m_cfmOrthoAng;
+                info->cfm[s] = m_cfmOrthoAng;
+        }
+        int nrow = 1; // last filled row
         int srow;
+        // check linear limits linear
+        btScalar limit_err = btScalar(0.0);
+        int limit = 0;
+        btScalar limit_err;
+        int limit;
+        int powered;
+        // next two rows.
+        // we want: velA + wA x relA == velB + wB x relB ... but this would
+        // result in three equations, so we project along two orthos to the slider axis
+        btTransform bodyA_trans = transA;
+        btTransform bodyB_trans = transB;
+        nrow++;
+        int s2 = nrow * s;
+        nrow++;
+        int s3 = nrow * s;
+        btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
+        if(m_useOffsetForConstraintFrame)
+        {
+                // get vector from bodyB to frameB in WCS
+                relB = trB.getOrigin() - bodyB_trans.getOrigin();
+                // get its projection to slider axis
+                btVector3 projB = ax1 * relB.dot(ax1);
+                // get vector directed from bodyB to slider axis (and orthogonal to it)
+                btVector3 orthoB = relB - projB;
+                // same for bodyA
+                relA = trA.getOrigin() - bodyA_trans.getOrigin();
+                btVector3 projA = ax1 * relA.dot(ax1);
+                btVector3 orthoA = relA - projA;
+                // get desired offset between frames A and B along slider axis
+                btScalar sliderOffs = m_linPos - m_depth[0];
+                // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
+                btVector3 totalDist = projA + ax1 * sliderOffs - projB;
+                // get offset vectors relA and relB
+                relA = orthoA + totalDist * factA;
+                relB = orthoB - totalDist * factB;
+                // now choose average ortho to slider axis
+                p = orthoB * factA + orthoA * factB;
+                btScalar len2 = p.length2();
+                if(len2 > SIMD_EPSILON)
+                {
+                        p /= btSqrt(len2);
+                }
+                else
+                {
+                        p = trA.getBasis().getColumn(1);
+                }
+                // make one more ortho
+                q = ax1.cross(p);
+                // fill two rows
+                tmpA = relA.cross(p);
+                tmpB = relB.cross(p);
+                for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+                for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+                tmpA = relA.cross(q);
+                tmpB = relB.cross(q);
+                if(hasStaticBody && getSolveAngLimit())
+                { // to make constraint between static and dynamic objects more rigid
+                        // remove wA (or wB) from equation if angular limit is hit
+                        tmpB *= factB;
+                        tmpA *= factA;
+                }
+                for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
+                for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+        }
+        else
+        {       // old way - maybe incorrect if bodies are not on the slider axis
+                // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
+                c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+                btVector3 tmp = c.cross(p);
+                for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+                for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+                tmp = c.cross(q);
+                for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+                for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+        }
+        // compute two elements of right hand side
+        //      k = info->fps * info->erp * getSoftnessOrthoLin();
+        currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
+        k = info->fps * currERP;
+        btScalar rhs = k * p.dot(ofs);
+        info->m_constraintError[s2] = rhs;
+        rhs = k * q.dot(ofs);
+        info->m_constraintError[s3] = rhs;
+        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
+        {
+                info->cfm[s2] = m_cfmOrthoLin;
+                info->cfm[s3] = m_cfmOrthoLin;
+        }
+        // check linear limits
+        limit_err = btScalar(0.0);
+        limit = 0;
         if(getSolveLinLimit())
+        {
 …
                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+        }
         int powered = 0;
+        powered = 0;
         if(getPoweredLinMotor())
+        {
 …
                 // a torque couple will result in limited slider-jointed free
                 // bodies from gaining angular momentum.
+                // the solution used here is to apply the constraint forces at the center of mass of the two bodies
+                btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
+//              c = btScalar(0.5) * c;
+                ltd = c.cross(ax1);
+                info->m_J1angularAxis[srow+0] = factA*ltd[0];
+                info->m_J1angularAxis[srow+1] = factA*ltd[1];
+                info->m_J1angularAxis[srow+2] = factA*ltd[2];
+                info->m_J2angularAxis[srow+0] = factB*ltd[0];
+                info->m_J2angularAxis[srow+1] = factB*ltd[1];
+                info->m_J2angularAxis[srow+2] = factB*ltd[2];
+                if(m_useOffsetForConstraintFrame)
+                {
+                        // this is needed only when bodyA and bodyB are both dynamic.
+                        if(!hasStaticBody)
+                        {
+                                tmpA = relA.cross(ax1);
+                                tmpB = relB.cross(ax1);
+                                info->m_J1angularAxis[srow+0] = tmpA[0];
+                                info->m_J1angularAxis[srow+1] = tmpA[1];
+                                info->m_J1angularAxis[srow+2] = tmpA[2];
+                                info->m_J2angularAxis[srow+0] = -tmpB[0];
+                                info->m_J2angularAxis[srow+1] = -tmpB[1];
+                                info->m_J2angularAxis[srow+2] = -tmpB[2];
+                        }
+                }
+                else
+                { // The old way. May be incorrect if bodies are not on the slider axis
+                        btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
+                        ltd = c.cross(ax1);
+                        info->m_J1angularAxis[srow+0] = factA*ltd[0];
+                        info->m_J1angularAxis[srow+1] = factA*ltd[1];
+                        info->m_J1angularAxis[srow+2] = factA*ltd[2];
+                        info->m_J2angularAxis[srow+0] = factB*ltd[0];
+                        info->m_J2angularAxis[srow+1] = factB*ltd[1];
+                        info->m_J2angularAxis[srow+2] = factB*ltd[2];
+                }
                 // right-hand part
                 btScalar lostop = getLowerLinLimit();
 …
                 info->m_lowerLimit[srow] = 0.;
                 info->m_upperLimit[srow] = 0.;
+                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
                 if(powered)
+                {
+            info->cfm[nrow] = btScalar(0.0);
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
+                        {
+                                info->cfm[srow] = m_cfmDirLin;
+                        }
                         btScalar tag_vel = getTargetLinMotorVelocity();
+                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
+//                      info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
+                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
 …
                 if(limit)
+                {
                         k = info->fps * info->erp;
+                        k = info->fps * currERP;
                         info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
+                        {
+                                info->cfm[srow] = m_cfmLimLin;
+                        }
                         if(lostop == histop)
                         {       // limited low and high simultaneously
 …
                         if(bounce > btScalar(0.0))
+                        {
                                 btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
                                 vel -= m_rbB.getLinearVelocity().dot(ax1);
+                                btScalar vel = linVelA.dot(ax1);
+                                vel -= linVelB.dot(ax1);
                                 vel *= signFact;
                                 // only apply bounce if the velocity is incoming, and if the
 …
                         powered = 0;
+                }
+                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
                 if(powered)
+                {
+            info->cfm[srow] = btScalar(0.0);
+                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
+                        {
+                                info->cfm[srow] = m_cfmDirAng;
+                        }
+                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
 …
                 if(limit)
+                {
                         k = info->fps * info->erp;
+                        k = info->fps * currERP;
                         info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
+                        {
+                                info->cfm[srow] = m_cfmLimAng;
+                        }
                         if(lostop == histop)
+                        {
 …
                 } // if(limit)
         } // if angular limit or powered
+} // btSliderConstraint::getInfo2()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                m_timeStep = timeStep;
+                if(m_useLinearReferenceFrameA)
+                {
+                        solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
+}
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
+void btSliderConstraint::setParam(int num, btScalar value, int axis)
+{
+        switch(num)
+        {
+        case BT_CONSTRAINT_STOP_ERP :
+                if(axis < 1)
+                {
+                        m_softnessLimLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
+                }
+                else if(axis < 3)
+                {
+                        m_softnessOrthoLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
+                }
+                else if(axis == 3)
+                {
+                        m_softnessLimAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
+                }
+                else if(axis < 6)
+                {
+                        m_softnessOrthoAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
+                }
                 else
+                {
+                        solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+                }
+        }
+} // btSliderConstraint::solveConstraint()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
+{
+    int i;
+    // linear
+    btVector3 velA;
+        bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
+    btVector3 velB;
+        bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
+    btVector3 vel = velA - velB;
+        for(i = 0; i < 3; i++)
+    {
+                const btVector3& normal = m_jacLin[i].m_linearJointAxis;
+                btScalar rel_vel = normal.dot(vel);
+                // calculate positional error
+                btScalar depth = m_depth[i];
+                // get parameters
+                btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
+                btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
+                btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
+                // calcutate and apply impulse
+                btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
+                btVector3 impulse_vector = normal * normalImpulse;
+                //rbA.applyImpulse( impulse_vector, m_relPosA);
+                //rbB.applyImpulse(-impulse_vector, m_relPosB);
+                {
+                        btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+                        btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+                        bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+                        bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+                }
+                if(m_poweredLinMotor && (!i))
+                { // apply linear motor
+                        if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
+                        {
+                                btScalar desiredMotorVel = m_targetLinMotorVelocity;
+                                btScalar motor_relvel = desiredMotorVel + rel_vel;
+                                normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
+                                // clamp accumulated impulse
+                                btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
+                                if(new_acc  > m_maxLinMotorForce)
+                                {
+                                        new_acc = m_maxLinMotorForce;
+                                }
+                                btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
+                                if(normalImpulse < btScalar(0.0))
+                                {
+                                        normalImpulse = -del;
+                                }
+                                else
+                                {
+                                        normalImpulse = del;
+                                }
+                                m_accumulatedLinMotorImpulse = new_acc;
+                                // apply clamped impulse
+                                impulse_vector = normal * normalImpulse;
+                                //rbA.applyImpulse( impulse_vector, m_relPosA);
+                                //rbB.applyImpulse(-impulse_vector, m_relPosB);
+                                {
+                                        btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+                                        btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+                                        bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+                                        bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+                                }
+                        }
+                }
+    }
+        // angular
+        // get axes in world space
+        btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
+        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
+        btVector3 angVelA;
+        bodyA.getAngularVelocity(angVelA);
+        btVector3 angVelB;
+        bodyB.getAngularVelocity(angVelB);
+        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
+        btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
+        btVector3 angAorthog = angVelA - angVelAroundAxisA;
+        btVector3 angBorthog = angVelB - angVelAroundAxisB;
+        btVector3 velrelOrthog = angAorthog-angBorthog;
+        //solve orthogonal angular velocity correction
+        btScalar len = velrelOrthog.length();
+        btScalar orthorImpulseMag = 0.f;
+        if (len > btScalar(0.00001))
+        {
+                btVector3 normal = velrelOrthog.normalized();
+                btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
+                //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+                orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+        }
+        //solve angular positional correction
+        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+        btVector3 angularAxis = angularError;
+        btScalar angularImpulseMag = 0;
+        btScalar len2 = angularError.length();
+        if (len2>btScalar(0.00001))
+        {
+                btVector3 normal2 = angularError.normalized();
+                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
+                angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+                angularError *= angularImpulseMag;
+        }
+        // apply impulse
+        //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+        //rbB.applyTorqueImpulse(velrelOrthog-angularError);
+        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
+        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
+        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
+        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
+        btScalar impulseMag;
+        //solve angular limits
+        if(m_solveAngLim)
+        {
+                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
+                impulseMag *= m_kAngle * m_softnessLimAng;
+        }
+        else
+        {
+                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
+                impulseMag *= m_kAngle * m_softnessDirAng;
+        }
+        btVector3 impulse = axisA * impulseMag;
+        //rbA.applyTorqueImpulse(impulse);
+        //rbB.applyTorqueImpulse(-impulse);
+        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
+        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
+        //apply angular motor
+        if(m_poweredAngMotor)
+        {
+                if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
+                {
+                        btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
+                        btScalar projRelVel = velrel.dot(axisA);
+                        btScalar desiredMotorVel = m_targetAngMotorVelocity;
+                        btScalar motor_relvel = desiredMotorVel - projRelVel;
+                        btScalar angImpulse = m_kAngle * motor_relvel;
+                        // clamp accumulated impulse
+                        btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
+                        if(new_acc  > m_maxAngMotorForce)
+                        {
+                                new_acc = m_maxAngMotorForce;
+                        }
+                        btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
+                        if(angImpulse < btScalar(0.0))
+                        {
+                                angImpulse = -del;
+                        }
+                        else
+                        {
+                                angImpulse = del;
+                        }
+                        m_accumulatedAngMotorImpulse = new_acc;
+                        // apply clamped impulse
+                        btVector3 motorImp = angImpulse * axisA;
+                        //rbA.applyTorqueImpulse(motorImp);
+                        //rbB.applyTorqueImpulse(-motorImp);
+                        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
+                        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
+                }
+        }
+} // btSliderConstraint::solveConstraint()
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+void btSliderConstraint::calculateTransforms(void){
+        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+        {
+                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+                m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+        }
+        else
+        {
+                m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
+                m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
+        }
+        m_realPivotAInW = m_calculatedTransformA.getOrigin();
+        m_realPivotBInW = m_calculatedTransformB.getOrigin();
+        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+        {
+                m_delta = m_realPivotBInW - m_realPivotAInW;
+        }
+        else
+        {
+                m_delta = m_realPivotAInW - m_realPivotBInW;
+        }
+        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+    btVector3 normalWorld;
+    int i;
+    //linear part
+    for(i = 0; i < 3; i++)
+    {
+                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+                m_depth[i] = m_delta.dot(normalWorld);
+    }
+} // btSliderConstraint::calculateTransforms()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::testLinLimits(void)
+{
+        m_solveLinLim = false;
+        m_linPos = m_depth[0];
+        if(m_lowerLinLimit <= m_upperLinLimit)
+        {
+                if(m_depth[0] > m_upperLinLimit)
+                {
+                        m_depth[0] -= m_upperLinLimit;
+                        m_solveLinLim = true;
+                }
+                else if(m_depth[0] < m_lowerLinLimit)
+                {
+                        m_depth[0] -= m_lowerLinLimit;
+                        m_solveLinLim = true;
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_CFM :
+                if(axis < 1)
+                {
+                        m_cfmDirLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
+                }
+                else if(axis == 3)
+                {
+                        m_cfmDirAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
+                }
                 else
+                {
+                        m_depth[0] = btScalar(0.);
+                }
+        }
+        else
+        {
+                m_depth[0] = btScalar(0.);
+        }
+} // btSliderConstraint::testLinLimits()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::testAngLimits(void)
+{
+        m_angDepth = btScalar(0.);
+        m_solveAngLim = false;
+        if(m_lowerAngLimit <= m_upperAngLimit)
+        {
+                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+                m_angPos = rot;
+                if(rot < m_lowerAngLimit)
+                {
+                        m_angDepth = rot - m_lowerAngLimit;
+                        m_solveAngLim = true;
+                }
+                else if(rot > m_upperAngLimit)
+                {
+                        m_angDepth = rot - m_upperAngLimit;
+                        m_solveAngLim = true;
+                }
+        }
+} // btSliderConstraint::testAngLimits()
+//-----------------------------------------------------------------------------
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+        btVector3 ancorInA;
+        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+        return ancorInA;
+} // btSliderConstraint::getAncorInA()
+//-----------------------------------------------------------------------------
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+        btVector3 ancorInB;
+        ancorInB = m_frameInB.getOrigin();
+        return ancorInB;
+} // btSliderConstraint::getAncorInB();
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_STOP_CFM :
+                if(axis < 1)
+                {
+                        m_cfmLimLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
+                }
+                else if(axis < 3)
+                {
+                        m_cfmOrthoLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
+                }
+                else if(axis == 3)
+                {
+                        m_cfmLimAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
+                }
+                else if(axis < 6)
+                {
+                        m_cfmOrthoAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        }
+}
+///return the local value of parameter
+btScalar btSliderConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal(SIMD_INFINITY);
+        switch(num)
+        {
+        case BT_CONSTRAINT_STOP_ERP :
+                if(axis < 1)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
+                        retVal = m_softnessLimLin;
+                }
+                else if(axis < 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
+                        retVal = m_softnessOrthoLin;
+                }
+                else if(axis == 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
+                        retVal = m_softnessLimAng;
+                }
+                else if(axis < 6)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
+                        retVal = m_softnessOrthoAng;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_CFM :
+                if(axis < 1)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
+                        retVal = m_cfmDirLin;
+                }
+                else if(axis == 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
+                        retVal = m_cfmDirAng;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_STOP_CFM :
+                if(axis < 1)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
+                        retVal = m_cfmLimLin;
+                }
+                else if(axis < 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
+                        retVal = m_cfmOrthoLin;
+                }
+                else if(axis == 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
+                        retVal = m_cfmLimAng;
+                }
+                else if(axis < 6)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
+                        retVal = m_cfmOrthoAng;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h

-                      r5781
+                      r8284
 #define SLIDER_CONSTRAINT_H
+//-----------------------------------------------------------------------------
 #include "LinearMath/btVector3.h"
 …
 #include "btTypedConstraint.h"
+//-----------------------------------------------------------------------------
 class btRigidBody;
+//-----------------------------------------------------------------------------
 #define SLIDER_CONSTRAINT_DEF_SOFTNESS          (btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_DAMPING           (btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_RESTITUTION       (btScalar(0.7))
+//-----------------------------------------------------------------------------
+#define SLIDER_CONSTRAINT_DEF_CFM                       (btScalar(0.f))
+enum btSliderFlags
+{
+        BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
+        BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
+        BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
+        BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
+        BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
+        BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
+        BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
+        BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
+        BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
+        BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
+        BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
+        BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
+};
 class btSliderConstraint : public btTypedConstraint
 …
         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
         bool            m_useSolveConstraintObsolete;
+        bool            m_useOffsetForConstraintFrame;
         btTransform     m_frameInA;
     btTransform m_frameInB;
 …
         btScalar m_restitutionDirLin;
         btScalar m_dampingDirLin;
+        btScalar m_cfmDirLin;
         btScalar m_softnessDirAng;
         btScalar m_restitutionDirAng;
         btScalar m_dampingDirAng;
+        btScalar m_cfmDirAng;
         btScalar m_softnessLimLin;
         btScalar m_restitutionLimLin;
         btScalar m_dampingLimLin;
+        btScalar m_cfmLimLin;
         btScalar m_softnessLimAng;
         btScalar m_restitutionLimAng;
         btScalar m_dampingLimAng;
+        btScalar m_cfmLimAng;
         btScalar m_softnessOrthoLin;
         btScalar m_restitutionOrthoLin;
         btScalar m_dampingOrthoLin;
+        btScalar m_cfmOrthoLin;
         btScalar m_softnessOrthoAng;
         btScalar m_restitutionOrthoAng;
         btScalar m_dampingOrthoAng;
+        btScalar m_cfmOrthoAng;
         // for interlal use
         bool m_solveLinLim;
         bool m_solveAngLim;
+        int m_flags;
         btJacobianEntry m_jacLin[3];
 …
         // constructors
     btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+    btSliderConstraint();
+    btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
         // overrides
+    virtual void        buildJacobian();
     virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual(btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
         // access
 …
     void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
     btScalar getLowerAngLimit() { return m_lowerAngLimit; }
     void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
+    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
     btScalar getUpperAngLimit() { return m_upperAngLimit; }
     void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
+    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
         bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
         btScalar getSoftnessDirLin() { return m_softnessDirLin; }
 …
         bool getSolveAngLimit() { return m_solveAngLim; }
         btScalar getAngDepth() { return m_angDepth; }
-        // internal
-    void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
-    void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
         // shared code used by ODE solver
+        void    calculateTransforms(void);
+        void    testLinLimits(void);
+        void    testLinLimits2(btConstraintInfo2* info);
+        void    testAngLimits(void);
+        void    calculateTransforms(const btTransform& transA,const btTransform& transB);
+        void    testLinLimits();
+        void    testAngLimits();
         // access for PE Solver
+        btVector3 getAncorInA(void);
+        btVector3 getAncorInB(void);
+        btVector3 getAncorInA();
+        btVector3 getAncorInB();
+        // access for UseFrameOffset
+        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+        ///If no axis is provided, it uses the default axis for this constraint.
+        virtual void    setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+//-----------------------------------------------------------------------------
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btSliderConstraintData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformFloatData m_rbBFrame;
+        float   m_linearUpperLimit;
+        float   m_linearLowerLimit;
+        float   m_angularUpperLimit;
+        float   m_angularLowerLimit;
+        int     m_useLinearReferenceFrameA;
+        int m_useOffsetForConstraintFrame;
+};
+SIMD_FORCE_INLINE               int     btSliderConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btSliderConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
+        btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
+        m_frameInA.serializeFloat(sliderData->m_rbAFrame);
+        m_frameInB.serializeFloat(sliderData->m_rbBFrame);
+        sliderData->m_linearUpperLimit = float(m_upperLinLimit);
+        sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
+        sliderData->m_angularUpperLimit = float(m_upperAngLimit);
+        sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
+        sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
+        sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
+        return "btSliderConstraintData";
+}
 #endif //SLIDER_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

-                      r5781
+                      r8284
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
 ATTRIBUTE_ALIGNED16 (struct)    btSolverBody
+ATTRIBUTE_ALIGNED64 (struct)    btSolverBodyObsolete
+{
         BT_DECLARE_ALIGNED_ALLOCATOR();
         btVector3               m_deltaLinearVelocity;
         btVector3               m_deltaAngularVelocity;
+        btScalar                m_angularFactor;
+        btScalar                m_invMass;
+        btScalar                m_friction;
+        btVector3               m_angularFactor;
+        btVector3               m_invMass;
         btRigidBody*    m_originalBody;
         btVector3               m_pushVelocity;
         //btVector3             m_turnVelocity;
+        btVector3               m_turnVelocity;
 …
+        }
+/*
+        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+        {
+                if (m_originalBody)
+                {
+                        m_pushVelocity += linearComponent*impulseMagnitude;
+                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                }
+        }
         void    writebackVelocity()
+        {
                 if (m_invMass)
+                if (m_originalBody)
+                {
                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
 …
+                }
+        }
-        */
+        void    writebackVelocity(btScalar timeStep=0)
+        void    writebackVelocity(btScalar timeStep)
+        {
+                if (m_invMass)
+        (void) timeStep;
+                if (m_originalBody)
+                {
                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
                         m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+                        //correct the position/orientation based on push/turn recovery
+                        btTransform newTransform;
+                        btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+                        m_originalBody->setWorldTransform(newTransform);
                         //m_originalBody->setCompanionId(-1);
+                }

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

-                      r5781
+                      r8284
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
 ATTRIBUTE_ALIGNED16 (struct)    btSolverConstraint
+ATTRIBUTE_ALIGNED64 (struct)    btSolverConstraint
+{
         BT_DECLARE_ALIGNED_ALLOCATOR();
 …
         union
+        {
                 int                     m_solverBodyIdA;
                 btScalar        m_unusedPadding2;
+                btRigidBody*    m_solverBodyA;
+                int                             m_companionIdA;
         };
         union
+        {
                 int                     m_solverBodyIdB;
                 btScalar        m_unusedPadding3;
+                btRigidBody*    m_solverBodyB;
+                int                             m_companionIdB;
         };
 …
         btScalar                m_lowerLimit;
         btScalar                m_upperLimit;
+        btScalar                m_rhsPenetration;
         enum            btSolverConstraintType

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

-                      r5781
+                      r8284
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btSerializer.h"
-static btRigidBody s_fixed(0, 0,0);
 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
+:m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_constraintType (type),
 m_rbA(s_fixed),
 m_rbB(s_fixed),
+m_needsFeedback(false),
+m_rbA(rbA),
+m_rbB(getFixedBody()),
 m_appliedImpulse(btScalar(0.)),
 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
-        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
-btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
-:m_userConstraintType(-1),
-m_userConstraintId(-1),
-m_constraintType (type),
-m_rbA(rbA),
-m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
-                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
+:m_userConstraintType(-1),
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_constraintType (type),
+m_needsFeedback(false),
 m_rbA(rbA),
 m_rbB(rbB),
 …
 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
-                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
+//-----------------------------------------------------------------------------
 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
 …
+        }
         return lim_fact;
 } // btTypedConstraint::getMotorFactor()
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
+        tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
+        tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
+        char* name = (char*) serializer->findNameForPointer(this);
+        tcd->m_name = (char*)serializer->getUniquePointer(name);
+        if (tcd->m_name)
+        {
+                serializer->serializeName(name);
+        }
+        tcd->m_objectType = m_objectType;
+        tcd->m_needsFeedback = m_needsFeedback;
+        tcd->m_userConstraintId =m_userConstraintId;
+        tcd->m_userConstraintType =m_userConstraintType;
+        tcd->m_appliedImpulse = float(m_appliedImpulse);
+        tcd->m_dbgDrawSize = float(m_dbgDrawSize );
+        tcd->m_disableCollisionsBetweenLinkedBodies = false;
+        int i;
+        for (i=0;i<m_rbA.getNumConstraintRefs();i++)
+                if (m_rbA.getConstraintRef(i) == this)
+                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
+        for (i=0;i<m_rbB.getNumConstraintRefs();i++)
+                if (m_rbB.getConstraintRef(i) == this)
+                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
+        return "btTypedConstraintData";
+}
+btRigidBody& btTypedConstraint::getFixedBody()
+{
+        static btRigidBody s_fixed(0, 0,0);
+        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+        return s_fixed;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "LinearMath/btScalar.h"
 #include "btSolverConstraint.h"
+struct  btSolverBody;
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btSerializer;
 enum btTypedConstraintType
+{
         POINT2POINT_CONSTRAINT_TYPE,
+        POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1,
         HINGE_CONSTRAINT_TYPE,
         CONETWIST_CONSTRAINT_TYPE,
         D6_CONSTRAINT_TYPE,
+        SLIDER_CONSTRAINT_TYPE
+        SLIDER_CONSTRAINT_TYPE,
+        CONTACT_CONSTRAINT_TYPE
 };
+enum btConstraintParams
+{
+        BT_CONSTRAINT_ERP=1,
+        BT_CONSTRAINT_STOP_ERP,
+        BT_CONSTRAINT_CFM,
+        BT_CONSTRAINT_STOP_CFM
+};
+#if 1
+        #define btAssertConstrParams(_par) btAssert(_par)
+#else
+        #define btAssertConstrParams(_par)
+#endif
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
 class btTypedConstraint
+class btTypedConstraint : public btTypedObject
+{
         int     m_userConstraintType;
+        int     m_userConstraintId;
+        btTypedConstraintType m_constraintType;
+        union
+        {
+                int     m_userConstraintId;
+                void* m_userConstraintPtr;
+        };
+        bool m_needsFeedback;
         btTypedConstraint&      operator=(btTypedConstraint&    other)
 …
         btScalar        m_dbgDrawSize;
+        ///internal method used by the constraint solver, don't use them directly
+        btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+        static btRigidBody& getFixedBody();
 public:
-        btTypedConstraint(btTypedConstraintType type);
         virtual ~btTypedConstraint() {};
         btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
 …
                 // the constraint.
                 int *findex;
+                // number of solver iterations
+                int m_numIterations;
+                //damping of the velocity
+                btScalar        m_damping;
         };
+        virtual void    buildJacobian() = 0;
+        ///internal method used by the constraint solver, don't use them directly
+        virtual void    buildJacobian() {};
+        ///internal method used by the constraint solver, don't use them directly
         virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+        {
+        }
+        (void)ca;
+        (void)solverBodyA;
+        (void)solverBodyB;
+        (void)timeStep;
+        }
+        ///internal method used by the constraint solver, don't use them directly
         virtual void getInfo1 (btConstraintInfo1* info)=0;
+        ///internal method used by the constraint solver, don't use them directly
         virtual void getInfo2 (btConstraintInfo2* info)=0;
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep) = 0;
+        btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+        ///internal method used by the constraint solver, don't use them directly
+        void    internalSetAppliedImpulse(btScalar appliedImpulse)
+        {
+                m_appliedImpulse = appliedImpulse;
+        }
+        ///internal method used by the constraint solver, don't use them directly
+        btScalar        internalGetAppliedImpulse()
+        {
+                return m_appliedImpulse;
+        }
+        ///internal method used by the constraint solver, don't use them directly
+        virtual void    solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar  /*timeStep*/) {};
         const btRigidBody& getRigidBodyA() const
 …
+        }
+        void    setUserConstraintPtr(void* ptr)
+        {
+                m_userConstraintPtr = ptr;
+        }
+        void*   getUserConstraintPtr()
+        {
+                return m_userConstraintPtr;
+        }
         int getUid() const
+        {
 …
+        }
+        bool    needsFeedback() const
+        {
+                return m_needsFeedback;
+        }
+        ///enableFeedback will allow to read the applied linear and angular impulse
+        ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
+        void    enableFeedback(bool needsFeedback)
+        {
+                m_needsFeedback = needsFeedback;
+        }
+        ///getAppliedImpulse is an estimated total applied impulse.
+        ///This feedback could be used to determine breaking constraints or playing sounds.
         btScalar        getAppliedImpulse() const
+        {
+                btAssert(m_needsFeedback);
                 return m_appliedImpulse;
+        }
 …
         btTypedConstraintType getConstraintType () const
+        {
                 return m_constraintType;
+                return btTypedConstraintType(m_objectType);
+        }
 …
                 return m_dbgDrawSize;
+        }
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+        ///If no axis is provided, it uses the default axis for this constraint.
+        virtual void    setParam(int num, btScalar value, int axis = -1) = 0;
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const = 0;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
+// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
+SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
+{
+        if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+        {
+                return angleInRadians;
+        }
+        else if(angleInRadians < angleLowerLimitInRadians)
+        {
+                btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
+                btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
+                return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
+        }
+        else if(angleInRadians > angleUpperLimitInRadians)
+        {
+                btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
+                btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+                return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
+        }
+        else
+        {
+                return angleInRadians;
+        }
+}
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btTypedConstraintData
+{
+        btRigidBodyData         *m_rbA;
+        btRigidBodyData         *m_rbB;
+        char    *m_name;
+        int     m_objectType;
+        int     m_userConstraintType;
+        int     m_userConstraintId;
+        int     m_needsFeedback;
+        float   m_appliedImpulse;
+        float   m_dbgDrawSize;
+        int     m_disableCollisionsBetweenLinkedBodies;
+        char    m_pad4[4];
+};
+SIMD_FORCE_INLINE       int     btTypedConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btTypedConstraintData);
+}
 #endif //TYPED_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp

-                      r5781
+                      r8284
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+#include "LinearMath/btStackAlloc.h"
 /*
 …
+{
         //capsule is convex hull of 2 spheres, so use btMultiSphereShape
         btVector3 inertiaHalfExtents(radius,height,radius);
         const int numSpheres = 2;
         btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
         btScalar radi[numSpheres] = {radius,radius};
         void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
         return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
+        return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
+}
 plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
 …
         worldTrans.setRotation(orn);
         body->setWorldTransform(worldTrans);
+}
+void    plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
+{
+        btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+        btAssert(body);
+        btTransform& worldTrans = body->getWorldTransform();
+        worldTrans.setFromOpenGLMatrix(matrix);
+}
 …
         btGjkPairDetector::ClosestPointInput input;
+        btStackAlloc gStackAlloc(1024*1024*2);
+        input.m_stackAlloc = &gStackAlloc;
         btTransform tr;
         tr.setIdentity();

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btActionInterface.h

-                      r5781
+                      r8284
 #include "LinearMath/btScalar.h"
+#include "btRigidBody.h"
 ///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
 class btActionInterface
+{
+        public:
+protected:
+        static btRigidBody& getFixedBody();
+public:
         virtual ~btActionInterface()
 …
 #endif //_BT_ACTION_INTERFACE_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp

-                      r5781
+                      r8284
         startProfiling(timeStep);
+        if(0 != m_internalPreTickCallback) {
+                (*m_internalPreTickCallback)(this, timeStep);
+        }
         ///update aabbs information

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 …
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+//for debug rendering
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
-#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
-#include "LinearMath/btIDebugDraw.h"
 …
 #include "LinearMath/btMotionState.h"
+#include "LinearMath/btSerializer.h"
 …
 m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
+m_localTime(btScalar(1.)/btScalar(60.)),
+m_localTime(0),
+m_synchronizeAllMotionStates(false),
 m_profileTimings(0)
+{
 …
 void    btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
+{
+///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+///to switch status _after_ adding kinematic objects to the world
+///fix it for Bullet 3.x release
         for (int i=0;i<m_collisionObjects.size();i++)
+        {
                 btCollisionObject* colObj = m_collisionObjects[i];
                 btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                                if (body->getActivationState() != ISLAND_SLEEPING)
+                                {
+                                        if (body->isKinematicObject())
+                                        {
+                                                //to calculate velocities next frame
+                                                body->saveKinematicState(timeStep);
+                                        }
+                                }
+                }
+        }
+                if (body && body->getActivationState() != ISLAND_SLEEPING)
+                {
+                        if (body->isKinematicObject())
+                        {
+                                //to calculate velocities next frame
+                                body->saveKinematicState(timeStep);
+                        }
+                }
+        }
+}
 …
         BT_PROFILE("debugDrawWorld");
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+        {
+                int numManifolds = getDispatcher()->getNumManifolds();
+                btVector3 color(0,0,0);
+                for (int i=0;i<numManifolds;i++)
+                {
+                        btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+                        //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+                        //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+                        int numContacts = contactManifold->getNumContacts();
+                        for (int j=0;j<numContacts;j++)
+                        {
+                                btManifoldPoint& cp = contactManifold->getContactPoint(j);
+                                getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+                        }
+                }
+        }
+        btCollisionWorld::debugDrawWorld();
         bool drawConstraints = false;
         if (getDebugDrawer())
 …
                 int i;
-                for (  i=0;i<m_collisionObjects.size();i++)
+                {
-                        btCollisionObject* colObj = m_collisionObjects[i];
-                        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+                        {
-                                btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
-                                switch(colObj->getActivationState())
+                                {
-                                case  ACTIVE_TAG:
-                                        color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
-                                case ISLAND_SLEEPING:
-                                        color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
-                                case WANTS_DEACTIVATION:
-                                        color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
-                                case DISABLE_DEACTIVATION:
-                                        color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
-                                case DISABLE_SIMULATION:
-                                        color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
-                                default:
+                                        {
-                                                color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
+                                        }
-                                };
-                                debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+                        }
-                        if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+                        {
-                                btVector3 minAabb,maxAabb;
-                                btVector3 colorvec(1,0,0);
-                                colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-                                m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+                        }
+                }
                 if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
+                {
 …
+{
         ///@todo: iterate over awake simulation islands!
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                        body->clearForces();
+                }
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                //need to check if next line is ok
+                //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
+                body->clearForces();
+        }
+}
 …
+{
         ///@todo: iterate over awake simulation islands!
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body && body->isActive())
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                if (body->isActive())
+                {
                         body->applyGravity();
 …
+{
         BT_PROFILE("synchronizeMotionStates");
+        {
+                //todo: iterate over awake simulation islands!
+        if (m_synchronizeAllMotionStates)
+        {
+                //iterate  over all collision objects
                 for ( int i=0;i<m_collisionObjects.size();i++)
+                {
                         btCollisionObject* colObj = m_collisionObjects[i];
                         btRigidBody* body = btRigidBody::upcast(colObj);
                         if (body)
                                 synchronizeSingleMotionState(body);
+                }
+        }
+/*
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+        {
+                for ( int i=0;i<this->m_vehicles.size();i++)
+                {
+                        for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+                        {
+                                //synchronize the wheels with the (interpolated) chassis worldtransform
+                                m_vehicles[i]->updateWheelTransform(v,true);
+                        }
+                }
+        }
+        */
+        } else
+        {
+                //iterate over all active rigid bodies
+                for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+                {
+                        btRigidBody* body = m_nonStaticRigidBodies[i];
+                        if (body->isActive())
+                                synchronizeSingleMotionState(body);
+                }
+        }
+}
 …
+        {
-                saveKinematicState(fixedTimeStep);
-                applyGravity();
                 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
                 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
+                saveKinematicState(fixedTimeStep*clampedSimulationSteps);
+                applyGravity();
                 for (int i=0;i<clampedSimulationSteps;i++)
+                {
 …
+                }
+        }
+        synchronizeMotionStates();
+        } else
+        {
+                synchronizeMotionStates();
+        }
         clearForces();
 …
         BT_PROFILE("internalSingleStepSimulation");
+        if(0 != m_internalPreTickCallback) {
+                (*m_internalPreTickCallback)(this, timeStep);
+        }
         ///apply gravity, predict motion
         predictUnconstraintMotion(timeStep);
 …
+{
         m_gravity = gravity;
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+                {
                         body->setGravity(gravity);
 …
+}
+void    btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+        btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
+}
+void    btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+        btRigidBody* body = btRigidBody::upcast(collisionObject);
+        if (body)
+                removeRigidBody(body);
+        else
+                btCollisionWorld::removeCollisionObject(collisionObject);
+}
 void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+        removeCollisionObject(body);
+}
+        m_nonStaticRigidBodies.remove(body);
+        btCollisionWorld::removeCollisionObject(body);
+}
 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
+{
         if (!body->isStaticOrKinematicObject())
+        if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+        {
                 body->setGravity(m_gravity);
 …
         if (body->getCollisionShape())
+        {
+                if (!body->isStaticObject())
+                {
+                        m_nonStaticRigidBodies.push_back(body);
+                } else
+                {
+                        body->setActivationState(ISLAND_SLEEPING);
+                }
                 bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
                 short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
 …
 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+{
         if (!body->isStaticOrKinematicObject())
+        if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+        {
                 body->setGravity(m_gravity);
 …
         if (body->getCollisionShape())
+        {
+                if (!body->isStaticObject())
+                {
+                        m_nonStaticRigidBodies.push_back(body);
+                }
+                 else
+                {
+                        body->setActivationState(ISLAND_SLEEPING);
+                }
                 addCollisionObject(body,group,mask);
+        }
 …
         BT_PROFILE("updateActivationState");
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
                 if (body)
+                {
 …
+                }
 };
 …
                 btStackAlloc*                   m_stackAlloc;
                 btDispatcher*                   m_dispatcher;
+                btAlignedObjectArray<btCollisionObject*> m_bodies;
+                btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+                btAlignedObjectArray<btTypedConstraint*> m_constraints;
                 InplaceSolverIslandCallback(
 …
+                }
                 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
+                {
 …
+                                }
+                                ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+                                if (numManifolds + numCurConstraints)
+                                {
+                                        m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                                }
+                        }
+                                if (m_solverInfo.m_minimumSolverBatchSize<=1)
+                                {
+                                        ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+                                        if (numManifolds + numCurConstraints)
+                                        {
+                                                m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                                        }
+                                } else
+                                {
+                                        for (i=0;i<numBodies;i++)
+                                                m_bodies.push_back(bodies[i]);
+                                        for (i=0;i<numManifolds;i++)
+                                                m_manifolds.push_back(manifolds[i]);
+                                        for (i=0;i<numCurConstraints;i++)
+                                                m_constraints.push_back(startConstraint[i]);
+                                        if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)
+                                        {
+                                                processConstraints();
+                                        } else
+                                        {
+                                                //printf("deferred\n");
+                                        }
+                                }
+                        }
+                }
+                void    processConstraints()
+                {
+                        if (m_manifolds.size() + m_constraints.size()>0)
+                        {
+                                m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                        }
+                        m_bodies.resize(0);
+                        m_manifolds.resize(0);
+                        m_constraints.resize(0);
+                }
         };
         //sorted version of all btTypedConstraint, based on islandId
 …
         m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
+        solverCallback.processConstraints();
         m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
+}
 …
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
 …
         btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
           btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
+                m_me(me),
                 m_allowedPenetration(0.0f),
-                m_me(me),
                 m_pairCache(pairCache),
                 m_dispatcher(dispatcher)
 …
         BT_PROFILE("integrateTransforms");
         btTransform predictedTrans;
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                        body->setHitFraction(1.f);
+                        if (body->isActive() && (!body->isStaticOrKinematicObject()))
+                        {
+                                body->predictIntegratedTransform(timeStep, predictedTrans);
+                                btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+                                if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+                                {
+                                        BT_PROFILE("CCD motion clamping");
+                                        if (body->getCollisionShape()->isConvex())
+                                        {
+                                                gNumClampedCcdMotions++;
+                                                btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+                                                btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                                btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                                sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
+                                                sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+                                                convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+                                                if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+                                                {
+                                                        body->setHitFraction(sweepResults.m_closestHitFraction);
+                                                        body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+                                                        body->setHitFraction(0.f);
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                body->setHitFraction(1.f);
+                if (body->isActive() && (!body->isStaticOrKinematicObject()))
+                {
+                        body->predictIntegratedTransform(timeStep, predictedTrans);
+                        btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+                        if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+                        {
+                                BT_PROFILE("CCD motion clamping");
+                                if (body->getCollisionShape()->isConvex())
+                                {
+                                        gNumClampedCcdMotions++;
+                                        btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+                                        //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                        btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                        sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
+                                        sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+                                        convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+                                        if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+                                        {
+                                                body->setHitFraction(sweepResults.m_closestHitFraction);
+                                                body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+                                                body->setHitFraction(0.f);
 //                                                      printf("clamped integration to hit fraction = %f\n",fraction);
+                                                }
+                                        }
+                                }
                                 body->proceedToTransform( predictedTrans);
+                        }
+                        }
+                        body->proceedToTransform( predictedTrans);
+                }
+        }
 …
+{
         BT_PROFILE("predictUnconstraintMotion");
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                        if (!body->isStaticOrKinematicObject())
+                        {
+                                body->integrateVelocities( timeStep);
+                                //damping
+                                body->applyDamping(timeStep);
+                                body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+                        }
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                if (!body->isStaticOrKinematicObject())
+                {
+                        body->integrateVelocities( timeStep);
+                        //damping
+                        body->applyDamping(timeStep);
+                        body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+                }
+        }
 …
-class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
+{
-        btIDebugDraw*   m_debugDrawer;
-        btVector3       m_color;
-        btTransform     m_worldTrans;
-public:
-        DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
-                m_debugDrawer(debugDrawer),
-                m_color(color),
-                m_worldTrans(worldTrans)
+        {
+        }
-        virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+        {
-                processTriangle(triangle,partId,triangleIndex);
+        }
-        virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
+        {
-                (void)partId;
-                (void)triangleIndex;
-                btVector3 wv0,wv1,wv2;
-                wv0 = m_worldTrans*triangle[0];
-                wv1 = m_worldTrans*triangle[1];
-                wv2 = m_worldTrans*triangle[2];
-                m_debugDrawer->drawLine(wv0,wv1,m_color);
-                m_debugDrawer->drawLine(wv1,wv2,m_color);
-                m_debugDrawer->drawLine(wv2,wv0,m_color);
+        }
-};
-void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+{
-        btVector3 start = transform.getOrigin();
-        const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
-        const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
-        const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
-        // XY
-        getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
-        getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
-        getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
-        getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
-        // XZ
-        getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
-        getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
-        getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
-        getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
-        // YZ
-        getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
-        getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
-        getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
-        getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
+}
-void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
-        // Draw a small simplex at the center of the object
+        {
-                btVector3 start = worldTransform.getOrigin();
-                getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
-                getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
-                getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
+        }
-        if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+        {
-                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
-                for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+                {
-                        btTransform childTrans = compoundShape->getChildTransform(i);
-                        const btCollisionShape* colShape = compoundShape->getChildShape(i);
-                        debugDrawObject(worldTransform*childTrans,colShape,color);
+                }
-        } else
+        {
-                switch (shape->getShapeType())
+                {
-                case SPHERE_SHAPE_PROXYTYPE:
+                        {
-                                const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
-                                btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
-                                debugDrawSphere(radius, worldTransform, color);
-                                break;
+                        }
-                case MULTI_SPHERE_SHAPE_PROXYTYPE:
+                        {
-                                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
-                                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+                                {
-                                        btTransform childTransform = worldTransform;
-                                        childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
-                                        debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
+                                }
-                                break;
+                        }
-                case CAPSULE_SHAPE_PROXYTYPE:
+                        {
-                                const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
-                                btScalar radius = capsuleShape->getRadius();
-                                btScalar halfHeight = capsuleShape->getHalfHeight();
-                                int upAxis = capsuleShape->getUpAxis();
-                                btVector3 capStart(0.f,0.f,0.f);
-                                capStart[upAxis] = -halfHeight;
-                                btVector3 capEnd(0.f,0.f,0.f);
-                                capEnd[upAxis] = halfHeight;
-                                // Draw the ends
+                                {
-                                        btTransform childTransform = worldTransform;
-                                        childTransform.getOrigin() = worldTransform * capStart;
-                                        debugDrawSphere(radius, childTransform, color);
+                                }
+                                {
-                                        btTransform childTransform = worldTransform;
-                                        childTransform.getOrigin() = worldTransform * capEnd;
-                                        debugDrawSphere(radius, childTransform, color);
+                                }
-                                // Draw some additional lines
-                                btVector3 start = worldTransform.getOrigin();
-                                capStart[(upAxis+1)%3] = radius;
-                                capEnd[(upAxis+1)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                capStart[(upAxis+1)%3] = -radius;
-                                capEnd[(upAxis+1)%3] = -radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                capStart[(upAxis+1)%3] = 0.f;
-                                capEnd[(upAxis+1)%3] = 0.f;
-                                capStart[(upAxis+2)%3] = radius;
-                                capEnd[(upAxis+2)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                capStart[(upAxis+2)%3] = -radius;
-                                capEnd[(upAxis+2)%3] = -radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                break;
+                        }
-                case CONE_SHAPE_PROXYTYPE:
+                        {
-                                const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
-                                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
-                                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
-                                btVector3 start = worldTransform.getOrigin();
-                                int upAxis= coneShape->getConeUpIndex();
-                                btVector3       offsetHeight(0,0,0);
-                                offsetHeight[upAxis] = height * btScalar(0.5);
-                                btVector3       offsetRadius(0,0,0);
-                                offsetRadius[(upAxis+1)%3] = radius;
-                                btVector3       offset2Radius(0,0,0);
-                                offset2Radius[(upAxis+2)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
-                                break;
+                        }
-                case CYLINDER_SHAPE_PROXYTYPE:
+                        {
-                                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
-                                int upAxis = cylinder->getUpAxis();
-                                btScalar radius = cylinder->getRadius();
-                                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
-                                btVector3 start = worldTransform.getOrigin();
-                                btVector3       offsetHeight(0,0,0);
-                                offsetHeight[upAxis] = halfHeight;
-                                btVector3       offsetRadius(0,0,0);
-                                offsetRadius[(upAxis+1)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-                                break;
+                        }
-                        case STATIC_PLANE_PROXYTYPE:
+                                {
-                                        const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
-                                        btScalar planeConst = staticPlaneShape->getPlaneConstant();
-                                        const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
-                                        btVector3 planeOrigin = planeNormal * planeConst;
-                                        btVector3 vec0,vec1;
-                                        btPlaneSpace1(planeNormal,vec0,vec1);
-                                        btScalar vecLen = 100.f;
-                                        btVector3 pt0 = planeOrigin + vec0*vecLen;
-                                        btVector3 pt1 = planeOrigin - vec0*vecLen;
-                                        btVector3 pt2 = planeOrigin + vec1*vecLen;
-                                        btVector3 pt3 = planeOrigin - vec1*vecLen;
-                                        getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
-                                        getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
-                                        break;
+                                }
-                default:
+                        {
-                                if (shape->isConcave())
+                                {
-                                        btConcaveShape* concaveMesh = (btConcaveShape*) shape;
-                                        ///@todo pass camera, for some culling? no -> we are not a graphics lib
-                                        btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-                                        btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-                                        concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+                                }
-                                if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+                                {
-                                        btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
-                                        //todo: pass camera for some culling
-                                        btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-                                        btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-                                        //DebugDrawcallback drawCallback;
-                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-                                        convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+                                }
-                                /// for polyhedral shapes
-                                if (shape->isPolyhedral())
+                                {
-                                        btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
-                                        int i;
-                                        for (i=0;i<polyshape->getNumEdges();i++)
+                                        {
-                                                btVector3 a,b;
-                                                polyshape->getEdge(i,a,b);
-                                                btVector3 wa = worldTransform * a;
-                                                btVector3 wb = worldTransform * b;
-                                                getDebugDrawer()->drawLine(wa,wb,color);
+                                        }
+                                }
+                        }
+                }
+        }
+}
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 …
                                 if(drawLimits)
+                                {
                                         btTransform tr = pSlider->getCalculatedTransformA();
+                                        btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
                                         btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
                                         btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
 …
+        }
         return;
 } // btDiscreteDynamicsWorld::debugDrawConstraint()
+}
 …
+void    btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
+{
+        int i;
+        //serialize all collision objects
+        for (i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
+                {
+                        int len = colObj->calculateSerializeBufferSize();
+                        btChunk* chunk = serializer->allocate(len,1);
+                        const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+                        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
+                }
+        }
+        for (i=0;i<m_constraints.size();i++)
+        {
+                btTypedConstraint* constraint = m_constraints[i];
+                int size = constraint->calculateSerializeBufferSize();
+                btChunk* chunk = serializer->allocate(size,1);
+                const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
+                serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
+        }
+}
+void    btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
+{
+        serializer->startSerialization();
+        serializeRigidBodies(serializer);
+        serializeCollisionObjects(serializer);
+        serializer->finishSerialization();
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #ifndef BT_DISCRETE_DYNAMICS_WORLD_H
 …
         btAlignedObjectArray<btTypedConstraint*> m_constraints;
+        btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
         btVector3       m_gravity;
 …
         bool    m_ownsIslandManager;
         bool    m_ownsConstraintSolver;
+        bool    m_synchronizeAllMotionStates;
         btAlignedObjectArray<btActionInterface*>        m_actions;
 …
         virtual void    saveKinematicState(btScalar timeStep);
+        void    debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
+        void    serializeRigidBodies(btSerializer* serializer);
 public:
 …
         virtual btVector3 getGravity () const;
+        virtual void    addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
         virtual void    addRigidBody(btRigidBody* body);
 …
         virtual void    removeRigidBody(btRigidBody* body);
+        void    debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+        ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
         void    debugDrawConstraint(btTypedConstraint* constraint);
 …
         virtual void    removeCharacter(btActionInterface* character);
+        void    setSynchronizeAllMotionStates(bool synchronizeAll)
+        {
+                m_synchronizeAllMotionStates = synchronizeAll;
+        }
+        bool getSynchronizeAllMotionStates() const
+        {
+                return m_synchronizeAllMotionStates;
+        }
+        ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
+        virtual void    serialize(btSerializer* serializer);
 };

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

-                      r5781
+                      r8284
 protected:
                 btInternalTickCallback m_internalTickCallback;
+                btInternalTickCallback m_internalPreTickCallback;
                 void*   m_worldUserInfo;
 …
                 btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
                 :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
+                :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
+                {
+                }
 …
                 /// Set the callback for when an internal tick (simulation substep) happens, optional user info
                 void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0)
+                void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false)
+                {
+                        m_internalTickCallback = cb;
+                        if (isPreTick)
+                        {
+                                m_internalPreTickCallback = cb;
+                        } else
+                        {
+                                m_internalTickCallback = cb;
+                        }
                         m_worldUserInfo = worldUserInfo;
+                }

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

-                      r5781
+                      r8284
 #include "LinearMath/btMotionState.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btSerializer.h"
 //'temporarily' global variables
 …
         m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
         m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+        m_angularFactor = btScalar(1.);
+        m_angularFactor.setValue(1,1,1);
+        m_linearFactor.setValue(1,1,1);
         m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
         m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 …
         updateInertiaTensor();
+        m_rigidbodyFlags = 0;
+        m_deltaLinearVelocity.setZero();
+        m_deltaAngularVelocity.setZero();
+        m_invMass = m_inverseMass*m_linearFactor;
+        m_pushVelocity.setZero();
+        m_turnVelocity.setZero();
+}
 …
 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
+{
         m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
         m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+        m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+        m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+}
 …
                 m_inverseMass = btScalar(1.0) / mass;
+        }
+        //Fg = m * a
+        m_gravity = mass * m_gravity_acceleration;
         m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
 …
                                    inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
+        m_invMass = m_linearFactor*m_inverseMass;
+}
 …
+}
+void    btRigidBody::internalWritebackVelocity(btScalar timeStep)
+{
+    (void) timeStep;
+        if (m_inverseMass)
+        {
+                setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+                setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+                //correct the position/orientation based on push/turn recovery
+                btTransform newTransform;
+                btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+                setWorldTransform(newTransform);
+                //m_originalBody->setCompanionId(-1);
+        }
+//      m_deltaLinearVelocity.setZero();
+//      m_deltaAngularVelocity .setZero();
+//      m_pushVelocity.setZero();
+//      m_turnVelocity.setZero();
+}
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
+{
 …
         m_checkCollideWith = m_constraintRefs.size() > 0;
+}
+int     btRigidBody::calculateSerializeBufferSize()     const
+{
+        int sz = sizeof(btRigidBodyData);
+        return sz;
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+        btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
+        btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
+        m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
+        m_linearVelocity.serialize(rbd->m_linearVelocity);
+        m_angularVelocity.serialize(rbd->m_angularVelocity);
+        rbd->m_inverseMass = m_inverseMass;
+        m_angularFactor.serialize(rbd->m_angularFactor);
+        m_linearFactor.serialize(rbd->m_linearFactor);
+        m_gravity.serialize(rbd->m_gravity);
+        m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
+        m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
+        m_totalForce.serialize(rbd->m_totalForce);
+        m_totalTorque.serialize(rbd->m_totalTorque);
+        rbd->m_linearDamping = m_linearDamping;
+        rbd->m_angularDamping = m_angularDamping;
+        rbd->m_additionalDamping = m_additionalDamping;
+        rbd->m_additionalDampingFactor = m_additionalDampingFactor;
+        rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
+        rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
+        rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
+        rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
+        rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
+        return btRigidBodyDataName;
+}
+void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
+{
+        btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
+        const char* structType = serialize(chunk->m_oldPtr, serializer);
+        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.h

-                      r5781
+                      r8284
 extern btScalar gDeactivationTime;
 extern bool gDisableDeactivation;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btRigidBodyData btRigidBodyDoubleData
+#define btRigidBodyDataName     "btRigidBodyDoubleData"
+#else
+#define btRigidBodyData btRigidBodyFloatData
+#define btRigidBodyDataName     "btRigidBodyFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+enum    btRigidBodyFlags
+{
+        BT_DISABLE_WORLD_GRAVITY = 1
+};
 …
         btVector3               m_angularVelocity;
         btScalar                m_inverseMass;
         btScalar                m_angularFactor;
+        btVector3               m_linearFactor;
         btVector3               m_gravity;
 …
         //keep track of typed constraints referencing this rigid body
         btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
+        int                             m_rigidbodyFlags;
+        int                             m_debugBodyId;
+protected:
+        ATTRIBUTE_ALIGNED64(btVector3           m_deltaLinearVelocity);
+        btVector3               m_deltaAngularVelocity;
+        btVector3               m_angularFactor;
+        btVector3               m_invMass;
+        btVector3               m_pushVelocity;
+        btVector3               m_turnVelocity;
 public:
 …
                 btScalar                        m_additionalAngularDampingFactor;
                 btRigidBodyConstructionInfo(    btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
                 m_mass(mass),
 …
         static const btRigidBody*       upcast(const btCollisionObject* colObj)
+        {
                 if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+                if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
                         return (const btRigidBody*)colObj;
                 return 0;
 …
         static btRigidBody*     upcast(btCollisionObject* colObj)
+        {
                 if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+                if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
                         return (btRigidBody*)colObj;
                 return 0;
 …
         void                    setMassProps(btScalar mass, const btVector3& inertia);
+        const btVector3& getLinearFactor() const
+        {
+                return m_linearFactor;
+        }
+        void setLinearFactor(const btVector3& linearFactor)
+        {
+                m_linearFactor = linearFactor;
+                m_invMass = m_linearFactor*m_inverseMass;
+        }
         btScalar                getInvMass() const { return m_inverseMass; }
         const btMatrix3x3& getInvInertiaTensorWorld() const {
 …
         void                    applyCentralForce(const btVector3& force)
+        {
                 m_totalForce += force;
+                m_totalForce += force*m_linearFactor;
+        }
 …
         void    applyTorque(const btVector3& torque)
+        {
                 m_totalTorque += torque;
+                m_totalTorque += torque*m_angularFactor;
+        }
 …
+        {
                 applyCentralForce(force);
                 applyTorque(rel_pos.cross(force)*m_angularFactor);
+                applyTorque(rel_pos.cross(force*m_linearFactor));
+        }
         void applyCentralImpulse(const btVector3& impulse)
+        {
                 m_linearVelocity += impulse * m_inverseMass;
+                m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
+        }
         void applyTorqueImpulse(const btVector3& torque)
+        {
                         m_angularVelocity += m_invInertiaTensorWorld * torque;
+                        m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+        }
 …
                         if (m_angularFactor)
+                        {
                                 applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor);
+                                applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
+                        }
+                }
+        }
-        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+        {
-                if (m_inverseMass != btScalar(0.))
+                {
-                        m_linearVelocity += linearComponent*impulseMagnitude;
-                        if (m_angularFactor)
+                        {
-                                m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
+                        }
+                }
+        }
         void clearForces()
+        {
 …
         int     m_frictionSolverType;
+        void    setAngularFactor(const btVector3& angFac)
+        {
+                m_angularFactor = angFac;
+        }
         void    setAngularFactor(btScalar angFac)
+        {
                 m_angularFactor = angFac;
+        }
         btScalar        getAngularFactor() const
+                m_angularFactor.setValue(angFac,angFac,angFac);
+        }
+        const btVector3&        getAngularFactor() const
+        {
                 return m_angularFactor;
 …
+        }
+        int     m_debugBodyId;
+        void    setFlags(int flags)
+        {
+                m_rigidbodyFlags = flags;
+        }
+        int getFlags() const
+        {
+                return m_rigidbodyFlags;
+        }
+        const btVector3& getDeltaLinearVelocity() const
+        {
+                return m_deltaLinearVelocity;
+        }
+        const btVector3& getDeltaAngularVelocity() const
+        {
+                return m_deltaAngularVelocity;
+        }
+        const btVector3& getPushVelocity() const
+        {
+                return m_pushVelocity;
+        }
+        const btVector3& getTurnVelocity() const
+        {
+                return m_turnVelocity;
+        }
+        ////////////////////////////////////////////////
+        ///some internal methods, don't use them
+        btVector3& internalGetDeltaLinearVelocity()
+        {
+                return m_deltaLinearVelocity;
+        }
+        btVector3& internalGetDeltaAngularVelocity()
+        {
+                return m_deltaAngularVelocity;
+        }
+        const btVector3& internalGetAngularFactor() const
+        {
+                return m_angularFactor;
+        }
+        const btVector3& internalGetInvMass() const
+        {
+                return m_invMass;
+        }
+        btVector3& internalGetPushVelocity()
+        {
+                return m_pushVelocity;
+        }
+        btVector3& internalGetTurnVelocity()
+        {
+                return m_turnVelocity;
+        }
+        SIMD_FORCE_INLINE void  internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+        {
+                velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+        }
+        SIMD_FORCE_INLINE void  internalGetAngularVelocity(btVector3& angVel) const
+        {
+                angVel = getAngularVelocity()+m_deltaAngularVelocity;
+        }
+        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+        {
+                if (m_inverseMass)
+                {
+                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                }
+        }
+        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+        {
+                if (m_inverseMass)
+                {
+                        m_pushVelocity += linearComponent*impulseMagnitude;
+                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                }
+        }
+        void    internalWritebackVelocity()
+        {
+                if (m_inverseMass)
+                {
+                        setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+                        setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+                        //m_deltaLinearVelocity.setZero();
+                        //m_deltaAngularVelocity .setZero();
+                        //m_originalBody->setCompanionId(-1);
+                }
+        }
+        void    internalWritebackVelocity(btScalar timeStep);
+        ///////////////////////////////////////////////
+        virtual int     calculateSerializeBufferSize()  const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer,  class btSerializer* serializer) const;
+        virtual void serializeSingleObject(class btSerializer* serializer) const;
 };
+//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btRigidBodyFloatData
+{
+        btCollisionObjectFloatData      m_collisionObjectData;
+        btMatrix3x3FloatData            m_invInertiaTensorWorld;
+        btVector3FloatData              m_linearVelocity;
+        btVector3FloatData              m_angularVelocity;
+        btVector3FloatData              m_angularFactor;
+        btVector3FloatData              m_linearFactor;
+        btVector3FloatData              m_gravity;
+        btVector3FloatData              m_gravity_acceleration;
+        btVector3FloatData              m_invInertiaLocal;
+        btVector3FloatData              m_totalForce;
+        btVector3FloatData              m_totalTorque;
+        float                                   m_inverseMass;
+        float                                   m_linearDamping;
+        float                                   m_angularDamping;
+        float                                   m_additionalDampingFactor;
+        float                                   m_additionalLinearDampingThresholdSqr;
+        float                                   m_additionalAngularDampingThresholdSqr;
+        float                                   m_additionalAngularDampingFactor;
+        float                                   m_linearSleepingThreshold;
+        float                                   m_angularSleepingThreshold;
+        int                                             m_additionalDamping;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btRigidBodyDoubleData
+{
+        btCollisionObjectDoubleData     m_collisionObjectData;
+        btMatrix3x3DoubleData           m_invInertiaTensorWorld;
+        btVector3DoubleData             m_linearVelocity;
+        btVector3DoubleData             m_angularVelocity;
+        btVector3DoubleData             m_angularFactor;
+        btVector3DoubleData             m_linearFactor;
+        btVector3DoubleData             m_gravity;
+        btVector3DoubleData             m_gravity_acceleration;
+        btVector3DoubleData             m_invInertiaLocal;
+        btVector3DoubleData             m_totalForce;
+        btVector3DoubleData             m_totalTorque;
+        double                                  m_inverseMass;
+        double                                  m_linearDamping;
+        double                                  m_angularDamping;
+        double                                  m_additionalDampingFactor;
+        double                                  m_additionalLinearDampingThresholdSqr;
+        double                                  m_additionalAngularDampingThresholdSqr;
+        double                                  m_additionalAngularDampingFactor;
+        double                                  m_linearSleepingThreshold;
+        double                                  m_angularSleepingThreshold;
+        int                                             m_additionalDamping;
+        char    m_padding[4];
+};

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

-                      r5781
+                      r8284
 void    btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+        removeCollisionObject(body);
+}
+        btCollisionWorld::removeCollisionObject(body);
+}
+void    btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+        btRigidBody* body = btRigidBody::upcast(collisionObject);
+        if (body)
+                removeRigidBody(body);
+        else
+                btCollisionWorld::removeCollisionObject(collisionObject);
+}
 void    btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

-                      r5781
+                      r8284
         virtual void    removeRigidBody(btRigidBody* body);
+        ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
         virtual void    updateAabbs();

code/branches/kicklib2/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

-                      r5781
+                      r8284
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+static btRigidBody s_fixedObject( 0,0,0);
+btRigidBody& btActionInterface::getFixedBody()
+{
+        static btRigidBody s_fixed(0, 0,0);
+        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+        return s_fixed;
+}
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,  btVehicleRaycaster* raycaster )
 …
         ci.m_bIsFrontWheel = isFrontWheel;
         ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
+        ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
         m_wheelInfo.push_back( btWheelInfo(ci));
 …
                 wheel.m_raycastInfo.m_isInContact = true;
                 wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!;
+                wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
                 //wheel.m_raycastInfo.m_groundObject = object;
 …
                 btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
+                btScalar gMaxSuspensionForce = btScalar(6000.);
+                if (suspensionForce > gMaxSuspensionForce)
+                {
+                        suspensionForce = gMaxSuspensionForce;
+                if (suspensionForce > wheel.m_maxSuspensionForce)
+                {
+                        suspensionForce = wheel.m_maxSuspensionForce;
+                }
                 btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
 …
                                         btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
                                         rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence;
+                                        rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
                                         m_chassisBody->applyImpulse(sideImp,rel_pos);
 …
         for (int v=0;v<this->getNumWheels();v++)
+        {
                 btVector3 wheelColor(0,255,255);
+                btVector3 wheelColor(0,1,1);
                 if (getWheelInfo(v).m_raycastInfo.m_isInContact)
+                {
                         wheelColor.setValue(0,0,255);
+                        wheelColor.setValue(0,0,1);
                 } else
+                {
                         wheelColor.setValue(255,0,255);
+                        wheelColor.setValue(1,0,1);
+                }

code/branches/kicklib2/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h

-                      r5781
+                      r8284
                 btAlignedObjectArray<btScalar>  m_forwardImpulse;
                 btAlignedObjectArray<btScalar>  m_sideImpulse;
+                ///backwards compatibility
+                int     m_userConstraintType;
+                int     m_userConstraintId;
 public:
 …
                                 m_suspensionDamping(btScalar(0.88)),
                                 m_maxSuspensionTravelCm(btScalar(500.)),
+                                m_frictionSlip(btScalar(10.5))
+                                m_frictionSlip(btScalar(10.5)),
+                                m_maxSuspensionForce(btScalar(6000.))
+                        {
+                        }
 …
                         btScalar        m_maxSuspensionTravelCm;
                         btScalar        m_frictionSlip;
+                        btScalar        m_maxSuspensionForce;
                 };
 …
         virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
+        {
+        (void) collisionWorld;
                 updateVehicle(step);
+        }
 …
+        ///backwards compatibility
+        int getUserConstraintType() const
+        {
+                return m_userConstraintType ;
+        }
+        void    setUserConstraintType(int userConstraintType)
+        {
+                m_userConstraintType = userConstraintType;
+        };
+        void    setUserConstraintId(int uid)
+        {
+                m_userConstraintId = uid;
+        }
+        int getUserConstraintId() const
+        {
+                return m_userConstraintId;
+        }
 };

code/branches/kicklib2/src/external/bullet/BulletDynamics/Vehicle/btWheelInfo.h

-                      r5781
+                      r8284
         btScalar                m_wheelsDampingRelaxation;
         btScalar                m_frictionSlip;
+        btScalar                m_maxSuspensionForce;
         bool m_bIsFrontWheel;
 …
         btScalar        m_deltaRotation;
         btScalar        m_rollInfluence;
+        btScalar        m_maxSuspensionForce;
         btScalar        m_engineForce;
 …
                 m_rollInfluence = btScalar(0.1);
                 m_bIsFrontWheel = ci.m_bIsFrontWheel;
+                m_maxSuspensionForce = ci.m_maxSuspensionForce;
+        }

code/branches/kicklib2/src/external/bullet/BulletLicense.txt

-                      r5781
+                      r8284
 /*
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
 This software is provided 'as-is', without any express or implied warranty.
 …
+Free for commercial use, but please mail bullet@erwincoumans.com to report projects, and join the forum at
+www.continuousphysics.com/Bullet/phpBB2
+Free for commercial use, please report projects in the forum at http://www.bulletphysics.org
+In case you want to display a Bullet logo in your software: you can download the Bullet logo in various vector formats and high resolution at the download section in http://bullet.googlecode.com

code/branches/kicklib2/src/external/bullet/CMakeLists.txt

r5929	r8284
31	31	NO_DLL_INTERFACE
32	32	VERSION
33		2.74
	33	2.77
34	34	SOURCE_FILES
35	35	${BULLET_FILES}

code/branches/kicklib2/src/external/bullet/ChangeLog

-                      r5781
+                      r8284
 Primary author and maintainer: Erwin Coumans
+Todo: update changelog from April - July 2008
+See http://code.google.com/p/bullet/source/list for more complete log in Subversion
+This ChangeLog is incomplete, for an up-to-date list of all fixed issues see http://bullet.googlecode.com
+using http://tinyurl.com/yabmjjj
+September 21
+        - Bullet 2.77 release based on revision 2218
+        - Added Visual Studio project files for OpenCL and Direct Compute in msvc folder
+September 7
+        - autotools now uses CamelCase naming for libraryes just like cmake:
+        libbulletdynamics -> libBulletDynamics, libbulletmath -> libLinearMath
+July 21
+        - Preparing for Bullet 2.77 release, around revision r2135
+        - Added an OpenCL particle demo, running on NVidia, AMD and MiniCL
+        Thanks to NVidia for the original particle demo from their OpenCL SDK
+        - Added GPU deformable object solvers for OpenCL and DirectCompute, and a DirectX 11 cloth demo
+        Thanks to AMD
+        - Create a separate library for MiniCL,
+        MiniCL is a rudimentary OpenCL wrapper that allows to compile OpenCL kernels for multi-core CPU, using Win32 Threads or Posix
+        - Moved vectormath into Bullet/src, and added a SSE implementation
+        - Added a btParallelConstraintSolver, mainly for PlayStation 3 Cell SPUs (although it runs fine on CPU too)
+March 6
+        - Dynamica Maya plugin (and COLLADA support) is moved to http://dynamica.googlecode.com
+February
+        - Bullet 2.76 release, revision 2010
+        - support for the .bullet binary file format
+        - btInternalEdgeUtility to adjust unwanted collisions against internal triangle edges
+        - Improved Maya Dynamica plugin with better constraint authoring and .bullet file export
+September 17
+        - Minor update to Bullet 2.75 release, revision 1776
+        - Support for btConvex2dShape, check out Bullet/Demos/Box2dDemo
+        - Fixes in build systems
+        - Minor fix in btGjkPairDetector
+        - Initialize world transform for btCollisionShape in constructor
+September 6
+        - Bullet 2.75 release
+        - Added SPH fluid simulation in Extras, not integrated with rigid body / soft body yet
+        Thanks to Rama Hoetzlein to make this contribution available under the ZLib license
+        - add special capsule-capsule collider code in btConvexConvexCollisionAlgorithm, to speed up capsule-ragdolls
+        - soft body improvement: faster building of bending constraints
+        - soft body improvement: allow to disable/enable cluster self-collision
+        - soft body fix: 'exploding' soft bodies when using cluster collision
+        - fix some degenerate cases in continuous convex cast, could impact ray cast/convex cast
+        Thanks to Jacob Langford for the report and reproduction cases, see http://code.google.com/p/bullet/issues/detail?id=250&can=1&start=200
+        - re-enabled split impulse
+        - added btHinge2Constraint, btUniversalConstraint, btGeneric6DofSpringConstraint
+        - demonstrate 2D physics with 2D/3D object interaction
+December 2
+        - Fix contact refresh issues with btCompoundShape, introduced with btDbvt acceleration structure in btCompoundCollisionAlgorithm
+        - Made btSequentialImpulseConstraintSolver 100% compatible with ODE quickstep
+        constraints can use 'solveConstraint' method or 'getInfo/getInfo2'
+November 30
+        - Add highly optimized SIMD branchless PGS/SI solver innerloop
 November 12

code/branches/kicklib2/src/external/bullet/LinearMath/CMakeLists.txt

-                      r5929
+                      r8284
 COMPILATION_BEGIN BulletLinearMathCompilation.cpp
+                btConvexHull.cpp
+                btQuickprof.cpp
+                btGeometryUtil.cpp
+                btAlignedAllocator.cpp
+        btAlignedAllocator.cpp
+        btConvexHull.cpp
+        btGeometryUtil.cpp
+        btQuickprof.cpp
+        btSerializer.cpp
 COMPILATION_END
+                # Headers
+                btAlignedObjectArray.h
+                btList.h
+                btPoolAllocator.h
+                btRandom.h
+                btVector3.h
+                btDefaultMotionState.h
+                btMatrix3x3.h
+                btQuadWord.h
+                btHashMap.h
+                btScalar.h
+                btAabbUtil2.h
+                btConvexHull.h
+                btMinMax.h
+                btQuaternion.h
+                btStackAlloc.h
+                btGeometryUtil.h
+                btMotionState.h
+                btTransform.h
+                btAlignedAllocator.h
+                btIDebugDraw.h
+                btQuickprof.h
+                btTransformUtil.h
+        # Headers
+        btAabbUtil2.h
+        btAlignedAllocator.h
+        btAlignedObjectArray.h
+        btConvexHull.h
+        btDefaultMotionState.h
+        btGeometryUtil.h
+        btHashMap.h
+        btIDebugDraw.h
+        btList.h
+        btMatrix3x3.h
+        btMinMax.h
+        btMotionState.h
+        btPoolAllocator.h
+        btQuadWord.h
+        btQuaternion.h
+        btQuickprof.h
+        btRandom.h
+        btScalar.h
+        btSerializer.h
+        btStackAlloc.h
+        btTransform.h
+        btTransformUtil.h
+        btVector3.h
+)

code/branches/kicklib2/src/external/bullet/LinearMath/btAlignedAllocator.cpp

-                      r5781
+                      r8284
+{
         gNumAlignedAllocs++;
+  void* ptr;
+#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+        void* ptr;
         ptr = sAlignedAllocFunc(size, alignment);
-#else
-  char *real;
-  unsigned long offset;
-  real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
-  if (real) {
-    offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
-    ptr = (void *)((real + sizeof(void *)) + offset);
-    *((void **)(ptr)-1) = (void *)(real);
-  } else {
-    ptr = (void *)(real);
+  }
-#endif  // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
 //      printf("btAlignedAllocInternal %d, %x\n",size,ptr);
         return ptr;
 …
         gNumAlignedFree++;
 //      printf("btAlignedFreeInternal %x\n",ptr);
-#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
         sAlignedFreeFunc(ptr);
-#else
-  void* real;
-  if (ptr) {
-    real = *((void **)(ptr)-1);
-    sFreeFunc(real);
+  }
-#endif  // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+}

code/branches/kicklib2/src/external/bullet/LinearMath/btAlignedObjectArray.h

-                      r5781
+                      r8284
+                }
+                SIMD_FORCE_INLINE const T& at(int n) const
+                {
+                        return m_data[n];
+                }
+                SIMD_FORCE_INLINE T& at(int n)
+                {
+                        return m_data[n];
+                }
                 SIMD_FORCE_INLINE const T& operator[](int n) const
+                {
 …
                         int curSize = size();
                         if (newsize < size())
+                        {
                                 for(int i = curSize; i < newsize; i++)
+                        if (newsize < curSize)
+                        {
+                                for(int i = newsize; i < curSize; i++)
+                                {
                                         m_data[i].~T();
 …
+                }
+                SIMD_FORCE_INLINE       T&  expandNonInitializing( )
+                {
+                        int sz = size();
+                        if( sz == capacity() )
+                        {
+                                reserve( allocSize(size()) );
+                        }
+                        m_size++;
+                        return m_data[sz];
+                }
                 SIMD_FORCE_INLINE       T&  expand( const T& fillValue=T())
 …
+        }
+        void copyFromArray(const btAlignedObjectArray& otherArray)
+        {
+                int otherSize = otherArray.size();
+                resize (otherSize);
+                otherArray.copy(0, otherSize, m_data);
+        }
 };

code/branches/kicklib2/src/external/bullet/LinearMath/btConvexHull.cpp

-                      r5781
+                      r8284
 #include "btConvexHull.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btMinMax.h"
 #include "LinearMath/btVector3.h"
+#include "btAlignedObjectArray.h"
+#include "btMinMax.h"
+#include "btVector3.h"
 …
                                 static btVector3 dif;
                 dif = p1-p0;
                                 btScalar dn= dot(plane.normal,dif);
                                 btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn;
+                                btScalar dn= btDot(plane.normal,dif);
+                                btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
                                 return p0 + (dif*t);
+}
 …
 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
+{
         return point - plane.normal * (dot(point,plane.normal)+plane.dist);
+        return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
+}
 …
         // return the normal of the triangle
         // inscribed by v0, v1, and v2
         btVector3 cp=cross(v1-v0,v2-v1);
+        btVector3 cp=btCross(v1-v0,v2-v1);
         btScalar m=cp.length();
         if(m==0) return btVector3(1,0,0);
 …
+{
         static btVector3 cp;
         cp = cross(udir,vdir).normalized();
         btScalar distu = -dot(cp,ustart);
         btScalar distv = -dot(cp,vstart);
+        cp = btCross(udir,vdir).normalized();
+        btScalar distu = -btDot(cp,ustart);
+        btScalar distv = -btDot(cp,vstart);
         btScalar dist = (btScalar)fabs(distu-distv);
         if(upoint)
+                {
                 btPlane plane;
                 plane.normal = cross(vdir,cp).normalized();
                 plane.dist = -dot(plane.normal,vstart);
+                plane.normal = btCross(vdir,cp).normalized();
+                plane.dist = -btDot(plane.normal,vstart);
                 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
+        }
 …
+                {
                 btPlane plane;
                 plane.normal = cross(udir,cp).normalized();
                 plane.dist = -dot(plane.normal,ustart);
+                plane.normal = btCross(udir,cp).normalized();
+                plane.dist = -btDot(plane.normal,ustart);
                 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
+        }
 …
 int PlaneTest(const btPlane &p, const btVector3 &v);
 int PlaneTest(const btPlane &p, const btVector3 &v) {
         btScalar a  = dot(v,p.normal)+p.dist;
+        btScalar a  = btDot(v,p.normal)+p.dist;
         int   flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
         return flag;
 …
                 if(allow[i])
+                {
                         if(m==-1 || dot(p[i],dir)>dot(p[m],dir))
+                        if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
                                 m=i;
+                }
 …
 btVector3 orth(const btVector3 &v)
+{
         btVector3 a=cross(v,btVector3(0,0,1));
         btVector3 b=cross(v,btVector3(0,1,0));
+        btVector3 a=btCross(v,btVector3(0,0,1));
+        btVector3 b=btCross(v,btVector3(0,1,0));
         if (a.length() > b.length())
+        {
 …
                 if(allow[m]==3) return m;
                 T u = orth(dir);
                 T v = cross(u,dir);
+                T v = btCross(u,dir);
                 int ma=-1;
                 for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
 …
+{
         btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
         return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
+        return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
+}
 int hasedge(const int3 &t, int a,int b);
 …
         if(p0==p1 || basis[0]==btVector3(0,0,0))
                 return int4(-1,-1,-1,-1);
         basis[1] = cross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
         basis[2] = cross(btVector3(btScalar(-0.02),     btScalar(1), btScalar(0)),basis[0]);
+        basis[1] = btCross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
+        basis[2] = btCross(btVector3(btScalar(-0.02),     btScalar(1), btScalar(0)),basis[0]);
         if (basis[1].length() > basis[2].length())
+        {
 …
                 return int4(-1,-1,-1,-1);
         basis[1] = verts[p2] - verts[p0];
         basis[2] = cross(basis[1],basis[0]).normalized();
+        basis[2] = btCross(basis[1],basis[0]).normalized();
         int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
         if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
 …
                 return int4(-1,-1,-1,-1);
         btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
         if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
+        if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
         return int4(p0,p1,p2,p3);
+}
 …
                 btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
                 t->vmax = maxdirsterid(verts,verts_count,n,allow);
                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
+                t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
+        }
         btHullTriangle *te;
 …
                         if(!hasvert(*m_tris[j],v)) break;
                         int3 nt=*m_tris[j];
                         if(above(verts,nt,center,btScalar(0.01)*epsilon)  || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
+                        if(above(verts,nt,center,btScalar(0.01)*epsilon)  || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
+                        {
                                 btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
 …
                         else
+                        {
                                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
+                                t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
+                        }
+                }
 …
         vcount = 0;
         btScalar recip[3];
+        btScalar recip[3]={0.f,0.f,0.f};
         if ( scale )
 …
                                 btScalar z = v[2];
                                 btScalar dx = fabsf(x - px );
                                 btScalar dy = fabsf(y - py );
                                 btScalar dz = fabsf(z - pz );
+                                btScalar dx = btFabs(x - px );
+                                btScalar dy = btFabs(y - py );
+                                btScalar dz = btFabs(z - pz );
                                 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
 …
         ocount = 0;
         for (i=0; i<indexcount; i++)
+        for (i=0; i<int (indexcount); i++)
+        {
                 unsigned int v = indices[i]; // original array index
 …
                         for (int k=0;k<m_vertexIndexMapping.size();k++)
+                        {
                                 if (tmpIndices[k]==v)
+                                if (tmpIndices[k]==int(v))
                                         m_vertexIndexMapping[k]=ocount;
+                        }

code/branches/kicklib2/src/external/bullet/LinearMath/btConvexHull.h

-                      r5781
+                      r8284
 #define CD_HULL_H
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedObjectArray.h"
+#include "btVector3.h"
+#include "btAlignedObjectArray.h"
 typedef btAlignedObjectArray<unsigned int> TUIntArray;

code/branches/kicklib2/src/external/bullet/LinearMath/btDefaultMotionState.h

r5781	r8284
1	1	#ifndef DEFAULT_MOTION_STATE_H
2	2	#define DEFAULT_MOTION_STATE_H
	3
	4	#include "btMotionState.h"
3	5
4	6	///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.

code/branches/kicklib2/src/external/bullet/LinearMath/btHashMap.h

-                      r5781
+                      r8284
 #include "btAlignedObjectArray.h"
+///very basic hashable string implementation, compatible with btHashMap
+struct btHashString
+{
+        const char* m_string;
+        unsigned int    m_hash;
+        SIMD_FORCE_INLINE       unsigned int getHash()const
+        {
+                return m_hash;
+        }
+        btHashString(const char* name)
+                :m_string(name)
+        {
+                /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
+                static const unsigned int  InitialFNV = 2166136261u;
+                static const unsigned int FNVMultiple = 16777619u;
+                /* Fowler / Noll / Vo (FNV) Hash */
+                unsigned int hash = InitialFNV;
+                for(int i = 0; m_string[i]; i++)
+                {
+                        hash = hash ^ (m_string[i]);       /* xor  the low 8 bits */
+                        hash = hash * FNVMultiple;  /* multiply by the magic number */
+                }
+                m_hash = hash;
+        }
+        int portableStringCompare(const char* src,      const char* dst) const
+        {
+                        int ret = 0 ;
+                        while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
+                                        ++src, ++dst;
+                        if ( ret < 0 )
+                                        ret = -1 ;
+                        else if ( ret > 0 )
+                                        ret = 1 ;
+                        return( ret );
+        }
+        bool equals(const btHashString& other) const
+        {
+                return (m_string == other.m_string) ||
+                        (0==portableStringCompare(m_string,other.m_string));
+        }
+};
 const int BT_HASH_NULL=0xffffffff;
+class btHashInt
+{
+        int     m_uid;
+public:
+        btHashInt(int uid)      :m_uid(uid)
+        {
+        }
+        int     getUid1() const
+        {
+                return m_uid;
+        }
+        void    setUid1(int uid)
+        {
+                m_uid = uid;
+        }
+        bool equals(const btHashInt& other) const
+        {
+                return getUid1() == other.getUid1();
+        }
+        //to our success
+        SIMD_FORCE_INLINE       unsigned int getHash()const
+        {
+                int key = m_uid;
+                // Thomas Wang's hash
+                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
+                return key;
+        }
+};
+class btHashPtr
+{
+        union
+        {
+                const void*     m_pointer;
+                int     m_hashValues[2];
+        };
+public:
+        btHashPtr(const void* ptr)
+                :m_pointer(ptr)
+        {
+        }
+        const void*     getPointer() const
+        {
+                return m_pointer;
+        }
+        bool equals(const btHashPtr& other) const
+        {
+                return getPointer() == other.getPointer();
+        }
+        //to our success
+        SIMD_FORCE_INLINE       unsigned int getHash()const
+        {
+                const bool VOID_IS_8 = ((sizeof(void*)==8));
+                int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
+                // Thomas Wang's hash
+                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
+                return key;
+        }
+};
+template <class Value>
+class btHashKeyPtr
+{
+        int     m_uid;
+public:
+        btHashKeyPtr(int uid)    :m_uid(uid)
+        {
+        }
+        int     getUid1() const
+        {
+                return m_uid;
+        }
+        bool equals(const btHashKeyPtr<Value>& other) const
+        {
+                return getUid1() == other.getUid1();
+        }
+        //to our success
+        SIMD_FORCE_INLINE       unsigned int getHash()const
+        {
+                int key = m_uid;
+                // Thomas Wang's hash
+                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
+                return key;
+        }
+};
 template <class Value>
 …
 public:
+        btHashKey(int uid)
+                :m_uid(uid)
+        {
+        }
+        int     getUid() const
+        btHashKey(int uid)      :m_uid(uid)
+        {
+        }
+        int     getUid1() const
+        {
                 return m_uid;
+        }
+        bool equals(const btHashKey<Value>& other) const
+        {
+                return getUid1() == other.getUid1();
+        }
         //to our success
         SIMD_FORCE_INLINE       unsigned int getHash()const
 …
                 int key = m_uid;
                 // Thomas Wang's hash
+                key += ~(key << 15);
+                key ^=  (key >> 10);
+                key +=  (key << 3);
+                key ^=  (key >> 6);
+                key += ~(key << 11);
+                key ^=  (key >> 16);
+                key += ~(key << 15);    key ^=  (key >> 10);    key +=  (key << 3);     key ^=  (key >> 6);     key += ~(key << 11);    key ^=  (key >> 16);
                 return key;
+        }
+        btHashKey       getKey(const Value& value) const
+        {
+                return btHashKey(value.getUid());
+        }
+};
+template <class Value>
+class btHashKeyPtr
+{
+        int     m_uid;
+public:
+        btHashKeyPtr(int uid)
+                :m_uid(uid)
+        {
+        }
+        int     getUid() const
+        {
+                return m_uid;
+        }
+        //to our success
+        SIMD_FORCE_INLINE       unsigned int getHash()const
+        {
+                int key = m_uid;
+                // Thomas Wang's hash
+                key += ~(key << 15);
+                key ^=  (key >> 10);
+                key +=  (key << 3);
+                key ^=  (key >> 6);
+                key += ~(key << 11);
+                key ^=  (key >> 16);
+                return key;
+        }
+        btHashKeyPtr    getKey(const Value& value) const
+        {
+                return btHashKeyPtr(value->getUid());
+        }
+};
+};
 ///The btHashMap template class implements a generic and lightweight hashmap.
 …
+{
+protected:
         btAlignedObjectArray<int>               m_hashTable;
         btAlignedObjectArray<int>               m_next;
         btAlignedObjectArray<Value>             m_valueArray;
+        void    growTables(const Key& key)
+        btAlignedObjectArray<Key>               m_keyArray;
+        void    growTables(const Key& /*key*/)
+        {
                 int newCapacity = m_valueArray.capacity();
 …
                         for(i=0;i<curHashtableSize;i++)
+                        {
+                                const Value& value = m_valueArray[i];
+                                int     hashValue = key.getKey(value).getHash() & (m_valueArray.capacity()-1);  // New hash value with new mask
+                                //const Value& value = m_valueArray[i];
+                                //const Key& key = m_keyArray[i];
+                                int     hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1);      // New hash value with new mask
                                 m_next[i] = m_hashTable[hashValue];
                                 m_hashTable[hashValue] = i;
 …
         void insert(const Key& key, const Value& value) {
                 int hash = key.getHash() & (m_valueArray.capacity()-1);
+                //don't add it if it is already there
+                if (find(key))
+                {
+                //replace value if the key is already there
+                int index = findIndex(key);
+                if (index != BT_HASH_NULL)
+                {
+                        m_valueArray[index]=value;
                         return;
+                }
                 int count = m_valueArray.size();
                 int oldCapacity = m_valueArray.capacity();
                 m_valueArray.push_back(value);
+                m_keyArray.push_back(key);
                 int newCapacity = m_valueArray.capacity();
                 if (oldCapacity < newCapacity)
 …
+                {
                         m_valueArray.pop_back();
+                        m_keyArray.pop_back();
                         return;
+                }
                 // Remove the last pair from the hash table.
+                const Value* lastValue = &m_valueArray[lastPairIndex];
+                int lastHash = key.getKey(*lastValue).getHash() & (m_valueArray.capacity()-1);
+                int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
                 index = m_hashTable[lastHash];
 …
                 // Copy the last pair into the remove pair's spot.
                 m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
+                m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
                 // Insert the last pair into the hash table
 …
                 m_valueArray.pop_back();
+                m_keyArray.pop_back();
+        }
 …
         int     findIndex(const Key& key) const
+        {
                 int hash = key.getHash() & (m_valueArray.capacity()-1);
                 if (hash >= m_hashTable.size())
+                unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
+                if (hash >= (unsigned int)m_hashTable.size())
+                {
                         return BT_HASH_NULL;
 …
                 int index = m_hashTable[hash];
                 while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false)
+                while ((index != BT_HASH_NULL) && key.equals(m_keyArray[index]) == false)
+                {
                         index = m_next[index];
 …
                 m_next.clear();
                 m_valueArray.clear();
+                m_keyArray.clear();
+        }

code/branches/kicklib2/src/external/bullet/LinearMath/btIDebugDraw.h

-                      r5781
+                      r8284
 /*
+Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+. This notice may not be removed or altered from any source distribution.
 */
 …
 ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
 ///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
+///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1]
 class   btIDebugDraw
+{
 …
                 DBG_DrawConstraints = (1 << 11),
                 DBG_DrawConstraintLimits = (1 << 12),
+                DBG_FastWireframe = (1<<13),
                 DBG_MAX_DEBUG_DRAW_MODE
         };
 …
         virtual ~btIDebugDraw() {};
+        virtual void    drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
         virtual void    drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
+        {
+        (void) toColor;
                 drawLine (from, to, fromColor);
+        }
+        virtual void    drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
+        {
+        }
+        virtual void    drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+        {
+                btVector3 start = transform.getOrigin();
+                const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
+                const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
+                const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
+                // XY
+                drawLine(start-xoffs, start+yoffs, color);
+                drawLine(start+yoffs, start+xoffs, color);
+                drawLine(start+xoffs, start-yoffs, color);
+                drawLine(start-yoffs, start-xoffs, color);
+                // XZ
+                drawLine(start-xoffs, start+zoffs, color);
+                drawLine(start+zoffs, start+xoffs, color);
+                drawLine(start+xoffs, start-zoffs, color);
+                drawLine(start-zoffs, start-xoffs, color);
+                // YZ
+                drawLine(start-yoffs, start+zoffs, color);
+                drawLine(start+zoffs, start+yoffs, color);
+                drawLine(start+yoffs, start-zoffs, color);
+                drawLine(start-zoffs, start-yoffs, color);
+        }
         virtual void    drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
+        {
+        }
+        virtual void    drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
+                btTransform tr;
+                tr.setIdentity();
+                tr.setOrigin(p);
+                drawSphere(radius,tr,color);
+        }
         virtual void    drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
 …
         virtual int             getDebugMode() const = 0;
         inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
+        virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
+        {
 …
+                }
+        }
         void drawTransform(const btTransform& transform, btScalar orthoLen)
+        virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
+        {
                 btVector3 start = transform.getOrigin();
 …
+        }
         void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
+        virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
                                 const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
+        {
 …
+                }
+        }
         void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
+        virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
                 btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
+        {
 …
+        }
         void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
+        virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
+        {
                 drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
 …
                 drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+        }
         void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
+        virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
+        {
                 drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);

code/branches/kicklib2/src/external/bullet/LinearMath/btMatrix3x3.h

-                      r5781
+                      r8284
+#ifndef btMatrix3x3_H
+#define btMatrix3x3_H
+#include "btScalar.h"
+#ifndef BT_MATRIX3x3_H
+#define BT_MATRIX3x3_H
 #include "btVector3.h"
 #include "btQuaternion.h"
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMatrix3x3Data btMatrix3x3DoubleData
+#else
+#define btMatrix3x3Data btMatrix3x3FloatData
+#endif //BT_USE_DOUBLE_PRECISION
 /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
  * Make sure to only include a pure orthogonal matrix without scaling. */
+* Make sure to only include a pure orthogonal matrix without scaling. */
 class btMatrix3x3 {
+        public:
+  /** @brief No initializaion constructor */
+                btMatrix3x3 () {}
+//              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
+  /**@brief Constructor from Quaternion */
+                explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
+                /*
+                template <typename btScalar>
+                Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+                {
+                        setEulerYPR(yaw, pitch, roll);
+                }
+                */
+  /** @brief Constructor with row major formatting */
+                btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+                                  const btScalar& yx, const btScalar& yy, const btScalar& yz,
+                                  const btScalar& zx, const btScalar& zy, const btScalar& zz)
+                {
+                        setValue(xx, xy, xz,
+                                         yx, yy, yz,
+                                         zx, zy, zz);
+                }
+  /** @brief Copy constructor */
+                SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
+                {
+                        m_el[0] = other.m_el[0];
+                        m_el[1] = other.m_el[1];
+                        m_el[2] = other.m_el[2];
+                }
+  /** @brief Assignment Operator */
+                SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
+                {
+                        m_el[0] = other.m_el[0];
+                        m_el[1] = other.m_el[1];
+                        m_el[2] = other.m_el[2];
+                        return *this;
+                }
+  /** @brief Get a column of the matrix as a vector
+   *  @param i Column number 0 indexed */
+                SIMD_FORCE_INLINE btVector3 getColumn(int i) const
+                {
+                        return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+                }
+  /** @brief Get a row of the matrix as a vector
+   *  @param i Row number 0 indexed */
+                SIMD_FORCE_INLINE const btVector3& getRow(int i) const
+                {
+                        btFullAssert(0 <= i && i < 3);
+                        return m_el[i];
+                }
+  /** @brief Get a mutable reference to a row of the matrix as a vector
+   *  @param i Row number 0 indexed */
+                SIMD_FORCE_INLINE btVector3&  operator[](int i)
+                {
+                        btFullAssert(0 <= i && i < 3);
+                        return m_el[i];
+                }
+  /** @brief Get a const reference to a row of the matrix as a vector
+   *  @param i Row number 0 indexed */
+                SIMD_FORCE_INLINE const btVector3& operator[](int i) const
+                {
+                        btFullAssert(0 <= i && i < 3);
+                        return m_el[i];
+                }
+  /** @brief Multiply by the target matrix on the right
+   *  @param m Rotation matrix to be applied
+   * Equivilant to this = this * m */
+                btMatrix3x3& operator*=(const btMatrix3x3& m);
+  /** @brief Set from a carray of btScalars
+   *  @param m A pointer to the beginning of an array of 9 btScalars */
+        ///Data storage for the matrix, each vector is a row of the matrix
+        btVector3 m_el[3];
+public:
+        /** @brief No initializaion constructor */
+        btMatrix3x3 () {}
+        //              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
+        /**@brief Constructor from Quaternion */
+        explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
+        /*
+        template <typename btScalar>
+        Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+        {
+        setEulerYPR(yaw, pitch, roll);
+        }
+        */
+        /** @brief Constructor with row major formatting */
+        btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+                const btScalar& yx, const btScalar& yy, const btScalar& yz,
+                const btScalar& zx, const btScalar& zy, const btScalar& zz)
+        {
+                setValue(xx, xy, xz,
+                        yx, yy, yz,
+                        zx, zy, zz);
+        }
+        /** @brief Copy constructor */
+        SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
+        {
+                m_el[0] = other.m_el[0];
+                m_el[1] = other.m_el[1];
+                m_el[2] = other.m_el[2];
+        }
+        /** @brief Assignment Operator */
+        SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
+        {
+                m_el[0] = other.m_el[0];
+                m_el[1] = other.m_el[1];
+                m_el[2] = other.m_el[2];
+                return *this;
+        }
+        /** @brief Get a column of the matrix as a vector
+        *  @param i Column number 0 indexed */
+        SIMD_FORCE_INLINE btVector3 getColumn(int i) const
+        {
+                return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+        }
+        /** @brief Get a row of the matrix as a vector
+        *  @param i Row number 0 indexed */
+        SIMD_FORCE_INLINE const btVector3& getRow(int i) const
+        {
+                btFullAssert(0 <= i && i < 3);
+                return m_el[i];
+        }
+        /** @brief Get a mutable reference to a row of the matrix as a vector
+        *  @param i Row number 0 indexed */
+        SIMD_FORCE_INLINE btVector3&  operator[](int i)
+        {
+                btFullAssert(0 <= i && i < 3);
+                return m_el[i];
+        }
+        /** @brief Get a const reference to a row of the matrix as a vector
+        *  @param i Row number 0 indexed */
+        SIMD_FORCE_INLINE const btVector3& operator[](int i) const
+        {
+                btFullAssert(0 <= i && i < 3);
+                return m_el[i];
+        }
+        /** @brief Multiply by the target matrix on the right
+        *  @param m Rotation matrix to be applied
+        * Equivilant to this = this * m */
+        btMatrix3x3& operator*=(const btMatrix3x3& m);
+        /** @brief Set from a carray of btScalars
+        *  @param m A pointer to the beginning of an array of 9 btScalars */
         void setFromOpenGLSubMatrix(const btScalar *m)
+                {
                         m_el[0].setValue(m[0],m[4],m[8]);
                         m_el[1].setValue(m[1],m[5],m[9]);
                         m_el[2].setValue(m[2],m[6],m[10]);
+                }
   /** @brief Set the values of the matrix explicitly (row major)
    *  @param xx Top left
    *  @param xy Top Middle
    *  @param xz Top Right
    *  @param yx Middle Left
    *  @param yy Middle Middle
    *  @param yz Middle Right
    *  @param zx Bottom Left
    *  @param zy Bottom Middle
    *  @param zz Bottom Right*/
                 void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
                                           const btScalar& yx, const btScalar& yy, const btScalar& yz,
                                           const btScalar& zx, const btScalar& zy, const btScalar& zz)
+                {
                         m_el[0].setValue(xx,xy,xz);
                         m_el[1].setValue(yx,yy,yz);
                         m_el[2].setValue(zx,zy,zz);
+                }
   /** @brief Set the matrix from a quaternion
    *  @param q The Quaternion to match */
                 void setRotation(const btQuaternion& q)
+                {
                         btScalar d = q.length2();
                         btFullAssert(d != btScalar(0.0));
                         btScalar s = btScalar(2.0) / d;
                         btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
                         btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
                         btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
                         btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
                         setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
                                          xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
                                          xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
+                }
   /** @brief Set the matrix from euler angles using YPR around YXZ respectively
    *  @param yaw Yaw about Y axis
    *  @param pitch Pitch about X axis
    *  @param roll Roll about Z axis
    */
                 void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+                {
                         setEulerZYX(roll, pitch, yaw);
+                }
+        {
+                m_el[0].setValue(m[0],m[4],m[8]);
+                m_el[1].setValue(m[1],m[5],m[9]);
+                m_el[2].setValue(m[2],m[6],m[10]);
+        }
+        /** @brief Set the values of the matrix explicitly (row major)
+        *  @param xx Top left
+        *  @param xy Top Middle
+        *  @param xz Top Right
+        *  @param yx Middle Left
+        *  @param yy Middle Middle
+        *  @param yz Middle Right
+        *  @param zx Bottom Left
+        *  @param zy Bottom Middle
+        *  @param zz Bottom Right*/
+        void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+                const btScalar& yx, const btScalar& yy, const btScalar& yz,
+                const btScalar& zx, const btScalar& zy, const btScalar& zz)
+        {
+                m_el[0].setValue(xx,xy,xz);
+                m_el[1].setValue(yx,yy,yz);
+                m_el[2].setValue(zx,zy,zz);
+        }
+        /** @brief Set the matrix from a quaternion
+        *  @param q The Quaternion to match */
+        void setRotation(const btQuaternion& q)
+        {
+                btScalar d = q.length2();
+                btFullAssert(d != btScalar(0.0));
+                btScalar s = btScalar(2.0) / d;
+                btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
+                btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
+                btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
+                btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
+                setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
+                        xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
+                        xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
+        }
+        /** @brief Set the matrix from euler angles using YPR around YXZ respectively
+        *  @param yaw Yaw about Y axis
+        *  @param pitch Pitch about X axis
+        *  @param roll Roll about Z axis
+        */
+        void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+        {
+                setEulerZYX(roll, pitch, yaw);
+        }
         /** @brief Set the matrix from euler angles YPR around ZYX axes
          * @param eulerX Roll about X axis
          * @param eulerY Pitch around Y axis
          * @param eulerZ Yaw aboud Z axis
+         *
          * These angles are used to produce a rotation matrix. The euler
          * angles are applied in ZYX order. I.e a vector is first rotated
          * about X then Y and then Z
          **/
+        * @param eulerX Roll about X axis
+        * @param eulerY Pitch around Y axis
+        * @param eulerZ Yaw aboud Z axis
+        *
+        * These angles are used to produce a rotation matrix. The euler
+        * angles are applied in ZYX order. I.e a vector is first rotated
+        * about X then Y and then Z
+        **/
         void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
   ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
+                ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
                 btScalar ci ( btCos(eulerX));
                 btScalar cj ( btCos(eulerY));
 …
                 btScalar sc = si * ch;
                 btScalar ss = si * sh;
                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+                                 cj * sh, sj * ss + cc, sj * cs - sc,
+                                 -sj,      cj * si,      cj * ci);
+        }
+  /**@brief Set the matrix to the identity */
+                void setIdentity()
+                        cj * sh, sj * ss + cc, sj * cs - sc,
+                        -sj,      cj * si,      cj * ci);
+        }
+        /**@brief Set the matrix to the identity */
+        void setIdentity()
+        {
+                setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
+                        btScalar(0.0), btScalar(1.0), btScalar(0.0),
+                        btScalar(0.0), btScalar(0.0), btScalar(1.0));
+        }
+        static const btMatrix3x3&       getIdentity()
+        {
+                static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
+                        btScalar(0.0), btScalar(1.0), btScalar(0.0),
+                        btScalar(0.0), btScalar(0.0), btScalar(1.0));
+                return identityMatrix;
+        }
+        /**@brief Fill the values of the matrix into a 9 element array
+        * @param m The array to be filled */
+        void getOpenGLSubMatrix(btScalar *m) const
+        {
+                m[0]  = btScalar(m_el[0].x());
+                m[1]  = btScalar(m_el[1].x());
+                m[2]  = btScalar(m_el[2].x());
+                m[3]  = btScalar(0.0);
+                m[4]  = btScalar(m_el[0].y());
+                m[5]  = btScalar(m_el[1].y());
+                m[6]  = btScalar(m_el[2].y());
+                m[7]  = btScalar(0.0);
+                m[8]  = btScalar(m_el[0].z());
+                m[9]  = btScalar(m_el[1].z());
+                m[10] = btScalar(m_el[2].z());
+                m[11] = btScalar(0.0);
+        }
+        /**@brief Get the matrix represented as a quaternion
+        * @param q The quaternion which will be set */
+        void getRotation(btQuaternion& q) const
+        {
+                btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
+                btScalar temp[4];
+                if (trace > btScalar(0.0))
+                {
+                        btScalar s = btSqrt(trace + btScalar(1.0));
+                        temp[3]=(s * btScalar(0.5));
+                        s = btScalar(0.5) / s;
+                        temp[0]=((m_el[2].y() - m_el[1].z()) * s);
+                        temp[1]=((m_el[0].z() - m_el[2].x()) * s);
+                        temp[2]=((m_el[1].x() - m_el[0].y()) * s);
+                }
+                else
+                {
+                        int i = m_el[0].x() < m_el[1].y() ?
+                                (m_el[1].y() < m_el[2].z() ? 2 : 1) :
+                                (m_el[0].x() < m_el[2].z() ? 2 : 0);
+                        int j = (i + 1) % 3;
+                        int k = (i + 2) % 3;
+                        btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
+                        temp[i] = s * btScalar(0.5);
+                        s = btScalar(0.5) / s;
+                        temp[3] = (m_el[k][j] - m_el[j][k]) * s;
+                        temp[j] = (m_el[j][i] + m_el[i][j]) * s;
+                        temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+                }
+                q.setValue(temp[0],temp[1],temp[2],temp[3]);
+        }
+        /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
+        * @param yaw Yaw around Y axis
+        * @param pitch Pitch around X axis
+        * @param roll around Z axis */
+        void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+        {
+                // first use the normal calculus
+                yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
+                pitch = btScalar(btAsin(-m_el[2].x()));
+                roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
+                // on pitch = +/-HalfPI
+                if (btFabs(pitch)==SIMD_HALF_PI)
+                {
+                        if (yaw>0)
+                                yaw-=SIMD_PI;
+                        else
+                                yaw+=SIMD_PI;
+                        if (roll>0)
+                                roll-=SIMD_PI;
+                        else
+                                roll+=SIMD_PI;
+                }
+        };
+        /**@brief Get the matrix represented as euler angles around ZYX
+        * @param yaw Yaw around X axis
+        * @param pitch Pitch around Y axis
+        * @param roll around X axis
+        * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
+        void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
+        {
+                struct Euler
+                {
+                        btScalar yaw;
+                        btScalar pitch;
+                        btScalar roll;
+                };
+                Euler euler_out;
+                Euler euler_out2; //second solution
+                //get the pointer to the raw data
+                // Check that pitch is not at a singularity
+                if (btFabs(m_el[2].x()) >= 1)
+                {
+                        euler_out.yaw = 0;
+                        euler_out2.yaw = 0;
+                        // From difference of angles formula
+                        btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
+                        if (m_el[2].x() > 0)  //gimbal locked up
+                        {
+                                euler_out.pitch = SIMD_PI / btScalar(2.0);
+                                euler_out2.pitch = SIMD_PI / btScalar(2.0);
+                                euler_out.roll = euler_out.pitch + delta;
+                                euler_out2.roll = euler_out.pitch + delta;
+                        }
+                        else // gimbal locked down
+                        {
+                                euler_out.pitch = -SIMD_PI / btScalar(2.0);
+                                euler_out2.pitch = -SIMD_PI / btScalar(2.0);
+                                euler_out.roll = -euler_out.pitch + delta;
+                                euler_out2.roll = -euler_out.pitch + delta;
+                        }
+                }
+                else
+                {
+                        euler_out.pitch = - btAsin(m_el[2].x());
+                        euler_out2.pitch = SIMD_PI - euler_out.pitch;
+                        euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
+                                m_el[2].z()/btCos(euler_out.pitch));
+                        euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
+                                m_el[2].z()/btCos(euler_out2.pitch));
+                        euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
+                                m_el[0].x()/btCos(euler_out.pitch));
+                        euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
+                                m_el[0].x()/btCos(euler_out2.pitch));
+                }
+                if (solution_number == 1)
+                {
                         setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
                                          btScalar(0.0), btScalar(1.0), btScalar(0.0),
                                          btScalar(0.0), btScalar(0.0), btScalar(1.0));
+                        yaw = euler_out.yaw;
+                        pitch = euler_out.pitch;
+                        roll = euler_out.roll;
+                }
+                static const btMatrix3x3&       getIdentity()
+                else
+                {
+                        yaw = euler_out2.yaw;
+                        pitch = euler_out2.pitch;
+                        roll = euler_out2.roll;
+                }
+        }
+        /**@brief Create a scaled copy of the matrix
+        * @param s Scaling vector The elements of the vector will scale each column */
+        btMatrix3x3 scaled(const btVector3& s) const
+        {
+                return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
+                        m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
+                        m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
+        }
+        /**@brief Return the determinant of the matrix */
+        btScalar            determinant() const;
+        /**@brief Return the adjoint of the matrix */
+        btMatrix3x3 adjoint() const;
+        /**@brief Return the matrix with all values non negative */
+        btMatrix3x3 absolute() const;
+        /**@brief Return the transpose of the matrix */
+        btMatrix3x3 transpose() const;
+        /**@brief Return the inverse of the matrix */
+        btMatrix3x3 inverse() const;
+        btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
+        btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
+        SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
+        {
+                return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+        }
+        SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
+        {
+                return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+        }
+        SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
+        {
+                return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+        }
+        /**@brief diagonalizes this matrix by the Jacobi method.
+        * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
+        * coordinate system, i.e., old_this = rot * new_this * rot^T.
+        * @param threshold See iteration
+        * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
+        * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
+        *
+        * Note that this matrix is assumed to be symmetric.
+        */
+        void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+        {
+                rot.setIdentity();
+                for (int step = maxSteps; step > 0; step--)
+                {
-                        static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
-                                         btScalar(0.0), btScalar(1.0), btScalar(0.0),
-                                         btScalar(0.0), btScalar(0.0), btScalar(1.0));
-                        return identityMatrix;
+                }
-  /**@brief Fill the values of the matrix into a 9 element array
-   * @param m The array to be filled */
-                void getOpenGLSubMatrix(btScalar *m) const
+                {
-                        m[0]  = btScalar(m_el[0].x());
-                        m[1]  = btScalar(m_el[1].x());
-                        m[2]  = btScalar(m_el[2].x());
-                        m[3]  = btScalar(0.0);
-                        m[4]  = btScalar(m_el[0].y());
-                        m[5]  = btScalar(m_el[1].y());
-                        m[6]  = btScalar(m_el[2].y());
-                        m[7]  = btScalar(0.0);
-                        m[8]  = btScalar(m_el[0].z());
-                        m[9]  = btScalar(m_el[1].z());
-                        m[10] = btScalar(m_el[2].z());
-                        m[11] = btScalar(0.0);
+                }
-  /**@brief Get the matrix represented as a quaternion
-   * @param q The quaternion which will be set */
-                void getRotation(btQuaternion& q) const
+                {
-                        btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
-                        btScalar temp[4];
-                        if (trace > btScalar(0.0))
+                        {
-                                btScalar s = btSqrt(trace + btScalar(1.0));
-                                temp[3]=(s * btScalar(0.5));
-                                s = btScalar(0.5) / s;
-                                temp[0]=((m_el[2].y() - m_el[1].z()) * s);
-                                temp[1]=((m_el[0].z() - m_el[2].x()) * s);
-                                temp[2]=((m_el[1].x() - m_el[0].y()) * s);
+                        }
-                        else
+                        {
-                                int i = m_el[0].x() < m_el[1].y() ?
-                                        (m_el[1].y() < m_el[2].z() ? 2 : 1) :
-                                        (m_el[0].x() < m_el[2].z() ? 2 : 0);
-                                int j = (i + 1) % 3;
-                                int k = (i + 2) % 3;
-                                btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
-                                temp[i] = s * btScalar(0.5);
-                                s = btScalar(0.5) / s;
-                                temp[3] = (m_el[k][j] - m_el[j][k]) * s;
-                                temp[j] = (m_el[j][i] + m_el[i][j]) * s;
-                                temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+                        }
-                        q.setValue(temp[0],temp[1],temp[2],temp[3]);
+                }
-  /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
-   * @param yaw Yaw around Y axis
-   * @param pitch Pitch around X axis
-   * @param roll around Z axis */
-                void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+                {
-                        // first use the normal calculus
-                        yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
-                        pitch = btScalar(btAsin(-m_el[2].x()));
-                        roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
-                        // on pitch = +/-HalfPI
-                        if (btFabs(pitch)==SIMD_HALF_PI)
+                        {
-                                if (yaw>0)
-                                        yaw-=SIMD_PI;
-                                else
-                                        yaw+=SIMD_PI;
-                                if (roll>0)
-                                        roll-=SIMD_PI;
-                                else
-                                        roll+=SIMD_PI;
+                        }
-                };
-  /**@brief Get the matrix represented as euler angles around ZYX
-   * @param yaw Yaw around X axis
-   * @param pitch Pitch around Y axis
-   * @param roll around X axis
-   * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
-  void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
+  {
-    struct Euler{btScalar yaw, pitch, roll;};
-    Euler euler_out;
-    Euler euler_out2; //second solution
-    //get the pointer to the raw data
-    // Check that pitch is not at a singularity
-    if (btFabs(m_el[2].x()) >= 1)
+    {
-      euler_out.yaw = 0;
-      euler_out2.yaw = 0;
-      // From difference of angles formula
-      btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
-      if (m_el[2].x() > 0)  //gimbal locked up
+      {
-        euler_out.pitch = SIMD_PI / btScalar(2.0);
-        euler_out2.pitch = SIMD_PI / btScalar(2.0);
-        euler_out.roll = euler_out.pitch + delta;
-        euler_out2.roll = euler_out.pitch + delta;
+      }
-      else // gimbal locked down
+      {
-        euler_out.pitch = -SIMD_PI / btScalar(2.0);
-        euler_out2.pitch = -SIMD_PI / btScalar(2.0);
-        euler_out.roll = -euler_out.pitch + delta;
-        euler_out2.roll = -euler_out.pitch + delta;
+      }
+    }
-    else
+    {
-      euler_out.pitch = - btAsin(m_el[2].x());
-      euler_out2.pitch = SIMD_PI - euler_out.pitch;
-      euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
-                               m_el[2].z()/btCos(euler_out.pitch));
-      euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
-                                m_el[2].z()/btCos(euler_out2.pitch));
-      euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
-                              m_el[0].x()/btCos(euler_out.pitch));
-      euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
-                               m_el[0].x()/btCos(euler_out2.pitch));
+    }
-    if (solution_number == 1)
+    {
-                yaw = euler_out.yaw;
-                pitch = euler_out.pitch;
-                roll = euler_out.roll;
+    }
-    else
+    {
-                yaw = euler_out2.yaw;
-                pitch = euler_out2.pitch;
-                roll = euler_out2.roll;
+    }
+  }
-  /**@brief Create a scaled copy of the matrix
-   * @param s Scaling vector The elements of the vector will scale each column */
-                btMatrix3x3 scaled(const btVector3& s) const
+                {
-                        return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
-                                                                         m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
-                                                                         m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
+                }
-  /**@brief Return the determinant of the matrix */
-                btScalar            determinant() const;
-  /**@brief Return the adjoint of the matrix */
-                btMatrix3x3 adjoint() const;
-  /**@brief Return the matrix with all values non negative */
-                btMatrix3x3 absolute() const;
-  /**@brief Return the transpose of the matrix */
-                btMatrix3x3 transpose() const;
-  /**@brief Return the inverse of the matrix */
-                btMatrix3x3 inverse() const;
-                btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
-                btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
-                SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
+                {
-                        return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+                }
-                SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
+                {
-                        return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+                }
-                SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
+                {
-                        return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+                }
-  /**@brief diagonalizes this matrix by the Jacobi method.
-   * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
-   * coordinate system, i.e., old_this = rot * new_this * rot^T.
-   * @param threshold See iteration
-   * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
-   * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
+   *
-   * Note that this matrix is assumed to be symmetric.
-   */
-                void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+                {
-                 rot.setIdentity();
-                 for (int step = maxSteps; step > 0; step--)
+                 {
                         // find off-diagonal element [p][q] with largest magnitude
                         int p = 0;
 …
                         if (v > max)
+                        {
                            q = 2;
                            r = 1;
                            max = v;
+                                q = 2;
+                                r = 1;
+                                max = v;
+                        }
                         v = btFabs(m_el[1][2]);
                         if (v > max)
+                        {
                            p = 1;
                            q = 2;
                            r = 0;
                            max = v;
+                                p = 1;
+                                q = 2;
+                                r = 0;
+                                max = v;
+                        }
 …
                         if (max <= t)
+                        {
                            if (max <= SIMD_EPSILON * t)
+                           {
                                   return;
+                           }
                            step = 1;
+                                if (max <= SIMD_EPSILON * t)
+                                {
+                                        return;
+                                }
+                                step = 1;
+                        }
 …
                         if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
+                        {
                            t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
                                                                                 : 1 / (theta - btSqrt(1 + theta2));
                            cos = 1 / btSqrt(1 + t * t);
                            sin = cos * t;
+                                t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
+                                        : 1 / (theta - btSqrt(1 + theta2));
+                                cos = 1 / btSqrt(1 + t * t);
+                                sin = cos * t;
+                        }
                         else
+                        {
                            // approximation for large theta-value, i.e., a nearly diagonal matrix
                            t = 1 / (theta * (2 + btScalar(0.5) / theta2));
                            cos = 1 - btScalar(0.5) * t * t;
                            sin = cos * t;
+                                // approximation for large theta-value, i.e., a nearly diagonal matrix
+                                t = 1 / (theta * (2 + btScalar(0.5) / theta2));
+                                cos = 1 - btScalar(0.5) * t * t;
+                                sin = cos * t;
+                        }
 …
                         for (int i = 0; i < 3; i++)
+                        {
+                           btVector3& row = rot[i];
+                           mrp = row[p];
+                           mrq = row[q];
+                           row[p] = cos * mrp - sin * mrq;
+                           row[q] = cos * mrq + sin * mrp;
+                        }
+                 }
+                                btVector3& row = rot[i];
+                                mrp = row[p];
+                                mrq = row[q];
+                                row[p] = cos * mrp - sin * mrq;
+                                row[q] = cos * mrq + sin * mrp;
+                        }
+                }
+        protected:
+  /**@brief Calculate the matrix cofactor
+   * @param r1 The first row to use for calculating the cofactor
+   * @param c1 The first column to use for calculating the cofactor
+   * @param r1 The second row to use for calculating the cofactor
+   * @param c1 The second column to use for calculating the cofactor
+   * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
+   */
+                btScalar cofac(int r1, int c1, int r2, int c2) const
+                {
+                        return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+                }
+  ///Data storage for the matrix, each vector is a row of the matrix
+                btVector3 m_el[3];
+        };
+        }
+        /**@brief Calculate the matrix cofactor
+        * @param r1 The first row to use for calculating the cofactor
+        * @param c1 The first column to use for calculating the cofactor
+        * @param r1 The second row to use for calculating the cofactor
+        * @param c1 The second column to use for calculating the cofactor
+        * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
+        */
+        btScalar cofac(int r1, int c1, int r2, int c2) const
+        {
+                return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+        }
+        void    serialize(struct        btMatrix3x3Data& dataOut) const;
+        void    serializeFloat(struct   btMatrix3x3FloatData& dataOut) const;
+        void    deSerialize(const struct        btMatrix3x3Data& dataIn);
+        void    deSerializeFloat(const struct   btMatrix3x3FloatData& dataIn);
+        void    deSerializeDouble(const struct  btMatrix3x3DoubleData& dataIn);
+};
+SIMD_FORCE_INLINE btMatrix3x3&
+btMatrix3x3::operator*=(const btMatrix3x3& m)
+{
+        setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+                m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+                m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+        return *this;
+}
+SIMD_FORCE_INLINE btScalar
+btMatrix3x3::determinant() const
+{
+        return btTriple((*this)[0], (*this)[1], (*this)[2]);
+}
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::absolute() const
+{
+        return btMatrix3x3(
+                btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
+                btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
+                btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
+}
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::transpose() const
+{
+        return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+                m_el[0].y(), m_el[1].y(), m_el[2].y(),
+                m_el[0].z(), m_el[1].z(), m_el[2].z());
+}
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::adjoint() const
+{
+        return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+                cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+                cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+}
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::inverse() const
+{
+        btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+        btScalar det = (*this)[0].dot(co);
+        btFullAssert(det != btScalar(0.0));
+        btScalar s = btScalar(1.0) / det;
+        return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+                co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+                co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+}
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
+{
+        return btMatrix3x3(
+                m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
+                m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
+                m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
+                m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
+                m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
+                m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
+                m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
+                m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
+                m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
+}
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
+{
+        return btMatrix3x3(
+                m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+                m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+                m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+}
+SIMD_FORCE_INLINE btVector3
+operator*(const btMatrix3x3& m, const btVector3& v)
+{
+        return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+}
+SIMD_FORCE_INLINE btVector3
+operator*(const btVector3& v, const btMatrix3x3& m)
+{
+        return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+}
+SIMD_FORCE_INLINE btMatrix3x3
+operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+        return btMatrix3x3(
+                m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
+                m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
+                m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+}
+/*
+SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
+return btMatrix3x3(
+m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+*/
+/**@brief Equality operator between two matrices
+* It will test all elements are equal.  */
+SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+        return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+                m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+                m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+}
+///for serialization
+struct  btMatrix3x3FloatData
+{
+        btVector3FloatData m_el[3];
+};
+///for serialization
+struct  btMatrix3x3DoubleData
+{
+        btVector3DoubleData m_el[3];
+};
+        SIMD_FORCE_INLINE btMatrix3x3&
+        btMatrix3x3::operator*=(const btMatrix3x3& m)
+        {
+                setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+                                 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+                                 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+                return *this;
+        }
+        SIMD_FORCE_INLINE btScalar
+        btMatrix3x3::determinant() const
+        {
+                return triple((*this)[0], (*this)[1], (*this)[2]);
+        }
+        SIMD_FORCE_INLINE btMatrix3x3
+        btMatrix3x3::absolute() const
+        {
+                return btMatrix3x3(
+                        btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
+                        btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
+                        btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
+        }
+        SIMD_FORCE_INLINE btMatrix3x3
+        btMatrix3x3::transpose() const
+        {
+                return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+                                                                 m_el[0].y(), m_el[1].y(), m_el[2].y(),
+                                                                 m_el[0].z(), m_el[1].z(), m_el[2].z());
+        }
+        SIMD_FORCE_INLINE btMatrix3x3
+        btMatrix3x3::adjoint() const
+        {
+                return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+                                                                 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+                                                                 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+        }
+        SIMD_FORCE_INLINE btMatrix3x3
+        btMatrix3x3::inverse() const
+        {
+                btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+                btScalar det = (*this)[0].dot(co);
+                btFullAssert(det != btScalar(0.0));
+                btScalar s = btScalar(1.0) / det;
+                return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+                                                                 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+                                                                 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+        }
+        SIMD_FORCE_INLINE btMatrix3x3
+        btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
+        {
+                return btMatrix3x3(
+                        m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
+                        m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
+                        m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
+                        m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
+                        m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
+                        m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
+                        m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
+                        m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
+                        m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
+        }
+        SIMD_FORCE_INLINE btMatrix3x3
+        btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
+        {
+                return btMatrix3x3(
+                        m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+                        m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+                        m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+        }
+        SIMD_FORCE_INLINE btVector3
+        operator*(const btMatrix3x3& m, const btVector3& v)
+        {
+                return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+        }
+        SIMD_FORCE_INLINE btVector3
+        operator*(const btVector3& v, const btMatrix3x3& m)
+        {
+                return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+        }
+        SIMD_FORCE_INLINE btMatrix3x3
+        operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
+        {
+                return btMatrix3x3(
+                        m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
+                        m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
+                        m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+        }
+/*
+        SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
+    return btMatrix3x3(
+        m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+        m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+        m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+        m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+        m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+        m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+        m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+        m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+        m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+*/
+/**@brief Equality operator between two matrices
+ * It will test all elements are equal.  */
+SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+   return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+            m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+            m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+}
+#endif
+SIMD_FORCE_INLINE       void    btMatrix3x3::serialize(struct   btMatrix3x3Data& dataOut) const
+{
+        for (int i=0;i<3;i++)
+                m_el[i].serialize(dataOut.m_el[i]);
+}
+SIMD_FORCE_INLINE       void    btMatrix3x3::serializeFloat(struct      btMatrix3x3FloatData& dataOut) const
+{
+        for (int i=0;i<3;i++)
+                m_el[i].serializeFloat(dataOut.m_el[i]);
+}
+SIMD_FORCE_INLINE       void    btMatrix3x3::deSerialize(const struct   btMatrix3x3Data& dataIn)
+{
+        for (int i=0;i<3;i++)
+                m_el[i].deSerialize(dataIn.m_el[i]);
+}
+SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeFloat(const struct      btMatrix3x3FloatData& dataIn)
+{
+        for (int i=0;i<3;i++)
+                m_el[i].deSerializeFloat(dataIn.m_el[i]);
+}
+SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeDouble(const struct     btMatrix3x3DoubleData& dataIn)
+{
+        for (int i=0;i<3;i++)
+                m_el[i].deSerializeDouble(dataIn.m_el[i]);
+}
+#endif //BT_MATRIX3x3_H

code/branches/kicklib2/src/external/bullet/LinearMath/btMinMax.h

-                      r5781
+                      r8284
 #define GEN_MINMAX_H
+#include "LinearMath/btScalar.h"
 template <class T>
 SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
 …
 template <class T>
 SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub)
+SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
+{
         return a < lb ? lb : (ub < a ? ub : a);
 …
 template <class T>
 SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub)
+SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
+{
         if (a < lb)

code/branches/kicklib2/src/external/bullet/LinearMath/btPoolAllocator.h

-                      r5781
+                      r8284
+        }
+        int getUsedCount() const
+        {
+                return m_maxElements - m_freeCount;
+        }
         void*   allocate(int size)
+        {
 …
+        }
+        unsigned char*  getPoolAddress()
+        {
+                return m_pool;
+        }
+        const unsigned char*    getPoolAddress() const
+        {
+                return m_pool;
+        }
 };

code/branches/kicklib2/src/external/bullet/LinearMath/btQuaternion.h

-                      r5781
+                      r8284
+        }
+  /**@brief Return the inverse of this quaternion */
+        /**@brief Return the axis of the rotation represented by this quaternion */
+        btVector3 getAxis() const
+        {
+                btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.));
+                if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
+                        return btVector3(1.0, 0.0, 0.0);  // Arbitrary
+                btScalar s = btSqrt(s_squared);
+                return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
+        }
+        /**@brief Return the inverse of this quaternion */
         btQuaternion inverse() const
+        {
 …
+        }
+        /**@todo document this and it's use */
+        SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const
+        {
+                btQuaternion diff,sum;
+                diff = *this - qd;
+                sum = *this + qd;
+                if( diff.dot(diff) < sum.dot(sum) )
+                        return qd;
+                return (-qd);
+        }
   /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
    * @param q The other quaternion to interpolate with
 …
                         btScalar s0 = btSin((btScalar(1.0) - t) * theta);
                         btScalar s1 = btSin(t * theta);
+                        return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d,
+                                (m_floats[1] * s0 + q.y() * s1) * d,
+                                (m_floats[2] * s0 + q.z() * s1) * d,
+                                (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+                        if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+                          return btQuaternion((m_floats[0] * s0 + -q.x() * s1) * d,
+                                              (m_floats[1] * s0 + -q.y() * s1) * d,
+                                              (m_floats[2] * s0 + -q.z() * s1) * d,
+                                              (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
+                        else
+                          return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d,
+                                              (m_floats[1] * s0 + q.y() * s1) * d,
+                                              (m_floats[2] * s0 + q.z() * s1) * d,
+                                              (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+                }
                 else
 …
         if (d < -1.0 + SIMD_EPSILON)
+                return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
+        {
+                btVector3 n,unused;
+                btPlaneSpace1(v0,n,unused);
+                return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
+        }
         btScalar  s = btSqrt((1.0f + d) * 2.0f);

code/branches/kicklib2/src/external/bullet/LinearMath/btQuickprof.cpp

-                      r5781
+                      r8284
 /*
 /***************************************************************************************************
+***************************************************************************************************
 **
 ** profile.cpp
 …
 // Ogre (www.ogre3d.org).
 #include "LinearMath/btQuickprof.h"
+#ifdef USE_BT_CLOCK
+#include "btQuickprof.h"
+#ifndef BT_NO_PROFILE
 static btClock gProfileClock;
+#ifdef __CELLOS_LV2__
+#include <sys/sys_time.h>
+#include <sys/time_util.h>
+#include <stdio.h>
+#endif
+#if defined (SUNOS) || defined (__SUNOS__)
+#include <stdio.h>
+#endif
+#if defined(WIN32) || defined(_WIN32)
+#define BT_USE_WINDOWS_TIMERS
+#define WIN32_LEAN_AND_MEAN
+#define NOWINRES
+#define NOMCX
+#define NOIME
+#ifdef _XBOX
+        #include <Xtl.h>
+#else //_XBOX
+        #include <windows.h>
+#endif //_XBOX
+#include <time.h>
+#else //_WIN32
+#include <sys/time.h>
+#endif //_WIN32
+#define mymin(a,b) (a > b ? a : b)
+struct btClockData
+{
+#ifdef BT_USE_WINDOWS_TIMERS
+        LARGE_INTEGER mClockFrequency;
+        DWORD mStartTick;
+        LONGLONG mPrevElapsedTime;
+        LARGE_INTEGER mStartTime;
+#else
+#ifdef __CELLOS_LV2__
+        uint64_t        mStartTime;
+#else
+        struct timeval mStartTime;
+#endif
+#endif //__CELLOS_LV2__
+};
+///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
+btClock::btClock()
+{
+        m_data = new btClockData;
+#ifdef BT_USE_WINDOWS_TIMERS
+        QueryPerformanceFrequency(&m_data->mClockFrequency);
+#endif
+        reset();
+}
+btClock::~btClock()
+{
+        delete m_data;
+}
+btClock::btClock(const btClock& other)
+{
+        m_data = new btClockData;
+        *m_data = *other.m_data;
+}
+btClock& btClock::operator=(const btClock& other)
+{
+        *m_data = *other.m_data;
+        return *this;
+}
+        /// Resets the initial reference time.
+void btClock::reset()
+{
+#ifdef BT_USE_WINDOWS_TIMERS
+        QueryPerformanceCounter(&m_data->mStartTime);
+        m_data->mStartTick = GetTickCount();
+        m_data->mPrevElapsedTime = 0;
+#else
+#ifdef __CELLOS_LV2__
+        typedef uint64_t  ClockSize;
+        ClockSize newTime;
+        //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+        SYS_TIMEBASE_GET( newTime );
+        m_data->mStartTime = newTime;
+#else
+        gettimeofday(&m_data->mStartTime, 0);
+#endif
+#endif
+}
+/// Returns the time in ms since the last call to reset or since
+/// the btClock was created.
+unsigned long int btClock::getTimeMilliseconds()
+{
+#ifdef BT_USE_WINDOWS_TIMERS
+        LARGE_INTEGER currentTime;
+        QueryPerformanceCounter(&currentTime);
+        LONGLONG elapsedTime = currentTime.QuadPart -
+                m_data->mStartTime.QuadPart;
+                // Compute the number of millisecond ticks elapsed.
+        unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
+                m_data->mClockFrequency.QuadPart);
+                // Check for unexpected leaps in the Win32 performance counter.
+        // (This is caused by unexpected data across the PCI to ISA
+                // bridge, aka south bridge.  See Microsoft KB274323.)
+                unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
+                signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+                if (msecOff < -100 || msecOff > 100)
+                {
+                        // Adjust the starting time forwards.
+                        LONGLONG msecAdjustment = mymin(msecOff *
+                                m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
+                                m_data->mPrevElapsedTime);
+                        m_data->mStartTime.QuadPart += msecAdjustment;
+                        elapsedTime -= msecAdjustment;
+                        // Recompute the number of millisecond ticks elapsed.
+                        msecTicks = (unsigned long)(1000 * elapsedTime /
+                                m_data->mClockFrequency.QuadPart);
+                }
+                // Store the current elapsed time for adjustments next time.
+                m_data->mPrevElapsedTime = elapsedTime;
+                return msecTicks;
+#else
+#ifdef __CELLOS_LV2__
+                uint64_t freq=sys_time_get_timebase_frequency();
+                double dFreq=((double) freq) / 1000.0;
+                typedef uint64_t  ClockSize;
+                ClockSize newTime;
+                SYS_TIMEBASE_GET( newTime );
+                //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+                return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
+#else
+                struct timeval currentTime;
+                gettimeofday(&currentTime, 0);
+                return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 +
+                        (currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000;
+#endif //__CELLOS_LV2__
+#endif
+}
+        /// Returns the time in us since the last call to reset or since
+        /// the Clock was created.
+unsigned long int btClock::getTimeMicroseconds()
+{
+#ifdef BT_USE_WINDOWS_TIMERS
+                LARGE_INTEGER currentTime;
+                QueryPerformanceCounter(&currentTime);
+                LONGLONG elapsedTime = currentTime.QuadPart -
+                        m_data->mStartTime.QuadPart;
+                // Compute the number of millisecond ticks elapsed.
+                unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
+                        m_data->mClockFrequency.QuadPart);
+                // Check for unexpected leaps in the Win32 performance counter.
+                // (This is caused by unexpected data across the PCI to ISA
+                // bridge, aka south bridge.  See Microsoft KB274323.)
+                unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
+                signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+                if (msecOff < -100 || msecOff > 100)
+                {
+                        // Adjust the starting time forwards.
+                        LONGLONG msecAdjustment = mymin(msecOff *
+                                m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
+                                m_data->mPrevElapsedTime);
+                        m_data->mStartTime.QuadPart += msecAdjustment;
+                        elapsedTime -= msecAdjustment;
+                }
+                // Store the current elapsed time for adjustments next time.
+                m_data->mPrevElapsedTime = elapsedTime;
+                // Convert to microseconds.
+                unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
+                        m_data->mClockFrequency.QuadPart);
+                return usecTicks;
+#else
+#ifdef __CELLOS_LV2__
+                uint64_t freq=sys_time_get_timebase_frequency();
+                double dFreq=((double) freq)/ 1000000.0;
+                typedef uint64_t  ClockSize;
+                ClockSize newTime;
+                //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+                SYS_TIMEBASE_GET( newTime );
+                return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
+#else
+                struct timeval currentTime;
+                gettimeofday(&currentTime, 0);
+                return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 +
+                        (currentTime.tv_usec - m_data->mStartTime.tv_usec);
+#endif//__CELLOS_LV2__
+#endif
+}
 inline void Profile_Get_Ticks(unsigned long int * ticks)
 …
+#endif //USE_BT_CLOCK
+#endif //BT_NO_PROFILE

code/branches/kicklib2/src/external/bullet/LinearMath/btQuickprof.h

-                      r5781
+                      r8284
 //#define BT_NO_PROFILE 1
 #ifndef BT_NO_PROFILE
+#include <stdio.h>//@todo remove this, backwards compatibility
 #include "btScalar.h"
 #include "LinearMath/btAlignedAllocator.h"
+#include "btAlignedAllocator.h"
 #include <new>
 …
+//if you don't need btClock, you can comment next line
 #define USE_BT_CLOCK 1
 #ifdef USE_BT_CLOCK
-#ifdef __CELLOS_LV2__
-#include <sys/sys_time.h>
-#include <sys/time_util.h>
-#include <stdio.h>
-#endif
-#if defined (SUNOS) || defined (__SUNOS__)
-#include <stdio.h>
-#endif
-#if defined(WIN32) || defined(_WIN32)
-#define USE_WINDOWS_TIMERS
-#define WIN32_LEAN_AND_MEAN
-#define NOWINRES
-#define NOMCX
-#define NOIME
-#ifdef _XBOX
-#include <Xtl.h>
-#else
-#include <windows.h>
-#endif
-#include <time.h>
-#else
-#include <sys/time.h>
-#endif
-#define mymin(a,b) (a > b ? a : b)
 ///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
 …
+{
 public:
+        btClock()
+        {
+#ifdef USE_WINDOWS_TIMERS
+                QueryPerformanceFrequency(&mClockFrequency);
+#endif
+                reset();
+        }
+        btClock();
+        ~btClock()
+        {
+        }
+        btClock(const btClock& other);
+        btClock& operator=(const btClock& other);
+        ~btClock();
         /// Resets the initial reference time.
+        void reset()
+        {
+#ifdef USE_WINDOWS_TIMERS
+                QueryPerformanceCounter(&mStartTime);
+                mStartTick = GetTickCount();
+                mPrevElapsedTime = 0;
+#else
+#ifdef __CELLOS_LV2__
+                typedef uint64_t  ClockSize;
+                ClockSize newTime;
+                //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+                SYS_TIMEBASE_GET( newTime );
+                mStartTime = newTime;
+#else
+                gettimeofday(&mStartTime, 0);
+#endif
+#endif
+        }
+        void reset();
         /// Returns the time in ms since the last call to reset or since
         /// the btClock was created.
+        unsigned long int getTimeMilliseconds()
+        {
+#ifdef USE_WINDOWS_TIMERS
+                LARGE_INTEGER currentTime;
+                QueryPerformanceCounter(&currentTime);
+                LONGLONG elapsedTime = currentTime.QuadPart -
+                        mStartTime.QuadPart;
+                // Compute the number of millisecond ticks elapsed.
+                unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
+                        mClockFrequency.QuadPart);
+                // Check for unexpected leaps in the Win32 performance counter.
+                // (This is caused by unexpected data across the PCI to ISA
+                // bridge, aka south bridge.  See Microsoft KB274323.)
+                unsigned long elapsedTicks = GetTickCount() - mStartTick;
+                signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+                if (msecOff < -100 || msecOff > 100)
+                {
+                        // Adjust the starting time forwards.
+                        LONGLONG msecAdjustment = mymin(msecOff *
+                                mClockFrequency.QuadPart / 1000, elapsedTime -
+                                mPrevElapsedTime);
+                        mStartTime.QuadPart += msecAdjustment;
+                        elapsedTime -= msecAdjustment;
+                        // Recompute the number of millisecond ticks elapsed.
+                        msecTicks = (unsigned long)(1000 * elapsedTime /
+                                mClockFrequency.QuadPart);
+                }
+                // Store the current elapsed time for adjustments next time.
+                mPrevElapsedTime = elapsedTime;
+                return msecTicks;
+#else
+#ifdef __CELLOS_LV2__
+                uint64_t freq=sys_time_get_timebase_frequency();
+                double dFreq=((double) freq) / 1000.0;
+                typedef uint64_t  ClockSize;
+                ClockSize newTime;
+                SYS_TIMEBASE_GET( newTime );
+                //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+                return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
+#else
+                struct timeval currentTime;
+                gettimeofday(&currentTime, 0);
+                return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 +
+                        (currentTime.tv_usec - mStartTime.tv_usec) / 1000;
+#endif //__CELLOS_LV2__
+#endif
+        }
+        unsigned long int getTimeMilliseconds();
         /// Returns the time in us since the last call to reset or since
         /// the Clock was created.
+        unsigned long int getTimeMicroseconds()
+        {
+#ifdef USE_WINDOWS_TIMERS
+                LARGE_INTEGER currentTime;
+                QueryPerformanceCounter(&currentTime);
+                LONGLONG elapsedTime = currentTime.QuadPart -
+                        mStartTime.QuadPart;
+                // Compute the number of millisecond ticks elapsed.
+                unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
+                        mClockFrequency.QuadPart);
+                // Check for unexpected leaps in the Win32 performance counter.
+                // (This is caused by unexpected data across the PCI to ISA
+                // bridge, aka south bridge.  See Microsoft KB274323.)
+                unsigned long elapsedTicks = GetTickCount() - mStartTick;
+                signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+                if (msecOff < -100 || msecOff > 100)
+                {
+                        // Adjust the starting time forwards.
+                        LONGLONG msecAdjustment = mymin(msecOff *
+                                mClockFrequency.QuadPart / 1000, elapsedTime -
+                                mPrevElapsedTime);
+                        mStartTime.QuadPart += msecAdjustment;
+                        elapsedTime -= msecAdjustment;
+                }
+                // Store the current elapsed time for adjustments next time.
+                mPrevElapsedTime = elapsedTime;
+                // Convert to microseconds.
+                unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
+                        mClockFrequency.QuadPart);
+                return usecTicks;
+#else
+#ifdef __CELLOS_LV2__
+                uint64_t freq=sys_time_get_timebase_frequency();
+                double dFreq=((double) freq)/ 1000000.0;
+                typedef uint64_t  ClockSize;
+                ClockSize newTime;
+                //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+                SYS_TIMEBASE_GET( newTime );
+                return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
+#else
+                struct timeval currentTime;
+                gettimeofday(&currentTime, 0);
+                return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 +
+                        (currentTime.tv_usec - mStartTime.tv_usec);
+#endif//__CELLOS_LV2__
+#endif
+        }
+        unsigned long int getTimeMicroseconds();
 private:
+#ifdef USE_WINDOWS_TIMERS
+        LARGE_INTEGER mClockFrequency;
+        DWORD mStartTick;
+        LONGLONG mPrevElapsedTime;
+        LARGE_INTEGER mStartTime;
+#else
+#ifdef __CELLOS_LV2__
+        uint64_t        mStartTime;
+#else
+        struct timeval mStartTime;
+#endif
+#endif //__CELLOS_LV2__
+        struct btClockData* m_data;
 };

code/branches/kicklib2/src/external/bullet/LinearMath/btScalar.h

-                      r5781
+                      r8284
 /*
 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bullet.googlecode.com
 This software is provided 'as-is', without any express or implied warranty.
 …
 #define SIMD___SCALAR_H
+#ifdef BT_MANAGED_CODE
+//Aligned data types not supported in managed code
+#pragma unmanaged
+#endif
 #include <math.h>
 #include <stdlib.h>//size_t for MSVC 6.0
 #include <cstdlib>
 #include <cfloat>
 #include <float.h>
+#define BT_BULLET_VERSION 274
+/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
+#define BT_BULLET_VERSION 277
 inline int      btGetVersion()
 …
 #ifdef WIN32
+#ifdef _WIN32
                 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
 …
                         #define SIMD_FORCE_INLINE inline
                         #define ATTRIBUTE_ALIGNED16(a) a
+                        #define ATTRIBUTE_ALIGNED64(a) a
                         #define ATTRIBUTE_ALIGNED128(a) a
                 #else
 …
                         #define SIMD_FORCE_INLINE __forceinline
                         #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+                        #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
                         #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
                 #ifdef _XBOX
 …
                 #else
 #if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
+#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
                         #define BT_USE_SSE
                         #include <emmintrin.h>
 …
 #if defined     (__CELLOS_LV2__)
                 #define SIMD_FORCE_INLINE inline
+                #define SIMD_FORCE_INLINE inline __attribute__((always_inline))
                 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+                #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+                #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+                #ifndef assert
+                #include <assert.h>
+                #endif
+#ifdef BT_DEBUG
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+        #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
+#else
+        #define btAssert assert
+#endif
+#else
+                #define btAssert(x)
+#endif
+                //btFullAssert is optional, slows down a lot
+                #define btFullAssert(x)
+                #define btLikely(_c)  _c
+                #define btUnlikely(_c) _c
+#else
+#ifdef USE_LIBSPE2
+                #define SIMD_FORCE_INLINE __inline
+                #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+                #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
                 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
                 #ifndef assert
 …
                 #define btFullAssert(x)
-                #define btLikely(_c)  _c
-                #define btUnlikely(_c) _c
-#else
-#ifdef USE_LIBSPE2
-                #define SIMD_FORCE_INLINE __inline
-                #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-                #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
-                #ifndef assert
-                #include <assert.h>
-                #endif
-#ifdef BT_DEBUG
-                #define btAssert assert
-#else
-                #define btAssert(x)
-#endif
-                //btFullAssert is optional, slows down a lot
-                #define btFullAssert(x)
                 #define btLikely(_c)   __builtin_expect((_c), 1)
 …
         //non-windows systems
+#if (defined (__APPLE__) && defined (__i386__) && (!defined (BT_USE_DOUBLE_PRECISION)))
+        #define BT_USE_SSE
+        #include <emmintrin.h>
+        #define SIMD_FORCE_INLINE inline
+///@todo: check out alignment methods for other platforms/compilers
+        #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+        #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+        #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+        #ifndef assert
+        #include <assert.h>
+        #endif
+        #if defined(DEBUG) || defined (_DEBUG)
+                #define btAssert assert
+        #else
+                #define btAssert(x)
+        #endif
+        //btFullAssert is optional, slows down a lot
+        #define btFullAssert(x)
+        #define btLikely(_c)  _c
+        #define btUnlikely(_c) _c
+#else
                 #define SIMD_FORCE_INLINE inline
                 ///@todo: check out alignment methods for other platforms/compilers
                 ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+                ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
                 ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
                 #define ATTRIBUTE_ALIGNED16(a) a
+                #define ATTRIBUTE_ALIGNED64(a) a
                 #define ATTRIBUTE_ALIGNED128(a) a
                 #ifndef assert
 …
                 #define btLikely(_c)  _c
                 #define btUnlikely(_c) _c
+#endif //__APPLE__
 #endif // LIBSPE2
 …
 #endif
-/// older compilers (gcc 3.x) and Sun needs double version of sqrt etc.
-/// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor)
-#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__))
-//use slow double float precision operation on those platforms
-#ifndef BT_USE_DOUBLE_PRECISION
-#define BT_FORCE_DOUBLE_FUNCTIONS
-#endif
-#endif
 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
 #if defined(BT_USE_DOUBLE_PRECISION)
 typedef double btScalar;
+//this number could be bigger in double precision
+#define BT_LARGE_FLOAT 1e30
 #else
 typedef float btScalar;
+//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
+#define BT_LARGE_FLOAT 1e18f
 #endif
 …
 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); }
 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return acos(x); }
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return asin(x); }
 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
 …
 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
 #else
 …
         *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
         x =  tempf;
         z =  y*btScalar(0.5);                        /* hoist out the /2    */
+        z =  y*btScalar(0.5);
         x = (btScalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
         x = (btScalar(1.5)*x)-(x*x)*(x*z);
 …
 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
+        btAssert(x <= btScalar(1.));
+        if (x<btScalar(-1))
+                x=btScalar(-1);
+        if (x>btScalar(1))
+                x=btScalar(1);
         return acosf(x);
+}
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) {
+        if (x<btScalar(-1))
+                x=btScalar(-1);
+        if (x>btScalar(1))
+                x=btScalar(1);
+        return asinf(x);
+}
 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
 …
   SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
   #endif
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
 #endif
 …
 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
 #define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
+#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
+#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))          /* reciprocal square root */
 #ifdef BT_USE_DOUBLE_PRECISION
 …
+}
+// returns normalized value in range [-SIMD_PI, SIMD_PI]
+SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
+{
+        angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
+        if(angleInRadians < -SIMD_PI)
+        {
+                return angleInRadians + SIMD_2_PI;
+        }
+        else if(angleInRadians > SIMD_PI)
+        {
+                return angleInRadians - SIMD_2_PI;
+        }
+        else
+        {
+                return angleInRadians;
+        }
+}
+///rudimentary class to provide type info
+struct btTypedObject
+{
+        btTypedObject(int objectType)
+                :m_objectType(objectType)
+        {
+        }
+        int     m_objectType;
+        inline int getObjectType() const
+        {
+                return m_objectType;
+        }
+};
 #endif //SIMD___SCALAR_H

code/branches/kicklib2/src/external/bullet/LinearMath/btTransform.h

-                      r5781
+                      r8284
 #define btTransform_H
+#include "btVector3.h"
 #include "btMatrix3x3.h"
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btTransformData btTransformDoubleData
+#else
+#define btTransformData btTransformFloatData
+#endif
 …
 class btTransform {
+  ///Storage for the rotation
+        btMatrix3x3 m_basis;
+  ///Storage for the translation
+        btVector3   m_origin;
 public:
 …
                 return identityTransform;
+        }
+private:
+  ///Storage for the rotation
+        btMatrix3x3 m_basis;
+  ///Storage for the translation
+        btVector3   m_origin;
+        void    serialize(struct        btTransformData& dataOut) const;
+        void    serializeFloat(struct   btTransformFloatData& dataOut) const;
+        void    deSerialize(const struct        btTransformData& dataIn);
+        void    deSerializeDouble(const struct  btTransformDoubleData& dataIn);
+        void    deSerializeFloat(const struct   btTransformFloatData& dataIn);
 };
 …
+///for serialization
+struct  btTransformFloatData
+{
+        btMatrix3x3FloatData    m_basis;
+        btVector3FloatData      m_origin;
+};
+struct  btTransformDoubleData
+{
+        btMatrix3x3DoubleData   m_basis;
+        btVector3DoubleData     m_origin;
+};
+SIMD_FORCE_INLINE       void    btTransform::serialize(btTransformData& dataOut) const
+{
+        m_basis.serialize(dataOut.m_basis);
+        m_origin.serialize(dataOut.m_origin);
+}
+SIMD_FORCE_INLINE       void    btTransform::serializeFloat(btTransformFloatData& dataOut) const
+{
+        m_basis.serializeFloat(dataOut.m_basis);
+        m_origin.serializeFloat(dataOut.m_origin);
+}
+SIMD_FORCE_INLINE       void    btTransform::deSerialize(const btTransformData& dataIn)
+{
+        m_basis.deSerialize(dataIn.m_basis);
+        m_origin.deSerialize(dataIn.m_origin);
+}
+SIMD_FORCE_INLINE       void    btTransform::deSerializeFloat(const btTransformFloatData& dataIn)
+{
+        m_basis.deSerializeFloat(dataIn.m_basis);
+        m_origin.deSerializeFloat(dataIn.m_origin);
+}
+SIMD_FORCE_INLINE       void    btTransform::deSerializeDouble(const btTransformDoubleData& dataIn)
+{
+        m_basis.deSerializeDouble(dataIn.m_basis);
+        m_origin.deSerializeDouble(dataIn.m_origin);
+}
 #endif

code/branches/kicklib2/src/external/bullet/LinearMath/btTransformUtil.h

-                      r5781
+                      r8284
-#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
-#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))          /* reciprocal square root */
 SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
 …
+SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
+{
+  if (btFabs(n.z()) > SIMDSQRT12) {
+    // choose p in y-z plane
+    btScalar a = n[1]*n[1] + n[2]*n[2];
+    btScalar k = btRecipSqrt (a);
+    p.setValue(0,-n[2]*k,n[1]*k);
+    // set q = n x p
+    q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
+  }
+  else {
+    // choose p in x-y plane
+    btScalar a = n.x()*n.x() + n.y()*n.y();
+    btScalar k = btRecipSqrt (a);
+    p.setValue(-n.y()*k,n.x()*k,0);
+    // set q = n x p
+    q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
+  }
+}
 …
         static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
+        {
                 btQuaternion orn1 = orn0.farthest(orn1a);
+                btQuaternion orn1 = orn0.nearest(orn1a);
                 btQuaternion dorn = orn1 * orn0.inverse();
-                ///floating point inaccuracy can lead to w component > 1..., which breaks
-                dorn.normalize();
                 angle = dorn.getAngle();
                 axis = btVector3(dorn.x(),dorn.y(),dorn.z());
 …
                         btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
                         btVector3 relLinVel = (linVelB-linVelA);
                         btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
+                        btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
                         if (relLinVelocLength<0.f)
+                        {
 …
         void    initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
+        {
-                m_separatingNormal = separatingVector;
                 m_separatingDistance = separatingDistance;
+                const btVector3& toPosA = transA.getOrigin();
+                const btVector3& toPosB = transB.getOrigin();
+                btQuaternion toOrnA = transA.getRotation();
+                btQuaternion toOrnB = transB.getRotation();
+                m_posA = toPosA;
+                m_posB = toPosB;
+                m_ornA = toOrnA;
+                m_ornB = toOrnB;
+                if (m_separatingDistance>0.f)
+                {
+                        m_separatingNormal = separatingVector;
+                        const btVector3& toPosA = transA.getOrigin();
+                        const btVector3& toPosB = transB.getOrigin();
+                        btQuaternion toOrnA = transA.getRotation();
+                        btQuaternion toOrnB = transB.getRotation();
+                        m_posA = toPosA;
+                        m_posB = toPosB;
+                        m_ornA = toOrnA;
+                        m_ornB = toOrnB;
+                }
+        }

code/branches/kicklib2/src/external/bullet/LinearMath/btVector3.h

-                      r5781
+                      r8284
 #include "btScalar.h"
-#include "btScalar.h"
 #include "btMinMax.h"
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btVector3Data btVector3DoubleData
+#define btVector3DataName "btVector3DoubleData"
+#else
+#define btVector3Data btVector3FloatData
+#define btVector3DataName "btVector3FloatData"
+#endif //BT_USE_DOUBLE_PRECISION
 /**@brief btVector3 can be used to represent 3D points and vectors.
  * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
  * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
  */
 ATTRIBUTE_ALIGNED16(class) btVector3
+{
 …
 #if defined (__SPU__) && defined (__CELLOS_LV2__)
-        union {
-                vec_float4 mVec128;
                 btScalar        m_floats[4];
-        };
 public:
         vec_float4      get128() const
+        {
                 return mVec128;
+        SIMD_FORCE_INLINE const vec_float4&     get128() const
+        {
+                return *((const vec_float4*)&m_floats[0]);
+        }
 public:
 #else //__CELLOS_LV2__ __SPU__
 #ifdef BT_USE_SSE // WIN32
+#ifdef BT_USE_SSE // _WIN32
         union {
                 __m128 mVec128;
 …
         SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
+        SIMD_FORCE_INLINE btVector3& safeNormalize()
+        {
+                btVector3 absVec = this->absolute();
+                int maxIndex = absVec.maxAxis();
+                if (absVec[maxIndex]>0)
+                {
+                        *this /= absVec[maxIndex];
+                        return *this /= length();
+                }
+                setValue(1,0,0);
+                return *this;
+        }
   /**@brief Normalize this vector
    * x^2 + y^2 + z^2 = 1 */
 …
         SIMD_FORCE_INLINE btVector3 normalized() const;
   /**@brief Rotate this vector
+  /**@brief Return a rotated version of this vector
    * @param wAxis The axis to rotate about
    * @param angle The angle to rotate by */
         SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
+        SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
   /**@brief Return the angle between this and another vector
 …
                         m_floats[1]=y;
                         m_floats[2]=z;
                         m_floats[3] = 0.f;
+                        m_floats[3] = btScalar(0.);
+                }
 …
                         v2->setValue(-y()       ,x()    ,0.);
+                }
+                void    setZero()
+                {
+                        setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+                }
+                SIMD_FORCE_INLINE bool isZero() const
+                {
+                        return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
+                }
+                SIMD_FORCE_INLINE bool fuzzyZero() const
+                {
+                        return length2() < SIMD_EPSILON;
+                }
+                SIMD_FORCE_INLINE       void    serialize(struct        btVector3Data& dataOut) const;
+                SIMD_FORCE_INLINE       void    deSerialize(const struct        btVector3Data& dataIn);
+                SIMD_FORCE_INLINE       void    serializeFloat(struct   btVector3FloatData& dataOut) const;
+                SIMD_FORCE_INLINE       void    deSerializeFloat(const struct   btVector3FloatData& dataIn);
+                SIMD_FORCE_INLINE       void    serializeDouble(struct  btVector3DoubleData& dataOut) const;
+                SIMD_FORCE_INLINE       void    deSerializeDouble(const struct  btVector3DoubleData& dataIn);
 };
 …
 /**@brief Return the dot product between two vectors */
 SIMD_FORCE_INLINE btScalar
 dot(const btVector3& v1, const btVector3& v2)
+btDot(const btVector3& v1, const btVector3& v2)
+{
         return v1.dot(v2);
 …
 /**@brief Return the distance squared between two vectors */
 SIMD_FORCE_INLINE btScalar
 distance2(const btVector3& v1, const btVector3& v2)
+btDistance2(const btVector3& v1, const btVector3& v2)
+{
         return v1.distance2(v2);
 …
 /**@brief Return the distance between two vectors */
 SIMD_FORCE_INLINE btScalar
 distance(const btVector3& v1, const btVector3& v2)
+btDistance(const btVector3& v1, const btVector3& v2)
+{
         return v1.distance(v2);
 …
 /**@brief Return the angle between two vectors */
 SIMD_FORCE_INLINE btScalar
 angle(const btVector3& v1, const btVector3& v2)
+btAngle(const btVector3& v1, const btVector3& v2)
+{
         return v1.angle(v2);
 …
 /**@brief Return the cross product of two vectors */
 SIMD_FORCE_INLINE btVector3
 cross(const btVector3& v1, const btVector3& v2)
+btCross(const btVector3& v1, const btVector3& v2)
+{
         return v1.cross(v2);
 …
 SIMD_FORCE_INLINE btScalar
 triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
+btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
+{
         return v1.triple(v2, v3);
 …
+}
 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
+SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) const
+{
         // wAxis must be a unit lenght vector
 …
+        {
                 int maxIndex = -1;
                 btScalar maxVal = btScalar(-1e30);
+                btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
                 if (m_floats[0] > maxVal)
+                {
 …
+        {
                 int minIndex = -1;
                 btScalar minVal = btScalar(1e30);
+                btScalar minVal = btScalar(BT_LARGE_FLOAT);
                 if (m_floats[0] < minVal)
+                {
 …
+                }
 };
 …
+}
+template <class T>
+SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
+{
+  if (btFabs(n[2]) > SIMDSQRT12) {
+    // choose p in y-z plane
+    btScalar a = n[1]*n[1] + n[2]*n[2];
+    btScalar k = btRecipSqrt (a);
+    p[0] = 0;
+        p[1] = -n[2]*k;
+        p[2] = n[1]*k;
+    // set q = n x p
+    q[0] = a*k;
+        q[1] = -n[0]*p[2];
+        q[2] = n[0]*p[1];
+  }
+  else {
+    // choose p in x-y plane
+    btScalar a = n[0]*n[0] + n[1]*n[1];
+    btScalar k = btRecipSqrt (a);
+    p[0] = -n[1]*k;
+        p[1] = n[0]*k;
+        p[2] = 0;
+    // set q = n x p
+    q[0] = -n[2]*p[1];
+        q[1] = n[2]*p[0];
+        q[2] = a*k;
+  }
+}
+struct  btVector3FloatData
+{
+        float   m_floats[4];
+};
+struct  btVector3DoubleData
+{
+        double  m_floats[4];
+};
+SIMD_FORCE_INLINE       void    btVector3::serializeFloat(struct        btVector3FloatData& dataOut) const
+{
+        ///could also do a memcpy, check if it is worth it
+        for (int i=0;i<4;i++)
+                dataOut.m_floats[i] = float(m_floats[i]);
+}
+SIMD_FORCE_INLINE void  btVector3::deSerializeFloat(const struct        btVector3FloatData& dataIn)
+{
+        for (int i=0;i<4;i++)
+                m_floats[i] = btScalar(dataIn.m_floats[i]);
+}
+SIMD_FORCE_INLINE       void    btVector3::serializeDouble(struct       btVector3DoubleData& dataOut) const
+{
+        ///could also do a memcpy, check if it is worth it
+        for (int i=0;i<4;i++)
+                dataOut.m_floats[i] = double(m_floats[i]);
+}
+SIMD_FORCE_INLINE void  btVector3::deSerializeDouble(const struct       btVector3DoubleData& dataIn)
+{
+        for (int i=0;i<4;i++)
+                m_floats[i] = btScalar(dataIn.m_floats[i]);
+}
+SIMD_FORCE_INLINE       void    btVector3::serialize(struct     btVector3Data& dataOut) const
+{
+        ///could also do a memcpy, check if it is worth it
+        for (int i=0;i<4;i++)
+                dataOut.m_floats[i] = m_floats[i];
+}
+SIMD_FORCE_INLINE void  btVector3::deSerialize(const struct     btVector3Data& dataIn)
+{
+        for (int i=0;i<4;i++)
+                m_floats[i] = dataIn.m_floats[i];
+}
 #endif //SIMD__VECTOR3_H

code/branches/kicklib2/src/external/bullet/NEWS

r5781	r8284
1	1
2		For news, visit the Bullet Physics ~~Forum~~ at
3		http://www.~~continuousphysics.com/Bullet/phpBB2/viewforum.php?f=9~~
	2	For news, visit the Bullet Physics forums at
	3	http://www.bulletphysics.org
4	4

code/branches/kicklib2/src/external/bullet/README

-                      r5781
+                      r8284
 Bullet is a 3D Collision Detection and Rigid Body Dynamics Library for games and animation.
 Free for commercial use, including Playstation 3, open source under the ZLib License.
-Discrete and continuous collision detection, integrated into Blender 3D, and COLLADA 1.4 Physics import.
 See the Bullet_User_Manual.pdf for more info and visit the Bullet Physics Forum at
 http://bulletphysics.com
+http://bulletphysics.org

code/branches/kicklib2/src/external/bullet/VERSION

r5781	r8284
1		2.73
	1	2.77

code/branches/kicklib2/src/external/bullet/btBulletCollisionCommon.h

r5781	r8284
62	62	#include "LinearMath/btQuickprof.h"
63	63	#include "LinearMath/btIDebugDraw.h"
	64	#include "LinearMath/btSerializer.h"
	65
64	66
65	67	#endif //BULLET_COLLISION_COMMON_H

code/branches/kicklib2/src/external/bullet/btBulletDynamicsCommon.h

-                      r5781
+                      r8284
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h"
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"

code/branches/kicklib2/src/external/bullet/changes_orxonox.diff

-                      r5781
+                      r8284
+Index: btScalar.h
+===================================================================
+--- btScalar.h  (revision 2882)
++++ btScalar.h  (working copy)
+@@ -236,7 +236,11 @@
+--- LinearMath/btScalar.h       Tue Aug 10 13:19:44 2010
++++ LinearMath/btScalar.h       Sun Feb 27 01:27:34 2011
+@@ -286,7 +286,11 @@
  SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
  SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
 …
 +  SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
 +  #endif
+ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
  #endif

code/branches/kicklib2/src/external/ogreceguirenderer/CMakeLists.txt

-                      r7163
+                      r8284
   DEFINE_SYMBOL
     "OGRE_GUIRENDERER_EXPORTS"
-  VERSION
-.4.9
   LINK_LIBRARIES
+    ${Boost_SYSTEM_LIBRARY}
+    ${Boost_THREAD_LIBRARY}
+    ${Boost_DATE_TIME_LIBRARY}
     ${OGRE_LIBRARY}
     ${CEGUI_LIBRARY}

code/branches/kicklib2/src/external/ois/CMakeLists.txt

-                      r5929
+                      r8284
   OISKeyboard.h
   OISMouse.h
+  OISMultiTouch.h
   OISObject.h
   OISPrereqs.h
 COMPILATION_BEGIN OISCompilation.cpp
+#COMPILATION_BEGIN OISCompilation.cpp
   OISEffect.cpp
   OISException.cpp
 …
   OISKeyboard.cpp
   OISObject.cpp
 COMPILATION_END
+#COMPILATION_END
+)
 IF(WIN32)
 …
 ENDIF()
+INCLUDE_DIRECTORIES(.)
+# Some unexplained hackery for Visual Studio 2005
+ADD_COMPILER_FLAGS("-D_WIN32_DCOM" MSVC8)
+# MinGW doesn't come with some required Windows headers
+IF(MINGW)
+  INCLUDE_DIRECTORIES(${WMI_INCLUDE_DIR})
+ENDIF()
 ORXONOX_ADD_LIBRARY(ois_orxonox
 …
     "OIS_NONCLIENT_BUILD"
   VERSION
 .2
+.3
   SOURCE_FILES
     ${OIS_FILES}
 …
 IF(WIN32)
+  TARGET_LINK_LIBRARIES(ois_orxonox ${DIRECTX_LIBRARIES})
+  TARGET_LINK_LIBRARIES(ois_orxonox ${DIRECTX_LIBRARIES} ${WMI_LIBRARY})
+ELSEIF(APPLE)
+  TARGET_LINK_LIBRARIES(ois_orxonox "/System/Library/Frameworks/IOKit.framework" "/System/Library/Frameworks/Carbon.framework")
 ENDIF()

code/branches/kicklib2/src/external/ois/OIS.h

r5781	r8284
29	29	#include "OISKeyboard.h"
30	30	#include "OISJoyStick.h"
	31	#include "OISMultiTouch.h"
31	32	#include "OISInputManager.h"
32	33	#include "OISFactoryCreator.h"

code/branches/kicklib2/src/external/ois/OISConfig.h

r5781	r8284
70	70	@remarks
71	71	Build in support for Win32 XInput (Xbox 360 Controller)
72		~~@notes~~
73		~~Not Yet Implemented~~
74	72	*/
75	73	//#define OIS_WIN32_XINPUT_SUPPORT

code/branches/kicklib2/src/external/ois/OISInputManager.cpp

-                      r5781
+                      r8284
 #elif defined OIS_APPLE_PLATFORM
 #  include "mac/MacInputManager.h"
+#elif defined OIS_IPHONE_PLATFORM
+#  include "iphone/iPhoneInputManager.h"
 #elif defined OIS_XBOX_PLATFORM
 #  include "xbox/XBoxInputManager.h"
 …
         m_wiiMoteSupport(0)
+{
+    mFactories.clear();
+    mFactoryObjects.clear();
+}
 …
 #elif defined OIS_APPLE_PLATFORM
         im = new MacInputManager();
+#elif defined OIS_IPHONE_PLATFORM
+        im = new iPhoneInputManager();
 #else
         OIS_EXCEPT(E_General, "No platform library.. check build platform defines!");

code/branches/kicklib2/src/external/ois/OISKeyboard.h

-                      r5781
+                      r8284
+        {
         public:
                 KeyEvent( Object* obj, KeyCode kc, unsigned int txt ) : EventArg(obj), key(kc), text(txt) {}
+                KeyEvent(Object* obj, KeyCode kc, unsigned int txt) : EventArg(obj), key(kc), text(txt) {}
                 virtual ~KeyEvent() {}
 …
         public:
                 virtual ~KeyListener() {}
                 virtual bool keyPressed( const KeyEvent &arg ) = 0;
                 virtual bool keyReleased( const KeyEvent &arg ) = 0;
+                virtual bool keyPressed(const KeyEvent &arg) = 0;
+                virtual bool keyReleased(const KeyEvent &arg) = 0;
         };
 …
                         A KeyCode to check
                 */
                 virtual bool isKeyDown( KeyCode key ) const = 0;
+                virtual bool isKeyDown(KeyCode key) const = 0;
                 /**
 …
                         Send a pointer to a class derived from KeyListener or 0 to clear the callback
                 */
                 virtual void setEventCallback( KeyListener *keyListener ) { mListener = keyListener;}
+                virtual void setEventCallback(KeyListener *keyListener) { mListener = keyListener;}
                 /**
 …
                         Off, Unicode, Ascii
                 */
                 virtual void setTextTranslation( TextTranslationMode mode );
+                virtual void setTextTranslation(TextTranslationMode mode);
                 /**
 …
                         The string as determined from the current locale
                 */
                 virtual const std::string& getAsString( KeyCode kc ) = 0;
+                virtual const std::string& getAsString(KeyCode kc) = 0;
                 //! Enum of bit position of modifer
 …
                         Check modifier status
                 */
                 bool isModifierDown( Modifier mod ) const;
+                bool isModifierDown(Modifier mod) const;
                 /**
 …
                         (in the form of 1 is down and 0 is up)
                 */
                 virtual void copyKeyStates( char keys[256] ) const = 0;
+                virtual void copyKeyStates(char keys[256]) const = 0;
         protected:

code/branches/kicklib2/src/external/ois/OISPrereqs.h

-                      r7163
+                      r8284
 #       endif
 #elif defined( __APPLE_CC__ ) // Apple OS X
+#       define OIS_APPLE_PLATFORM
+#       undef _OISExport
+#       define _OISExport __attribute__((visibility("default")))
+    // Device                                       Simulator
+#   if __IPHONE_OS_VERSION_MIN_REQUIRED >= 20201 || __IPHONE_OS_VERSION_MIN_REQUIRED >= 20000
+//#   if __IPHONE_OS_VERSION_MIN_REQUIRED >= 30000 || __IPHONE_OS_VERSION_MIN_REQUIRED >= 30000
+#       define OIS_IPHONE_PLATFORM
+#   else
+#       define OIS_APPLE_PLATFORM
+#   endif
+#   undef _OISExport
+#   define _OISExport __attribute__((visibility("default")))
 #else //Probably Linux
 #       define OIS_LINUX_PLATFORM
 …
 //-------------- Common Classes, Enums, and Typdef's -------------------------//
 #define OIS_VERSION_MAJOR 1
 #define OIS_VERSION_MINOR 2
+#define OIS_VERSION_MINOR 3
 #define OIS_VERSION_PATCH 0
 #define OIS_VERSION_NAME "Smash"
+#define OIS_VERSION_NAME "1.3.0"
 #define OIS_VERSION ((OIS_VERSION_MAJOR << 16) | (OIS_VERSION_MINOR << 8) | OIS_VERSION_PATCH)
 …
         class Mouse;
         class JoyStick;
+    class MultiTouch;
         class KeyListener;
         class MouseListener;
+    class MultiTouchListener;
         class JoyStickListener;
         class Interface;
 …
     enum Type
+        {
+                OISUnknown   = 0,
+                OISKeyboard  = 1,
+                OISMouse     = 2,
+                OISJoyStick  = 3,
+                OISTablet    = 4
+                OISUnknown       = 0,
+                OISKeyboard      = 1,
+                OISMouse         = 2,
+                OISJoyStick      = 3,
+                OISTablet        = 4,
+                OISMultiTouch    = 5
         };

code/branches/kicklib2/src/external/ois/ReadMe.txt

-                      r5781
+                      r8284
 The zlib/libpng License
 Copyright (c) 2005-2007 Phillip Castaneda (pjcast -- www.wreckedgames.com)
+Copyright (c) 2005-2010 Phillip Castaneda (pjcast -- www.wreckedgames.com)
 This software is provided 'as-is', without any express or implied warranty. In no
 …
 Win32/
         Contains Visual Studio .Net Solution Files
         Contains CodeBlocks + MinGW + StlPort project files for OIS
+        Contains CodeBlocks project files for OIS
         ---- Dependencies ------------------------------------------------------
         DirectInput 8
-        Ogre & CEGUI 0.4.0 If building CEGUIOgre OIS Demo
-        SDL/
-                A test bed for an OIS InputManager with SDL as the backend. Not recommended;
-                however, useful for platforms with non-native OIS ports for temporary use.
 Linux/
         ---- Dependencies ------------------------------------------------------
         X11
+        Ogre (GLX Platform) & CEGUI 0.4.0 If building CEGUIOgre OIS Demo
+        Newer Linux Kernel (2.6+ ?) for Event API - else, use --disable-joyevents
+        Newer Linux Kernel (2.6+ ?) for Event API
         Steps to build on Linux:
 …
         ---- Configure build options --------------------------------------------
         ./configure --help              --- List all configure options
+        ./configure --disable-ogre      --- Disables CEGUIOgre ActionMapping Demo
+        ./configure --disable-joyevents --- Uses /dev/input/jsX instead of
+                                            /dev/input/eventX
 LinuxCB/
 …
 Mac/
-        XCode-1.5/
-                Non-complete native OIS port.
         XCode-2.2/
                 Working, complete, OIS port to OSX using SDL as a backend.
+                Working, mostly complete OSX vackend.

code/branches/kicklib2/src/external/ois/VERSION

r5781	r8284
1		OIS v1_2 CVS updated on 2009/01/24
2		Note that this is not CVS HEAD, but v1_2 branch!
3
	1	OIS SVN v1.3 branch updated on 2011/02/20 (revision 32)
	2	https://wgois.svn.sourceforge.net/svnroot/wgois/ois/branches/v1-3/

code/branches/kicklib2/src/external/ois/changes_orxonox.diff

-                      r7163
+                      r8284
                         //Ignorable Dll interface warning...
  #           if !defined(OIS_MINGW_COMPILER)
 --- linux/EventHelpers.cpp      (revision 5668)
 +++ linux/EventHelpers.cpp      (working copy)
 …
 +++ win32/Win32ForceFeedback.cpp
@@ -25,7 +25,7 @@
  #include <Math.h>
+ #include <math.h>
  // 0 = No trace; 1 = Important traces; 2 = Debug traces
 …
  #if (defined (_DEBUG) || defined(OIS_WIN32_JOYFF_DEBUG))
    #include <iostream>
+--- win32/Win32JoyStick.cpp
++++ win32/Win32JoyStick.cpp
+@@ -26,6 +26,14 @@
+ #include "OISEvents.h"
+ #include "OISException.h"
++// (Orxonox): Required for MinGW to compile properly
++#ifdef __MINGW32__
++#  include <oaidl.h>
++#  ifndef __MINGW_EXTENSION
++#    define __MINGW_EXTENSION __extension__
++#  endif
++#endif
++
+ #include <cassert>
+ #include <wbemidl.h>
+ #include <oleauto.h>
+@@ -39,6 +47,11 @@
+    }
+ #endif
++// (Orxonox): MinGW doesn't have swscanf_s
++#ifdef __MINGW32__
++#      define swscanf_s swscanf
++#endif
++
+ #ifdef OIS_WIN32_XINPUT_SUPPORT
+ #      pragma comment(lib, "xinput.lib")
+ #endif
+@@ -583,7 +596,12 @@
+     bool bCleanupCOM = SUCCEEDED(hr);
+     // Create WMI
++    // (Orxonox): Fix for MinGW
++#ifdef __MINGW32__
++    hr = CoCreateInstance(CLSID_WbemLocator, NULL, CLSCTX_INPROC_SERVER, IID_IWbemLocator, (LPVOID*)&pIWbemLocator);
++#else
+     hr = CoCreateInstance(__uuidof(WbemLocator), NULL, CLSCTX_INPROC_SERVER, __uuidof(IWbemLocator), (LPVOID*)&pIWbemLocator);
++#endif
+     if( FAILED(hr) || pIWbemLocator == NULL )
+         goto LCleanup;
+--- mac/MacHIDManager.cpp
++++ mac/MacHIDManager.cpp
+@@ -406,6 +406,7 @@
+                        switch(iType)
+                        {
+                                case OISJoyStick:
++                {
+                                        int totalDevs = totalDevices(iType);
+                                        int freeDevs = freeDevices(iType);
+                                        int devID = totalDevs - freeDevs;
+@@ -413,6 +414,7 @@
+                                        obj = new MacJoyStick((*it)->combinedKey, bufferMode, *it, creator, devID);
+                                        (*it)->inUse = true;
+                                        return obj;
++                }
+                                case OISTablet:
+                                        //Create MacTablet
+                                        break;

code/branches/kicklib2/src/external/ois/linux/LinuxInputManager.cpp

-                      r5781
+                      r8284
         hideMouse = true;
         mGrabs = true;
-        useXRepeat = false;
         keyboardUsed = mouseUsed = false;
 …
         //--------- Keyboard Settings ------------//
-        i = paramList.find("XAutoRepeatOn");
-        if( i != paramList.end() )
-                if( i->second == "true" )
-                        useXRepeat = true;
         i = paramList.find("x11_keyboard_grab");
         if( i != paramList.end() )
 …
+        {
                 if( keyboardUsed == false )
                         obj = new LinuxKeyboard(this, bufferMode, grabKeyboard, useXRepeat);
+                        obj = new LinuxKeyboard(this, bufferMode, grabKeyboard);
                 break;
+        }

code/branches/kicklib2/src/external/ois/linux/LinuxInputManager.h

-                      r5781
+                      r8284
                 bool mGrabs;
                 bool hideMouse;
-                //! By default, keyboard disables XRepeatRate
-                bool useXRepeat;
         };
+}

code/branches/kicklib2/src/external/ois/linux/LinuxJoyStickEvents.cpp

-                      r5781
+                      r8284
         //We are in non blocking mode - we just read once, and try to fill up buffer
         input_event js[JOY_BUFFERSIZE];
+        int ret = read(mJoyStick, &js, sizeof(struct input_event) * JOY_BUFFERSIZE);
+        if( ret <= 0 )
+                return;
+        //Determine how many whole events re read up
+        ret /= sizeof(struct input_event);
+        for(int i = 0; i < ret; ++i)
+        while(true)
+        {
+                switch(js[i].type)
+                int ret = read(mJoyStick, &js, sizeof(struct input_event) * JOY_BUFFERSIZE);
+        if( ret < 0 )
+                        break;
+                //Determine how many whole events re read up
+                ret /= sizeof(struct input_event);
+                for(int i = 0; i < ret; ++i)
+                {
+                case EV_KEY:  //Button
+                {
+                        int button = mButtonMap[js[i].code];
+                        #ifdef OIS_LINUX_JOY_DEBUG
+                          cout << "\nButton Code: " << js[i].code << ", OIS Value: " << button << endl;
+                        #endif
+                        //Check to see whether push or released event...
+                        if(js[i].value)
+                        {
+                                mState.mButtons[button] = true;
+                                if( mBuffered && mListener )
+                                        if(!mListener->buttonPressed(JoyStickEvent(this,mState), button)) return;
+                        }
+                        else
+                        {
+                                mState.mButtons[button] = false;
+                                if( mBuffered && mListener )
+                                        if(!mListener->buttonReleased(JoyStickEvent(this,mState), button)) return;
+                        }
+                        break;
+                }
+                case EV_ABS:  //Absolute Axis
+                {
+                        //A Stick (BrakeDefine is the highest possible Axis)
+                        if( js[i].code <= ABS_BRAKE )
+                        {
+                                int axis = mAxisMap[js[i].code];
+                                assert( axis < 32 && "Too many axes (Max supported is 32). Report this to OIS forums!" );
+                                axisMoved[axis] = true;
+                                //check for rescaling:
+                                if( mRanges[axis].min == JoyStick::MIN_AXIS && mRanges[axis].max != JoyStick::MAX_AXIS )
+                                {       //Scale is perfect
+                                        mState.mAxes[axis].abs = js[i].value;
+                        switch(js[i].type)
+                        {
+                        case EV_KEY:  //Button
+                        {
+                                int button = mButtonMap[js[i].code];
+                                #ifdef OIS_LINUX_JOY_DEBUG
+                                  cout << "\nButton Code: " << js[i].code << ", OIS Value: " << button << endl;
+                                #endif
+                                //Check to see whether push or released event...
+                                if(js[i].value)
+                                {
+                                        mState.mButtons[button] = true;
+                                        if( mBuffered && mListener )
+                                                if(!mListener->buttonPressed(JoyStickEvent(this,mState), button)) return;
+                                }
                                 else
+                                {       //Rescale
+                                        float proportion = (float)(js[i].value-mRanges[axis].max)/(float)(mRanges[axis].min-mRanges[axis].max);
+                                        mState.mAxes[axis].abs = (int)(32767.0f - (65535.0f * proportion));
+                                {
+                                        mState.mButtons[button] = false;
+                                        if( mBuffered && mListener )
+                                                if(!mListener->buttonReleased(JoyStickEvent(this,mState), button)) return;
+                                }
+                        }
+                        else if( js[i].code <= ABS_HAT3Y ) //A POV - Max four POVs allowed
+                        {
+                                //Normalise the POV to between 0-7
+                                //Even is X Axis, Odd is Y Axis
+                                unsigned char LinuxPovNumber = js[i].code - 16;
+                                short OIS_POVIndex = POV_MASK[LinuxPovNumber];
+                                //Handle X Axis first (Even) (left right)
+                                if((LinuxPovNumber & 0x0001) == 0)
+                                break;
+                        }
+                        case EV_ABS:  //Absolute Axis
+                        {
+                                //A Stick (BrakeDefine is the highest possible Axis)
+                                if( js[i].code <= ABS_BRAKE )
+                                {
+                                        //Why do this? Because, we use a bit field, and when this axis is east,
+                                        //it can't possibly be west too. So clear out the two X axes, then refil
+                                        //it in with the new direction bit.
+                                        //Clear the East/West Bit Flags first
+                                        mState.mPOV[OIS_POVIndex].direction &= 0x11110011;
+                                        if( js[i].value == -1 ) //Left
+                                                mState.mPOV[OIS_POVIndex].direction |= Pov::West;
+                                        else if( js[i].value == 1 ) //Right
+                                                mState.mPOV[OIS_POVIndex].direction |= Pov::East;
+                                        int axis = mAxisMap[js[i].code];
+                                        assert( axis < 32 && "Too many axes (Max supported is 32). Report this to OIS forums!" );
+                                        axisMoved[axis] = true;
+                                        //check for rescaling:
+                                        if( mRanges[axis].min == JoyStick::MIN_AXIS && mRanges[axis].max != JoyStick::MAX_AXIS )
+                                        {       //Scale is perfect
+                                                mState.mAxes[axis].abs = js[i].value;
+                                        }
+                                        else
+                                        {       //Rescale
+                                                float proportion = (float)(js[i].value-mRanges[axis].max)/(float)(mRanges[axis].min-mRanges[axis].max);
+                                                mState.mAxes[axis].abs = (int)(32767.0f - (65535.0f * proportion));
+                                        }
+                                }
+                                //Handle Y Axis (Odd) (up down)
+                                else
+                                else if( js[i].code <= ABS_HAT3Y ) //A POV - Max four POVs allowed
+                                {
+                                        //Clear the North/South Bit Flags first
+                                        mState.mPOV[OIS_POVIndex].direction &= 0x11111100;
+                                        if( js[i].value == -1 ) //Up
+                                                mState.mPOV[OIS_POVIndex].direction |= Pov::North;
+                                        else if( js[i].value == 1 ) //Down
+                                                mState.mPOV[OIS_POVIndex].direction |= Pov::South;
+                                        //Normalise the POV to between 0-7
+                                        //Even is X Axis, Odd is Y Axis
+                                        unsigned char LinuxPovNumber = js[i].code - 16;
+                                        short OIS_POVIndex = POV_MASK[LinuxPovNumber];
+                                        //Handle X Axis first (Even) (left right)
+                                        if((LinuxPovNumber & 0x0001) == 0)
+                                        {
+                                                //Why do this? Because, we use a bit field, and when this axis is east,
+                                                //it can't possibly be west too. So clear out the two X axes, then refil
+                                                //it in with the new direction bit.
+                                                //Clear the East/West Bit Flags first
+                                                mState.mPOV[OIS_POVIndex].direction &= 0x11110011;
+                                                if( js[i].value == -1 ) //Left
+                                                        mState.mPOV[OIS_POVIndex].direction |= Pov::West;
+                                                else if( js[i].value == 1 ) //Right
+                                                        mState.mPOV[OIS_POVIndex].direction |= Pov::East;
+                                        }
+                                        //Handle Y Axis (Odd) (up down)
+                                        else
+                                        {
+                                                //Clear the North/South Bit Flags first
+                                                mState.mPOV[OIS_POVIndex].direction &= 0x11111100;
+                                                if( js[i].value == -1 ) //Up
+                                                        mState.mPOV[OIS_POVIndex].direction |= Pov::North;
+                                                else if( js[i].value == 1 ) //Down
+                                                        mState.mPOV[OIS_POVIndex].direction |= Pov::South;
+                                        }
+                                        if( mBuffered && mListener )
+                                                if( mListener->povMoved( JoyStickEvent(this,mState), OIS_POVIndex) == false )
+                                                        return;
+                                }
+                                if( mBuffered && mListener )
+                                        if( mListener->povMoved( JoyStickEvent(this,mState), OIS_POVIndex) == false )
+                                                return;
+                        }
+                        break;
+                }
+                case EV_REL: //Relative Axes (Do any joystick actually have a relative axis?)
+#ifdef OIS_LINUX_JOY_DEBUG
+                    cout << "\nWarning: Relatives axes not supported yet" << endl;
+#endif
+                        break;
+                default: break;
+                                break;
+                        }
+                        case EV_REL: //Relative Axes (Do any joystick actually have a relative axis?)
+        #ifdef OIS_LINUX_JOY_DEBUG
+                                cout << "\nWarning: Relatives axes not supported yet" << endl;
+        #endif
+                                break;
+                        default: break;
+                        }
+                }
+        }

code/branches/kicklib2/src/external/ois/linux/LinuxKeyboard.cpp

-                      r5781
+                      r8284
 #include <iostream>
 //-------------------------------------------------------------------//
 LinuxKeyboard::LinuxKeyboard(InputManager* creator, bool buffered, bool grab, bool useXRepeat)
+LinuxKeyboard::LinuxKeyboard(InputManager* creator, bool buffered, bool grab)
         : Keyboard(creator->inputSystemName(), buffered, 0, creator)
+{
 …
         grabKeyboard = grab;
         keyFocusLost = false;
-        xAutoRepeat = useXRepeat;
-        oldXAutoRepeat = false;
         //X Key Map to KeyCode
 …
         keyFocusLost = false;
-        if( xAutoRepeat == false )
+        {
-                //We do not want to blindly turn on autorepeat later when quiting if
-                //it was not on to begin with.. So, let us check and see first
-                XKeyboardState old;
-                XGetKeyboardControl( display, &old );
-                oldXAutoRepeat = false;
-                if( old.global_auto_repeat == AutoRepeatModeOn )
-                        oldXAutoRepeat = true;
-                XAutoRepeatOff( display );
+        }
+}
 …
         if( display )
+        {
-                if( oldXAutoRepeat )
-                        XAutoRepeatOn(display);
                 if( grabKeyboard )
                         XUngrabKeyboard(display, CurrentTime);
 …
         while( XPending(display) > 0 )
+        {               XNextEvent(display, &event);            if( KeyPress == event.type )
+        {
+                XNextEvent(display, &event);
+                if(KeyPress == event.type)
+                {
                         unsigned int character = 0;
                         if( mTextMode != Off )
+                        if(mTextMode != Off)
+                        {
                                 unsigned char buffer[6] = {0,0,0,0,0,0};
 …
                         if( event.xkey.state & Mod1Mask && key == XK_Tab )
                                 linMan->_setGrabState(false);
+                }               else if( KeyRelease == event.type )
+                }
+                else if(KeyRelease == event.type)
+                {
+                        //Mask out the modifier states X sets.. or we will get improper values
+                        event.xkey.state &= ~ShiftMask;
+                        event.xkey.state &= ~LockMask;
+                        //Else, it is a valid key release
+                        XLookupString(&event.xkey,NULL,0,&key,NULL);
+                        _injectKeyUp(key);              }
+                        if(!_isKeyRepeat(event))
+                        {
+                                //Mask out the modifier states X sets.. or we will get improper values
+                                event.xkey.state &= ~ShiftMask;
+                                event.xkey.state &= ~LockMask;
+                                XLookupString(&event.xkey,NULL,0,&key,NULL);
+                                _injectKeyUp(key);
+                        }
+                }
+        }

code/branches/kicklib2/src/external/ois/linux/LinuxKeyboard.h

-                      r5781
+                      r8284
+        {
         public:
                 LinuxKeyboard(InputManager* creator, bool buffered, bool grab, bool useXRepeat );
+                LinuxKeyboard(InputManager* creator, bool buffered, bool grab);
                 virtual ~LinuxKeyboard();
 …
         protected:
+                inline bool _isKeyRepeat(XEvent &event)
+                {
+                        //When a key is repeated, there will be two events: released, followed by another immediate pressed. So check to see if another pressed is present
+                        if(!XPending(display))
+                                return false;
+                        XEvent e;
+                        XPeekEvent(display, &e);
+                        if(e.type == KeyPress && e.xkey.keycode == event.xkey.keycode && (e.xkey.time - event.xkey.time) < 2)
+                        {
+                                XNextEvent(display, &e);
+                                return true;
+                        }
+                        return false;
+                }
                 bool _injectKeyDown( KeySym key, int text );
                 bool _injectKeyUp( KeySym key );
 …
                 bool keyFocusLost;
-                bool xAutoRepeat;
-                bool oldXAutoRepeat;
                 std::string mGetString;
         };

code/branches/kicklib2/src/external/ois/linux/LinuxMouse.cpp

-                      r5781
+                      r8284
                         //Compute this frames Relative X & Y motion
                         mState.X.rel = event.xmotion.x - oldXMouseX;
                         mState.Y.rel = event.xmotion.y - oldXMouseY;
+                        int dx = event.xmotion.x - oldXMouseX;
+                        int dy = event.xmotion.y - oldXMouseY;
                         //Store old values for next time to compute relative motion
 …
                         oldXMouseY = event.xmotion.y;
+                        mState.X.abs += mState.X.rel;
+                        mState.Y.abs += mState.Y.rel;
+                        mState.X.abs += dx;
+                        mState.Y.abs += dy;
+                        mState.X.rel += dx;
+                        mState.Y.rel += dy;
                         //Check to see if we are grabbing the mouse to the window (requires clipping and warping)

code/branches/kicklib2/src/external/ois/linux/LinuxPrereqs.h

r5781	r8284
32	32
33	33	//! Max number of elements to collect from buffered input
34		#define JOY_BUFFERSIZE 10
	34	#define JOY_BUFFERSIZE 64
35	35
36	36	namespace OIS

code/branches/kicklib2/src/external/ois/mac/CMakeLists.txt

-                      r5929
+                      r8284
   MacHIDManager.h
   MacInputManager.h
+  MacJoyStick.h
   MacKeyboard.h
   MacMouse.h
   MacPrereqs.h
 COMPILATION_BEGIN OISMacCompilation.cpp
+#COMPILATION_BEGIN OISMacCompilation.cpp
   MacHelpers.cpp
   MacHIDManager.cpp
   MacInputManager.cpp
+  MacJoyStick.cpp
   MacKeyboard.cpp
   MacMouse.cpp
 COMPILATION_END
+#COMPILATION_END
+)

code/branches/kicklib2/src/external/ois/mac/MacHIDManager.cpp

-                      r5781
+                      r8284
 . This notice may not be removed or altered from any source distribution.
 */
+ */
 #include "mac/MacHIDManager.h"
+#include "mac/MacJoyStick.h"
 #include "OISException.h"
 #include "OISObject.h"
 …
+{
         CFTypeRef temp = CFDictionaryGetValue(dict, OIS_CFString(keyName));
         if(temp && CFGetTypeID(temp) == CFArrayGetTypeID())
                 return (CFArrayRef)temp;
 …
+{
         CFTypeRef temp = CFDictionaryGetValue(dict, OIS_CFString(keyName));
         if(temp && CFGetTypeID(temp) == CFStringGetTypeID())
                 return (CFStringRef)temp;
 …
+{
         CFTypeRef temp = CFDictionaryGetValue(dict, OIS_CFString(keyName));
         if(temp && CFGetTypeID(temp) == CFNumberGetTypeID())
                 return (CFNumberRef)temp;
 …
+{
         CFTypeRef temp = CFArrayGetValueAtIndex(array, idx);
         if(temp && CFGetTypeID(temp) == CFDictionaryGetTypeID())
                 return (CFDictionaryRef)temp;
 …
 int getInt32(CFNumberRef ref)
+{
    int r = 0;
    if (r)
       CFNumberGetValue(ref, kCFNumberIntType, &r);
    return r;
+        int r = 0;
+        if (r)
+                CFNumberGetValue(ref, kCFNumberIntType, &r);
+        return r;
+}
 …
 //------------------------------------------------------------------------------------------------------//
 void MacHIDManager::initialize()
+{
+        //Make the search more specific by adding usage flags
+        int usage = kHIDUsage_GD_Joystick;
+        int page = kHIDPage_GenericDesktop;
+        io_iterator_t iterator = lookUpDevices(usage, page);
+        if(iterator)
+                iterateAndOpenDevices(iterator);
+        //Doesn't support multiple usage flags, iterate twice
+        usage = kHIDUsage_GD_GamePad;
+        iterator = lookUpDevices(usage, page);
+        if(iterator)
+                iterateAndOpenDevices(iterator);
+}
+//------------------------------------------------------------------------------------------------------//
+io_iterator_t MacHIDManager::lookUpDevices(int usage, int page)
+{
         CFMutableDictionaryRef deviceLookupMap = IOServiceMatching(kIOHIDDeviceKey);
         if(!deviceLookupMap)
                 OIS_EXCEPT(E_General, "Could not setup HID device search parameters");
+        //Make the search more specific by adding usage flags
+        int usage = kHIDUsage_GD_GamePad | kHIDUsage_GD_Joystick,
+            page  = kHIDPage_GenericDesktop;
+        CFNumberRef usageRef = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &usage),
+                                pageRef  = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &page);
+        CFNumberRef usageRef = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &usage);
+        CFNumberRef pageRef  = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &page);
         CFDictionarySetValue(deviceLookupMap, CFSTR(kIOHIDPrimaryUsageKey), usageRef);
         CFDictionarySetValue(deviceLookupMap, CFSTR(kIOHIDPrimaryUsagePageKey), pageRef);
         //IOServiceGetMatchingServices consumes the map so we do not have to release it ourself
         io_iterator_t iterator = 0;
         IOReturn result = IOServiceGetMatchingServices(kIOMasterPortDefault, deviceLookupMap, &iterator);
+        if (result == kIOReturnSuccess && iterator)
+        {
+                io_object_t hidDevice = 0;
+                while ((hidDevice = IOIteratorNext(iterator)) !=0)
+        CFRelease(usageRef);
+        CFRelease(pageRef);
+        if(result == kIOReturnSuccess)
+        {
+                return iterator;
+        }
+        //TODO: Throw exception instead?
+        else
+        {
+                return 0;
+        }
+}
+//------------------------------------------------------------------------------------------------------//
+void MacHIDManager::iterateAndOpenDevices(io_iterator_t iterator)
+{
+        io_object_t hidDevice = 0;
+        while ((hidDevice = IOIteratorNext(iterator)) !=0)
+        {
+                //Get the current registry items property map
+                CFMutableDictionaryRef propertyMap = 0;
+                if (IORegistryEntryCreateCFProperties(hidDevice, &propertyMap, kCFAllocatorDefault, kNilOptions) == KERN_SUCCESS && propertyMap)
+                {
+                        //Get the current registry items property map
+                        CFMutableDictionaryRef propertyMap = 0;
+                        if (IORegistryEntryCreateCFProperties(hidDevice, &propertyMap, kCFAllocatorDefault, kNilOptions) == KERN_SUCCESS && propertyMap)
+                        //Go through device to find all needed info
+                        HidInfo* hid = enumerateDeviceProperties(propertyMap);
+                        if(hid)
+                        {
-                                //Go through device to find all needed info
-                                HidInfo* hid = enumerateDeviceProperties(propertyMap);
-                                if(hid)
-                                        mDeviceList.push_back(hid);
                                 //todo - we need to hold an open interface so we do not have to enumerate again later
                                 //should be able to watch for device removals also
+                                /// Testing opening / closing interface
+                                //IOCFPlugInInterface **pluginInterface = NULL;
+                                //SInt32 score = 0;
+                                //if (IOCreatePlugInInterfaceForService(hidDevice, kIOHIDDeviceUserClientTypeID, kIOCFPlugInInterfaceID, &pluginInterface, &score) == kIOReturnSuccess)
+                                //{
+                                //      IOHIDDeviceInterface **interface;
+                                //      HRESULT pluginResult = (*pluginInterface)->QueryInterface(pluginInterface, CFUUIDGetUUIDBytes(kIOHIDDeviceInterfaceID), (void **)&(interface));
+                                //      if(pluginResult == S_OK)
+                                //              cout << "Successfully created plugin interface for device\n";
+                                //      else
+                                //              cout << "Not able to create plugin interface\n";
+                                //      IODestroyPlugInInterface(pluginInterface);
+                                //      if ((*interface)->open(interface, 0) == KERN_SUCCESS)
+                                //              cout << "Opened interface.\n";
+                                //      else
+                                //              cout << "Failed to open\n";
+                                //      (*interface)->close(interface);
+                                //}
+                                //
+                                // Testing opening / closing interface
+                                IOCFPlugInInterface **pluginInterface = NULL;
+                                SInt32 score = 0;
+                                if (IOCreatePlugInInterfaceForService(hidDevice, kIOHIDDeviceUserClientTypeID, kIOCFPlugInInterfaceID, &pluginInterface, &score) == kIOReturnSuccess)
+                                {
+                                        IOHIDDeviceInterface **interface;
+                                        HRESULT pluginResult = (*pluginInterface)->QueryInterface(pluginInterface, CFUUIDGetUUIDBytes(kIOHIDDeviceInterfaceID), (void **)&(interface));
+                                        if(pluginResult != S_OK)
+                                                OIS_EXCEPT(E_General, "Not able to create plugin interface");
+                                        IODestroyPlugInInterface(pluginInterface);
+                                        hid->interface = interface;
+                                        //Check for duplicates - some devices have multiple usage
+                                        if(std::find(mDeviceList.begin(), mDeviceList.end(), hid) == mDeviceList.end())
+                                                mDeviceList.push_back(hid);
+                                }
+                        }
+                }
+                IOObjectRelease(iterator);
+        }
+        CFRelease(usageRef);
+        CFRelease(pageRef);
+        }
+        IOObjectRelease(iterator);
+}
 …
         if (str)
                 info->vendor = CFStringGetCStringPtr(str, CFStringGetSystemEncoding());
         str = getDictionaryItemAsRef<CFStringRef>(propertyMap, kIOHIDProductKey);
         if (str)
                 info->productKey = CFStringGetCStringPtr(str, CFStringGetSystemEncoding());
         info->combinedKey = info->vendor + " " + info->productKey;
         //Go through all items in this device (i.e. buttons, hats, sticks, axes, etc)
         CFArrayRef array = getDictionaryItemAsRef<CFArrayRef>(propertyMap, kIOHIDElementKey);
 …
                 for (int i = 0; i < CFArrayGetCount(array); i++)
                         parseDeviceProperties(getArrayItemAsRef<CFDictionaryRef>(array, i));
         return info;
+}
 …
         if(!properties)
                 return;
         CFArrayRef array = getDictionaryItemAsRef<CFArrayRef>(properties, kIOHIDElementKey);
         if (array)
 …
                                         switch(getInt32(getDictionaryItemAsRef<CFNumberRef>(element, kIOHIDElementUsagePageKey)))
+                                        {
                                         case kHIDPage_GenericDesktop:
                                                 switch(getInt32(getDictionaryItemAsRef<CFNumberRef>(element, kIOHIDElementUsageKey)))
+                                                case kHIDPage_GenericDesktop:
+                                                        switch(getInt32(getDictionaryItemAsRef<CFNumberRef>(element, kIOHIDElementUsageKey)))
+                                                {
+                                                case kHIDUsage_GD_Pointer:
+                                                        cout << "\tkHIDUsage_GD_Pointer\n";
+                                                        parseDevicePropertiesGroup(element);
+                                                        case kHIDUsage_GD_Pointer:
+                                                                cout << "\tkHIDUsage_GD_Pointer\n";
+                                                                parseDevicePropertiesGroup(element);
+                                                                break;
+                                                        case kHIDUsage_GD_X:
+                                                        case kHIDUsage_GD_Y:
+                                                        case kHIDUsage_GD_Z:
+                                                        case kHIDUsage_GD_Rx:
+                                                        case kHIDUsage_GD_Ry:
+                                                        case kHIDUsage_GD_Rz:
+                                                                cout << "\tAxis\n";
+                                                                break;
+                                                        case kHIDUsage_GD_Slider:
+                                                        case kHIDUsage_GD_Dial:
+                                                        case kHIDUsage_GD_Wheel:
+                                                                cout << "\tUnsupported kHIDUsage_GD_Wheel\n";
+                                                                break;
+                                                        case kHIDUsage_GD_Hatswitch:
+                                                                cout << "\tUnsupported - kHIDUsage_GD_Hatswitch\n";
+                                                                break;
+                                                }
                                                         break;
+                                                case kHIDUsage_GD_X:
+                                                case kHIDUsage_GD_Y:
+                                                case kHIDUsage_GD_Z:
+                                                case kHIDUsage_GD_Rx:
+                                                case kHIDUsage_GD_Ry:
+                                                case kHIDUsage_GD_Rz:
+                                                        cout << "\tAxis\n";
+                                                case kHIDPage_Button:
+                                                        cout << "\tkHIDPage_Button\n";
                                                         break;
-                                                case kHIDUsage_GD_Slider:
-                                                case kHIDUsage_GD_Dial:
-                                                case kHIDUsage_GD_Wheel:
-                                                        cout << "\tUnsupported kHIDUsage_GD_Wheel\n";
-                                                        break;
-                                                case kHIDUsage_GD_Hatswitch:
-                                                        cout << "\tUnsupported - kHIDUsage_GD_Hatswitch\n";
-                                                        break;
+                                                }
-                                                break;
-                                        case kHIDPage_Button:
-                                                cout << "\tkHIDPage_Button\n";
-                                                break;
+                                        }
+                                }
 …
         if(!properties)
                 return;
         CFArrayRef array = getDictionaryItemAsRef<CFArrayRef>(properties, kIOHIDElementKey);
         if(array)
 …
                                 switch(getInt32(getDictionaryItemAsRef<CFNumberRef>(element, kIOHIDElementUsagePageKey)))
+                                {
                                 case kHIDPage_GenericDesktop:
                                         switch(getInt32(getDictionaryItemAsRef<CFNumberRef>(element, kIOHIDElementUsageKey)))
+                                        case kHIDPage_GenericDesktop:
+                                                switch(getInt32(getDictionaryItemAsRef<CFNumberRef>(element, kIOHIDElementUsageKey)))
+                                        {
+                                        case kHIDUsage_GD_X:
+                                        case kHIDUsage_GD_Y:
+                                        case kHIDUsage_GD_Z:
+                                        case kHIDUsage_GD_Rx:
+                                        case kHIDUsage_GD_Ry:
+                                        case kHIDUsage_GD_Rz:
+                                                cout << "\t\tAxis\n";
+                                                case kHIDUsage_GD_X:
+                                                case kHIDUsage_GD_Y:
+                                                case kHIDUsage_GD_Z:
+                                                case kHIDUsage_GD_Rx:
+                                                case kHIDUsage_GD_Ry:
+                                                case kHIDUsage_GD_Rz:
+                                                        cout << "\t\tAxis\n";
+                                                        break;
+                                                case kHIDUsage_GD_Slider:
+                                                case kHIDUsage_GD_Dial:
+                                                case kHIDUsage_GD_Wheel:
+                                                        cout << "\tUnsupported - kHIDUsage_GD_Wheel\n";
+                                                        break;
+                                                case kHIDUsage_GD_Hatswitch:
+                                                        cout << "\tUnsupported - kHIDUsage_GD_Hatswitch\n";
+                                                        break;
+                                        }
                                                 break;
+                                        case kHIDUsage_GD_Slider:
+                                        case kHIDUsage_GD_Dial:
+                                        case kHIDUsage_GD_Wheel:
+                                                cout << "\tUnsupported - kHIDUsage_GD_Wheel\n";
+                                        case kHIDPage_Button:
                                                 break;
-                                        case kHIDUsage_GD_Hatswitch:
-                                                cout << "\tUnsupported - kHIDUsage_GD_Hatswitch\n";
-                                                break;
+                                        }
-                                        break;
-                                case kHIDPage_Button:
-                                        break;
+                                }
+                        }
 …
                         ret.insert(std::make_pair((*it)->type, (*it)->combinedKey));
+        }
         return ret;
+}
 …
         int ret = 0;
         HidInfoList::iterator it = mDeviceList.begin(), end = mDeviceList.end();
         for(; it != end; ++it)
+        {
 …
         int ret = 0;
         HidInfoList::iterator it = mDeviceList.begin(), end = mDeviceList.end();
         for(; it != end; ++it)
+        {
 …
                         ret++;
+        }
         return ret;
+}
 …
+{
         HidInfoList::iterator it = mDeviceList.begin(), end = mDeviceList.end();
         for(; it != end; ++it)
+        {
 …
 //--------------------------------------------------------------------------------//
 Object* MacHIDManager::createObject(InputManager* creator, Type iType, bool bufferMode,
                                                                           const std::string & vendor)
+                                                                        const std::string & vendor)
+{
         Object *obj = 0;
         HidInfoList::iterator it = mDeviceList.begin(), end = mDeviceList.end();
         for(; it != end; ++it)
 …
                 if((*it)->inUse == false && (*it)->type == iType && (vendor == "" || (*it)->combinedKey == vendor))
+                {
+                        //create device
+                        switch(iType)
+                        {
+                                case OISJoyStick:
+                {
+                                        int totalDevs = totalDevices(iType);
+                                        int freeDevs = freeDevices(iType);
+                                        int devID = totalDevs - freeDevs;
+                                        obj = new MacJoyStick((*it)->combinedKey, bufferMode, *it, creator, devID);
+                                        (*it)->inUse = true;
+                                        return obj;
+                }
+                                case OISTablet:
+                                        //Create MacTablet
+                                        break;
+                                default:
+                                        break;
+                        }
+                }
+        }
+        if( obj == 0 )
+                OIS_EXCEPT(E_InputDeviceNonExistant, "No devices match requested type.");
         return obj;
+}

code/branches/kicklib2/src/external/ois/mac/MacHIDManager.h

-                      r5781
+                      r8284
                 void initialize();
+                void iterateAndOpenDevices(io_iterator_t iterator);
+                io_iterator_t lookUpDevices(int usage, int page);
                 //FactoryCreator Overrides

code/branches/kicklib2/src/external/ois/mac/MacInputManager.cpp

r5781	r8284
188	188	}
189	189	default:
	190	{
	191	obj = mHIDManager->createObject(creator, iType, bufferMode, vendor);

Context Navigation

Legend: