diff options
189 files changed, 13650 insertions, 7979 deletions
diff --git a/.gitignore b/.gitignore index 6d65a4f58..636ce8302 100644 --- a/.gitignore +++ b/.gitignore @@ -33,3 +33,6 @@ CTestTestFile.cmake # Apple Finder metadata *.DS_Store + +# vim temp files +*.swp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ad1aa1aa..e432f929b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG") #MESSAGE("CMAKE_CXX_FLAGS_DEBUG="+${CMAKE_CXX_FLAGS_DEBUG}) OPTION(USE_DOUBLE_PRECISION "Use double precision" OFF) +SET(CLAMP_VELOCITIES "0" CACHE STRING "Clamp rigid bodies' velocity to this value, if larger than zero. Useful to prevent floating point errors or in general runaway velocities in complex scenarios") OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON) OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF) OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON) @@ -62,6 +63,7 @@ IF (CMAKE_SYSTEM_NAME STREQUAL WindowsPhone OR CMAKE_SYSTEM_NAME STREQUAL Window ELSE () OPTION(USE_MSVC_RUNTIME_LIBRARY_DLL "Use MSVC Runtime Library DLL (/MD or /MDd)" OFF) ENDIF (CMAKE_SYSTEM_NAME STREQUAL WindowsPhone OR CMAKE_SYSTEM_NAME STREQUAL WindowsStore) +OPTION(USE_MSVC_RELEASE_RUNTIME_ALWAYS "Use MSVC Release Runtime Library even in Debug" OFF) #SET(CMAKE_EXE_LINKER_FLAGS_INIT "/STACK:10000000 /INCREMENTAL:NO") #SET(CMAKE_EXE_LINKER_FLAGS "/STACK:10000000 /INCREMENTAL:NO") @@ -104,6 +106,21 @@ IF(MSVC) ENDFOREACH(flag_var) ENDIF (NOT USE_MSVC_RUNTIME_LIBRARY_DLL) + IF (USE_MSVC_RELEASE_RUNTIME_ALWAYS) + FOREACH(flag_var CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO ) + IF(${flag_var} MATCHES "/MDd") + STRING(REGEX REPLACE "/MDd" "/MD" ${flag_var} "${${flag_var}}") + ENDIF(${flag_var} MATCHES "/MDd") + IF(${flag_var} MATCHES "/MTd") + STRING(REGEX REPLACE "/MTd" "/MT" ${flag_var} "${${flag_var}}") + ENDIF(${flag_var} MATCHES "/MTd") + # Need to remove _DEBUG too otherwise things like _ITERATOR_DEBUG_LEVEL mismatch + IF(${flag_var} MATCHES "-D_DEBUG") + STRING(REGEX REPLACE "-D_DEBUG" "" ${flag_var} "${${flag_var}}") + ENDIF(${flag_var} MATCHES "-D_DEBUG") + ENDFOREACH(flag_var) + ENDIF (USE_MSVC_RELEASE_RUNTIME_ALWAYS) + IF (CMAKE_CL_64) ADD_DEFINITIONS(-D_WIN64) ELSE() @@ -211,6 +228,10 @@ IF (INTERNAL_UPDATE_SERIALIZATION_STRUCTURES) ADD_DEFINITIONS( -DBT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES) ENDIF (INTERNAL_UPDATE_SERIALIZATION_STRUCTURES) +IF (CLAMP_VELOCITIES) +ADD_DEFINITIONS( -DBT_CLAMP_VELOCITY_TO=${CLAMP_VELOCITIES}) +ENDIF (CLAMP_VELOCITIES) + IF (USE_DOUBLE_PRECISION) ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION) SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION") @@ -465,6 +486,7 @@ IF (BUILD_UNIT_TESTS) ENDIF() set (BULLET_CONFIG_CMAKE_PATH lib${LIB_SUFFIX}/cmake/bullet ) +list (APPEND BULLET_DEFINITIONS ${BULLET_DOUBLE_DEF}) list (APPEND BULLET_LIBRARIES LinearMath) list (APPEND BULLET_LIBRARIES Bullet3Common) list (APPEND BULLET_LIBRARIES BulletInverseDynamics) diff --git a/build3/findOpenGLGlewGlut.lua b/build3/findOpenGLGlewGlut.lua index 5d53a25d9..0a3eb4e59 100644 --- a/build3/findOpenGLGlewGlut.lua +++ b/build3/findOpenGLGlewGlut.lua @@ -43,6 +43,37 @@ configuration{} end + function initX11() + if os.is("Linux") then + if _OPTIONS["enable_system_x11"] and (os.isdir("/usr/include") and os.isfile("/usr/include/X11/X.h")) then + links{"X11","pthread"} + else + print("No X11/X.h found, using dynamic loading of X11") + includedirs { + projectRootDir .. "examples/ThirdPartyLibs/optionalX11" + } + defines {"DYNAMIC_LOAD_X11_FUNCTIONS"} + links {"dl","pthread"} + end + end + end + + + function initX11() + if os.is("Linux") then + if _OPTIONS["enable_system_x11"] and (os.isdir("/usr/include") and os.isfile("/usr/include/X11/X.h")) then + links{"X11","pthread"} + else + print("No X11/X.h found, using dynamic loading of X11") + includedirs { + projectRootDir .. "examples/ThirdPartyLibs/optionalX11" + } + defines {"DYNAMIC_LOAD_X11_FUNCTIONS"} + links {"dl","pthread"} + end + end + end + function initGlew() configuration {} @@ -63,8 +94,9 @@ if os.is("Linux") then configuration{"Linux"} + initX11() if _OPTIONS["enable_system_glx"] then --# and (os.isdir("/usr/include") and os.isfile("/usr/include/GL/glx.h")) then - links{"X11","pthread"} + links{"pthread"} print("Using system GL/glx.h") else print("Using glad_glx") @@ -86,18 +118,4 @@ configuration{} end - function initX11() - if os.is("Linux") then - if _OPTIONS["enable_system_x11"] and (os.isdir("/usr/include") and os.isfile("/usr/include/X11/X.h")) then - links{"X11","pthread"} - else - print("No X11/X.h found, using dynamic loading of X11") - includedirs { - projectRootDir .. "examples/ThirdPartyLibs/optionalX11" - } - defines {"DYNAMIC_LOAD_X11_FUNCTIONS"} - links {"dl","pthread"} - end - end - end diff --git a/build3/premake4.lua b/build3/premake4.lua index e1167b0aa..8a6de1938 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -337,6 +337,12 @@ end trigger = "double", description = "Double precision version of Bullet" } + + newoption + { + trigger = "clamp-velocities", + description = "Limit maximum velocities to reduce FP exception risk" + } newoption { @@ -360,13 +366,28 @@ end if _OPTIONS["double"] then defines {"BT_USE_DOUBLE_PRECISION"} end + if _OPTIONS["clamp-velocities"] then + defines {"BT_CLAMP_VELOCITY_TO=9999"} + end + newoption + { + trigger = "dynamic-runtime", + description = "Enable dynamic DLL CRT runtime" + } configurations {"Release", "Debug"} configuration "Release" - flags { "Optimize", "EnableSSE2","StaticRuntime", "NoMinimalRebuild", "FloatFast"} + flags { "Optimize", "EnableSSE2", "NoMinimalRebuild", "FloatFast"} + if not _OPTIONS["dynamic-runtime"] then + flags { "StaticRuntime" } + end configuration "Debug" defines {"_DEBUG=1"} - flags { "Symbols", "StaticRuntime" , "NoMinimalRebuild", "NoEditAndContinue" ,"FloatFast"} + flags { "Symbols" , "NoMinimalRebuild", "NoEditAndContinue" ,"FloatFast"} + if not _OPTIONS["dynamic-runtime"] then + flags { "StaticRuntime" } + end + if os.is("Linux") or os.is("macosx") then if os.is64bit() then diff --git a/build_visual_studio_vr_pybullet_double_cmake.bat b/build_visual_studio_vr_pybullet_double_cmake.bat index 72158083c..32cd01735 100644 --- a/build_visual_studio_vr_pybullet_double_cmake.bat +++ b/build_visual_studio_vr_pybullet_double_cmake.bat @@ -1,4 +1,4 @@ mkdir build_cmake cd build_cmake -cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.3.amd64\include -DPYTHON_LIBRARY=c:\python-3.5.3.amd64\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.3.amd64\libs\python35_d.lib -G "Visual Studio 14 2015 Win64" .. +cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.3.amd64\include -DPYTHON_LIBRARY=c:\python-3.5.3.amd64\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.3.amd64\libs\python35_d.lib -G "Visual Studio 16 2019" .. start .
\ No newline at end of file diff --git a/build_visual_studio_vr_pybullet_double_dynamic.bat b/build_visual_studio_vr_pybullet_double_dynamic.bat new file mode 100644 index 000000000..034a46034 --- /dev/null +++ b/build_visual_studio_vr_pybullet_double_dynamic.bat @@ -0,0 +1,28 @@ +IF NOT EXIST bin mkdir bin +IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin +IF NOT EXIST bin\openvr64pi.dll copy examples\ThirdPartyLibs\openvr\bin\win64\openvr_api.dll bin\openvr64pi.dll + +#aargh, see https://github.com/ValveSoftware/openvr/issues/412 + + +#find a python version (hopefully just 1) and use this +dir c:\python* /b /ad > tmp1234.txt + +set /p myvar1= < tmp1234.txt +set myvar=c:/%myvar1% +del tmp1234.txt + +rem you can also override and hardcode the Python path like this (just remove the # hashmark in next line) +rem SET myvar=c:\python-3.5.2 + +cd build3 + + +premake4 --dynamic-runtime --double --standalone-examples --enable_stable_pd --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010 + +rem premake4 --double --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../binserver" vs2010 +rem premake4 --double --enable_grpc --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../binserver" vs2010 +rem premake4 --serial --audio --double --midi --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010 + +start vs2010 + diff --git a/data/Base_1.urdf b/data/Base_1.urdf new file mode 100644 index 000000000..e7b251a1d --- /dev/null +++ b/data/Base_1.urdf @@ -0,0 +1,65 @@ +<?xml version="1.0"?> +<!--https://valerolab.org/--> +<!-- +Code used for PID control of an inverted pendulum actuated by strings. +--> +<robot name="myfirst"> + <material name="blue"> + <color rgba="0 0 0.8 1"/> + </material> + <material name="white"> + <color rgba="1 1 1 1"/> + </material> + <material name="other"> + <color rgba="1 0 0.8 1"/> + </material> + <velocity name="vel"> + <speed spd="50"/> + </velocity> + + + <link name="Base"> + <visual> + <geometry> + <box size="0.3 0.3 1.5"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <box size="0.3 0.3 1.5"/> + </geometry> + </collision> + <inertial> + <mass value="100"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + + <link name="pulley1"> + <visual> + <geometry> + <cylinder length=".1" radius=".5"/> + </geometry> + <material name="blue"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".5"/> + </geometry> + </collision> + <inertial> + <mass value="2"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="Base_pulley1" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="Base"/> + <child link="pulley1"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="-1.57075 0 0" xyz="0 .35 .15"/> + </joint> +<!----> +</robot>
\ No newline at end of file diff --git a/data/Base_2.urdf b/data/Base_2.urdf new file mode 100644 index 000000000..44ff0d44d --- /dev/null +++ b/data/Base_2.urdf @@ -0,0 +1,415 @@ +<?xml version="1.0"?> +<!--https://valerolab.org/--> +<!-- +Code used for PID control of an inverted pendulum actuated by strings. +--> +<robot name="myfirst"> + <material name="blue"> + <color rgba="0 0 0.8 1"/> + </material> + <material name="white"> + <color rgba="1 1 1 1"/> + </material> + <material name="other"> + <color rgba="1 0 0.8 1"/> + </material> + <velocity name="vel"> + <speed spd="50"/> + </velocity> + + + <link name="Base"> + <visual> + <geometry> + <box size="0.3 0.3 1.5"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <box size="0.3 0.3 1.5"/> + </geometry> + </collision> + <inertial> + <mass value="100"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + + <link name="pulley1"> + <visual> + <geometry> + <cylinder length=".1" radius=".5"/> + </geometry> + <material name="blue"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".5"/> + </geometry> + </collision> + <inertial> + <mass value="2"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="Base_pulley1" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="Base"/> + <child link="pulley1"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="-1.57075 1.57075 0" xyz="0 .35 .15"/> + </joint> + + <link name="tendon1_1"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="blue"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="pulley1_tendon1_1" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="pulley1"/> + <child link="tendon1_1"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 3.1416" xyz="0 .55 0"/> + </joint> + + <link name="tendon1_2"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_1_tendon1_2" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_1"/> + <child link="tendon1_2"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_3"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_2_tendon1_3" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_2"/> + <child link="tendon1_3"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_4"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_3_tendon1_4" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_3"/> + <child link="tendon1_4"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 1" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_5"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_4_tendon1_5" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_4"/> + <child link="tendon1_5"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 1" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_6"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_5_tendon1_6" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_5"/> + <child link="tendon1_6"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_7"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_6_tendon1_7" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_6"/> + <child link="tendon1_7"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_8"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_7_tendon1_8" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_7"/> + <child link="tendon1_8"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_9"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_8_tendon1_9" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_8"/> + <child link="tendon1_9"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_10"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_9_tendon1_10" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_9"/> + <child link="tendon1_10"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_11"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_10_tendon1_11" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_10"/> + <child link="tendon1_11"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_12"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_11_tendon1_12" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_11"/> + <child link="tendon1_12"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_13"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_12_tendon1_13" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_12"/> + <child link="tendon1_13"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + + <link name="tendon1_14"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_13_tendon1_14" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_13"/> + <child link="tendon1_14"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz=".2 0 0"/> + </joint> + +</robot>
\ No newline at end of file diff --git a/data/Pendulum_Tendon_1_Cart_Rail.urdf b/data/Pendulum_Tendon_1_Cart_Rail.urdf new file mode 100644 index 000000000..a680987a5 --- /dev/null +++ b/data/Pendulum_Tendon_1_Cart_Rail.urdf @@ -0,0 +1,485 @@ +<?xml version="1.0"?> +<!--https://valerolab.org/--> +<!-- +Code used for PID control of an inverted pendulum actuated by strings. +--> +<robot name="myfirst"> + <material name="blue"> + <color rgba="0 0 0.8 1"/> + </material> + <material name="white"> + <color rgba="1 1 1 1"/> + </material> + <material name="other"> + <color rgba="1 0 0.8 1"/> + </material> + <velocity name="vel"> + <speed spd="50"/> + </velocity> + + + <link name="Rail"> + <visual> + <geometry> + <cylinder length="4" radius=".05"/> + </geometry> + <material name="blue"/> + </visual> + <collision> + <geometry> + <cylinder length="4" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="100"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + + <link name="slider"> + <visual> + <geometry> + <box size="0.1 0.6 0.3"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <box size="0.1 0.6 0.3"/> + </geometry> + </collision> + <inertial> + <mass value="100"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + + <joint name="rail_slider" type="prismatic"> + <axis xyz="0 1 0"/> + <parent link="Rail"/> + <child link="slider"/> + <limit effort="0" lower="-2" upper="2" velocity="25"/> + <origin rpy="-1.570796 0 0" xyz="0 0 0"/> + </joint> + + <link name="cart"> + <visual> + <geometry> + <box size="0.1 0.6 0.3"/> + </geometry> + <material name="blue"/> + </visual> + <collision> + <geometry> + <box size="0.1 0.6 0.3"/> + </geometry> + </collision> + <inertial> + <mass value="100"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + + <joint name="slider_cart" type="fixed"> + <parent link="slider"/> + <child link="cart"/> + <origin rpy="0 0 0" xyz="0 0 0.3"/> + </joint> +<!--*****************************************--> + + <link name="pendulumAxis"> + <visual> + <geometry> + <box size="0.1 0.1 .1"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <box size="0.1 0.1 .1"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + + <joint name="cart_pendulumAxis" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="cart"/> + <child link="pendulumAxis"/> + <limit effort="0" lower="1" upper="0" velocity="205"/> + <origin rpy="0 0 0" xyz="0 0 .21"/> + </joint> + + <link name="pendulum"> + <visual> + <geometry> + <box size="0.1 2 .1"/> + </geometry> + <material name="blue"/> + </visual> + <collision> + <geometry> + <box size="0.1 2 .1"/> + </geometry> + </collision> + <inertial> + <mass value="1"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + + <joint name="pendulumAxis_pendulum" type="fixed"> + <parent link="pendulumAxis"/> + <child link="pendulum"/> + <origin rpy="0 0 1.570796" xyz="1 0 0"/> + </joint> + + <!--*****************************************--> + <link name="tendon1_1"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="cart_tendon1_1" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="cart"/> + <child link="tendon1_1"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .55 0"/> + </joint> + + <link name="tendon1_2"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_1_tendon1_2" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_1"/> + <child link="tendon1_2"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_3"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_2_tendon1_3" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_2"/> + <child link="tendon1_3"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_4"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_3_tendon1_4" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_3"/> + <child link="tendon1_4"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_5"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_4_tendon1_5" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_4"/> + <child link="tendon1_5"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_6"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_5_tendon1_6" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_5"/> + <child link="tendon1_6"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_7"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_6_tendon1_7" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_6"/> + <child link="tendon1_7"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_8"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_7_tendon1_8" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_7"/> + <child link="tendon1_8"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_9"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_8_tendon1_9" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_8"/> + <child link="tendon1_9"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_10"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_9_tendon1_10" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_9"/> + <child link="tendon1_10"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_11"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_10_tendon1_11" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_10"/> + <child link="tendon1_11"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_12"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_11_tendon1_12" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_11"/> + <child link="tendon1_12"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_13"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="other"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_12_tendon1_13" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_12"/> + <child link="tendon1_13"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + <link name="tendon1_14"> + <visual> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + <material name="white"/> + </visual> + <collision> + <geometry> + <cylinder length=".1" radius=".05"/> + </geometry> + </collision> + <inertial> + <mass value="10"/> + <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0" iyz="0.0" izz="0"/> + </inertial> + </link> + <joint name="tendon1_13_tendon1_14" type="revolute"> + <axis xyz="0 0 1"/> + <parent link="tendon1_13"/> + <child link="tendon1_14"/> + <limit effort="0" lower="1" upper="0" velocity="50"/> + <origin rpy="0 0 0" xyz="0 .2 0"/> + </joint> + + <!----> +</robot>
\ No newline at end of file diff --git a/data/multibody.bullet b/data/multibody.bullet Binary files differindex f4295c210..018d5eb52 100644 --- a/data/multibody.bullet +++ b/data/multibody.bullet diff --git a/data/torus_deform.urdf b/data/torus_deform.urdf index c5d661993..a3b49dc94 100644 --- a/data/torus_deform.urdf +++ b/data/torus_deform.urdf @@ -2,12 +2,13 @@ <robot name="torus"> <deformable name="torus"> <inertial> - <mass value="1" /> + <mass value="3" /> <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" /> </inertial> <collision_margin value="0.006"/> + <repulsion_stiffness value="800.0"/> <friction value= "0.5"/> - <neohookean mu= "60" lambda= "200" damping= "0.01" /> + <neohookean mu= "180" lambda= "600" damping= "0.01" /> <visual filename="torus.vtk"/> </deformable> </robot> diff --git a/examples/CommonInterfaces/CommonDeformableBodyBase.h b/examples/CommonInterfaces/CommonDeformableBodyBase.h index 686f12a72..90dbf8f06 100644 --- a/examples/CommonInterfaces/CommonDeformableBodyBase.h +++ b/examples/CommonInterfaces/CommonDeformableBodyBase.h @@ -21,11 +21,13 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase btAlignedObjectArray<btDeformableLagrangianForce*> m_forces; btSoftBody* m_pickedSoftBody; btDeformableMousePickingForce* m_mouseForce; - btScalar m_maxPickingForce; + btScalar m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, m_maxPickingForce; CommonDeformableBodyBase(GUIHelperInterface* helper) : CommonMultiBodyBase(helper), m_pickedSoftBody(0), m_mouseForce(0), + m_pickingForceElasticStiffness(100), + m_pickingForceDampingStiffness(0.0), m_maxPickingForce(0.3) { } @@ -115,7 +117,7 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase m_pickedSoftBody = psb; psb->setActivationState(DISABLE_DEACTIVATION); const btSoftBody::Face& f = psb->m_faces[face_id]; - btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0.2, f, m_hitPos, m_maxPickingForce); + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, f, m_hitPos, m_maxPickingForce); m_mouseForce = mouse_force; getDeformableDynamicsWorld()->addForce(psb, mouse_force); } diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index d65cb6581..d5b72a0c8 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -45,6 +45,7 @@ struct GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeInstanceFlags(int instanceUid, int flags) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} + virtual void changeScaling(int instanceUid, const double scaling[3]) {} virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {} virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height) {} virtual void updateShape(int shapeIndex, float* vertices) {} @@ -151,6 +152,7 @@ struct DummyGUIHelper : public GUIHelperInterface virtual void removeAllGraphicsInstances() {} virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} + virtual void changeScaling(int instanceUid, const double scaling[3]) {} virtual Common2dCanvasInterface* get2dCanvasInterface() { diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp index 8418b58fe..1d091fa38 100644 --- a/examples/DeformableDemo/ClothFriction.cpp +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -144,14 +144,15 @@ void ClothFriction::initPhysics() 10,10, 0, true); - psb->getCollisionShape()->setMargin(0.05); + psb->getCollisionShape()->setMargin(0.06); psb->generateBendingConstraints(2); psb->setTotalMass(1); - psb->setSpringStiffness(10); + psb->setSpringStiffness(100); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 3; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; getDeformableDynamicsWorld()->addSoftBody(psb); @@ -172,19 +173,20 @@ void ClothFriction::initPhysics() btVector3(+s, h, +s), 5,5, 0, true); - psb2->getCollisionShape()->setMargin(0.05); + psb2->getCollisionShape()->setMargin(0.06); psb2->generateBendingConstraints(2); psb2->setTotalMass(1); - psb2->setSpringStiffness(10); + psb2->setSpringStiffness(100); psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body - psb2->m_cfg.kDF = 20; + psb2->m_cfg.kDF = 1; psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; psb->translate(btVector3(0,0,0)); getDeformableDynamicsWorld()->addSoftBody(psb2); - btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,1, true); + btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,.1, true); getDeformableDynamicsWorld()->addForce(psb2, mass_spring2); m_forces.push_back(mass_spring2); diff --git a/examples/DeformableDemo/Collide.cpp b/examples/DeformableDemo/Collide.cpp new file mode 100644 index 000000000..01590ce7a --- /dev/null +++ b/examples/DeformableDemo/Collide.cpp @@ -0,0 +1,273 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. 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: + 1. 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. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "Collide.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonDeformableBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The Collide shows the contact between volumetric deformable objects and rigid objects. +static btScalar E = 50; +static btScalar nu = 0.3; +static btScalar damping_alpha = 0.1; +static btScalar damping_beta = 0.01; +static btScalar COLLIDING_VELOCITY = 15; + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class Collide : public CommonDeformableBodyBase +{ + btDeformableLinearElasticityForce* m_linearElasticity; + +public: + Collide(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper) + { + m_linearElasticity = 0; + } + virtual ~Collide() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = 0; + float yaw = 90; + float targetPos[3] = {0, 3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void Ctor_RbUpStack() + { + float mass = 0.5; + btCollisionShape* shape = new btBoxShape(btVector3(2, 2, 2)); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,-2,0)); + btRigidBody* rb = createRigidBody(mass, startTransform, shape); + rb->setLinearVelocity(btVector3(0,+COLLIDING_VELOCITY, 0)); + } + + void stepSimulation(float deltaTime) + { + m_linearElasticity->setPoissonRatio(nu); + m_linearElasticity->setYoungsModulus(E); + m_linearElasticity->setDamping(damping_alpha, damping_beta); + float internalTimeStep = 1. / 60.f; + m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + } + + virtual void renderScene() + { + CommonDeformableBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void Collide::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, 0, 0); + m_dynamicsWorld->setGravity(gravity); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + // create volumetric soft body + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 7, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(0.5); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->m_sleepingThreshold = 0; + btSoftBodyHelpers::generateBoundaryFaces(psb); + + psb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); + + btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); + m_linearElasticity = linearElasticity; + getDeformableDynamicsWorld()->addForce(psb, linearElasticity); + m_forces.push_back(linearElasticity); + } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); + getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; + // add a few rigid bodies + Ctor_RbUpStack(); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + +// { +// SliderParams slider("Young's Modulus", &E); +// slider.m_minVal = 0; +// slider.m_maxVal = 2000; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Poisson Ratio", &nu); +// slider.m_minVal = 0.05; +// slider.m_maxVal = 0.49; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Mass Damping", &damping_alpha); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Stiffness Damping", &damping_beta); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } + { + SliderParams slider("Young's Modulus", &E); + slider.m_minVal = 0; + slider.m_maxVal = 2000; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Poisson Ratio", &nu); + slider.m_minVal = 0.05; + slider.m_maxVal = 0.49; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Mass Damping", &damping_alpha); + slider.m_minVal = 0; + slider.m_maxVal = 1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Stiffness Damping", &damping_beta); + slider.m_minVal = 0; + slider.m_maxVal = 0.1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } +} + +void Collide::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + removePickingConstraint(); + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* CollideCreateFunc(struct CommonExampleOptions& options) +{ + return new Collide(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/Collide.h b/examples/DeformableDemo/Collide.h new file mode 100644 index 000000000..97a432e5c --- /dev/null +++ b/examples/DeformableDemo/Collide.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. 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: + 1. 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. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _COLLIDE_H +#define _COLLIDE_H + +class CommonExampleInterface* CollideCreateFunc(struct CommonExampleOptions& options); + +#endif //_COLLIDE_H diff --git a/examples/DeformableDemo/DeformableClothAnchor.cpp b/examples/DeformableDemo/DeformableClothAnchor.cpp index ef2c495d9..1ed164240 100644 --- a/examples/DeformableDemo/DeformableClothAnchor.cpp +++ b/examples/DeformableDemo/DeformableClothAnchor.cpp @@ -134,7 +134,7 @@ void DeformableClothAnchor::initPhysics() { const btScalar s = 4; const btScalar h = 6; - const int r = 9; + const int r = 8; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), btVector3(+s, h, -s), btVector3(-s, h, +s), @@ -146,6 +146,7 @@ void DeformableClothAnchor::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(100,1, true); @@ -159,7 +160,7 @@ void DeformableClothAnchor::initPhysics() btTransform startTransform; startTransform.setIdentity(); startTransform.setOrigin(btVector3(0, h, -(s + 3.5))); - btRigidBody* body = createRigidBody(2, startTransform, new btBoxShape(btVector3(s, 1, 3))); + btRigidBody* body = createRigidBody(1, startTransform, new btBoxShape(btVector3(s, 1, 3))); psb->appendDeformableAnchor(0, body); psb->appendDeformableAnchor(r - 1, body); } diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index 37107186d..b1ad0b67b 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -143,7 +143,7 @@ void DeformableContact::initPhysics() 20,20, 1 + 2 + 4 + 8, true); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.05); psb->generateBendingConstraints(2); psb->setSpringStiffness(10); psb->setTotalMass(1); @@ -151,6 +151,7 @@ void DeformableContact::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; getDeformableDynamicsWorld()->addSoftBody(psb); @@ -171,7 +172,7 @@ void DeformableContact::initPhysics() btVector3(+s, h, +s), 10,10, 0, true); - psb2->getCollisionShape()->setMargin(0.1); + psb2->getCollisionShape()->setMargin(0.05); psb2->generateBendingConstraints(2); psb2->setSpringStiffness(10); psb2->setTotalMass(1); @@ -179,6 +180,7 @@ void DeformableContact::initPhysics() psb2->m_cfg.kCHR = 1; // collision hardness with rigid body psb2->m_cfg.kDF = 0.5; psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; psb->translate(btVector3(3.5,0,0)); getDeformableDynamicsWorld()->addSoftBody(psb2); diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 6e67b9e27..041624b00 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -192,13 +192,14 @@ void DeformableMultibody::initPhysics() // 3,3, 1 + 2 + 4 + 8, true); - psb->getCollisionShape()->setMargin(0.25); + psb->getCollisionShape()->setMargin(0.025); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; psb->setCollisionFlags(0); getDeformableDynamicsWorld()->addSoftBody(psb); @@ -340,6 +341,7 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; btCollisionShape* box = new btBoxShape(baseHalfExtents); + box->setMargin(0.01); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); col->setCollisionShape(box); diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 2d8a9dd90..7efe118a6 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -53,11 +53,53 @@ public: //use a smaller internal timestep, there are stability issues float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + +// +// btCollisionShape* boxShape = new btBoxShape(btVector3(1, 1, 1)); +// boxShape->setMargin(1e-3); +// if (0) +// { +// btVector3 p(0.99,1.01,0.99); +// for (int i = 0; i < 40; ++i) +// { +// p[1] -= 0.001; +// btScalar margin(.000001); +// btTransform trans; +// trans.setIdentity(); +// btGjkEpaSolver2::sResults results; +// const btConvexShape* csh = static_cast<const btConvexShape*>(boxShape); +// btScalar d = btGjkEpaSolver2::SignedDistance(p, margin, csh, trans, results); +// printf("d = %f\n", d); +// printf("----\n"); +// } +// } +// +// btVector3 p(.991,1.01,.99); +// for (int i = 0; i < 40; ++i) +// { +// p[1] -= 0.001; +// btScalar margin(.006); +// btTransform trans; +// trans.setIdentity(); +// btScalar dst; +// btGjkEpaSolver2::sResults results; +// btTransform point_transform; +// point_transform.setIdentity(); +// point_transform.setOrigin(p); +// btSphereShape sphere(margin); +// btVector3 guess(0,0,0); +// const btConvexShape* csh = static_cast<const btConvexShape*>(boxShape); +// btGjkEpaSolver2::SignedDistance(&sphere, point_transform, csh, trans, guess, results); +// dst = results.distance-csh->getMargin(); +// dst -= margin; +// printf("d = %f\n", dst); +// printf("----\n"); +// } } void Ctor_RbUpStack(int count) { - float mass = 0.2; + float mass = .2; btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); @@ -72,8 +114,8 @@ public: btCollisionShape* shape[] = { new btBoxShape(btVector3(1, 1, 1)), -// new btSphereShape(0.75), -// cylinderCompound + new btSphereShape(0.75), + cylinderCompound }; // static const int nshapes = sizeof(shape) / sizeof(shape[0]); // for (int i = 0; i < count; ++i) @@ -167,7 +209,7 @@ void DeformableRigid::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -32, 0)); + groundTransform.setOrigin(btVector3(0, -42, 0)); groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -190,6 +232,7 @@ void DeformableRigid::initPhysics() } // create a piece of cloth + if(1) { bool onGround = false; const btScalar s = 4; @@ -200,8 +243,8 @@ void DeformableRigid::initPhysics() btVector3(-s, h, +s), btVector3(+s, h, +s), // 3,3, - 20,20, - 1 + 2 + 4 + 8, true); + 20,20, + 1 + 2 + 4 + 8, true); // 0, true); if (onGround) @@ -213,17 +256,17 @@ void DeformableRigid::initPhysics() 2,2, 0, true); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.05); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = .4; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(15,0.5, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); @@ -231,8 +274,8 @@ void DeformableRigid::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); // add a few rigid bodies - Ctor_RbUpStack(1); } + Ctor_RbUpStack(10); getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index 699289b1c..99180ea0c 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -31,6 +31,7 @@ public: DeformableSelfCollision(struct GUIHelperInterface* helper) : CommonDeformableBodyBase(helper) { + m_maxPickingForce = 0.004; } virtual ~DeformableSelfCollision() { @@ -41,7 +42,7 @@ public: void resetCamera() { - float dist = 1.0; + float dist = 2.0; float pitch = -8; float yaw = 100; float targetPos[3] = {0, -1.0, 0}; @@ -93,7 +94,7 @@ void DeformableSelfCollision::initPhysics() { ///create a ground btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.))); - + groundShape->setMargin(0.02); m_collisionShapes.push_back(groundShape); btTransform groundTransform; @@ -119,8 +120,8 @@ void DeformableSelfCollision::initPhysics() //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); } + addCloth(btVector3(0, -0.2, 0)); addCloth(btVector3(0, -0.1, 0)); - addCloth(btVector3(0, 1, 0)); getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); @@ -128,7 +129,7 @@ void DeformableSelfCollision::initPhysics() void DeformableSelfCollision::addCloth(btVector3 origin) // create a piece of cloth { - const btScalar s = 0.3; + const btScalar s = 0.6; const btScalar h = 0; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2*s), @@ -146,23 +147,26 @@ void DeformableSelfCollision::addCloth(btVector3 origin) psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0.1; - psb->rotate(btQuaternion(0, SIMD_PI / 4, 0)); +// psb->rotate(btQuaternion(0, SIMD_PI / 2, 0)); btTransform clothTransform; clothTransform.setIdentity(); clothTransform.setOrigin(btVector3(0,0.2,0)+origin); psb->transform(clothTransform); psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; getDeformableDynamicsWorld()->addSoftBody(psb); psb->setSelfCollision(true); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true); - psb->setSpringStiffness(2); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.2, true); + psb->setSpringStiffness(4); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); btVector3 gravity = btVector3(0, -9.8, 0); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); + getDeformableDynamicsWorld()->setUseProjection(true); m_forces.push_back(gravity_force); } diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 79cdcc76d..f52f67fe9 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -73,10 +73,10 @@ public: void resetCamera() { - float dist = 2; + float dist = 0.3; float pitch = -45; float yaw = 100; - float targetPos[3] = {0, -0, 0}; + float targetPos[3] = {0, -0.1, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } @@ -105,12 +105,12 @@ public: if (dofIndex == 6) { motor->setVelocityTarget(-fingerTargetVelocities[1], 1); - motor->setMaxAppliedImpulse(2); + motor->setMaxAppliedImpulse(20); } if (dofIndex == 7) { motor->setVelocityTarget(fingerTargetVelocities[1], 1); - motor->setMaxAppliedImpulse(2); + motor->setMaxAppliedImpulse(20); } motor->setMaxAppliedImpulse(1); } @@ -198,6 +198,9 @@ void GraspDeformable::initPhysics() btVector3 gravity = btVector3(0, -9.81, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_maxPickingForce = 0.001; // build a gripper @@ -208,9 +211,9 @@ void GraspDeformable::initPhysics() bool canSleep = false; bool selfCollide = true; int numLinks = 2; - btVector3 linkHalfExtents(.1, .2, .04); - btVector3 baseHalfExtents(.1, 0.02, .2); - btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, .7f,0.f), linkHalfExtents, baseHalfExtents, false); + btVector3 linkHalfExtents(0.02, 0.018, .003); + btVector3 baseHalfExtents(0.02, 0.002, .002); + btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), linkHalfExtents, baseHalfExtents, false); mbC->setCanSleep(canSleep); mbC->setHasSelfCollision(selfCollide); @@ -219,7 +222,7 @@ void GraspDeformable::initPhysics() for (int i = 0; i < numLinks; i++) { int mbLinkIndex = i; - float maxMotorImpulse = 1.f; + double maxMotorImpulse = 1; if (supportsJointMotor(mbC, mbLinkIndex)) { @@ -252,13 +255,13 @@ void GraspDeformable::initPhysics() //create a ground { - btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); - + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.))); + groundShape->setMargin(0.001); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -25-.6, 0)); + groundTransform.setOrigin(btVector3(0, -5.1, 0)); groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -274,47 +277,49 @@ void GraspDeformable::initPhysics() btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); - body->setFriction(0.1); + body->setFriction(0.5); //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body,1,1+2); } // create a soft block - if (1) + if (0) { char absolute_path[1024]; b3BulletDefaultFileIO fileio; // fileio.findResourcePath("ditto.vtk", absolute_path, 1024); // fileio.findResourcePath("banana.vtk", absolute_path, 1024); -// fileio.findResourcePath("ball.vtk", absolute_path, 1024); + fileio.findResourcePath("ball.vtk", absolute_path, 1024); // fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024); // fileio.findResourcePath("single_tet.vtk", absolute_path, 1024); - fileio.findResourcePath("tube.vtk", absolute_path, 1024); +// fileio.findResourcePath("tube.vtk", absolute_path, 1024); // fileio.findResourcePath("torus.vtk", absolute_path, 1024); // fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024); // fileio.findResourcePath("bread.vtk", absolute_path, 1024); // fileio.findResourcePath("boot.vtk", absolute_path, 1024); - // btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), - // TetraCube::getElements(), - // 0, - // TetraCube::getNodes(), - // false, true, true); - btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), absolute_path); + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + btSoftBodyHelpers::generateBoundaryFaces(psb); +// btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), absolute_path); // psb->scale(btVector3(30, 30, 30)); // for banana // psb->scale(btVector3(.7, .7, .7)); -// psb->scale(btVector3(2, 2, 2)); - psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot + psb->scale(btVector3(.2, .2, .2)); +// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot // psb->scale(btVector3(.1, .1, .1)); // for ditto // psb->translate(btVector3(.25, 10, 0.4)); - psb->getCollisionShape()->setMargin(0.0005); + psb->getCollisionShape()->setMargin(0.01); psb->setMaxStress(50); - psb->setTotalMass(.01); + psb->setTotalMass(.1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; getDeformableDynamicsWorld()->addSoftBody(psb); @@ -322,23 +327,23 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,8,.02); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(20,80,.01); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } getDeformableDynamicsWorld()->setImplicit(false); // create a piece of cloth - if(0) + if(1) { bool onGround = false; - const btScalar s = .1; - const btScalar h = 1; + const btScalar s = .05; + const btScalar h = -0.02; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), btVector3(+s, h, -s), btVector3(-s, h, +s), btVector3(+s, h, +s), - 20,20, + 10,10, 0, true); if (onGround) @@ -350,35 +355,35 @@ void GraspDeformable::initPhysics() 2,2, 0, true); - psb->getCollisionShape()->setMargin(0.005); + psb->getCollisionShape()->setMargin(0.001); psb->generateBendingConstraints(2); - psb->setTotalMass(.01); + psb->setTotalMass(0.01); psb->setSpringStiffness(10); psb->setDampingCoefficient(0.05); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 1; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.0,0.0, true)); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.05, false)); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); { SliderParams slider("Moving velocity", &sGripperVerticalVelocity); - slider.m_minVal = -.2; - slider.m_maxVal = .2; + slider.m_minVal = -.02; + slider.m_maxVal = .02; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity); - slider.m_minVal = -.3; - slider.m_maxVal = .3; + slider.m_minVal = -1; + slider.m_maxVal = 1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } @@ -432,8 +437,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor { //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 100.f; - float linkMass = 100.f; + float baseMass = 0.1; + float linkMass = 0.1; int numLinks = 2; if (baseMass) @@ -461,8 +466,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor //y-axis assumed up btAlignedObjectArray<btVector3> parentComToCurrentCom; - parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 4.f)); - parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 4.f));//par body's COM to cur body's COM offset + parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 2.f)); + parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 2.f));//par body's COM to cur body's COM offset btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*8.f, 0); //cur body's COM to cur body's PIV offset @@ -503,6 +508,7 @@ void GraspDeformable::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsW if (1) { btCollisionShape* box = new btBoxShape(baseHalfExtents); + box->setMargin(0.001); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); col->setCollisionShape(box); @@ -533,6 +539,7 @@ void GraspDeformable::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsW btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; btCollisionShape* box = new btBoxShape(linkHalfExtents); + box->setMargin(0.001); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); col->setCollisionShape(box); diff --git a/examples/DeformableDemo/LargeDeformation.cpp b/examples/DeformableDemo/LargeDeformation.cpp new file mode 100644 index 000000000..f1a6a29f5 --- /dev/null +++ b/examples/DeformableDemo/LargeDeformation.cpp @@ -0,0 +1,262 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. 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: + 1. 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. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "LargeDeformation.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonDeformableBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The LargeDeformation shows the contact between volumetric deformable objects and rigid objects. +static btScalar E = 50; +static btScalar nu = 0.3; +static btScalar damping_alpha = 0.1; +static btScalar damping_beta = 0.01; + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class LargeDeformation : public CommonDeformableBodyBase +{ + btDeformableLinearElasticityForce* m_linearElasticity; + +public: + LargeDeformation(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper) + { + m_linearElasticity = 0; + } + virtual ~LargeDeformation() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, 3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + m_linearElasticity->setPoissonRatio(nu); + m_linearElasticity->setYoungsModulus(E); + m_linearElasticity->setDamping(damping_alpha, damping_beta); + float internalTimeStep = 1. / 60.f; + m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + } + + virtual void renderScene() + { + CommonDeformableBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void LargeDeformation::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + // create volumetric soft body + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 5, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(0.5); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0.5; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->m_sleepingThreshold = 0; + btSoftBodyHelpers::generateBoundaryFaces(psb); + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + for (int j = 0; j < 3; ++j) + psb->m_nodes[i].m_x[j] = ((double) 2*rand() / (RAND_MAX))-1.0; + psb->m_nodes[i].m_x[1]+=8; + } + + btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); + m_linearElasticity = linearElasticity; + getDeformableDynamicsWorld()->addForce(psb, linearElasticity); + m_forces.push_back(linearElasticity); + } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); + getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(20); + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; + // add a few rigid bodies + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + { + SliderParams slider("Young's Modulus", &E); + slider.m_minVal = 0; + slider.m_maxVal = 2000; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Poisson Ratio", &nu); + slider.m_minVal = 0.05; + slider.m_maxVal = 0.49; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Mass Damping", &damping_alpha); + slider.m_minVal = 0; + slider.m_maxVal = 1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Stiffness Damping", &damping_beta); + slider.m_minVal = 0; + slider.m_maxVal = 0.1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } +// { +// SliderParams slider("Young's Modulus", &E); +// slider.m_minVal = 0; +// slider.m_maxVal = 200; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Poisson Ratio", &nu); +// slider.m_minVal = 0.05; +// slider.m_maxVal = 0.40; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Mass Damping", &damping_alpha); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Stiffness Damping", &damping_beta); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +} + +void LargeDeformation::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + removePickingConstraint(); + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* LargeDeformationCreateFunc(struct CommonExampleOptions& options) +{ + return new LargeDeformation(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/LargeDeformation.h b/examples/DeformableDemo/LargeDeformation.h new file mode 100644 index 000000000..bb68082f4 --- /dev/null +++ b/examples/DeformableDemo/LargeDeformation.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. 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: + 1. 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. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _LARGE_DEFORMATION_H +#define _LARGE_DEFORMATION_H + +class CommonExampleInterface* LargeDeformationCreateFunc(struct CommonExampleOptions& options); + +#endif //_LARGE_DEFORMATION_H diff --git a/examples/DeformableDemo/MultibodyClothAnchor.cpp b/examples/DeformableDemo/MultibodyClothAnchor.cpp index 5efdf8d28..ab78efa68 100644 --- a/examples/DeformableDemo/MultibodyClothAnchor.cpp +++ b/examples/DeformableDemo/MultibodyClothAnchor.cpp @@ -143,13 +143,14 @@ void MultibodyClothAnchor::initPhysics() btVector3(+s, h, -s), btVector3(-s, h, +s), btVector3(+s, h, +s), r, r, 4 + 8, true); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.01); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true); diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index c6b942fe3..dbf1e6c80 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -296,24 +296,25 @@ void Pinch::initPhysics() psb->scale(btVector3(2, 2, 2)); psb->translate(btVector3(0, 4, 0)); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.01); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 2; + psb->m_cfg.kDF = .5; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); btSoftBodyHelpers::generateBoundaryFaces(psb); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1,0.05); - getDeformableDynamicsWorld()->addForce(psb, mass_spring); - m_forces.push_back(mass_spring); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.2,1); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,3, 0.02); + neohookean->setPoissonRatio(0.3); + neohookean->setYoungsModulus(25); + neohookean->setDamping(0.01); + psb->m_cfg.drag = 0.001; getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); // add a grippers diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index 16b6e30e1..5d31331b6 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -265,7 +265,7 @@ void PinchFriction::initPhysics() psb->scale(btVector3(2, 2, 1)); psb->translate(btVector3(0, 2.1, 2.2)); - psb->getCollisionShape()->setMargin(0.05); + psb->getCollisionShape()->setMargin(0.025); psb->setSpringStiffness(10); psb->setTotalMass(.6); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects @@ -273,6 +273,7 @@ void PinchFriction::initPhysics() psb->m_cfg.kDF = 2; btSoftBodyHelpers::generateBoundaryFaces(psb); psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; getDeformableDynamicsWorld()->addSoftBody(psb); @@ -295,13 +296,14 @@ void PinchFriction::initPhysics() psb2->scale(btVector3(2, 2, 1)); psb2->translate(btVector3(0, 2.1, -2.2)); - psb2->getCollisionShape()->setMargin(0.05); + psb2->getCollisionShape()->setMargin(0.025); psb2->setTotalMass(.6); psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body psb2->m_cfg.kDF = 2; psb2->setSpringStiffness(10); psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; btSoftBodyHelpers::generateBoundaryFaces(psb2); getDeformableDynamicsWorld()->addSoftBody(psb2); @@ -325,13 +327,14 @@ void PinchFriction::initPhysics() psb3->scale(btVector3(2, 2, 1)); psb3->translate(btVector3(0, 2.1, 0)); - psb3->getCollisionShape()->setMargin(0.05); + psb3->getCollisionShape()->setMargin(0.025); psb3->setTotalMass(.6); psb3->setSpringStiffness(10); psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb3->m_cfg.kCHR = 1; // collision hardness with rigid body psb3->m_cfg.kDF = 2; psb3->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb3->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb3->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; btSoftBodyHelpers::generateBoundaryFaces(psb3); getDeformableDynamicsWorld()->addSoftBody(psb3); diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp index a213918d4..962d49d66 100644 --- a/examples/DeformableDemo/SplitImpulse.cpp +++ b/examples/DeformableDemo/SplitImpulse.cpp @@ -160,13 +160,14 @@ void SplitImpulse::initPhysics() // 0, true); - psb->getCollisionShape()->setMargin(0.15); + psb->getCollisionShape()->setMargin(0.015); psb->generateBendingConstraints(2); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0.1; + psb->m_cfg.kDF = 1; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true); diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 6fc822a36..bfc51a156 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -27,9 +27,10 @@ #include "../Utils/b3ResourcePath.h" ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects. -static btScalar E = 100; +static btScalar E = 50; static btScalar nu = 0.3; -static btScalar damping = 0.01; +static btScalar damping_alpha = 0.1; +static btScalar damping_beta = 0.01; struct TetraCube { @@ -38,13 +39,16 @@ struct TetraCube class VolumetricDeformable : public CommonDeformableBodyBase { - btDeformableNeoHookeanForce* m_neohookean; + btDeformableLinearElasticityForce* m_linearElasticity; public: VolumetricDeformable(struct GUIHelperInterface* helper) : CommonDeformableBodyBase(helper) { - m_neohookean = 0; + m_linearElasticity = 0; + m_pickingForceElasticStiffness = 100; + m_pickingForceDampingStiffness = 0; + m_maxPickingForce = 1e10; // allow large picking force with implicit scheme. } virtual ~VolumetricDeformable() { @@ -64,12 +68,12 @@ public: void stepSimulation(float deltaTime) { - m_neohookean->setPoissonRatio(nu); - m_neohookean->setYoungsModulus(E); - m_neohookean->setDamping(damping); + m_linearElasticity->setPoissonRatio(nu); + m_linearElasticity->setYoungsModulus(E); + m_linearElasticity->setDamping(damping_alpha, damping_beta); //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 600.f; - m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); + float internalTimeStep = 1. / 240; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } void createStaticBox(const btVector3& halfEdge, const btVector3& translation) @@ -100,7 +104,7 @@ public: void Ctor_RbUpStack(int count) { - float mass = 1; + float mass = 2; btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); @@ -187,7 +191,7 @@ void VolumetricDeformable::initPhysics() btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); - body->setFriction(0.5); + body->setFriction(1); //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); @@ -208,28 +212,32 @@ void VolumetricDeformable::initPhysics() getDeformableDynamicsWorld()->addSoftBody(psb); psb->scale(btVector3(2, 2, 2)); psb->translate(btVector3(0, 5, 0)); - psb->getCollisionShape()->setMargin(0.25); - psb->setTotalMass(1); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(0.5); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0.5; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; psb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(psb); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.01); - m_neohookean = neohookean; - getDeformableDynamicsWorld()->addForce(psb, neohookean); - m_forces.push_back(neohookean); - + btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); + m_linearElasticity = linearElasticity; + getDeformableDynamicsWorld()->addForce(psb, linearElasticity); + m_forces.push_back(linearElasticity); } - getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setImplicit(true); getDeformableDynamicsWorld()->setLineSearch(false); + getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; // add a few rigid bodies Ctor_RbUpStack(4); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); @@ -237,24 +245,31 @@ void VolumetricDeformable::initPhysics() { SliderParams slider("Young's Modulus", &E); slider.m_minVal = 0; - slider.m_maxVal = 200; + slider.m_maxVal = 2000; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Poisson Ratio", &nu); - slider.m_minVal = 0.1; - slider.m_maxVal = 0.4; + slider.m_minVal = 0.05; + slider.m_maxVal = 0.49; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { - SliderParams slider("Damping", &damping); - slider.m_minVal = 0.01; - slider.m_maxVal = 0.2; + SliderParams slider("Mass Damping", &damping_alpha); + slider.m_minVal = 0; + slider.m_maxVal = 1; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + { + SliderParams slider("Stiffness Damping", &damping_beta); + slider.m_minVal = 0; + slider.m_maxVal = 0.1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } } void VolumetricDeformable::exitPhysics() diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index fc34707fc..ca06b7d11 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -373,6 +373,10 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/SplitImpulse.h ../DeformableDemo/VolumetricDeformable.cpp ../DeformableDemo/VolumetricDeformable.h + ../DeformableDemo/Collide.cpp + ../DeformableDemo/Collide.h + ../DeformableDemo/LargeDeformation.cpp + ../DeformableDemo/LargeDeformation.h ../DeformableDemo/DeformableClothAnchor.cpp ../DeformableDemo/DeformableClothAnchor.h ../DeformableDemo/MultibodyClothAnchor.cpp diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index ee106cdd5..7ba53d62e 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -82,7 +82,13 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e59641d18..127ba5172 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -52,6 +52,8 @@ #include "../DeformableDemo/PinchFriction.h" #include "../DeformableDemo/DeformableMultibody.h" #include "../DeformableDemo/VolumetricDeformable.h" +#include "../DeformableDemo/LargeDeformation.h" +#include "../DeformableDemo/Collide.h" #include "../DeformableDemo/GraspDeformable.h" #include "../DeformableDemo/DeformableContact.h" #include "../DeformableDemo/DeformableClothAnchor.h" @@ -202,6 +204,8 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), + ExampleEntry(1, "Extreme Deformation", "Recovery from extreme deformation", LargeDeformationCreateFunc), + ExampleEntry(1, "Colliding Test", "Volumetric deformable collide with rigid box", CollideCreateFunc), ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc), ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc), ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 610329425..8afdad793 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -388,6 +388,14 @@ void OpenGLGuiHelper::changeInstanceFlags(int instanceUid, int flags) m_data->m_glApp->m_renderer->writeSingleInstanceFlagsToCPU( flags, instanceUid); } } +void OpenGLGuiHelper::changeScaling(int instanceUid, const double scaling[3]) +{ + if (instanceUid >= 0) + { + m_data->m_glApp->m_renderer->writeSingleInstanceScaleToCPU(scaling, instanceUid); + }; +} + void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { if (instanceUid >= 0) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index bdaa22772..8c9b0e70e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -29,6 +29,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid); virtual void changeInstanceFlags(int instanceUid, int flags); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); + virtual void changeScaling(int instanceUid, const double scaling[3]); virtual void changeSpecularColor(int instanceUid, const double specularColor[3]); virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height); virtual void removeTexture(int textureUid); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index fe48dc8a6..b8142d90a 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -724,7 +724,14 @@ struct BulletMJCFImporterInternalData } { - geom.m_localMaterial.m_matColor.m_rgbaColor = sGoogleColors[linkIndex & 3]; + if (m_flags & CUF_GOOGLEY_UNDEFINED_COLORS) + { + geom.m_localMaterial.m_matColor.m_rgbaColor = sGoogleColors[linkIndex & 3]; + } + else + { + geom.m_localMaterial.m_matColor.m_rgbaColor.setValue(1, 1, 1, 1); + } geom.m_localMaterial.m_matColor.m_specularColor.setValue(1, 1, 1); geom.m_hasLocalMaterial = true; } @@ -1597,7 +1604,8 @@ bool BulletMJCFImporter::getLinkColor2(int linkIndex, struct UrdfMaterialColor& if (!hasLinkColor) { - matCol.m_rgbaColor = sGoogleColors[linkIndex & 3]; + + matCol.m_rgbaColor = (m_data->m_flags & CUF_GOOGLEY_UNDEFINED_COLORS) ? sGoogleColors[linkIndex & 3] : btVector4(1,1,1,1); matCol.m_specularColor.setValue(1, 1, 1); hasLinkColor = true; } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index ec0cba28f..42cc102f4 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -636,6 +636,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength); btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); shape = cylZShape; + shape->setMargin(gUrdfDefaultCollisionMargin); } else { @@ -1283,45 +1284,40 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures,meshData); - if (m_data->m_flags&CUF_USE_MATERIAL_COLORS_FROM_MTL) + bool mtlOverridesUrdfColor = false; + if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) && + (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) { - if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) && - (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) + mtlOverridesUrdfColor = (m_data->m_flags & CUF_USE_MATERIAL_COLORS_FROM_MTL) != 0; + UrdfMaterialColor matCol; + if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) { - UrdfMaterialColor matCol; - - if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) - { - matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], - meshData.m_rgbaColor[1], - meshData.m_rgbaColor[2], - meshData.m_rgbaColor[3]); - } else - { - matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], - meshData.m_rgbaColor[1], - meshData.m_rgbaColor[2], - 1); - } - - matCol.m_specularColor.setValue(meshData.m_specularColor[0], - meshData.m_specularColor[1], - meshData.m_specularColor[2]); - m_data->m_linkColors.insert(linkIndex, matCol); - } - } else - { - if (matPtr) + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + meshData.m_rgbaColor[3]); + } else { - UrdfMaterial* const mat = *matPtr; - //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); - UrdfMaterialColor matCol; - matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; - matCol.m_specularColor = mat->m_matColor.m_specularColor; - m_data->m_linkColors.insert(linkIndex, matCol); + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + 1); } - } + matCol.m_specularColor.setValue(meshData.m_specularColor[0], + meshData.m_specularColor[1], + meshData.m_specularColor[2]); + m_data->m_linkColors.insert(linkIndex, matCol); + } + if (matPtr && !mtlOverridesUrdfColor) + { + UrdfMaterial* const mat = *matPtr; + //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); + UrdfMaterialColor matCol; + matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; + matCol.m_specularColor = mat->m_matColor.m_specularColor; + m_data->m_linkColors.insert(linkIndex, matCol); + } } } if (vertices.size() && indices.size()) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 1a85c1928..c21e7fc9a 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -21,12 +21,12 @@ //static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); static bool enableConstraints = true; -static btVector4 colors[4] = - { - btVector4(1, 0, 0, 1), - btVector4(0, 1, 0, 1), - btVector4(0, 1, 1, 1), - btVector4(1, 1, 0, 1), +static btVector4 gGoogleyColors[4] = +{ + btVector4(60. / 256., 186. / 256., 84. / 256., 1), + btVector4(244. / 256., 194. / 256., 13. / 256., 1), + btVector4(219. / 256., 50. / 256., 54. / 256., 1), + btVector4(72. / 256., 133. / 256., 237. / 256., 1), }; static btVector4 selectColor2() @@ -36,7 +36,7 @@ static btVector4 selectColor2() sMutex.lock(); #endif static int curColor = 0; - btVector4 color = colors[curColor]; + btVector4 color = gGoogleyColors[curColor]; curColor++; curColor &= 3; #ifdef BT_THREADSAFE @@ -303,7 +303,8 @@ btTransform ConvertURDF2BulletInternal( if (compoundShape) { UrdfMaterialColor matColor; - btVector4 color2 = selectColor2(); + + btVector4 color2 = (flags & CUF_GOOGLEY_UNDEFINED_COLORS) ? selectColor2() : btVector4(1, 1, 1, 1); btVector3 specular(0.5, 0.5, 0.5); if (u2b.getLinkColor2(urdfLinkIndex, matColor)) { @@ -642,7 +643,7 @@ btTransform ConvertURDF2BulletInternal( } world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); - btVector4 color2 = selectColor2(); //(0.0,0.0,0.5); + btVector4 color2 = (flags & CUF_GOOGLEY_UNDEFINED_COLORS) ? selectColor2() : btVector4(1, 1, 1, 1); btVector3 specularColor(1, 1, 1); UrdfMaterialColor matCol; if (u2b.getLinkColor2(urdfLinkIndex, matCol)) diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index ed163ec34..fff0fee2e 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -104,6 +104,7 @@ enum ConvertURDFFlags CUF_IGNORE_VISUAL_SHAPES = 1 << 20, CUF_IGNORE_COLLISION_SHAPES = 1 << 21, CUF_PRINT_URDF_INFO = 1 << 22, + CUF_GOOGLEY_UNDEFINED_COLORS = 1 << 23, }; diff --git a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h index 147d8c911..3050cadde 100644 --- a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h +++ b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h @@ -63,48 +63,35 @@ static bool UrdfFindMeshFile( fn = fn.substr(drop_it_model.length()); std::list<std::string> shorter; - shorter.push_back("../.."); - shorter.push_back(".."); - shorter.push_back("."); + shorter.push_back("../../"); + shorter.push_back("../"); + shorter.push_back("./"); int cnt = urdf_path.size(); for (int i = 0; i < cnt; ++i) { if (urdf_path[i] == '/' || urdf_path[i] == '\\') { - shorter.push_back(urdf_path.substr(0, i)); + shorter.push_back(urdf_path.substr(0, i) + "/"); } } + shorter.push_back(""); // no prefix shorter.reverse(); std::string existing_file; - - + for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x) { - for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x) - { - std::string attempt = *x + "/" + fn; - int f = fileIO->fileOpen(attempt.c_str(), "rb"); - if (f<0) - { - //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); - continue; - } - fileIO->fileClose(f); - existing_file = attempt; - //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); - break; - } - } - if (existing_file.empty()) - { - std::string attempt = fn; + std::string attempt = *x + fn; int f = fileIO->fileOpen(attempt.c_str(), "rb"); - if (f>=0) + if (f<0) { - existing_file = attempt; - fileIO->fileClose(f); + //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); + continue; } + fileIO->fileClose(f); + existing_file = attempt; + //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); + break; } if (existing_file.empty()) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index f656e4fde..c898fbb65 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1163,6 +1163,28 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, } deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value")); } + + XMLElement* repulsion_xml = config->FirstChildElement("repulsion_stiffness"); + if (repulsion_xml) + { + if (!repulsion_xml->Attribute("value")) + { + logger->reportError("repulsion_stiffness element must have value attribute"); + return false; + } + deformable.m_repulsionStiffness = urdfLexicalCast<double>(repulsion_xml->Attribute("value")); + } + + XMLElement* grav_xml = config->FirstChildElement("gravity_factor"); + if (grav_xml) + { + if (!grav_xml->Attribute("value")) + { + logger->reportError("gravity_factor element must have value attribute"); + return false; + } + deformable.m_gravFactor = urdfLexicalCast<double>(grav_xml->Attribute("value")); + } XMLElement* spring_xml = config->FirstChildElement("spring"); if (spring_xml) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 9b99bc36f..b11578113 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -205,10 +205,12 @@ struct SpringCoeffcients{ double elastic_stiffness; double damping_stiffness; double bending_stiffness; + int damp_all_directions; SpringCoeffcients(): - elastic_stiffness(0.), - damping_stiffness(0.), - bending_stiffness(0.){} + elastic_stiffness(0.), + damping_stiffness(0.), + bending_stiffness(0.), + damp_all_directions(0){} }; struct LameCoefficients @@ -225,6 +227,8 @@ struct UrdfDeformable double m_mass; double m_collisionMargin; double m_friction; + double m_repulsionStiffness; + double m_gravFactor; SpringCoeffcients m_springCoefficients; LameCoefficients m_corotatedCoefficients; @@ -234,7 +238,7 @@ struct UrdfDeformable std::string m_simFileName; btHashMap<btHashString, std::string> m_userData; - UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_visualFileName(""), m_simFileName("") + UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_visualFileName(""), m_simFileName("") { } }; diff --git a/examples/OpenGLWindow/MacOpenGLWindowObjC.m b/examples/OpenGLWindow/MacOpenGLWindowObjC.m index 6294f354c..c3ae97e2b 100644 --- a/examples/OpenGLWindow/MacOpenGLWindowObjC.m +++ b/examples/OpenGLWindow/MacOpenGLWindowObjC.m @@ -190,7 +190,7 @@ void dumpInfo(void) [m_context setView: self]; [m_context makeCurrentContext]; - + [m_context update]; // Draw //display(); @@ -240,13 +240,14 @@ void dumpInfo(void) m_context = [[NSOpenGLContext alloc] initWithFormat: fmt shareContext: nil]; [fmt release]; [m_context makeCurrentContext]; - + [m_context update]; //checkError("makeCurrentContext"); } -(void) MakeCurrent { - [m_context makeCurrentContext]; + [m_context makeCurrentContext]; + [m_context update]; } -(void)windowWillClose:(NSNotification *)note { @@ -1177,7 +1178,7 @@ int Mac_fileOpenDialog(char* filename, int maxNameLength) NSInteger zIntResult = [zOpenPanel runModal]; [foo makeCurrentContext]; - + [foo update]; if (zIntResult == NSFileHandlingPanelCancelButton) { NSLog(@"readUsingOpenPanel cancelled"); return 0; diff --git a/examples/SharedMemory/GraphicsServerExample.cpp b/examples/SharedMemory/GraphicsServerExample.cpp index b63d36c65..3e92b75c4 100644 --- a/examples/SharedMemory/GraphicsServerExample.cpp +++ b/examples/SharedMemory/GraphicsServerExample.cpp @@ -488,6 +488,18 @@ void TCPThreadFunc(void* userPtr, void* lsMemory) printf("GFX_CMD_CHANGE_RGBA_COLOR\n"); break; } + case GFX_CMD_CHANGE_SCALING: + { + args->submitCommand(); + while (args->isCommandOutstanding()) + { + clock.usleep(0); + } + if (gVerboseNetworkMessagesServer) + printf("GFX_CMD_CHANGE_SCALING\n"); + break; + } + case GFX_CMD_GET_CAMERA_INFO: { args->submitCommand(); @@ -833,6 +845,14 @@ public: m_args.processCommand(); break; } + + case GFX_CMD_CHANGE_SCALING: + { + m_guiHelper->changeScaling(clientCmd.m_changeScalingCommand.m_graphicsUid, clientCmd.m_changeScalingCommand.m_scaling); + m_args.processCommand(); + break; + } + case GFX_CMD_GET_CAMERA_INFO: { serverStatusOut.m_type = GFX_CMD_GET_CAMERA_INFO_FAILED; diff --git a/examples/SharedMemory/GraphicsSharedMemoryCommands.h b/examples/SharedMemory/GraphicsSharedMemoryCommands.h index c612d9b96..2fbaf459f 100644 --- a/examples/SharedMemory/GraphicsSharedMemoryCommands.h +++ b/examples/SharedMemory/GraphicsSharedMemoryCommands.h @@ -112,6 +112,14 @@ struct GraphicsChangeRGBAColorCommand double m_rgbaColor[4]; }; +struct GraphicsChangeScalingCommand +{ + int m_graphicsUid; + double m_scaling[3]; +}; + + + struct GraphicsGetCameraInfoStatus { int width; @@ -150,6 +158,7 @@ struct GraphicsSharedMemoryCommand struct GraphicsSyncTransformsCommand m_syncTransformsCommand; struct GraphicsRemoveInstanceCommand m_removeGraphicsInstanceCommand; struct GraphicsChangeRGBAColorCommand m_changeRGBAColorCommand; + struct GraphicsChangeScalingCommand m_changeScalingCommand; }; }; diff --git a/examples/SharedMemory/GraphicsSharedMemoryPublic.h b/examples/SharedMemory/GraphicsSharedMemoryPublic.h index 486721c72..4a80736bd 100644 --- a/examples/SharedMemory/GraphicsSharedMemoryPublic.h +++ b/examples/SharedMemory/GraphicsSharedMemoryPublic.h @@ -22,6 +22,7 @@ enum EnumGraphicsSharedMemoryClientCommand GFX_CMD_REMOVE_SINGLE_GRAPHICS_INSTANCE, GFX_CMD_CHANGE_RGBA_COLOR, GFX_CMD_GET_CAMERA_INFO, + GFX_CMD_CHANGE_SCALING, //don't go beyond this command! GFX_CMD_MAX_CLIENT_COMMANDS, }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1ec54f498..23fe53071 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -381,8 +381,8 @@ B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle c { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); - command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness; - command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness; + command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness; + command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness; command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE; return 0; } @@ -404,6 +404,24 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle return 0; } +B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_repulsionStiffness = stiffness; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS; + return 0; +} + +B3_SHARED_API int b3LoadSoftBodySetGravityFactor(b3SharedMemoryCommandHandle commandHandle, double gravFactor) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_gravFactor = gravFactor; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_GRAVITY_FACTOR; + return 0; +} + B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; @@ -432,6 +450,15 @@ B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle co return 0; } +B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int allDirectionDamping) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_dampAllDirections = allDirectionDamping; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE; + return 0; +} + B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; @@ -2231,6 +2258,22 @@ B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle return 0; } +B3_SHARED_API int b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle, double scaling[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_SCALING; + + command->m_initPoseArgs.m_scaling[0] = scaling[0]; + command->m_initPoseArgs.m_scaling[1] = scaling[1]; + command->m_initPoseArgs.m_scaling[2] = scaling[2]; + + return 0; +} + + + B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; @@ -3131,6 +3174,32 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryComma return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLowerLimit, double jointUpperLimit) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_jointLowerLimit = jointLowerLimit; + command->m_changeDynamicsInfoArgs.m_jointUpperLimit = jointUpperLimit; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS; + return 0; +} + +B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_jointLimitForce = jointLimitForce; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE; + return 0; +} + + + + B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[]) { @@ -3641,6 +3710,11 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[0] = rayToWorldX; command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[1] = rayToWorldY; command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[2] = rayToWorldZ; + command->m_requestRaycastIntersections.m_reportHitNumber = -1; + command->m_requestRaycastIntersections.m_collisionFilterMask = -1; + command->m_requestRaycastIntersections.m_fractionEpsilon = B3_EPSILON; + + return (b3SharedMemoryCommandHandle)command; } @@ -3659,6 +3733,10 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys command->m_requestRaycastIntersections.m_numThreads = 1; command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1; command->m_requestRaycastIntersections.m_parentLinkIndex=-1; + command->m_requestRaycastIntersections.m_reportHitNumber = -1; + command->m_requestRaycastIntersections.m_collisionFilterMask = -1; + command->m_requestRaycastIntersections.m_fractionEpsilon = B3_EPSILON; + return (b3SharedMemoryCommandHandle)command; } @@ -3717,6 +3795,29 @@ B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle com command->m_requestRaycastIntersections.m_parentLinkIndex = parentLinkIndex; } +B3_SHARED_API void b3RaycastBatchSetReportHitNumber(b3SharedMemoryCommandHandle commandHandle, int reportHitNumber) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_reportHitNumber= reportHitNumber; +} +B3_SHARED_API void b3RaycastBatchSetCollisionFilterMask(b3SharedMemoryCommandHandle commandHandle, int collisionFilterMask) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_collisionFilterMask = collisionFilterMask; +} + +B3_SHARED_API void b3RaycastBatchSetFractionEpsilon(b3SharedMemoryCommandHandle commandHandle, double fractionEpsilon) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_fractionEpsilon = fractionEpsilon; +} + B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 7c52708a3..0011337e4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -152,8 +152,11 @@ extern "C" B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[]); B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[]); + B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLowerLimit, double jointUpperLimit); + B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce); + B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); @@ -574,6 +577,8 @@ extern "C" B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, const double linVel[/*3*/]); B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, const double angVel[/*3*/]); + B3_SHARED_API int b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle, double scaling[/* 3*/]); + B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); @@ -625,7 +630,10 @@ extern "C" //max num rays for b3RaycastBatchAddRays is MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays); B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex); - + B3_SHARED_API void b3RaycastBatchSetReportHitNumber(b3SharedMemoryCommandHandle commandHandle, int reportHitNumber); + B3_SHARED_API void b3RaycastBatchSetCollisionFilterMask(b3SharedMemoryCommandHandle commandHandle, int collisionFilterMask); + B3_SHARED_API void b3RaycastBatchSetFractionEpsilon(b3SharedMemoryCommandHandle commandHandle, double fractionEpsilon); + B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); /// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates. @@ -648,9 +656,11 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision); - B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact); + B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness); + B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness); + B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int useAllDirectionDamping); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]); diff --git a/examples/SharedMemory/PhysicsClientTCP.cpp b/examples/SharedMemory/PhysicsClientTCP.cpp index 6a722cafb..3a8f3fdd1 100644 --- a/examples/SharedMemory/PhysicsClientTCP.cpp +++ b/examples/SharedMemory/PhysicsClientTCP.cpp @@ -64,9 +64,10 @@ struct TcpNetworkedInternalData { m_tcpSocket.SetSendTimeout(m_timeOutInSeconds, 0); m_tcpSocket.SetReceiveTimeout(m_timeOutInSeconds, 0); + int key = SHARED_MEMORY_MAGIC_NUMBER; + m_tcpSocket.Send((uint8*)&key, 4); } - int key = SHARED_MEMORY_MAGIC_NUMBER; - m_tcpSocket.Send((uint8*)&key, 4); + return m_isConnected; } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 4b842b29b..7c0edfbf1 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1049,7 +1049,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: { - b3Warning("requestOpenGLVisualizeCamera failed"); + //b3Warning("requestOpenGLVisualizeCamera failed"); break; } case CMD_REMOVE_USER_CONSTRAINT_FAILED: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d27b6f4d8..7b4193ac3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -14,6 +14,9 @@ #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" + + #include "../Utils/b3BulletDefaultFileIO.h" #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" @@ -93,11 +96,10 @@ #include "../TinyAudio/b3SoundEngine.h" #endif - #ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #define SKIP_DEFORMABLE_BODY 1 #endif - + #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" #include "BulletSoftBody/btSoftBodySolvers.h" @@ -107,18 +109,16 @@ #include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" #include "../SoftDemo/BunnyMesh.h" -#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_DEFORMABLE_BODY #include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" #include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" -#endif//SKIP_DEFORMABLE_BODY +#endif //SKIP_DEFORMABLE_BODY #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" - - int gInternalSimFlags = 0; bool gResetSimulation = 0; int gVRTrackingObjectUniqueId = -1; @@ -310,9 +310,16 @@ struct InteralUserConstraintData b3UserConstraint m_userConstraintData; + int m_sbHandle; + int m_sbNodeIndex; + btScalar m_sbNodeMass; + InteralUserConstraintData() : m_rbConstraint(0), - m_mbConstraint(0) + m_mbConstraint(0), + m_sbHandle(-1), + m_sbNodeIndex(-1), + m_sbNodeMass(-1) { } }; @@ -1607,6 +1614,13 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; btScalar m_numSimulationSubSteps; + + btScalar getDeltaTimeSubStep() const + { + btScalar deltaTimeSubStep = m_numSimulationSubSteps > 0 ? m_physicsDeltaTime / m_numSimulationSubSteps : m_physicsDeltaTime; + return deltaTimeSubStep; + } + btScalar m_simulationTimestamp; btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks; b3HashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies; @@ -1636,13 +1650,14 @@ struct PhysicsServerCommandProcessorInternalData btDefaultCollisionConfiguration* m_collisionConfiguration; - #ifndef SKIP_DEFORMABLE_BODY - + btSoftBody* m_pickedSoftBody; + btDeformableMousePickingForce* m_mouseForce; + btScalar m_maxPickingForce; btDeformableBodySolver* m_deformablebodySolver; - btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; + btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; #endif - + btMultiBodyDynamicsWorld* m_dynamicsWorld; int m_constraintSolverType; @@ -1668,6 +1683,7 @@ struct PhysicsServerCommandProcessorInternalData //data for picking objects class btRigidBody* m_pickedBody; + int m_savedActivationState; class btTypedConstraint* m_pickedConstraint; class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; @@ -1710,6 +1726,9 @@ struct PhysicsServerCommandProcessorInternalData m_solver(0), m_collisionConfiguration(0), #ifndef SKIP_DEFORMABLE_BODY + m_pickedSoftBody(0), + m_mouseForce(0), + m_maxPickingForce(0.3), m_deformablebodySolver(0), #endif m_dynamicsWorld(0), @@ -2019,6 +2038,12 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() if (m_data->m_threadPool) delete m_data->m_threadPool; + for (int i = 0; i < m_data->m_savedStates.size(); i++) + { + delete m_data->m_savedStates[i].m_bulletFile; + delete m_data->m_savedStates[i].m_serializer; + } + delete m_data; } @@ -2496,6 +2521,17 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface } } } + + //delete textures + for (int i = 0; i < textures.size(); i++) + { + B3_PROFILE("free textureData"); + if (!textures[i].m_isCached) + { + free(textures[i].textureData1); + } + } + return graphicsIndex; } @@ -2608,9 +2644,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld() { btDeformableMultiBodyDynamicsWorld* world = 0; - if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD) + if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType() == BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD) { - world = (btDeformableMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world = (btDeformableMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; } return world; } @@ -2618,9 +2654,9 @@ btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformable btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld() { btSoftMultiBodyDynamicsWorld* world = 0; - if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD) + if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType() == BT_SOFT_MULTIBODY_DYNAMICS_WORLD) { - world = (btSoftMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world = (btSoftMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; } return world; } @@ -2646,43 +2682,44 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags) m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback); //int maxProxies = 32768; - if (flags&RESET_USE_SIMPLE_BROADPHASE) + if (flags & RESET_USE_SIMPLE_BROADPHASE) { m_data->m_broadphase = new btSimpleBroadphase(65536, m_data->m_pairCache); - } else + } + else { btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache); bv->setVelocityPrediction(0); m_data->m_broadphase = bv; } - if (flags & RESET_USE_DEFORMABLE_WORLD) + if (flags & RESET_USE_DEFORMABLE_WORLD) { #ifndef SKIP_DEFORMABLE_BODY - m_data->m_deformablebodySolver = new btDeformableBodySolver(); - btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; - m_data->m_solver = solver; - solver->setDeformableSolver(m_data->m_deformablebodySolver); - m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); + m_data->m_deformablebodySolver = new btDeformableBodySolver(); + btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; + m_data->m_solver = solver; + solver->setDeformableSolver(m_data->m_deformablebodySolver); + m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); #endif } - if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD))) + if ((0 == m_data->m_dynamicsWorld) && (0 == (flags & RESET_USE_DISCRETE_DYNAMICS_WORLD))) { - m_data->m_solver = new btMultiBodyConstraintSolver; + m_data->m_solver = new btMultiBodyConstraintSolver; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); #else - m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); #endif } - if (0==m_data->m_dynamicsWorld) + if (0 == m_data->m_dynamicsWorld) { m_data->m_solver = new btMultiBodyConstraintSolver; m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); } - + //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024); @@ -2840,43 +2877,42 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() delete mb; } #ifndef SKIP_DEFORMABLE_BODY - for (int j = 0; j < m_data->m_lf.size(); j++) - { - btDeformableLagrangianForce* force = m_data->m_lf[j]; - delete force; - } - m_data->m_lf.clear(); + for (int j = 0; j < m_data->m_lf.size(); j++) + { + btDeformableLagrangianForce* force = m_data->m_lf[j]; + delete force; + } + m_data->m_lf.clear(); #endif #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD { btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); if (softWorld) { - for (i =softWorld->getSoftBodyArray().size() - 1; i >= 0; i--) + for (i = softWorld->getSoftBodyArray().size() - 1; i >= 0; i--) { - btSoftBody* sb =softWorld->getSoftBodyArray()[i]; + btSoftBody* sb = softWorld->getSoftBodyArray()[i]; softWorld->removeSoftBody(sb); delete sb; } } } -#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_DEFORMABLE_BODY - { + { btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { - for (i =deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) + for (i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) { - btSoftBody* sb =deformWorld->getSoftBodyArray()[i]; + btSoftBody* sb = deformWorld->getSoftBodyArray()[i]; deformWorld->removeSoftBody(sb); delete sb; } } } #endif - } for (int i = 0; i < constraints.size(); i++) @@ -3014,7 +3050,8 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) } } -int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, const char* valueBytes, int valueLength, int valueType) { +int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, const char* valueBytes, int valueLength, int valueType) +{ InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (!body) { @@ -3045,13 +3082,16 @@ int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex, return userDataHandle; } -void PhysicsServerCommandProcessor::addUserData(const btHashMap<btHashString, std::string>& user_data_entries, int bodyUniqueId, int linkIndex, int visualShapeIndex) { - for (int i = 0; i < user_data_entries.size(); ++i) { +void PhysicsServerCommandProcessor::addUserData(const btHashMap<btHashString, std::string>& user_data_entries, int bodyUniqueId, int linkIndex, int visualShapeIndex) +{ + for (int i = 0; i < user_data_entries.size(); ++i) + { const std::string key = user_data_entries.getKeyAtIndex(i).m_string1; const std::string* value = user_data_entries.getAtIndex(i); - if (value) { + if (value) + { addUserData(bodyUniqueId, linkIndex, visualShapeIndex, key.c_str(), value->c_str(), - value->size()+1, USER_DATA_VALUE_TYPE_STRING); + value->size() + 1, USER_DATA_VALUE_TYPE_STRING); } } } @@ -3213,15 +3253,15 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, #ifdef B3_ENABLE_TINY_AUDIO { SDFAudioSource audioSource; - int urdfLinkIndex = creation.m_mb2urdfLink[link]; + int urdfLinkIndex = creation.m_mb2urdfLink[i]; if (u2b.getLinkAudioSource(urdfLinkIndex, audioSource)) { - int flags = mb->getLink(link).m_collider->getCollisionFlags(); + int flags = mb->getLink(i).m_collider->getCollisionFlags(); mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); if (audioSource.m_userIndex >= 0) { - bodyHandle->m_audioSources.insert(link, audioSource); + bodyHandle->m_audioSources.insert(i, audioSource); } } } @@ -3330,17 +3370,38 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, } } - const UrdfModel* urdfModel = u2b.getUrdfModel(); - if (urdfModel) { - addUserData(urdfModel->m_userData, bodyUniqueId); - for (int linkIndex = 0; linkIndex < urdfModel->m_links.size(); ++linkIndex) { - const UrdfLink* link = *urdfModel->m_links.getAtIndex(linkIndex); - addUserData(link->m_userData, bodyUniqueId, linkIndex - 1); - for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) { - addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, linkIndex - 1, visualShapeIndex); + // Because the link order between UrdfModel and MultiBody may be different, + // create a mapping from link name to link index in order to apply the user + // data to the correct link in the MultiBody. + btHashMap<btHashString, int> linkNameToIndexMap; + if (bodyHandle->m_multiBody) + { + btMultiBody* mb = bodyHandle->m_multiBody; + linkNameToIndexMap.insert(mb->getBaseName(), -1); + for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex) + { + linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex); + } + } + + const UrdfModel* urdfModel = u2b.getUrdfModel(); + if (urdfModel) + { + addUserData(urdfModel->m_userData, bodyUniqueId); + for (int i = 0; i < urdfModel->m_links.size(); ++i) + { + const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); + int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str()); + if (linkIndex) + { + addUserData(link->m_userData, bodyUniqueId, *linkIndex); + for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) + { + addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex); + } + } } } - } b3Notification notification; notification.m_notificationType = BODY_ADDED; @@ -3484,7 +3545,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (loadOk) { - btTransform rootTrans; rootTrans.setOrigin(pos); rootTrans.setRotation(orn); @@ -4993,30 +5053,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); glmesh = LoadMeshFromObj(relativeFileName, pathPrefix, fileIO); } - if (glmesh) - { - if (compound == 0) - { - compound = worldImporter->createCompoundShape(); - } - - btTriangleMesh* meshInterface = new btTriangleMesh(); - m_data->m_meshInterfaces.push_back(meshInterface); - - for (int i = 0; i < glmesh->m_numIndices / 3; i++) - { - float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3)).xyzw; - float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 1)).xyzw; - float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 2)).xyzw; - meshInterface->addTriangle( - btVector3(v0[0], v0[1], v0[2])* meshScale, - btVector3(v1[0], v1[1], v1[2])* meshScale, - btVector3(v2[0], v2[1], v2[2])* meshScale); - } - - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true); - compound->addChildShape(childTransform, trimesh); - } + //btBvhTriangleMeshShape is created below } else { @@ -5049,6 +5086,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str convexHull->optimizeConvexHull(); compound->addChildShape(childTransform, convexHull); + delete glmesh; + glmesh = 0; } if (out_type == UrdfGeometry::FILE_OBJ) { @@ -5078,19 +5117,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { btVector3 pt; pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0], - attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], - attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], - attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], - attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], - attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], - attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); } @@ -5286,7 +5325,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S if (bodyHandle->m_multiBody) { //collision shape - if (clientCmd.m_requestMeshDataArgs.m_linkIndex == -1) { @@ -5308,20 +5346,22 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S btTransform tr; tr.setIdentity(); int collisionShapeIndex = -1; - if (clientCmd.m_updateFlags& B3_MESH_DATA_COLLISIONSHAPEINDEX) + if (clientCmd.m_updateFlags & B3_MESH_DATA_COLLISIONSHAPEINDEX) { collisionShapeIndex = clientCmd.m_requestMeshDataArgs.m_collisionShapeIndex; } gatherVertices(tr, colShape, vertices, collisionShapeIndex); - + int numVertices = vertices.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); - for (int i = 0; i < verticesCopied; ++i) + + if (verticesCopied > 0) { - verticesOut[i] = vertices[i]; + memcpy(verticesOut, &vertices[0], sizeof(btVector3) * verticesCopied); } + sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; @@ -5334,13 +5374,13 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S if (bodyHandle->m_softBody) { btSoftBody* psb = bodyHandle->m_softBody; - - bool separateRenderMesh = (psb->m_renderNodes.size() != 0); - int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); + + bool separateRenderMesh = (psb->m_renderNodes.size() != 0); + int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); - + for (int i = 0; i < verticesCopied; ++i) { if (separateRenderMesh) @@ -5799,7 +5839,6 @@ bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct Sha serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; } - if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) { m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); @@ -5955,6 +5994,65 @@ struct CastSyncInfo }; #endif // __cplusplus >= 201103L +struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback +{ + FilteredClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask) + : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), + m_collisionFilterMask(collisionFilterMask) + { + } + + int m_collisionFilterMask; + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) + { + bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; + if (!collides) + return m_closestHitFraction; + return btCollisionWorld::ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); + } +}; + +struct FilteredAllHitsRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback +{ + FilteredAllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask, btScalar fractionEpsilon) + : btCollisionWorld::AllHitsRayResultCallback(rayFromWorld, rayToWorld), + m_collisionFilterMask(collisionFilterMask), + m_fractionEpsilon(fractionEpsilon) + { + } + + int m_collisionFilterMask; + btScalar m_fractionEpsilon; + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) + { + bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; + if (!collides) + return m_closestHitFraction; + //remove duplicate hits: + //same collision object, link index and hit fraction + bool isDuplicate = false; + + for (int i = 0; i < m_collisionObjects.size(); i++) + { + if (m_collisionObjects[i] == rayResult.m_collisionObject) + { + btScalar diffFraction = m_hitFractions[i] - rayResult.m_hitFraction; + if (btEqual(diffFraction, m_fractionEpsilon)) + { + isDuplicate = true; + break; + } + } + } + if (isDuplicate) + return m_closestHitFraction; + + return btCollisionWorld::AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); + } +}; + struct BatchRayCaster { b3ThreadPool* m_threadPool; @@ -5963,9 +6061,12 @@ struct BatchRayCaster const b3RayData* m_rayInputBuffer; b3RayHitInfo* m_hitInfoOutputBuffer; int m_numRays; + int m_reportHitNumber; + int m_collisionFilterMask; + btScalar m_fractionEpsilon; - BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays) - : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays) + BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber, int collisionFilterMask, btScalar fractionEpsilon) + : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays), m_reportHitNumber(reportHitNumber), m_collisionFilterMask(collisionFilterMask), m_fractionEpsilon(fractionEpsilon) { m_syncInfo = new CastSyncInfo; } @@ -6034,10 +6135,26 @@ struct BatchRayCaster btVector3 rayFromWorld(from[0], from[1], from[2]); btVector3 rayToWorld(to[0], to[1], to[2]); - btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld, rayToWorld); + FilteredClosestRayResultCallback rayResultCallback(rayFromWorld, rayToWorld, m_collisionFilterMask); rayResultCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; - - m_world->rayTest(rayFromWorld, rayToWorld, rayResultCallback); + if (m_reportHitNumber >= 0) + { + //compute all hits, and select the m_reportHitNumber, if available + FilteredAllHitsRayResultCallback allResultsCallback(rayFromWorld, rayToWorld, m_collisionFilterMask, m_fractionEpsilon); + allResultsCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; + m_world->rayTest(rayFromWorld, rayToWorld, allResultsCallback); + if (allResultsCallback.m_collisionObjects.size() > m_reportHitNumber) + { + rayResultCallback.m_collisionObject = allResultsCallback.m_collisionObjects[m_reportHitNumber]; + rayResultCallback.m_closestHitFraction = allResultsCallback.m_hitFractions[m_reportHitNumber]; + rayResultCallback.m_hitNormalWorld = allResultsCallback.m_hitNormalWorld[m_reportHitNumber]; + rayResultCallback.m_hitPointWorld = allResultsCallback.m_hitPointWorld[m_reportHitNumber]; + } + } + else + { + m_world->rayTest(rayFromWorld, rayToWorld, rayResultCallback); + } b3RayHitInfo& hit = m_hitInfoOutputBuffer[ray]; if (rayResultCallback.hasHit()) @@ -6048,8 +6165,14 @@ struct BatchRayCaster int linkIndex = -1; const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD const btSoftBody* softBody = btSoftBody::upcast(rayResultCallback.m_collisionObject); - if (body || softBody) + if (softBody) + { + objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); + } +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + if (body) { objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); } @@ -6108,6 +6231,9 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co const int numStreamingRays = clientCmd.m_requestRaycastIntersections.m_numStreamingRays; const int totalRays = numCommandRays + numStreamingRays; int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads; + int reportHitNumber = clientCmd.m_requestRaycastIntersections.m_reportHitNumber; + int collisionFilterMask = clientCmd.m_requestRaycastIntersections.m_collisionFilterMask; + btScalar fractionEpsilon = clientCmd.m_requestRaycastIntersections.m_fractionEpsilon; if (numThreads == 0) { // When 0 is specified, Bullet can decide how many threads to use. @@ -6176,7 +6302,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co } } - BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays); + BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays, reportHitNumber, collisionFilterMask, fractionEpsilon); batchRayCaster.castRays(numThreads); serverStatusOut.m_numDataStreamBytes = totalRays * sizeof(b3RayData); @@ -6282,7 +6408,7 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar int usz = m_data->m_userConstraints.size(); int* constraintUid = bodyUids + actualNumBodies; serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; - + for (int i = 0; i < usz; i++) { int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1(); @@ -6307,14 +6433,16 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar } else { - for (int i=0; i<clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies; ++i) { + for (int i = 0; i < clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies; ++i) + { const int bodyUniqueId = clientCmd.m_syncUserDataRequestArgs.m_requestedBodyIds[i]; InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (!body) { return hasStatus; } - for (int j=0; j < body->m_userDataHandles.size(); ++j) { + for (int j = 0; j < body->m_userDataHandles.size(); ++j) + { userDataHandles.push_back(body->m_userDataHandles[j]); } } @@ -6329,7 +6457,7 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size(); serverStatusOut.m_numDataStreamBytes = sizeInBytes; serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; - + return hasStatus; } @@ -6379,7 +6507,8 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share addUserDataArgs.m_visualShapeIndex, addUserDataArgs.m_key, bufferServerToClient, addUserDataArgs.m_valueLength, addUserDataArgs.m_valueType); - if (userDataHandle < 0) { + if (userDataHandle < 0) + { return hasStatus; } @@ -6465,34 +6594,37 @@ bool PhysicsServerCommandProcessor::processCollisionFilterCommand(const struct S if (clientCmd.m_updateFlags & B3_COLLISION_FILTER_GROUP_MASK) { InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA); - btCollisionObject* colObj = 0; - if (body->m_multiBody) + if (body) { - if (clientCmd.m_collisionFilterArgs.m_linkIndexA == -1) + btCollisionObject* colObj = 0; + if (body->m_multiBody) { - colObj = body->m_multiBody->getBaseCollider(); + if (clientCmd.m_collisionFilterArgs.m_linkIndexA == -1) + { + colObj = body->m_multiBody->getBaseCollider(); + } + else + { + if (clientCmd.m_collisionFilterArgs.m_linkIndexA >= 0 && clientCmd.m_collisionFilterArgs.m_linkIndexA < body->m_multiBody->getNumLinks()) + { + colObj = body->m_multiBody->getLinkCollider(clientCmd.m_collisionFilterArgs.m_linkIndexA); + } + } } else { - if (clientCmd.m_collisionFilterArgs.m_linkIndexA >= 0 && clientCmd.m_collisionFilterArgs.m_linkIndexA < body->m_multiBody->getNumLinks()) + if (body->m_rigidBody) { - colObj = body->m_multiBody->getLinkCollider(clientCmd.m_collisionFilterArgs.m_linkIndexA); + colObj = body->m_rigidBody; } } - } - else - { - if (body->m_rigidBody) + if (colObj) { - colObj = body->m_rigidBody; + colObj->getBroadphaseHandle()->m_collisionFilterGroup = clientCmd.m_collisionFilterArgs.m_collisionFilterGroup; + colObj->getBroadphaseHandle()->m_collisionFilterMask = clientCmd.m_collisionFilterArgs.m_collisionFilterMask; + m_data->m_dynamicsWorld->refreshBroadphaseProxy(colObj); } } - if (colObj) - { - colObj->getBroadphaseHandle()->m_collisionFilterGroup = clientCmd.m_collisionFilterArgs.m_collisionFilterGroup; - colObj->getBroadphaseHandle()->m_collisionFilterMask = clientCmd.m_collisionFilterArgs.m_collisionFilterMask; - m_data->m_dynamicsWorld->refreshBroadphaseProxy(colObj); - } } } return true; @@ -6703,10 +6835,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct //disable velocity clamp in velocity mode motor->setRhsClamp(SIMD_INFINITY); - btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f * m_data->getDeltaTimeSubStep(); if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) { - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] * m_data->m_physicsDeltaTime; + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] * m_data->getDeltaTimeSubStep(); } motor->setMaxAppliedImpulse(maxImp); } @@ -6786,10 +6918,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct } motor->setPositionTarget(desiredPosition, kp); - btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f * m_data->getDeltaTimeSubStep(); if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime; + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->getDeltaTimeSubStep(); motor->setMaxAppliedImpulse(maxImp); } @@ -6854,10 +6986,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct //} motor->setPositionTarget(desiredPosition, kp); - btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f * m_data->getDeltaTimeSubStep(); if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime; + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->getDeltaTimeSubStep(); motor->setMaxAppliedImpulse(maxImp); } @@ -6968,7 +7100,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct Eigen::MatrixXd M = rbdModel->GetMassMat(); //rbdModel->UpdateBiasForce(); const Eigen::VectorXd& C = rbdModel->GetBiasForce(); - M.diagonal() += m_data->m_physicsDeltaTime * mKd; + M.diagonal() += m_data->getDeltaTimeSubStep() * mKd; Eigen::VectorXd pose_inc; @@ -7228,7 +7360,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct } } } //fi - //break; + //break; } } } //if (body && body->m_rigidBody) @@ -7370,7 +7502,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor) { btScalar impulse = motor->getAppliedImpulse(d); - btScalar force = impulse / m_data->m_physicsDeltaTime; + btScalar force = impulse / m_data->getDeltaTimeSubStep(); stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; } } @@ -7382,7 +7514,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor && m_data->m_physicsDeltaTime > btScalar(0)) { - btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime; + btScalar force = motor->getAppliedImpulse(0) / m_data->getDeltaTimeSubStep(); stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; } } @@ -7420,7 +7552,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor && m_data->m_physicsDeltaTime > btScalar(0)) { - btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime; + btScalar force = motor->getAppliedImpulse(0) / m_data->getDeltaTimeSubStep(); stateDetails->m_jointMotorForce[l] = force; //if (force>0) @@ -7542,7 +7674,6 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage); serverCmd.m_sendActualStateArgs.m_stateDetails = 0; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = @@ -7559,7 +7690,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3]; - btVector3 center_of_mass(sb->getCenterOfMass()); + btVector3 center_of_mass(sb->getCenterOfMass()); btTransform tr = sb->getRigidTransform(); //base position in world space, cartesian stateDetails->m_actualStateQ[0] = center_of_mass[0]; @@ -7709,13 +7840,22 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand pt.m_linkIndexB = linkIndexB; for (int j = 0; j < 3; j++) { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + if (swap) + { + pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j]; + } + else + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } } - pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; - pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->m_physicsDeltaTime; - pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_data->m_physicsDeltaTime; + pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->getDeltaTimeSubStep(); + pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->getDeltaTimeSubStep(); + pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_data->getDeltaTimeSubStep(); for (int j = 0; j < 3; j++) { pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j]; @@ -7962,7 +8102,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand cb.m_bodyUniqueIdA = bodyUniqueIdA; cb.m_bodyUniqueIdB = bodyUniqueIdB; - cb.m_deltaTime = m_data->m_physicsDeltaTime; + cb.m_deltaTime = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime; for (int i = 0; i < setA.size(); i++) { @@ -8281,6 +8421,11 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness; } + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE) + { + deformable.m_springCoefficients.damp_all_directions = loadSoftBodyArgs.m_dampAllDirections; + } + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) { deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu; @@ -8297,12 +8442,20 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe { deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff; } - + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS) + { + deformable.m_repulsionStiffness = loadSoftBodyArgs.m_repulsionStiffness; + } + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_GRAVITY_FACTOR) + { + deformable.m_gravFactor = loadSoftBodyArgs.m_gravFactor; + } #endif } bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision) { +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btSoftBody* psb = NULL; CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); char relativeFileName[1024]; @@ -8380,7 +8533,8 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness, deformable.m_springCoefficients.damping_stiffness, - true, deformable.m_springCoefficients.bending_stiffness); + !deformable.m_springCoefficients.damp_all_directions, + deformable.m_springCoefficients.bending_stiffness); deformWorld->addForce(psb, springForce); m_data->m_lf.push_back(springForce); } @@ -8507,13 +8661,17 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo // turn on the collision flag for deformable // collision between deformable and rigid psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - // turn on face contact only for multibodies + // turn on face contact for multibodies psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + /// turn on face contact for rigid body + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; // collion between deformable and deformable and self-collision psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; psb->setCollisionFlags(0); psb->setTotalMass(deformable.m_mass); psb->setSelfCollision(useSelfCollision); + psb->setSpringStiffness(deformable.m_repulsionStiffness); + psb->setGravityFactor(deformable.m_gravFactor); psb->initializeFaceTree(); } #endif //SKIP_DEFORMABLE_BODY @@ -8610,6 +8768,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId; m_data->m_pluginManager.addNotification(notification); } +#endif return true; } @@ -8669,8 +8828,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; -#endif } +#endif return hasStatus; } @@ -9166,6 +9325,78 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { if (linkIndex >= 0 && linkIndex < mb->getNumLinks()) { + + if ((clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) || + (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS)) + { + + btMultiBodyJointLimitConstraint* limC = 0; + + int numConstraints = m_data->m_dynamicsWorld->getNumMultiBodyConstraints(); + for (int c = 0; c < numConstraints; c++) + { + btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(c); + if (mbc->getConstraintType() == MULTIBODY_CONSTRAINT_LIMIT) + { + if (((mbc->getMultiBodyA() == mb) && (mbc->getLinkA() == linkIndex)) + || + ((mbc->getMultiBodyB() == mb) && ((mbc->getLinkB() == linkIndex))) + ) + { + limC = (btMultiBodyJointLimitConstraint*)mbc; + } + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS) + { + //find a joint limit + btScalar prevUpper = mb->getLink(linkIndex).m_jointUpperLimit; + btScalar prevLower = mb->getLink(linkIndex).m_jointLowerLimit; + btScalar lower = clientCmd.m_changeDynamicsInfoArgs.m_jointLowerLimit; + btScalar upper = clientCmd.m_changeDynamicsInfoArgs.m_jointUpperLimit; + bool enableLimit = lower <= upper; + + if (enableLimit) + { + if (limC == 0) + { + limC = new btMultiBodyJointLimitConstraint(mb, linkIndex, lower, upper); + m_data->m_dynamicsWorld->addMultiBodyConstraint(limC); + } + else + { + limC->setLowerBound(lower); + limC->setUpperBound(upper); + } + mb->getLink(linkIndex).m_jointLowerLimit = lower; + mb->getLink(linkIndex).m_jointUpperLimit = upper; + } + else + { + if (limC) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(limC); + delete limC; + limC = 0; + } + mb->getLink(linkIndex).m_jointLowerLimit = 1; + mb->getLink(linkIndex).m_jointUpperLimit = -1; + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) + { + btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime; + btScalar maxImpulse = clientCmd.m_changeDynamicsInfoArgs.m_jointLimitForce * fixedTimeSubStep; + if (limC) + { + //convert from force to impulse + limC->setMaxAppliedImpulse(maxImpulse); + } + } + } + if (mb->getLinkCollider(linkIndex)) { if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) @@ -9425,8 +9656,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY; - + serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY; + btMultiBody* mb = body->m_multiBody; if (linkIndex == -1) { @@ -9544,7 +9775,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY; + serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY; btRigidBody* rb = body->m_rigidBody; serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction(); @@ -9556,7 +9787,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S serverCmd.m_dynamicsInfo.m_collisionMargin = rb->getCollisionShape() ? rb->getCollisionShape()->getMargin() : 0; } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - else if (body && body->m_softBody){ + else if (body && body->m_softBody) + { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY; @@ -9691,9 +9923,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st gforce->m_gravity = grav; } } - } - #endif if (m_data->m_verboseOutput) @@ -9705,8 +9935,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; } - - if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_NONCONTACT_INNER_ITERATIONS) + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_NONCONTACT_INNER_ITERATIONS) { m_data->m_dynamicsWorld->getSolverInfo().m_numNonContactInnerIterations = clientCmd.m_physSimParamArgs.m_numNonContactInnerIterations; } @@ -9771,7 +10001,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { } }; - + if (newSolver) { delete oldSolver; @@ -9836,23 +10066,23 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { #ifndef SKIP_DEFORMABLE_BODY { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) - { - deformWorld ->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); - deformWorld ->getWorldInfo().m_sparsesdf.Reset(); - } - } + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + deformWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); + deformWorld->getWorldInfo().m_sparsesdf.Reset(); + } + } #endif #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - { - btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); - if (softWorld) - { - softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); - softWorld->getWorldInfo().m_sparsesdf.Reset(); - } - } + { + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); + softWorld->getWorldInfo().m_sparsesdf.Reset(); + } + } #endif } @@ -9890,7 +10120,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; - + BT_PROFILE("CMD_INIT_POSE"); if (m_data->m_verboseOutput) @@ -9935,6 +10165,20 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe { btMultiBody* mb = body->m_multiBody; + if (clientCmd.m_updateFlags & INIT_POSE_HAS_SCALING) + { + btVector3 scaling(clientCmd.m_initPoseArgs.m_scaling[0], clientCmd.m_initPoseArgs.m_scaling[1], clientCmd.m_initPoseArgs.m_scaling[2]); + + mb->getBaseCollider()->getCollisionShape()->setLocalScaling(scaling); + //refresh broadphase + m_data->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs( + mb->getBaseCollider()->getBroadphaseHandle(), + m_data->m_dynamicsWorld->getDispatcher()); + //also visuals + int graphicsIndex = mb->getBaseCollider()->getUserIndex(); + m_data->m_guiHelper->changeScaling(graphicsIndex, clientCmd.m_initPoseArgs.m_scaling); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) { mb->setBaseVel(baseLinVel); @@ -10107,7 +10351,7 @@ bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct S bool hasStatus = true; BT_PROFILE("CMD_RESET_SIMULATION"); m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0); - + resetSimulation(clientCmd.m_updateFlags); m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 1); @@ -10796,7 +11040,7 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc int link = clientCmd.m_externalForceArguments.m_linkIds[i]; btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpForce : tmpForce; - btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); + btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpPosition : tmpPosition - mb->getLink(link).m_cachedWorldTransform.getOrigin(); mb->addLinkForce(link, forceWorld); mb->addLinkTorque(link, relPosWorld.cross(forceWorld)); //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); @@ -10852,6 +11096,32 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc rb->applyTorque(torqueWorld); } } + +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + if (body && body->m_softBody) + { + btSoftBody* sb = body->m_softBody; + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]); + + btVector3 forceWorld = isLinkFrame ? forceLocal : sb->getWorldTransform().getBasis() * forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : sb->getWorldTransform().getBasis() * positionLocal; + if (link >= 0 && link < sb->m_nodes.size()) + { + sb->addForce(forceWorld, link); + } + } + } +#endif + } SharedMemoryStatus& serverCmd = serverStatusOut; @@ -10986,7 +11256,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared { deformWorld->removeSoftBody(psb); } - + int graphicsInstance = psb->getUserIndex2(); m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); delete psb; @@ -11080,30 +11350,36 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str if (sbodyHandle->m_softBody) { int nodeIndex = clientCmd.m_userConstraintArguments.m_parentJointIndex; - if (nodeIndex>=0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size()) + if (nodeIndex >= 0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size()) { int bodyUniqueId = clientCmd.m_userConstraintArguments.m_childBodyIndex; - if (bodyUniqueId<=0) + if (bodyUniqueId <= 0) { //fixed anchor (mass = 0) - sbodyHandle->m_softBody->setMass(nodeIndex,0.0); + InteralUserConstraintData userConstraintData; + userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex; + userConstraintData.m_sbNodeIndex = nodeIndex; + userConstraintData.m_sbNodeMass = sbodyHandle->m_softBody->getMass(nodeIndex); + sbodyHandle->m_softBody->setMass(nodeIndex, 0.0); int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid, userConstraintData); serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } else + } + else { InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (mbodyHandle && mbodyHandle->m_multiBody) { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex; - if (linkIndex<0) + if (linkIndex < 0) { sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider()); - } else + } + else { if (linkIndex < mbodyHandle->m_multiBody->getNumLinks()) { @@ -11129,22 +11405,22 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str { bool disableCollisionBetweenLinkedBodies = true; btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0], - clientCmd.m_userConstraintArguments.m_childFrame[1], - clientCmd.m_userConstraintArguments.m_childFrame[2]); - + clientCmd.m_userConstraintArguments.m_childFrame[1], + clientCmd.m_userConstraintArguments.m_childFrame[2]); + sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies); - } + } #endif } int uid = m_data->m_userConstraintUIDGenerator++; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + InteralUserConstraintData userConstraintData; + userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex; + userConstraintData.m_sbNodeIndex = nodeIndex; + m_data->m_userConstraints.insert(uid, userConstraintData); serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - } - - } } #endif @@ -11493,6 +11769,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) { + btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime; + serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED; int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); @@ -11525,7 +11803,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str } if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * m_data->m_physicsDeltaTime; + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * fixedTimeSubStep; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); } @@ -11556,7 +11835,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str { if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * m_data->m_physicsDeltaTime; + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * fixedTimeSubStep; userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); } @@ -11595,6 +11874,27 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str delete userConstraintPtr->m_rbConstraint; m_data->m_userConstraints.remove(userConstraintUidRemove); } +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + + if (userConstraintPtr->m_sbHandle >= 0) + { + InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(userConstraintPtr->m_sbHandle); + if (sbodyHandle) + { + if (sbodyHandle->m_softBody) + { + if (userConstraintPtr->m_sbNodeMass >= 0) + { + sbodyHandle->m_softBody->setMass(userConstraintPtr->m_sbNodeIndex, userConstraintPtr->m_sbNodeMass); + } + else + { + sbodyHandle->m_softBody->removeAnchor(userConstraintPtr->m_sbNodeIndex); + } + } + } + } +#endif serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; } @@ -12448,7 +12748,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(cons { //extract shape info from base collider int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects); - serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes*sizeof(b3CollisionShapeData); + serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes * sizeof(b3CollisionShapeData); serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes; serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED; } @@ -12683,6 +12983,20 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct m_data->m_guiHelper->changeSpecularColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_specularColor); } } +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + + else if (bodyHandle->m_softBody) + { + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + if (m_data->m_pluginManager.getRenderInterface()) + { + m_data->m_pluginManager.getRenderInterface()->changeRGBAColor(bodyUniqueId, linkIndex, + clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + } + } +#endif } } } @@ -13519,35 +13833,35 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) - { - for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++) + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) { - btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i]; - if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++) { - //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); - btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),deformWorld->getDrawFlags()); + btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i]; + if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), deformWorld->getDrawFlags()); + } } } } - } { - btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); - if (softWorld) - { - for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++) + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) { - btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i]; - if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++) { - //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); - btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),softWorld->getDrawFlags()); + btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i]; + if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), softWorld->getDrawFlags()); + } } } } - } #endif } } @@ -13555,8 +13869,11 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback { + int m_faceId; + MyResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) - : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld) + : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), + m_faceId(-1) { } @@ -13564,6 +13881,34 @@ struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback { return true; } + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) + { + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_collisionObject = rayResult.m_collisionObject; + if (rayResult.m_localShapeInfo) + { + m_faceId = rayResult.m_localShapeInfo->m_triangleIndex; + } + else + { + m_faceId = -1; + } + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } + else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; + } + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } }; bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) @@ -13621,6 +13966,31 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons world->addMultiBodyConstraint(p2p); m_data->m_pickingMultiBodyPoint2Point = p2p; } + else + { +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + //deformable/soft body? + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (psb) + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + int face_id = rayCallback.m_faceId; + if (face_id >= 0 && face_id < psb->m_faces.size()) + { + m_data->m_pickedSoftBody = psb; + psb->setActivationState(DISABLE_DEACTIVATION); + const btSoftBody::Face& f = psb->m_faces[face_id]; + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0, f, pickPos, m_data->m_maxPickingForce); + m_data->m_mouseForce = mouse_force; + + deformWorld->addForce(psb, mouse_force); + } + } + } +#endif + } } // pickObject(pickPos, rayCallback.m_collisionObject); @@ -13664,6 +14034,21 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); } +#ifndef SKIP_DEFORMABLE_BODY + if (m_data->m_pickedSoftBody) + { + if (m_data->m_pickedSoftBody && m_data->m_mouseForce) + { + btVector3 newPivot; + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_data->m_oldPickingDist; + newPivot = rayFromWorld + dir; + m_data->m_mouseForce->setMousePos(newPivot); + } + } +#endif + return false; } @@ -13685,6 +14070,18 @@ void PhysicsServerCommandProcessor::removePickingConstraint() delete m_data->m_pickingMultiBodyPoint2Point; m_data->m_pickingMultiBodyPoint2Point = 0; } + +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + //deformable/soft body? + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld && m_data->m_mouseForce) + { + deformWorld->removeForce(m_data->m_pickedSoftBody, m_data->m_mouseForce); + delete m_data->m_mouseForce; + m_data->m_mouseForce = 0; + m_data->m_pickedSoftBody = 0; + } +#endif } void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName) @@ -13951,7 +14348,7 @@ void PhysicsServerCommandProcessor::resetSimulation(int flags) btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { - deformWorld ->getWorldInfo().m_sparsesdf.Reset(); + deformWorld->getWorldInfo().m_sparsesdf.Reset(); } } { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 18db2cdd7..cbc4cbab4 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -19,7 +19,7 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface { struct PhysicsServerCommandProcessorInternalData* m_data; - void resetSimulation(int flags=0); + void resetSimulation(int flags = 0); void createThreadPool(); class btDeformableMultiBodyDynamicsWorld* getDeformableWorld(); @@ -120,7 +120,7 @@ public: void createJointMotors(class btMultiBody* body); - virtual void createEmptyDynamicsWorld(int flags=0); + virtual void createEmptyDynamicsWorld(int flags = 0); virtual void deleteDynamicsWorld(); virtual bool connect() diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 7cde642a2..3a159f581 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -128,6 +128,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperRemoveTexture, eGUIHelperSetVisualizerFlagCheckRenderedFrame, eGUIHelperUpdateShape, + eGUIHelperChangeGraphicsInstanceScaling, eGUIUserDebugRemoveAllParameters, }; @@ -1035,6 +1036,19 @@ public: workerThreadWait(); } + int m_graphicsInstanceChangeScaling; + double m_baseScaling[3]; + virtual void changeScaling(int instanceUid, const double scaling[3]) + { + m_graphicsInstanceChangeScaling = instanceUid; + m_baseScaling[0] = scaling[0]; + m_baseScaling[1] = scaling[1]; + m_baseScaling[2] = scaling[2]; + m_cs->lock(); + m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceScaling); + workerThreadWait(); + } + double m_specularColor[3]; int m_graphicsInstanceChangeSpecular; virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) @@ -2181,6 +2195,16 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->mainThreadRelease(); break; } + + case eGUIHelperChangeGraphicsInstanceScaling: + { + B3_PROFILE("eGUIHelperChangeGraphicsInstanceScaling"); + + m_multiThreadedHelper->m_childGuiHelper->changeScaling(m_multiThreadedHelper->m_graphicsInstanceChangeScaling, m_multiThreadedHelper->m_baseScaling); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + case eGUIHelperChangeGraphicsInstanceSpecularColor: { B3_PROFILE("eGUIHelperChangeGraphicsInstanceSpecularColor"); diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp index a7bed235d..b5754e185 100644 --- a/examples/SharedMemory/RemoteGUIHelper.cpp +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -529,6 +529,11 @@ void RemoteGUIHelper::removeGraphicsInstance(int graphicsUid) } } } + +void RemoteGUIHelper::changeScaling(int instanceUid, const double scaling[3]) +{ + +} void RemoteGUIHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); diff --git a/examples/SharedMemory/RemoteGUIHelper.h b/examples/SharedMemory/RemoteGUIHelper.h index 1931091e5..3d74b03fd 100644 --- a/examples/SharedMemory/RemoteGUIHelper.h +++ b/examples/SharedMemory/RemoteGUIHelper.h @@ -38,6 +38,7 @@ struct RemoteGUIHelper : public GUIHelperInterface virtual void removeAllGraphicsInstances(); virtual void removeGraphicsInstance(int graphicsUid); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); + virtual void changeScaling(int instanceUid, const double scaling[3]); virtual Common2dCanvasInterface* get2dCanvasInterface(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c0d00ac72..71529e5b1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -167,6 +167,8 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768, CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY = 1<<16, CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN = 1 << 17, + CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS = 1 << 18, + CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE = 1 << 19, }; struct ChangeDynamicsInfoArgs @@ -192,6 +194,10 @@ struct ChangeDynamicsInfoArgs double m_anisotropicFriction[3]; double m_maxJointVelocity; double m_collisionMargin; + + double m_jointLowerLimit; + double m_jointUpperLimit; + double m_jointLimitForce; }; struct GetDynamicsInfoArgs @@ -215,6 +221,7 @@ enum EnumInitPoseFlags INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8, INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16, INIT_POSE_HAS_JOINT_VELOCITY = 32, + INIT_POSE_HAS_SCALING=64, }; ///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position @@ -228,6 +235,7 @@ struct InitPoseArgs double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_scaling[3]; }; struct RequestDebugLinesArgs @@ -304,6 +312,9 @@ struct RequestRaycastIntersections //optional m_parentObjectUniqueId (-1 for unused) int m_parentObjectUniqueId; int m_parentLinkIndex; + int m_reportHitNumber; + int m_collisionFilterMask; + double m_fractionEpsilon; //streaming ray data stored in shared memory streaming part. (size m_numStreamingRays ) }; @@ -497,17 +508,20 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_UPDATE_MASS = 1<<2, LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 1<<3, LOAD_SOFT_BODY_INITIAL_POSITION = 1<<4, - LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5, - LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6, - LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7, - LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8, - LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9, - LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10, - LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11, - LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12, - LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13, - LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14, - LOAD_SOFT_BODY_SIM_MESH = 1<<15, + LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5, + LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6, + LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7, + LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8, + LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9, + LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10, + LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11, + LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12, + LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13, + LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14, + LOAD_SOFT_BODY_SIM_MESH = 1<<15, + LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16, + LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE = 1<<17, + LOAD_SOFT_BODY_SET_GRAVITY_FACTOR = 1<<18, }; enum EnumSimParamInternalSimFlags @@ -528,6 +542,7 @@ struct LoadSoftBodyArgs double m_initialOrientation[4]; double m_springElasticStiffness; double m_springDampingStiffness; + int m_dampAllDirections; double m_springBendingStiffness; double m_corotatedMu; double m_corotatedLambda; @@ -540,6 +555,8 @@ struct LoadSoftBodyArgs double m_NeoHookeanDamping; int m_useFaceContact; char m_simFileName[MAX_FILENAME_LENGTH]; + double m_repulsionStiffness; + double m_gravFactor; }; struct b3LoadSoftBodyResultArgs @@ -772,7 +789,7 @@ struct CalculateInverseKinematicsArgs // double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; double m_targetPositions[MAX_DEGREE_OF_FREEDOM*3]; int m_numEndEffectorLinkIndices; - double m_targetOrientation[4]; //orientation represented as quaternion, x,y,z,w + double m_targetOrientation[MAX_DEGREE_OF_FREEDOM*4]; //orientation represented as quaternion, x,y,z,w int m_endEffectorLinkIndices[MAX_DEGREE_OF_FREEDOM]; double m_lowerLimit[MAX_DEGREE_OF_FREEDOM]; double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 17f947048..b1689c0b3 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,7 +7,11 @@ //Please don't replace an existing magic number: //instead, only ADD a new one at the top, comment-out previous one -#define SHARED_MEMORY_MAGIC_NUMBER 202002030 + + +#define SHARED_MEMORY_MAGIC_NUMBER 202007060 +//#define SHARED_MEMORY_MAGIC_NUMBER 202005070 +//#define SHARED_MEMORY_MAGIC_NUMBER 202002030 //#define SHARED_MEMORY_MAGIC_NUMBER 202001230 //#define SHARED_MEMORY_MAGIC_NUMBER 201911280 //#define SHARED_MEMORY_MAGIC_NUMBER 201911180 @@ -930,6 +934,7 @@ enum eURDF_Flags URDF_IGNORE_VISUAL_SHAPES = 1 << 20, URDF_IGNORE_COLLISION_SHAPES = 1 << 21, URDF_PRINT_URDF_INFO = 1 << 22, + URDF_GOOGLEY_UNDEFINED_COLORS = 1 << 23, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 7398d930b..f2777e129 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -2309,6 +2309,29 @@ int b3RobotSimulatorClientAPI_NoDirect::createCollisionShape(int shapeType, stru scalarToDouble3(args.m_meshScale, meshScale); shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale); } + if (shapeType == GEOM_HEIGHTFIELD) + { + double meshScale[3]; + scalarToDouble3(args.m_meshScale, meshScale); + if (args.m_fileName) + { + shapeIndex = b3CreateCollisionShapeAddHeightfield(command, args.m_fileName, meshScale, args.m_heightfieldTextureScaling); + } + else + { + if (args.m_heightfieldData.size() && args.m_numHeightfieldRows>0 && args.m_numHeightfieldColumns>0) + { + shapeIndex = b3CreateCollisionShapeAddHeightfield2(sm, command, meshScale, args.m_heightfieldTextureScaling, + &args.m_heightfieldData[0], + args.m_numHeightfieldRows, + args.m_numHeightfieldColumns, + args.m_replaceHeightfieldIndex); + } + } + + + } + if (shapeType == GEOM_PLANE) { double planeConstant = 0; @@ -2546,6 +2569,27 @@ void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId) statusType = b3GetStatusType(statusHandle); } +void b3RobotSimulatorClientAPI_NoDirect::removeState(int stateUniqueId) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) + { + b3Warning("Not connected"); + return; + } + + if (stateUniqueId >= 0) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (b3CanSubmitCommand(sm)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitRemoveStateCommand(sm, stateUniqueId)); + statusType = b3GetStatusType(statusHandle); + } + } +} + bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation& visualShapeInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 1c92746b4..5c8f9e2ff 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -471,12 +471,23 @@ struct b3RobotSimulatorCreateCollisionShapeArgs btVector3 m_meshScale; btVector3 m_planeNormal; int m_flags; + + double m_heightfieldTextureScaling; + btAlignedObjectArray<float> m_heightfieldData; + int m_numHeightfieldRows; + int m_numHeightfieldColumns; + int m_replaceHeightfieldIndex; + b3RobotSimulatorCreateCollisionShapeArgs() : m_shapeType(-1), m_radius(0.5), m_height(1), m_fileName(NULL), - m_flags(0) + m_flags(0), + m_heightfieldTextureScaling(1), + m_numHeightfieldRows(0), + m_numHeightfieldColumns(0), + m_replaceHeightfieldIndex(-1) { m_halfExtents.m_floats[0] = 1; m_halfExtents.m_floats[1] = 1; @@ -889,6 +900,7 @@ public: int saveStateToMemory(); void restoreStateFromMemory(int stateId); + void removeState(int stateUniqueId); int getAPIVersion() const { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp index 224afaf9a..cb6bacd33 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp @@ -93,6 +93,7 @@ bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eige collisionShape = compound->getChildShape(0); } } + switch (collisionShape->getShapeType()) { case BOX_SHAPE_PROXYTYPE: @@ -102,6 +103,7 @@ bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eige param0 = box->getHalfExtentsWithMargin()[0] * 2; param1 = box->getHalfExtentsWithMargin()[1] * 2; param2 = box->getHalfExtentsWithMargin()[2] * 2; + break; } case SPHERE_SHAPE_PROXYTYPE: @@ -124,7 +126,20 @@ bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eige } default: { - btAssert(0); + //approximate by its box + btTransform identity; + identity.setIdentity(); + btVector3 aabbMin, aabbMax; + collisionShape->getAabb(identity, aabbMin, aabbMax); + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); + btScalar margin = collisionShape->getMargin(); + btScalar lx = btScalar(2.) * (halfExtents.x() + margin); + btScalar ly = btScalar(2.) * (halfExtents.y() + margin); + btScalar lz = btScalar(2.) * (halfExtents.z() + margin); + param0 = lx; + param1 = ly; + param2 = lz; + shapeType = cShape::eShapeBox; } } } diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 6d0ce3fa2..937ac28fb 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -66,6 +66,8 @@ struct TinyRendererVisualShapeConverterInternalData // Maps bodyUniqueId to a list of visual shapes belonging to the body. btHashMap<btHashInt, btAlignedObjectArray<b3VisualShapeData> > m_visualShapesMap; + btAlignedObjectArray<unsigned char> m_checkeredTexels; + int m_uidGenerator; int m_upAxis; int m_swWidth; @@ -560,10 +562,61 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa } // case mesh case URDF_GEOM_PLANE: - // TODO: plane in tiny renderer - // TODO: export visualShapeOut for external render - break; + { + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray<int>(); + glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); + glmesh->m_indices->push_back(0); + glmesh->m_indices->push_back(1); + glmesh->m_indices->push_back(2); + glmesh->m_indices->push_back(0); + glmesh->m_indices->push_back(2); + glmesh->m_indices->push_back(3); + glmesh->m_scaling[0] = 1; + glmesh->m_scaling[1] = 1; + glmesh->m_scaling[2] = 1; + glmesh->m_scaling[3] = 1; + + btScalar planeConst = 0; + btVector3 planeNormal = visual->m_geometry.m_planeNormal; + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0, vec1; + btPlaneSpace1(planeNormal, vec0, vec1); + + btScalar vecLen = 128; + btVector3 verts[4]; + + verts[0] = planeOrigin + vec0 * vecLen + vec1 * vecLen; + verts[1] = planeOrigin - vec0 * vecLen + vec1 * vecLen; + verts[2] = planeOrigin - vec0 * vecLen - vec1 * vecLen; + verts[3] = planeOrigin + vec0 * vecLen - vec1 * vecLen; + + GLInstanceVertex vtx; + vtx.xyzw[0] = verts[0][0]; vtx.xyzw[1] = verts[0][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = vecLen; vtx.uv[1] = vecLen; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[1][0]; vtx.xyzw[1] = verts[1][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = 0; vtx.uv[1] = vecLen; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[2][0]; vtx.xyzw[1] = verts[2][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = 0; vtx.uv[1] = 0; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[3][0]; vtx.xyzw[1] = verts[3][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = vecLen; vtx.uv[1] = 0; + glmesh->m_vertices->push_back(vtx); + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + break; + } default: { b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type); @@ -646,7 +699,7 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa delete glmesh; } -static btVector4 sColors[4] = +static btVector4 sGoogleyColors[4] = { btVector4(60. / 256., 186. / 256., 84. / 256., 1), btVector4(244. / 256., 194. / 256., 13. / 256., 1), @@ -705,7 +758,7 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( colorIndex = 0; colorIndex &= 3; btVector4 color; - color = sColors[colorIndex]; + color = (m_data->m_flags & URDF_GOOGLEY_UNDEFINED_COLORS) ? sGoogleyColors[colorIndex] : btVector4(1, 1, 1, 1); float rgbaColor[4] = {(float)color[0], (float)color[1], (float)color[2], (float)color[3]}; //if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) //{ @@ -783,6 +836,45 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( { B3_PROFILE("convertURDFToVisualShape"); convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags); + if (vis->m_geometry.m_type == URDF_GEOM_PLANE) + { + int texWidth = 1024; + int texHeight = 1024; + if (m_data->m_checkeredTexels.size() == 0) + { + + int red = 173; + int green = 199; + int blue = 255; + //create a textured surface + + m_data->m_checkeredTexels.resize(texWidth * texHeight * 3); + for (int i = 0; i < texWidth * texHeight * 3; i++) + m_data->m_checkeredTexels[i] = 255; + + for (int i = 0; i < texWidth; i++) + { + for (int j = 0; j < texHeight; j++) + { + int a = i < texWidth / 2 ? 1 : 0; + int b = j < texWidth / 2 ? 1 : 0; + + if (a == b) + { + m_data->m_checkeredTexels[(i + j * texWidth) * 3 + 0] = red; + m_data->m_checkeredTexels[(i + j * texWidth) * 3 + 1] = green; + m_data->m_checkeredTexels[(i + j * texWidth) * 3 + 2] = blue; + } + } + } + } + MyTexture2 texData; + texData.m_width = texWidth; + texData.m_height = texHeight; + texData.textureData1 = &m_data->m_checkeredTexels[0]; + texData.m_isCached = true; + textures.push_back(texData); + } } rgbaColor[0] = visualShape.m_rgbaColor[0]; diff --git a/examples/ThirdPartyLibs/cpp_base64/CMakeLists.txt b/examples/ThirdPartyLibs/cpp_base64/CMakeLists.txt new file mode 100644 index 000000000..7e8cd1f87 --- /dev/null +++ b/examples/ThirdPartyLibs/cpp_base64/CMakeLists.txt @@ -0,0 +1,6 @@ +
+ADD_LIBRARY(cpp_base64 base64.cpp)
+
+target_include_directories(cpp_base64 PUBLIC include PRIVATE include/cpp_base64)
+
+
diff --git a/examples/ThirdPartyLibs/cpp_base64/LICENSE b/examples/ThirdPartyLibs/cpp_base64/LICENSE new file mode 100644 index 000000000..f258136d8 --- /dev/null +++ b/examples/ThirdPartyLibs/cpp_base64/LICENSE @@ -0,0 +1,19 @@ +Copyright © 2004-2017 by René Nyffenegger
+
+This source code is provided 'as-is', without any express or implied
+warranty. In no event will the author 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:
+
+1. The origin of this source code must not be misrepresented; you must not
+ claim that you wrote the original source code. If you use this source code
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original source code.
+
+3. This notice may not be removed or altered from any source distribution.
diff --git a/examples/ThirdPartyLibs/cpp_base64/base64.cpp b/examples/ThirdPartyLibs/cpp_base64/base64.cpp new file mode 100644 index 000000000..40b555c44 --- /dev/null +++ b/examples/ThirdPartyLibs/cpp_base64/base64.cpp @@ -0,0 +1,195 @@ +/*
+ base64.cpp and base64.h
+
+ base64 encoding and decoding with C++.
+ More information at
+ https://renenyffenegger.ch/notes/development/Base64/Encoding-and-decoding-base-64-with-cpp
+
+ Version: 2.rc.00 (release candidate)
+
+ Copyright (C) 2004-2017, 2020 René Nyffenegger
+
+ This source code is provided 'as-is', without any express or implied
+ warranty. In no event will the author 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:
+
+ 1. The origin of this source code must not be misrepresented; you must not
+ claim that you wrote the original source code. If you use this source code
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original source code.
+
+ 3. This notice may not be removed or altered from any source distribution.
+
+ René Nyffenegger rene.nyffenegger@adp-gmbh.ch
+
+*/
+
+#include "base64.h"
+
+static std::string base64_chars =
+ "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+ "abcdefghijklmnopqrstuvwxyz"
+ "0123456789"
+ "??"; // These two question marks will be replaced based on the value of url in base64_encode
+
+
+static std::size_t pos_of_char(const unsigned char chr) {
+ //
+ // Return the position of chr within base64_chars.
+ //
+
+ if (chr >= 'A' && chr <= 'Z') return chr - 'A';
+ else if (chr >= 'a' && chr <= 'z') return chr - 'a' + ('Z' - 'A') + 1;
+ else if (chr >= '0' && chr <= '9') return chr - '0' + ('Z' - 'A') + ('z' - 'a') + 2;
+ else if (chr == '+' || chr == '-') return 62; // Be liberal with input and accept both url ('-') and non-url ('+') base 64 characters (
+ else if (chr == '/' || chr == '_') return 63; // Ditto for '/' and '_'
+
+ //throw "If input is correct, this line should never be reached.";
+ return 0;
+}
+
+static std::string insert_linebreaks(std::string str, size_t distance) {
+ //
+ // Provided by https://github.com/JomaCorpFX, adapted by me.
+ //
+ if (!str.length()) {
+ return "";
+ }
+
+ size_t pos = distance;
+
+ while (pos < str.size()) {
+ str.insert(pos, "\n");
+ pos += distance + 1;
+ }
+
+ return str;
+}
+
+std::string base64_encode(unsigned char const* bytes_to_encode, unsigned int in_len, bool url) {
+ //
+ // Replace question marks in base64_chars:
+ //
+ if (url) {
+ base64_chars[62] = '-';
+ base64_chars[63] = '_';
+ }
+ else {
+ base64_chars[62] = '+';
+ base64_chars[63] = '/';
+ }
+
+ unsigned int len_encoded = (in_len +2) / 3 * 4;
+
+ unsigned char trailing_char = url ? '.' : '=';
+
+ std::string ret;
+ ret.reserve(len_encoded);
+
+ unsigned int pos = 0;
+
+ while (pos < in_len) {
+ ret.push_back(base64_chars[(bytes_to_encode[pos + 0] & 0xfc) >> 2]);
+
+ if (pos+1 < in_len) {
+ ret.push_back(base64_chars[((bytes_to_encode[pos + 0] & 0x03) << 4) + ((bytes_to_encode[pos + 1] & 0xf0) >> 4)]);
+
+ if (pos+2 < in_len) {
+ ret.push_back(base64_chars[((bytes_to_encode[pos + 1] & 0x0f) << 2) + ((bytes_to_encode[pos + 2] & 0xc0) >> 6)]);
+ ret.push_back(base64_chars[ bytes_to_encode[pos + 2] & 0x3f]);
+ }
+ else {
+ ret.push_back(base64_chars[(bytes_to_encode[pos + 1] & 0x0f) << 2]);
+ ret.push_back(trailing_char);
+ }
+ }
+ else {
+
+ ret.push_back(base64_chars[(bytes_to_encode[pos + 0] & 0x03) << 4]);
+ ret.push_back(trailing_char);
+ ret.push_back(trailing_char);
+ }
+
+ pos += 3;
+ }
+
+
+ return ret;
+}
+
+
+std::string base64_decode(std::string const& encoded_string, bool remove_linebreaks) {
+
+ if (remove_linebreaks) {
+
+ if (! encoded_string.length() ) {
+ return "";
+ }
+
+ std::string copy(encoded_string);
+
+ size_t pos=0;
+ while ((pos = copy.find("\n", pos)) != std::string::npos) {
+ copy.erase(pos, 1);
+ }
+
+ return base64_decode(copy, false);
+
+ }
+
+ int length_of_string = encoded_string.length();
+ if (!length_of_string) return std::string("");
+
+ size_t in_len = length_of_string;
+ size_t pos = 0;
+
+ //
+ // The approximate length (bytes) of the decoded string might be one ore
+ // two bytes smaller, depending on the amount of trailing equal signs
+ // in the encoded string. This approximation is needed to reserve
+ // enough space in the string to be returned.
+ //
+ size_t approx_length_of_decoded_string = length_of_string / 4 * 3;
+ std::string ret;
+ ret.reserve(approx_length_of_decoded_string);
+
+ while (pos < in_len) {
+
+ unsigned int pos_of_char_1 = pos_of_char(encoded_string[pos+1] );
+
+ ret.push_back( ( (pos_of_char(encoded_string[pos+0]) ) << 2 ) + ( (pos_of_char_1 & 0x30 ) >> 4));
+
+ if (encoded_string[pos+2] != '=' && encoded_string[pos+2] != '.') { // accept URL-safe base 64 strings, too, so check for '.' also.
+
+ unsigned int pos_of_char_2 = pos_of_char(encoded_string[pos+2] );
+ ret.push_back( (( pos_of_char_1 & 0x0f) << 4) + (( pos_of_char_2 & 0x3c) >> 2));
+
+ if (encoded_string[pos+3] != '=') {
+ ret.push_back( ( (pos_of_char_2 & 0x03 ) << 6 ) + pos_of_char(encoded_string[pos+3]) );
+ }
+ }
+
+ pos += 4;
+ }
+
+ return ret;
+}
+
+std::string base64_encode(std::string const& s, bool url) {
+ return base64_encode(reinterpret_cast<const unsigned char*>(s.c_str()), s.length(), url);
+}
+
+std::string base64_encode_pem (std::string const& s) {
+ return insert_linebreaks(base64_encode(s, false), 64);
+}
+
+std::string base64_encode_mime(std::string const& s) {
+ return insert_linebreaks(base64_encode(s, false), 76);
+}
diff --git a/examples/ThirdPartyLibs/cpp_base64/include/cpp_base64/base64.h b/examples/ThirdPartyLibs/cpp_base64/include/cpp_base64/base64.h new file mode 100644 index 000000000..ea1f22d39 --- /dev/null +++ b/examples/ThirdPartyLibs/cpp_base64/include/cpp_base64/base64.h @@ -0,0 +1,18 @@ +//
+// base64 encoding and decoding with C++.
+// Version: 2.rc.00 (release candidate)
+//
+
+#ifndef BASE64_H_C0CE2A47_D10E_42C9_A27C_C883944E704A
+#define BASE64_H_C0CE2A47_D10E_42C9_A27C_C883944E704A
+
+#include <string>
+
+std::string base64_encode (std::string const& s, bool url = false);
+std::string base64_encode_pem (std::string const& s);
+std::string base64_encode_mime(std::string const& s);
+
+std::string base64_decode(std::string const& encoded_string, bool remove_linebreaks = false);
+std::string base64_encode(unsigned char const*, unsigned int len, bool url = false);
+
+#endif /* BASE64_H_C0CE2A47_D10E_42C9_A27C_C883944E704A */
diff --git a/examples/ThirdPartyLibs/cpp_base64/test.cpp b/examples/ThirdPartyLibs/cpp_base64/test.cpp new file mode 100644 index 000000000..0665691e4 --- /dev/null +++ b/examples/ThirdPartyLibs/cpp_base64/test.cpp @@ -0,0 +1,91 @@ +#include "base64.h"
+#include <iostream>
+
+int main() {
+
+ bool all_tests_passed = true;
+
+ const std::string orig =
+ "René Nyffenegger\n"
+ "http://www.renenyffenegger.ch\n"
+ "passion for data\n";
+
+ std::string encoded = base64_encode(reinterpret_cast<const unsigned char*>(orig.c_str()), orig.length());
+ std::string decoded = base64_decode(encoded);
+
+ if (encoded != "UmVuw6kgTnlmZmVuZWdnZXIKaHR0cDovL3d3dy5yZW5lbnlmZmVuZWdnZXIuY2gKcGFzc2lvbiBmb3IgZGF0YQo=") {
+ std::cout << "Encoding is wrong" << std::endl;
+ all_tests_passed = false;
+ }
+
+ if (decoded != orig) {
+ std::cout << "decoded != orig" << std::endl;
+ all_tests_passed = false;
+ }
+
+ // Test all possibilites of fill bytes (none, one =, two ==)
+ // References calculated with: https://www.base64encode.org/
+
+ std::string rest0_original = "abc";
+ std::string rest0_reference = "YWJj";
+
+ std::string rest0_encoded = base64_encode(reinterpret_cast<const unsigned char*>(rest0_original.c_str()),
+ rest0_original.length());
+ std::string rest0_decoded = base64_decode(rest0_encoded);
+
+ if (rest0_decoded != rest0_original) {
+ std::cout << "rest0_decoded != rest0_original" << std::endl;
+ all_tests_passed = false;
+ }
+ if (rest0_reference != rest0_encoded) {
+ std::cout << "rest0_reference != rest0_encoded" << std::endl;
+ all_tests_passed = false;
+ }
+
+ // std::cout << "encoded: " << rest0_encoded << std::endl;
+ // std::cout << "reference: " << rest0_reference << std::endl;
+ // std::cout << "decoded: " << rest0_decoded << std::endl << std::endl;
+
+ std::string rest1_original = "abcd";
+ std::string rest1_reference = "YWJjZA==";
+
+ std::string rest1_encoded = base64_encode(reinterpret_cast<const unsigned char*>(rest1_original.c_str()),
+ rest1_original.length());
+ std::string rest1_decoded = base64_decode(rest1_encoded);
+
+ if (rest1_decoded != rest1_original) {
+ std::cout << "rest1_decoded != rest1_original" << std::endl;
+ all_tests_passed = false;
+ }
+ if (rest1_reference != rest1_encoded) {
+ std::cout << "rest1_reference != rest1_encoded" << std::endl;
+ all_tests_passed = false;
+ }
+
+ // std::cout << "encoded: " << rest1_encoded << std::endl;
+ // std::cout << "reference: " << rest1_reference << std::endl;
+ // std::cout << "decoded: " << rest1_decoded << std::endl << std::endl;
+
+ std::string rest2_original = "abcde";
+ std::string rest2_reference = "YWJjZGU=";
+
+ std::string rest2_encoded = base64_encode(reinterpret_cast<const unsigned char*>(rest2_original.c_str()),
+ rest2_original.length());
+ std::string rest2_decoded = base64_decode(rest2_encoded);
+
+ if (rest2_decoded != rest2_original) {
+ std::cout << "rest2_decoded != rest2_original" << std::endl;
+ all_tests_passed = false;
+ }
+ if (rest2_reference != rest2_encoded) {
+ std::cout << "rest2_reference != rest2_encoded" << std::endl;
+ all_tests_passed = false;
+ }
+
+ // std::cout << "encoded: " << rest2_encoded << std::endl;
+ // std::cout << "reference: " << rest2_reference << std::endl;
+ // std::cout << "decoded: " << rest2_decoded << std::endl << std::endl;
+
+ if (all_tests_passed) return 0;
+ return 1;
+}
diff --git a/examples/ThirdPartyLibs/crossguid/crossguid/guid.hpp b/examples/ThirdPartyLibs/crossguid/crossguid/guid.hpp new file mode 100644 index 000000000..1679a8f08 --- /dev/null +++ b/examples/ThirdPartyLibs/crossguid/crossguid/guid.hpp @@ -0,0 +1,147 @@ +/* +The MIT License (MIT) + +Copyright (c) 2014 Graeme Hill (http://graemehill.ca) + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +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 AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#pragma once + +#ifdef GUID_ANDROID +#include <thread> +#include <jni.h> +#endif + +#include <functional> +#include <iostream> +#include <array> +#include <sstream> +#include <utility> +#include <iomanip> + +#define BEGIN_XG_NAMESPACE namespace xg { +#define END_XG_NAMESPACE } + +BEGIN_XG_NAMESPACE + +// Class to represent a GUID/UUID. Each instance acts as a wrapper around a +// 16 byte value that can be passed around by value. It also supports +// conversion to string (via the stream operator <<) and conversion from a +// string via constructor. +class Guid +{ +public: + explicit Guid(const std::array<unsigned char, 16> &bytes); + explicit Guid(std::array<unsigned char, 16> &&bytes); + + Guid(); + + Guid(const Guid &other) = default; + Guid &operator=(const Guid &other) = default; + Guid(Guid &&other) = default; + Guid &operator=(Guid &&other) = default; + + bool operator==(const Guid &other) const; + bool operator!=(const Guid &other) const; + + std::string str() const; + operator std::string() const; + const std::array<unsigned char, 16>& bytes() const; + void swap(Guid &other); + bool isValid() const; + +private: + void zeroify(); + + // actual data + std::array<unsigned char, 16> _bytes; + + // make the << operator a friend so it can access _bytes + friend std::ostream &operator<<(std::ostream &s, const Guid &guid); + friend bool operator<(const Guid &lhs, const Guid &rhs); +}; + +Guid newGuid(); + +#ifdef GUID_ANDROID +struct AndroidGuidInfo +{ + static AndroidGuidInfo fromJniEnv(JNIEnv *env); + + JNIEnv *env; + jclass uuidClass; + jmethodID newGuidMethod; + jmethodID mostSignificantBitsMethod; + jmethodID leastSignificantBitsMethod; + std::thread::id initThreadId; +}; + +extern AndroidGuidInfo androidInfo; + +void initJni(JNIEnv *env); + +// overloading for multi-threaded calls +Guid newGuid(JNIEnv *env); +#endif + +namespace details +{ + template <typename...> struct hash; + + template<typename T> + struct hash<T> : public std::hash<T> + { + using std::hash<T>::hash; + }; + + + template <typename T, typename... Rest> + struct hash<T, Rest...> + { + inline std::size_t operator()(const T& v, const Rest&... rest) { + std::size_t seed = hash<Rest...>{}(rest...); + seed ^= hash<T>{}(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + return seed; + } + }; +} + +END_XG_NAMESPACE + +namespace std +{ + // Template specialization for std::swap<Guid>() -- + // See guid.cpp for the function definition + template <> + void swap(xg::Guid &guid0, xg::Guid &guid1) noexcept; + + // Specialization for std::hash<Guid> -- this implementation + // uses std::hash<std::string> on the stringification of the guid + // to calculate the hash + template <> + struct hash<xg::Guid> + { + std::size_t operator()(xg::Guid const &guid) const + { + const uint64_t* p = reinterpret_cast<const uint64_t*>(guid.bytes().data()); + return xg::details::hash<uint64_t, uint64_t>{}(p[0], p[1]); + } + }; +} diff --git a/examples/ThirdPartyLibs/crossguid/guid.cpp b/examples/ThirdPartyLibs/crossguid/guid.cpp new file mode 100644 index 000000000..ea28e0a40 --- /dev/null +++ b/examples/ThirdPartyLibs/crossguid/guid.cpp @@ -0,0 +1,363 @@ +/* +The MIT License (MIT) + +Copyright (c) 2014 Graeme Hill (http://graemehill.ca) + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +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 AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#include <cstring> +#include "crossguid/guid.hpp" + +#ifdef GUID_LIBUUID +#include <uuid/uuid.h> +#endif + +#ifdef GUID_CFUUID +#include <CoreFoundation/CFUUID.h> +#endif + +#ifdef GUID_WINDOWS +#include <objbase.h> +#endif + +#ifdef GUID_ANDROID +#include <jni.h> +#include <cassert> +#endif + +BEGIN_XG_NAMESPACE + +#ifdef GUID_ANDROID +AndroidGuidInfo androidInfo; + +AndroidGuidInfo AndroidGuidInfo::fromJniEnv(JNIEnv *env) +{ + AndroidGuidInfo info; + info.env = env; + auto localUuidClass = env->FindClass("java/util/UUID"); + info.uuidClass = (jclass)env->NewGlobalRef(localUuidClass); + env->DeleteLocalRef(localUuidClass); + info.newGuidMethod = env->GetStaticMethodID( + info.uuidClass, "randomUUID", "()Ljava/util/UUID;"); + info.mostSignificantBitsMethod = env->GetMethodID( + info.uuidClass, "getMostSignificantBits", "()J"); + info.leastSignificantBitsMethod = env->GetMethodID( + info.uuidClass, "getLeastSignificantBits", "()J"); + info.initThreadId = std::this_thread::get_id(); + return info; +} + +void initJni(JNIEnv *env) +{ + androidInfo = AndroidGuidInfo::fromJniEnv(env); +} +#endif + +// overload << so that it's easy to convert to a string +std::ostream &operator<<(std::ostream &s, const Guid &guid) +{ + std::ios_base::fmtflags f(s.flags()); // politely don't leave the ostream in hex mode + s << std::hex << std::setfill('0') + << std::setw(2) << (int)guid._bytes[0] + << std::setw(2) << (int)guid._bytes[1] + << std::setw(2) << (int)guid._bytes[2] + << std::setw(2) << (int)guid._bytes[3] + << "-" + << std::setw(2) << (int)guid._bytes[4] + << std::setw(2) << (int)guid._bytes[5] + << "-" + << std::setw(2) << (int)guid._bytes[6] + << std::setw(2) << (int)guid._bytes[7] + << "-" + << std::setw(2) << (int)guid._bytes[8] + << std::setw(2) << (int)guid._bytes[9] + << "-" + << std::setw(2) << (int)guid._bytes[10] + << std::setw(2) << (int)guid._bytes[11] + << std::setw(2) << (int)guid._bytes[12] + << std::setw(2) << (int)guid._bytes[13] + << std::setw(2) << (int)guid._bytes[14] + << std::setw(2) << (int)guid._bytes[15]; + s.flags(f); + return s; +} + +bool operator<(const xg::Guid &lhs, const xg::Guid &rhs) +{ + return lhs.bytes() < rhs.bytes(); +} + +bool Guid::isValid() const +{ + xg::Guid empty; + return *this != empty; +} + +// convert to string using std::snprintf() and std::string +std::string Guid::str() const +{ + char one[10], two[6], three[6], four[6], five[14]; + + snprintf(one, 10, "%02x%02x%02x%02x", + _bytes[0], _bytes[1], _bytes[2], _bytes[3]); + snprintf(two, 6, "%02x%02x", + _bytes[4], _bytes[5]); + snprintf(three, 6, "%02x%02x", + _bytes[6], _bytes[7]); + snprintf(four, 6, "%02x%02x", + _bytes[8], _bytes[9]); + snprintf(five, 14, "%02x%02x%02x%02x%02x%02x", + _bytes[10], _bytes[11], _bytes[12], _bytes[13], _bytes[14], _bytes[15]); + const std::string sep("-"); + std::string out(one); + + out += sep + two; + out += sep + three; + out += sep + four; + out += sep + five; + + return out; +} + +// conversion operator for std::string +Guid::operator std::string() const +{ + return str(); +} + +// Access underlying bytes +const std::array<unsigned char, 16>& Guid::bytes() const +{ + return _bytes; +} + +// create a guid from vector of bytes +Guid::Guid(const std::array<unsigned char, 16> &bytes) : _bytes(bytes) +{ } + +// create a guid from vector of bytes +Guid::Guid(std::array<unsigned char, 16> &&bytes) : _bytes(std::move(bytes)) +{ } + +// converts a single hex char to a number (0 - 15) +unsigned char hexDigitToChar(char ch) +{ + // 0-9 + if (ch > 47 && ch < 58) + return ch - 48; + + // a-f + if (ch > 96 && ch < 103) + return ch - 87; + + // A-F + if (ch > 64 && ch < 71) + return ch - 55; + + return 0; +} + +bool isValidHexChar(char ch) +{ + // 0-9 + if (ch > 47 && ch < 58) + return true; + + // a-f + if (ch > 96 && ch < 103) + return true; + + // A-F + if (ch > 64 && ch < 71) + return true; + + return false; +} + +// converts the two hexadecimal characters to an unsigned char (a byte) +unsigned char hexPairToChar(char a, char b) +{ + return hexDigitToChar(a) * 16 + hexDigitToChar(b); +} + + + +// create empty guid +Guid::Guid() : _bytes{ {0} } +{ } + +// set all bytes to zero +void Guid::zeroify() +{ + std::fill(_bytes.begin(), _bytes.end(), static_cast<unsigned char>(0)); +} + +// overload equality operator +bool Guid::operator==(const Guid &other) const +{ + return _bytes == other._bytes; +} + +// overload inequality operator +bool Guid::operator!=(const Guid &other) const +{ + return !((*this) == other); +} + +// member swap function +void Guid::swap(Guid &other) +{ + _bytes.swap(other._bytes); +} + +// This is the linux friendly implementation, but it could work on other +// systems that have libuuid available +#ifdef GUID_LIBUUID +Guid newGuid() +{ + std::array<unsigned char, 16> data; + static_assert(std::is_same<unsigned char[16], uuid_t>::value, "Wrong type!"); + uuid_generate(data.data()); + return Guid{std::move(data)}; +} +#endif + +// this is the mac and ios version +#ifdef GUID_CFUUID +Guid newGuid() +{ + auto newId = CFUUIDCreate(NULL); + auto bytes = CFUUIDGetUUIDBytes(newId); + CFRelease(newId); + + std::array<unsigned char, 16> byteArray = + {{ + bytes.byte0, + bytes.byte1, + bytes.byte2, + bytes.byte3, + bytes.byte4, + bytes.byte5, + bytes.byte6, + bytes.byte7, + bytes.byte8, + bytes.byte9, + bytes.byte10, + bytes.byte11, + bytes.byte12, + bytes.byte13, + bytes.byte14, + bytes.byte15 + }}; + return Guid{std::move(byteArray)}; +} +#endif + +// obviously this is the windows version +#ifdef GUID_WINDOWS +Guid newGuid() +{ + GUID newId; + CoCreateGuid(&newId); + + std::array<unsigned char, 16> bytes = + { + (unsigned char)((newId.Data1 >> 24) & 0xFF), + (unsigned char)((newId.Data1 >> 16) & 0xFF), + (unsigned char)((newId.Data1 >> 8) & 0xFF), + (unsigned char)((newId.Data1) & 0xff), + + (unsigned char)((newId.Data2 >> 8) & 0xFF), + (unsigned char)((newId.Data2) & 0xff), + + (unsigned char)((newId.Data3 >> 8) & 0xFF), + (unsigned char)((newId.Data3) & 0xFF), + + (unsigned char)newId.Data4[0], + (unsigned char)newId.Data4[1], + (unsigned char)newId.Data4[2], + (unsigned char)newId.Data4[3], + (unsigned char)newId.Data4[4], + (unsigned char)newId.Data4[5], + (unsigned char)newId.Data4[6], + (unsigned char)newId.Data4[7] + }; + + return Guid{std::move(bytes)}; +} +#endif + +// android version that uses a call to a java api +#ifdef GUID_ANDROID +Guid newGuid(JNIEnv *env) +{ + assert(env != androidInfo.env || std::this_thread::get_id() == androidInfo.initThreadId); + + jobject javaUuid = env->CallStaticObjectMethod( + androidInfo.uuidClass, androidInfo.newGuidMethod); + jlong mostSignificant = env->CallLongMethod(javaUuid, + androidInfo.mostSignificantBitsMethod); + jlong leastSignificant = env->CallLongMethod(javaUuid, + androidInfo.leastSignificantBitsMethod); + + std::array<unsigned char, 16> bytes = + { + (unsigned char)((mostSignificant >> 56) & 0xFF), + (unsigned char)((mostSignificant >> 48) & 0xFF), + (unsigned char)((mostSignificant >> 40) & 0xFF), + (unsigned char)((mostSignificant >> 32) & 0xFF), + (unsigned char)((mostSignificant >> 24) & 0xFF), + (unsigned char)((mostSignificant >> 16) & 0xFF), + (unsigned char)((mostSignificant >> 8) & 0xFF), + (unsigned char)((mostSignificant) & 0xFF), + (unsigned char)((leastSignificant >> 56) & 0xFF), + (unsigned char)((leastSignificant >> 48) & 0xFF), + (unsigned char)((leastSignificant >> 40) & 0xFF), + (unsigned char)((leastSignificant >> 32) & 0xFF), + (unsigned char)((leastSignificant >> 24) & 0xFF), + (unsigned char)((leastSignificant >> 16) & 0xFF), + (unsigned char)((leastSignificant >> 8) & 0xFF), + (unsigned char)((leastSignificant) & 0xFF) + }; + + env->DeleteLocalRef(javaUuid); + + return Guid{std::move(bytes)}; +} + +Guid newGuid() +{ + return newGuid(androidInfo.env); +} +#endif + + +END_XG_NAMESPACE + +// Specialization for std::swap<Guid>() -- +// call member swap function of lhs, passing rhs +namespace std +{ + template <> + void swap(xg::Guid &lhs, xg::Guid &rhs) noexcept + { + lhs.swap(rhs); + } +} diff --git a/examples/TinyAudio/b3AudioListener.cpp b/examples/TinyAudio/b3AudioListener.cpp index 184b169ec..4e2022ffa 100644 --- a/examples/TinyAudio/b3AudioListener.cpp +++ b/examples/TinyAudio/b3AudioListener.cpp @@ -2,6 +2,7 @@ #include "b3SoundSource.h" #include "Bullet3Common/b3Logging.h" #include "b3WriteWavFile.h" +#include <math.h> template <class T> inline const T& MyMin(const T& a, const T& b) diff --git a/examples/TinyAudio/b3SoundEngine.cpp b/examples/TinyAudio/b3SoundEngine.cpp index e683bf19a..6e36b0d9c 100644 --- a/examples/TinyAudio/b3SoundEngine.cpp +++ b/examples/TinyAudio/b3SoundEngine.cpp @@ -164,7 +164,7 @@ int b3SoundEngine::loadWavFile(const char* fileName) } char resourcePath[1024]; - if (b3ResourcePath::findResourcePath(fileName, resourcePath, 1024)) + if (b3ResourcePath::findResourcePath(fileName, resourcePath, 1024, 0)) { b3ReadWavFile* wavFile = new b3ReadWavFile(); wavFile->getWavInfo(resourcePath); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 0e383a54b..6412888a4 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -167,11 +167,11 @@ struct Shader : public IShader Vec2f uv = varying_uv * bar; Vec3f reflection_direction = (bn * (bn * m_light_dir_local * 2.f) - m_light_dir_local).normalize(); - float specular = std::pow(b3Max(reflection_direction.z, 0.f), - m_model->specular(uv)); - float diffuse = b3Max(0.f, bn * m_light_dir_local); + float specular = std::pow(b3Max(reflection_direction.z, 0.f), + m_model->specular(uv)); + float diffuse = b3Max(0.f, bn * m_light_dir_local); - color = m_model->diffuse(uv); + color = m_model->diffuse(uv); color[0] *= m_colorRGBA[0]; color[1] *= m_colorRGBA[1]; color[2] *= m_colorRGBA[2]; diff --git a/examples/TinyRenderer/geometry.h b/examples/TinyRenderer/geometry.h index f2100e57e..c17518c9f 100644 --- a/examples/TinyRenderer/geometry.h +++ b/examples/TinyRenderer/geometry.h @@ -315,8 +315,10 @@ template <size_t DimRows,size_t DimCols,class T> std::ostream& operator<<(std::o ///////////////////////////////////////////////////////////////////////////////// typedef vec<2, float> Vec2f; +typedef vec<2, double> Vec2d; typedef vec<2, int> Vec2i; typedef vec<3, float> Vec3f; +typedef vec<3, double> Vec3d; typedef vec<3, int> Vec3i; typedef vec<4, float> Vec4f; typedef mat<4, 4, float> Matrix; diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 29f13dd4d..6bbaa5167 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -166,10 +166,10 @@ TGAColor Model::diffuse(Vec2f uvf) // bool repeat = true; // if (repeat) { - uvf[0] = std::modf(uvf[0], &val); - uvf[1] = std::modf(uvf[1], &val); - } - Vec2i uv(uvf[0] * diffusemap_.get_width(), uvf[1] * diffusemap_.get_height()); + uvf[0] = std::modf(uvf[0], &val); + uvf[1] = std::modf(uvf[1], &val); + } + Vec2i uv(uvf[0] * diffusemap_.get_width(), uvf[1] * diffusemap_.get_height()); return diffusemap_.get(uv[0], uv[1]); } return TGAColor(255, 255, 255, 255); diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index b71c09e75..30d2024e8 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -61,19 +61,25 @@ Matrix lookat(Vec3f eye, Vec3f center, Vec3f up) return ModelView; } -Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) +Vec3d barycentric(Vec2f A1, Vec2f B1, Vec2f C1, Vec2f P1) { - Vec3f s[2]; + + Vec2d A(A1.x, A1.y); + Vec2d B(B1.x, B1.y); + Vec2d C(C1.x, C1.y); + Vec2d P(P1.x, P1.y);; + + Vec3d s[2]; for (int i = 2; i--;) { s[i][0] = C[i] - A[i]; s[i][1] = B[i] - A[i]; s[i][2] = A[i] - P[i]; } - Vec3f u = cross(s[0], s[1]); + Vec3d u = cross(s[0], s[1]); if (std::abs(u[2]) > 1e-2) // dont forget that u[2] is integer. If it is zero then triangle ABC is degenerate - return Vec3f(1.f - (u.x + u.y) / u.z, u.y / u.z, u.x / u.z); - return Vec3f(-1, 1, 1); // in this case generate negative coordinates, it will be thrown away by the rasterizator + return Vec3d(1. - (u.x + u.y) / u.z, u.y / u.z, u.x / u.z); + return Vec3d(-1., 1., 1.); // in this case generate negative coordinates, it will be thrown away by the rasterizator } void triangleClipped(mat<4, 3, float> &clipc, mat<4, 3, float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix &viewPortMatrix) @@ -119,25 +125,28 @@ void triangleClipped(mat<4, 3, float> &clipc, mat<4, 3, float> &orgClipc, IShade { for (P.y = bboxmin.y; P.y <= bboxmax.y; P.y++) { - float frag_depth = 0; + double frag_depth = 0; { - Vec3f bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); - Vec3f bc_clip = Vec3f(bc_screen.x / screenSpacePts[0][3], bc_screen.y / screenSpacePts[1][3], bc_screen.z / screenSpacePts[2][3]); + Vec3d bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); + Vec3d bc_clip = Vec3d(bc_screen.x / screenSpacePts[0][3], bc_screen.y / screenSpacePts[1][3], bc_screen.z / screenSpacePts[2][3]); bc_clip = bc_clip / (bc_clip.x + bc_clip.y + bc_clip.z); - frag_depth = -1 * (clipc[2] * bc_clip); + Vec3d clipd(clipc[2].x, clipc[2].y, clipc[2].z); + frag_depth = -1. * (clipd * bc_clip); if (bc_screen.x < 0 || bc_screen.y < 0 || bc_screen.z < 0 || zbuffer[P.x + P.y * image.get_width()] > frag_depth) continue; } - Vec3f bc_screen2 = barycentric(orgPts2[0], orgPts2[1], orgPts2[2], P); - Vec3f bc_clip2 = Vec3f(bc_screen2.x / orgScreenSpacePts[0][3], bc_screen2.y / orgScreenSpacePts[1][3], bc_screen2.z / orgScreenSpacePts[2][3]); + Vec3d bc_screen2 = barycentric(orgPts2[0], orgPts2[1], orgPts2[2], P); + Vec3d bc_clip2 = Vec3d(bc_screen2.x / orgScreenSpacePts[0][3], bc_screen2.y / orgScreenSpacePts[1][3], bc_screen2.z / orgScreenSpacePts[2][3]); bc_clip2 = bc_clip2 / (bc_clip2.x + bc_clip2.y + bc_clip2.z); - float frag_depth2 = -1 * (orgClipc[2] * bc_clip2); - - bool discard = shader.fragment(bc_clip2, color); + Vec3d orgClipd(orgClipc[2].x, orgClipc[2].y, orgClipc[2].z); + double frag_depth2 = -1. * (orgClipd * bc_clip2); + Vec3f bc_clip2f(bc_clip2.x, bc_clip2.y, bc_clip2.z); + bool discard = shader.fragment(bc_clip2f, color); + if (!discard) { zbuffer[P.x + P.y * image.get_width()] = frag_depth; @@ -182,14 +191,16 @@ void triangle(mat<4, 3, float> &clipc, IShader &shader, TGAImage &image, float * { for (P.y = bboxmin.y; P.y <= bboxmax.y; P.y++) { - Vec3f bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); - Vec3f bc_clip = Vec3f(bc_screen.x / pts[0][3], bc_screen.y / pts[1][3], bc_screen.z / pts[2][3]); + Vec3d bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); + Vec3d bc_clip = Vec3d(bc_screen.x / pts[0][3], bc_screen.y / pts[1][3], bc_screen.z / pts[2][3]); bc_clip = bc_clip / (bc_clip.x + bc_clip.y + bc_clip.z); - float frag_depth = -1 * (clipc[2] * bc_clip); + Vec3d clipd(clipc[2].x, clipc[2].y, clipc[2].z); + double frag_depth = -1. * (clipd * bc_clip); if (bc_screen.x < 0 || bc_screen.y < 0 || bc_screen.z < 0 || zbuffer[P.x + P.y * image.get_width()] > frag_depth) continue; - bool discard = shader.fragment(bc_clip, color); + Vec3f bc_clipf(bc_clip.x, bc_clip.y, bc_clip.z); + bool discard = shader.fragment(bc_clipf, color); if (frag_depth < -shader.m_farPlane) discard = true; if (frag_depth > shader.m_nearPlane) diff --git a/examples/TinyRenderer/tgaimage.cpp b/examples/TinyRenderer/tgaimage.cpp index 706baefe3..68e91054b 100644 --- a/examples/TinyRenderer/tgaimage.cpp +++ b/examples/TinyRenderer/tgaimage.cpp @@ -296,6 +296,23 @@ bool TGAImage::unload_rle_data(std::ofstream &out) const TGAColor TGAImage::get(int x, int y) const { + if (x < 0) + { + x = 0; + } + if (y < 0) + { + y = 0; + } + if (x >= width) + { + x = width - 1; + } + if (y >= height) + { + y = height - 1; + } + if (!data || x < 0 || y < 0 || x >= width || y >= height) { return TGAColor(128.f, 128.f, 128.f, 255.f); diff --git a/examples/Utils/ChromeTraceUtil.cpp b/examples/Utils/ChromeTraceUtil.cpp index a7eca7604..68a84effb 100644 --- a/examples/Utils/ChromeTraceUtil.cpp +++ b/examples/Utils/ChromeTraceUtil.cpp @@ -5,6 +5,7 @@ #include "LinearMath/btAlignedObjectArray.h" #include "Bullet3Common/b3Logging.h" #include <stdio.h> +#include <climits> struct btTiming { @@ -111,16 +112,15 @@ struct btTimings sprintf(newname, "%s%d", name, counter2++); #ifdef _WIN32 - fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", threadId, startTimeDiv1000, startTimeRem1000Str, newname); fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", threadId, endTimeDiv1000, endTimeRem1000Str, newname); - #else - fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", + // Note: on 64b build, PRIu64 resolves in 'lu' whereas timings ('ts') have to be printed as 'llu'. + fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%llu.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", threadId, startTimeDiv1000, startTimeRem1000Str, newname); - fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", + fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%llu.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", threadId, endTimeDiv1000, endTimeRem1000Str, newname); #endif #endif diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py index 70d4a6fe9..60b883a5c 100644 --- a/examples/pybullet/examples/constraint.py +++ b/examples/pybullet/examples/constraint.py @@ -13,7 +13,6 @@ p.setRealTimeSimulation(1) cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1]) print(cid) print(p.getConstraintUniqueId(0)) -prev = [0, 0, 1] a = -math.pi while 1: a = a + 0.01 diff --git a/examples/pybullet/examples/deformable_anchor.py b/examples/pybullet/examples/deformable_anchor.py index 1c01ed6d4..fa3d75867 100644 --- a/examples/pybullet/examples/deformable_anchor.py +++ b/examples/pybullet/examples/deformable_anchor.py @@ -15,7 +15,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True) -clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1) +clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, springDampingAllDirections = 1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1) p.createSoftBodyAnchor(clothId ,0,-1,-1) diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py index ffda4e6b7..68dd39aaa 100644 --- a/examples/pybullet/examples/deformable_ball.py +++ b/examples/pybullet/examples/deformable_ball.py @@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) -ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5) +ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 4, useNeoHookean = 1, NeoHookeanMu = 400, NeoHookeanLambda = 600, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5, collisionMargin = 0.001) p.setTimeStep(0.001) p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) p.setRealTimeSimulation(1) diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index 2801944d8..fe64ae648 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/examples/pybullet/examples/deformable_torus.py @@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2]) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) -bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5) +bunnyId = p.loadSoftBody("torus.vtk", mass = 3, useNeoHookean = 1, NeoHookeanMu = 180, NeoHookeanLambda = 600, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800) bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION) @@ -23,4 +23,3 @@ p.setRealTimeSimulation(1) while p.isConnected(): p.setGravity(0,0,-10) - sleep(1./240.) diff --git a/examples/pybullet/examples/externalTorqueControlledSphere.py b/examples/pybullet/examples/externalTorqueControlledSphere.py index 0182dbcc3..45b2cfe70 100644 --- a/examples/pybullet/examples/externalTorqueControlledSphere.py +++ b/examples/pybullet/examples/externalTorqueControlledSphere.py @@ -1,7 +1,6 @@ import pybullet as p import pybullet_data import time -import pybullet_data p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) diff --git a/examples/pybullet/examples/inverted_pendulum_tendon_actuation.py b/examples/pybullet/examples/inverted_pendulum_tendon_actuation.py new file mode 100644 index 000000000..d9b075931 --- /dev/null +++ b/examples/pybullet/examples/inverted_pendulum_tendon_actuation.py @@ -0,0 +1,167 @@ +""" +https://valerolab.org/ + +PID control of an inverted pendulum actuated by strings. +""" + +import pybullet as p +import time +import math as m +import numpy as np +import pybullet_data +import matplotlib.pyplot as plt + + +p.connect(p.GUI) +plane = p.loadURDF("plane.urdf") + +"""_____________________________________________________________________________________________________________________________""" +"""Gains, motor forces, daq and timing parameters""" + +""" Gains for pendulum angle""" +proportional_gain = 30000 +integral_gain = 18000 +derivative_gain = 22000 + +"""Motor force parameters""" +tension_force = 600 + +"""Control input parameters""" +u_factor = 1.5 +u_lower_limit = tension_force +u_upper_limit=9000 + +"""Data aquisition, timing and history""" +time_steps = 2000 +history = np.array( [[1000,-1000,0]] ) +time_history = np.array([[0]]) +previous_pendulum_angle = 0 +previous_cart_position = 0 + +"""_____________________________________________________________________________________________________________________________""" +"""Loading URDF files""" + +cubeStartPos = [-2.15,0,.75] +cubeStartPos2 = [0,0,1.4] +cubeStartPos3 = [2.15,0,.75] + +PulleyStartOrientation = p.getQuaternionFromEuler([1.570796, 0, 0]) +cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) +cubeStartOrientation2 = p.getQuaternionFromEuler([0,-1.570796,0]) + +base_1 = p.loadURDF("Base_1.urdf",cubeStartPos3, cubeStartOrientation, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION) #Base 1: magenta base and tendon +base_2 = p.loadURDF("Base_2.urdf",cubeStartPos, cubeStartOrientation, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION) #Base 2: white base and tendon +pendulum = p.loadURDF("Pendulum_Tendon_1_Cart_Rail.urdf",cubeStartPos2, cubeStartOrientation2, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION) + + +"""_____________________________________________________________________________________________________________________________""" +"""Getting access and information from specific joints in each body (each body has links and joint described in the URDF files):""" +nJoints = p.getNumJoints(base_1) #Base 1: magenta base and tendon +jointNameToId = {} +for i in range(nJoints): + jointInfo = p.getJointInfo(base_1, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] +Base_pulley_1 = jointNameToId['Base_pulley1'] + +nJoints = p.getNumJoints(pendulum) +jointNameToId = {} +for i in range(nJoints): + jointInfo = p.getJointInfo(pendulum, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] +last_tendon_link_1 = jointNameToId['tendon1_13_tendon1_14'] +cart_pendulumAxis = jointNameToId['cart_pendulumAxis'] +cart = jointNameToId['slider_cart'] + +nJoints = p.getNumJoints(base_2) #Base 2: white base and tendon +jointNameToId = {} +for i in range(nJoints): + jointInfo = p.getJointInfo(base_2, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] +last_tendon_link_2 = jointNameToId['tendon1_13_tendon1_14'] +Base_pulley_2 = jointNameToId['Base_pulley1'] +"""_____________________________________________________________________________________________________________________________""" +"""Creating new contraints (joints), with the information obtained in the previous step""" + +pulley_1_tendon_magenta = p.createConstraint(base_1, Base_pulley_1, pendulum, last_tendon_link_1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], [-.56, 0, 0]) +tendon_white_cart = p.createConstraint(base_2, last_tendon_link_2, pendulum, cart, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], [0,-.55, 0]) + +"""_____________________________________________________________________________________________________________________________""" +"""Defining some motor conditions""" +p.setJointMotorControl2(pendulum, cart_pendulumAxis, p.VELOCITY_CONTROL, targetVelocity=0, force=0) +p.setJointMotorControl2(base_1, Base_pulley_1, p.VELOCITY_CONTROL, targetVelocity=10, force=1000) #Base 1: magenta base and tendon +p.setJointMotorControl2(base_2, Base_pulley_2, p.VELOCITY_CONTROL, targetVelocity=10, force=-1000)#Base 2: white base and tendon + + +"""_____________________________________________________________________________________________________________________________""" + + +p.setGravity(0,0,-10) + + +for i in range (time_steps): + p.stepSimulation() + pendulum_angle = p.getJointState(pendulum,cart_pendulumAxis) + pendulum_angle = pendulum_angle[0] + angle_delta_error = -pendulum_angle + + #PROPPORTIONAL + p_correction = proportional_gain * pendulum_angle + + #INTEGRAL + i_correction = integral_gain * (previous_pendulum_angle + pendulum_angle) + previous_pendulum_angle = pendulum_angle + + #DERIVATIVE + d_correction = derivative_gain * angle_delta_error + + u = p_correction + i_correction + d_correction + 10 + + u = abs(u) + if u<u_lower_limit: + u=u_lower_limit + elif u>u_upper_limit: + u=u_upper_limit + + if pendulum_angle > 0: + u_pulley_1 = u * u_factor #Base 1: magenta base and tendon + u_pulley_2 = -tension_force #Base 2: white base and tendon + #print(">0") + else: + u_pulley_1 = tension_force #Base 1: magenta base and tendon + u_pulley_2 = -u * u_factor #Base 2: white base and tendon + #print("<0") + + p.setJointMotorControl2(base_1, Base_pulley_1, p.VELOCITY_CONTROL, targetVelocity=100, force = u_pulley_1) + p.setJointMotorControl2(base_2, Base_pulley_2, p.VELOCITY_CONTROL, targetVelocity=100, force = u_pulley_2) + + history = np.append(history , [[ u_pulley_1, u_pulley_2, pendulum_angle]] , axis = 0) + + time.sleep(1./240.) + +print("Done with simulation") + +fig, ax1 = plt.subplots() +ax1.set_xlabel("Time Steps") +ax1.set_ylabel("Activation Values") +ax1.plot(history[:,0],label="u_pulley_1") +ax1.plot(history[:,1],label="u_pulley_2") +ax1.set_ylim((-12000,12000)) + +plt.legend(loc='best', bbox_to_anchor=(0.5, 0., 0.5, 0.5), + ncol=1, mode=None, borderaxespad=0.) +plt.title("Ctrl Input and Angle History") +plt.grid(True) +color = 'tab:red' +ax2 = ax1.twinx() +ax2.set_ylabel('Pendulum Angle', color=color) +ax2.plot(np.rad2deg(history[:,2]),label="Angle",color=color) +ax2.tick_params(axis='y', labelcolor=color) +ax2.set_ylim((-90,90)) + +fig.tight_layout() +plt.show() + +p.disconnect() + + + diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py index cbcda6e5b..41ced4716 100644 --- a/examples/pybullet/examples/testrender.py +++ b/examples/pybullet/examples/testrender.py @@ -15,7 +15,7 @@ import pybullet_data pybullet.connect(pybullet.DIRECT) -p.setAdditionalSearchPath(pybullet_data.getDataPath()) +pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) #pybullet.loadPlugin("eglRendererPlugin") pybullet.loadURDF("plane.urdf", [0, 0, -1]) diff --git a/examples/pybullet/examples/video_sync_mp4.py b/examples/pybullet/examples/video_sync_mp4.py index 3859de538..754101744 100644 --- a/examples/pybullet/examples/video_sync_mp4.py +++ b/examples/pybullet/examples/video_sync_mp4.py @@ -2,10 +2,15 @@ import pybullet as p import time import pybullet_data +#Once the video is recorded, you can extract all individual frames using ffmpeg +#mkdir frames +#ffmpeg -i test.mp4 "frames/out-%03d.png" + #by default, PyBullet runs at 240Hz -p.connect(p.GUI, options="--mp4=\"test.mp4\" --mp4fps=240") +p.connect(p.GUI, options="--width=1920 --height=1080 --mp4=\"test.mp4\" --mp4fps=240") p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1) p.loadURDF("plane.urdf") diff --git a/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_walk_COMenabled.ckpt.data-00000-of-00001 b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_walk_COMenabled.ckpt.data-00000-of-00001 Binary files differnew file mode 100644 index 000000000..22e8b639a --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_walk_COMenabled.ckpt.data-00000-of-00001 diff --git a/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_walk_COMenabled.ckpt.index b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_walk_COMenabled.ckpt.index Binary files differnew file mode 100644 index 000000000..9115ee029 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_walk_COMenabled.ckpt.index diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index b6713bfd6..031efef74 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -229,4 +229,10 @@ register(id='HumanoidFlagrunHarderBulletEnv-v0', def getList(): btenvs = ['- ' + spec.id for spec in gym.envs.registry.all() if spec.id.find('Bullet') >= 0] + btenvs.extend([ + '- MinitaurExtendedEnv-v0', '- MinitaurReactiveEnv-v0', + '- MinitaurBallGymEnv-v0', '- MinitaurTrottingEnv-v0', + '- MinitaurStandGymEnv-v0', '- MinitaurAlternatingLegsEnv-v0', + '- MinitaurFourLegStandEnv-v0', '- KukaDiverseObjectGrasping-v0' + ]) return btenvs diff --git a/examples/pybullet/gym/pybullet_envs/bullet/__init__.py b/examples/pybullet/gym/pybullet_envs/bullet/__init__.py index 42932a7ed..a50b9b5d6 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/__init__.py @@ -6,3 +6,4 @@ from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv from pybullet_envs.bullet.kukaGymEnv import KukaGymEnv from pybullet_envs.bullet.kukaCamGymEnv import KukaCamGymEnv +from pybullet_envs.bullet.kuka_diverse_object_gym_env import KukaDiverseObjectEnv diff --git a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py index 1fe2cfe65..d0f833a2a 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py @@ -17,7 +17,7 @@ import time import subprocess import pybullet as p2 import pybullet_data -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc from pkg_resources import parse_version logger = logging.getLogger(__name__) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py index 696dc268a..294a5984a 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py @@ -102,6 +102,9 @@ class KukaDiverseObjectEnv(KukaGymEnv): self.action_space = spaces.Box(low=-1, high=1, shape=(3,)) # dx, dy, da if self._removeHeightHack: self.action_space = spaces.Box(low=-1, high=1, shape=(4,)) # dx, dy, dz, da + self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, + self._width, + 3)) self.viewer = None def reset(self): diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py index eefb74693..6c2d9dabb 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py @@ -17,7 +17,7 @@ from gym import spaces from gym.utils import seeding import numpy as np import pybullet -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc from . import minitaur import pybullet_data from . import minitaur_env_randomizer diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py index ee63044c1..1770fa5e3 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -14,7 +14,7 @@ from gym import spaces from gym.utils import seeding import numpy as np import pybullet -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc from . import minitaur import os import pybullet_data diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index af9d5d545..445ae7d65 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -12,7 +12,7 @@ import numpy as np import pybullet from . import racecar import random -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc import pybullet_data from pkg_resources import parse_version diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index 5e8641cef..ebd67f156 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -10,7 +10,7 @@ from gym.utils import seeding import numpy as np import time import pybullet -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc from . import racecar import random import pybullet_data diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py index 2e9f95c18..fcbf2154b 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py @@ -1,6 +1,7 @@ from pybullet_utils import pd_controller_stable from pybullet_envs.deep_mimic.env import humanoid_pose_interpolator import math +import numpy as np chest = 1 neck = 2 @@ -20,7 +21,7 @@ jointFrictionForce = 0 class HumanoidStablePD(object): def __init__( self, pybullet_client, mocap_data, timeStep, - useFixedBase=True, arg_parser=None): + useFixedBase=True, arg_parser=None, useComReward=False): self._pybullet_client = pybullet_client self._mocap_data = mocap_data self._arg_parser = arg_parser @@ -148,6 +149,8 @@ class HumanoidStablePD(object): self._totalDofs += dof self.setSimTime(0) + self._useComReward = useComReward + self.resetPose() def resetPose(self): @@ -357,6 +360,7 @@ class HumanoidStablePD(object): ) def computePDForces(self, desiredPositions, desiredVelocities, maxForces): + """Compute torques from the PD controller.""" if desiredVelocities == None: desiredVelocities = [0] * self._totalDofs @@ -371,6 +375,7 @@ class HumanoidStablePD(object): return taus def applyPDForces(self, taus): + """Apply pre-computed torques.""" dofIndex = 7 scaling = 1 useArray = True @@ -733,13 +738,19 @@ class HumanoidStablePD(object): return angle * angle def getReward(self, pose): + """Compute and return the pose-based reward.""" #from DeepMimic double cSceneImitate::CalcRewardImitate #todo: compensate for ground height in some parts, once we move to non-flat terrain + # not values from the paper, but from the published code. pose_w = 0.5 vel_w = 0.05 end_eff_w = 0.15 + # does not exist in paper root_w = 0.2 - com_w = 0 #0.1 + if self._useComReward: + com_w = 0.1 + else: + com_w = 0 total_w = pose_w + vel_w + end_eff_w + root_w + com_w pose_w /= total_w @@ -753,7 +764,7 @@ class HumanoidStablePD(object): end_eff_scale = 40 root_scale = 5 com_scale = 10 - err_scale = 1 + err_scale = 1 # error scale reward = 0 @@ -779,6 +790,9 @@ class HumanoidStablePD(object): #tMatrix kin_origin_trans = kin_char.BuildOriginTrans(); # #tVector com0_world = sim_char.CalcCOM(); + if self._useComReward: + comSim, comSimVel = self.computeCOMposVel(self._sim_model) + comKin, comKinVel = self.computeCOMposVel(self._kin_model) #tVector com_vel0_world = sim_char.CalcCOMVel(); #tVector com1_world; #tVector com_vel1_world; @@ -910,6 +924,10 @@ class HumanoidStablePD(object): root_err = root_pos_err + 0.1 * root_rot_err + 0.01 * root_vel_err + 0.001 * root_ang_vel_err + # COM error in initial code -> COM velocities + if self._useComReward: + com_err = 0.1 * np.sum(np.square(comKinVel - comSimVel)) + # com_err = 0.1 * np.sum(np.square(comKin - comSim)) #com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm() #print("pose_err=",pose_err) @@ -929,5 +947,40 @@ class HumanoidStablePD(object): #print("end_eff_reward=",end_eff_reward) #print("root_reward=",root_reward) #print("com_reward=",com_reward) - + + info_rew = dict( + pose_reward=pose_reward, + vel_reward=vel_reward, + end_eff_reward=end_eff_reward, + root_reward=root_reward, + com_reward=com_reward + ) + + info_errs = dict( + pose_err=pose_err, + vel_err=vel_err, + end_eff_err=end_eff_err, + root_err=root_err, + com_err=com_err + ) + return reward + + def computeCOMposVel(self, uid: int): + """Compute center-of-mass position and velocity.""" + pb = self._pybullet_client + num_joints = 15 + jointIndices = range(num_joints) + link_states = pb.getLinkStates(uid, jointIndices, computeLinkVelocity=1) + link_pos = np.array([s[0] for s in link_states]) + link_vel = np.array([s[-2] for s in link_states]) + tot_mass = 0. + masses = [] + for j in jointIndices: + mass_, *_ = pb.getDynamicsInfo(uid, j) + masses.append(mass_) + tot_mass += mass_ + masses = np.asarray(masses)[:, None] + com_pos = np.sum(masses * link_pos, axis=0) / tot_mass + com_vel = np.sum(masses * link_vel, axis=0) / tot_mass + return com_pos, com_vel diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py index 309de425c..6cdbb7e0c 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py @@ -10,16 +10,28 @@ import pybullet_data import pybullet as p1 import random +from enum import Enum + +class InitializationStrategy(Enum): + """Set how the environment is initialized.""" + START = 0 + RANDOM = 1 # random state initialization (RSI) + class PyBulletDeepMimicEnv(Env): - def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None): + def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None, + time_step=1./240, + init_strategy=InitializationStrategy.RANDOM): super().__init__(arg_parser, enable_draw) self._num_agents = 1 self._pybullet_client = pybullet_client self._isInitialized = False self._useStablePD = True self._arg_parser = arg_parser + self.timeStep = time_step + self._init_strategy = init_strategy + print("Initialization strategy: {:s}".format(init_strategy)) self.reset() def reset(self): @@ -52,7 +64,7 @@ class PyBulletDeepMimicEnv(Env): motionPath = pybullet_data.getDataPath() + "/" + motion_file[0] #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" self._mocapData.Load(motionPath) - timeStep = 1. / 240. + timeStep = self.timeStep useFixedBase = False self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase, self._arg_parser) @@ -77,9 +89,14 @@ class PyBulletDeepMimicEnv(Env): time.sleep(timeStep) #print("numframes = ", self._humanoid._mocap_data.NumFrames()) #startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2) - rnrange = 1000 - rn = random.randint(0, rnrange) - startTime = float(rn) / rnrange * self._humanoid.getCycleTime() + + if self._init_strategy == InitializationStrategy.RANDOM: + rnrange = 1000 + rn = random.randint(0, rnrange) + startTime = float(rn) / rnrange * self._humanoid.getCycleTime() + elif self._init_strategy == InitializationStrategy.START: + startTime = 0 + self.t = startTime self._humanoid.setSimTime(startTime) @@ -263,6 +280,7 @@ class PyBulletDeepMimicEnv(Env): #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t) self._pybullet_client.setTimeStep(timeStep) self._humanoid._timeStep = timeStep + self.timeStep = timeStep for i in range(1): self.t += timeStep @@ -314,7 +332,7 @@ class PyBulletDeepMimicEnv(Env): def check_valid_episode(self): #could check if limbs exceed velocity threshold - return true + return True def getKeyboardEvents(self): return self._pybullet_client.getKeyboardEvents() diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py index 55a50ef50..ec060dd95 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py @@ -17,20 +17,31 @@ import time import subprocess import pybullet as p2 import pybullet_data -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc from pkg_resources import parse_version -from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv +from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv, InitializationStrategy from pybullet_utils.arg_parser import ArgParser from pybullet_utils.logger import Logger +from typing import Optional + logger = logging.getLogger(__name__) class HumanoidDeepBulletEnv(gym.Env): + """Base Gym environment for DeepMimic.""" metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50} - def __init__(self, renders=False, arg_file=''): - + def __init__(self, renders=False, arg_file='', test_mode=False, + time_step=1./240, + rescale_actions=True, + rescale_observations=True): + """ + Args: + test_mode (bool): in test mode, the `reset()` method will always set the mocap clip time + to 0. + time_step (float): physics time step. + """ self._arg_parser = ArgParser() Logger.print2("===========================================================") succ = False @@ -39,15 +50,33 @@ class HumanoidDeepBulletEnv(gym.Env): succ = self._arg_parser.load_file(path) Logger.print2(arg_file) assert succ, Logger.print2('Failed to load args from: ' + arg_file) - - self._p = None - self._time_step = 1./240. - self._internal_env = None + + self._p: Optional[BulletClient] = None + self._time_step = time_step + self._internal_env: Optional[PyBulletDeepMimicEnv] = None self._renders = renders self._discrete_actions = False - self._arg_file=arg_file - self._render_height = 200 - self._render_width = 320 + self._arg_file = arg_file + self._render_height = 400 + self._render_width = 640 + self._rescale_actions = rescale_actions + self._rescale_observations = rescale_observations + self.agent_id = -1 + + self._numSteps = None + self.test_mode = test_mode + if self.test_mode: + print("Environment running in TEST mode") + + self.reset() + + # Query the policy at 30Hz + self.policy_query_30 = True + if self.policy_query_30: + self._policy_step = 1./30 + else: + self._policy_step = 1./240 + self._num_env_steps = int(self._policy_step / self._time_step) self.theta_threshold_radians = 12 * 2 * math.pi / 360 self.x_threshold = 0.4 #2.4 @@ -93,6 +122,7 @@ class HumanoidDeepBulletEnv(gym.Env): self.viewer = None self._configure() + def _configure(self, display=None): self.display = display @@ -102,34 +132,73 @@ class HumanoidDeepBulletEnv(gym.Env): return [seed] def step(self, action): - - #apply control action - agent_id = -1 + agent_id = self.agent_id + + if self._rescale_actions: + # Rescale the action + mean = -self._action_offset + std = 1./self._action_scale + action = action * std + mean + + # Record reward + reward = self._internal_env.calc_reward(agent_id) + + # Apply control action self._internal_env.set_action(agent_id, action) - - #step sim - self._internal_env.update(self._time_step) - - #record state + start_time = self._internal_env.t + + # step sim + for i in range(self._num_env_steps): + self._internal_env.update(self._time_step) + + elapsed_time = self._internal_env.t - start_time + + self._numSteps += 1 + + # Record state self.state = self._internal_env.record_state(agent_id) - #record reward - reward = self._internal_env.calc_reward(agent_id) - - #record done + if self._rescale_observations: + state = np.array(self.state) + mean = -self._state_offset + std = 1./self._state_scale + state = (state - mean) / (std + 1e-8) + + # Record done done = self._internal_env.is_episode_end() - return np.array(self.state), reward, done, {} + info = {} + return state, reward, done, info def reset(self): - # print("-----------reset simulation---------------") - if self._internal_env==None: - self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders) + # use the initialization strategy + if self._internal_env is None: + if self.test_mode: + init_strat = InitializationStrategy.START + else: + init_strat = InitializationStrategy.RANDOM + self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders, + time_step=self._time_step, + init_strategy=init_strat) + self._internal_env.reset() self._p = self._internal_env._pybullet_client - agent_id = -1 #unused here - state = self._internal_env.record_state(agent_id) + agent_id = self.agent_id # unused here + self._state_offset = self._internal_env.build_state_offset(self.agent_id) + self._state_scale = self._internal_env.build_state_scale(self.agent_id) + self._action_offset = self._internal_env.build_action_offset(self.agent_id) + self._action_scale = self._internal_env.build_action_scale(self.agent_id) + self._numSteps = 0 + # Record state + self.state = self._internal_env.record_state(agent_id) + + # return state as ndarray + state = np.array(self.state) + if self._rescale_observations: + mean = -self._state_offset + std = 1./self._state_scale + state = (state - mean) / (std + 1e-8) return state def render(self, mode='human', close=False): @@ -137,8 +206,15 @@ class HumanoidDeepBulletEnv(gym.Env): self._renders = True if mode != "rgb_array": return np.array([]) - base_pos=[0,0,0] - self._cam_dist = 2 + human = self._internal_env._humanoid + base_pos, orn = self._p.getBasePositionAndOrientation(human._sim_model) + base_pos = np.asarray(base_pos) + # track the position + base_pos[1] += 0.3 + rpy = self._p.getEulerFromQuaternion(orn) # rpy, in radians + rpy = 180 / np.pi * np.asarray(rpy) # convert rpy in degrees + + self._cam_dist = 3 self._cam_pitch = 0.3 self._cam_yaw = 0 if (not self._p == None): @@ -148,10 +224,9 @@ class HumanoidDeepBulletEnv(gym.Env): yaw=self._cam_yaw, pitch=self._cam_pitch, roll=0, - upAxisIndex=2) + upAxisIndex=1) proj_matrix = self._p.computeProjectionMatrixFOV(fov=60, - aspect=float(self._render_width) / - self._render_height, + aspect=float(self._render_width) / self._render_height, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = self._p.getCameraImage( @@ -160,6 +235,12 @@ class HumanoidDeepBulletEnv(gym.Env): renderer=self._p.ER_BULLET_HARDWARE_OPENGL, viewMatrix=view_matrix, projectionMatrix=proj_matrix) + # self._p.resetDebugVisualizerCamera( + # cameraDistance=2 * self._cam_dist, + # cameraYaw=self._cam_yaw, + # cameraPitch=self._cam_pitch, + # cameraTargetPosition=base_pos + # ) else: px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8) rgb_array = np.array(px, dtype=np.uint8) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index f9c51c051..ac7c2c067 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -26,7 +26,7 @@ class MJCFBaseBulletEnv(gym.Env): self.scene = None self.physicsClientId = -1 self.ownsPhysicsClient = 0 - self.camera = Camera() + self.camera = Camera(self) self.isRender = render self.robot = robot self.seed() @@ -88,16 +88,23 @@ class MJCFBaseBulletEnv(gym.Env): self.potential = self.robot.calc_potential() return s + def camera_adjust(self): + pass + def render(self, mode='human', close=False): + if mode == "human": self.isRender = True + if self.physicsClientId>=0: + self.camera_adjust() + if mode != "rgb_array": return np.array([]) base_pos = [0, 0, 0] if (hasattr(self, 'robot')): - if (hasattr(self.robot, 'body_xyz')): - base_pos = self.robot.body_xyz + if (hasattr(self.robot, 'body_real_xyz')): + base_pos = self.robot.body_real_xyz if (self.physicsClientId>=0): view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos, distance=self._cam_dist, @@ -115,15 +122,8 @@ class MJCFBaseBulletEnv(gym.Env): viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) - try: - # Keep the previous orientation of the camera set by the user. - con_mode = self._p.getConnectionInfo()['connectionMethod'] - if con_mode==self._p.SHARED_MEMORY or con_mode == self._p.GUI: - [yaw, pitch, dist] = self._p.getDebugVisualizerCamera()[8:11] - self._p.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos) - except: - pass + self._p.configureDebugVisualizer(self._p.COV_ENABLE_SINGLE_STEP_RENDERING,1) else: px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8) rgb_array = np.array(px, dtype=np.uint8) @@ -160,11 +160,15 @@ class MJCFBaseBulletEnv(gym.Env): class Camera: - def __init__(self): + def __init__(self, env): + self.env = env pass def move_and_look_at(self, i, j, k, x, y, z): lookat = [x, y, z] - distance = 10 - yaw = 10 - self._p.resetDebugVisualizerCamera(distance, yaw, -20, lookat) + camInfo = self.env._p.getDebugVisualizerCamera() + + distance = camInfo[10] + pitch = camInfo[9] + yaw = camInfo[8] + self.env._p.resetDebugVisualizerCamera(distance, yaw, pitch, lookat) diff --git a/examples/pybullet/gym/pybullet_envs/examples/dominoes.py b/examples/pybullet/gym/pybullet_envs/examples/dominoes.py index e27f6aec9..c3e645c4b 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/dominoes.py +++ b/examples/pybullet/gym/pybullet_envs/examples/dominoes.py @@ -1,7 +1,7 @@ import pybullet_data as pd import pybullet_utils as pu import pybullet -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc import time p = bc.BulletClient(connection_mode=pybullet.GUI) diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py index 9fc5d7962..29f5db65d 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py @@ -4,7 +4,6 @@ import inspect currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0, parentdir) -import pybullet import gym import numpy as np import pybullet_envs @@ -32,8 +31,8 @@ class SmallReactivePolicy: def main(): - pybullet.connect(pybullet.DIRECT) env = gym.make("AntBulletEnv-v0") + env.render(mode="human") pi = SmallReactivePolicy(env.observation_space, env.action_space) @@ -55,7 +54,6 @@ def main(): frame += 1 distance = 5 yaw = 0 - still_open = env.render("human") if still_open == False: return diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py index 50eacc552..b95ff2e31 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py @@ -49,7 +49,6 @@ def main(): obs, r, done, _ = env.step(a) score += r frame += 1 - still_open = env.render("human") if still_open == False: return diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py index a58b79411..f341aaf00 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py @@ -51,7 +51,6 @@ def main(): obs, r, done, _ = env.step(a) score += r frame += 1 - still_open = env.render("human") if still_open == False: return diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py index 572d495ca..18b902d5a 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py @@ -43,16 +43,15 @@ def main(): obs = env.reset() while 1: + time.sleep(1. / 60.) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r frame += 1 - time.sleep(1. / 60.) - still_open = env.render("human") if still_open == False: return - if not done: continue + continue if restart_delay == 0: print("score=%0.2f in %i frames" % (score, frame)) restart_delay = 60 * 2 # 2 sec at 60 fps diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py index 84220d533..cfbbbfd73 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py @@ -45,12 +45,13 @@ def demo_run(): obs = env.reset() while 1: + if (gui): + time.sleep(1. / 60) + a = pi.act(obs) obs, r, done, _ = env.step(a) score += r frame += 1 - if (gui): - time.sleep(1. / 60) still_open = env.render("human") diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index cbf096600..ba22fa61c 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -23,6 +23,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): frame_skip=4) return self.stadium_scene + def reset(self): if (self.stateId >= 0): #print("restoreState self.stateId:",self.stateId) @@ -125,9 +126,10 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): return state, sum(self.rewards), bool(done), {} def camera_adjust(self): - x, y, z = self.body_xyz - self.camera_x = 0.98 * self.camera_x + (1 - 0.98) * x - self.camera.move_and_look_at(self.camera_x, y - 2.0, 1.4, x, y, 1.0) + x, y, z = self.robot.body_real_xyz + + self.camera_x = x + self.camera.move_and_look_at(self.camera_x, y , 1.4, x, y, 1.0) class HopperBulletEnv(WalkerBaseBulletEnv): @@ -163,8 +165,11 @@ class AntBulletEnv(WalkerBaseBulletEnv): class HumanoidBulletEnv(WalkerBaseBulletEnv): - def __init__(self, robot=Humanoid(), render=False): - self.robot = robot + def __init__(self, robot=None, render=False): + if robot is None: + self.robot = Humanoid() + else: + self.robot = robot WalkerBaseBulletEnv.__init__(self, self.robot, render) self.electricity_cost = 4.25 * WalkerBaseBulletEnv.electricity_cost self.stall_torque_cost = 4.25 * WalkerBaseBulletEnv.stall_torque_cost diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/__init__.py index 72c619547..103471d35 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/__init__.py @@ -5,3 +5,4 @@ from pybullet_envs.minitaur.envs.minitaur_reactive_env import MinitaurReactiveEn from pybullet_envs.minitaur.envs.minitaur_stand_gym_env import MinitaurStandGymEnv from pybullet_envs.minitaur.envs.minitaur_trotting_env import MinitaurTrottingEnv from pybullet_envs.minitaur.envs.minitaur_four_leg_stand_env import MinitaurFourLegStandEnv +from pybullet_envs.minitaur.envs.minitaur_extended_env import MinitaurExtendedEnv, MinitaurReactiveEnv diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py index a48d46a03..608b8eab0 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py @@ -14,7 +14,7 @@ from gym import spaces from gym.utils import seeding import numpy as np import pybullet -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc import pybullet_data from pybullet_envs.minitaur.envs import minitaur from pybullet_envs.minitaur.envs import minitaur_derpy diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto index 79258352a..75face4af 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.proto @@ -27,7 +27,7 @@ message MinitaurStateAction { bool info_valid = 6; // The time stamp of this step. It is computed since the reset of the // environment. - google.protobuf.Timestamp time = 1; + robotics.messages.Timestamp time = 1; // The position of the base of the minitaur. robotics.messages.Vector3d base_position = 2; // The orientation of the base of the minitaur. It is represented as (roll, diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py index 500bbdc71..b6f1c5a3d 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py @@ -19,7 +19,7 @@ import datetime import os import time -import tf.compat.v1 as tf +import tensorflow.compat.v1 as tf from pybullet_envs.minitaur.envs import minitaur_logging_pb2 NUM_MOTORS = 8 diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py index 9650c4b24..afb70dd33 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging_pb2.py @@ -1,288 +1,182 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: minitaur_logging.proto - -import sys - -import os, inspect -currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) -parentdir = os.path.dirname(os.path.dirname(currentdir)) -os.sys.path.insert(0, parentdir) - -_b = sys.version_info[0] < 3 and (lambda x: x) or (lambda x: x.encode('latin1')) +"""Generated protocol buffer code.""" from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database -from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() + from pybullet_envs.minitaur.envs import timestamp_pb2 as timestamp__pb2 -from pybullet_envs.minitaur.envs import vector_pb2 as vector__pb2 +from pybullet_envs.minitaur.envs import vector_pb2 as vector__pb2 + DESCRIPTOR = _descriptor.FileDescriptor( - name='minitaur_logging.proto', - package='robotics.reinforcement_learning.minitaur.envs', - syntax='proto3', - serialized_pb=_b( - '\n\x16minitaur_logging.proto\x12-robotics.reinforcement_learning.minitaur.envs\x1a\x0ftimestamp.proto\x1a\x0cvector.proto\"k\n\x0fMinitaurEpisode\x12X\n\x0cstate_action\x18\x01 \x03(\x0b\x32\x42.robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction\"U\n\x12MinitaurMotorState\x12\r\n\x05\x61ngle\x18\x01 \x01(\x01\x12\x10\n\x08velocity\x18\x02 \x01(\x01\x12\x0e\n\x06torque\x18\x03 \x01(\x01\x12\x0e\n\x06\x61\x63tion\x18\x04 \x01(\x01\"\xce\x02\n\x13MinitaurStateAction\x12\x12\n\ninfo_valid\x18\x06 \x01(\x08\x12(\n\x04time\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x32\n\rbase_position\x18\x02 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12\x35\n\x10\x62\x61se_orientation\x18\x03 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12\x35\n\x10\x62\x61se_angular_vel\x18\x04 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12W\n\x0cmotor_states\x18\x05 \x03(\x0b\x32\x41.robotics.reinforcement_learning.minitaur.envs.MinitaurMotorStateb\x06proto3' - ), - dependencies=[ - timestamp__pb2.DESCRIPTOR, - vector__pb2.DESCRIPTOR, - ]) + name='minitaur_logging.proto', + package='robotics.reinforcement_learning.minitaur.envs', + syntax='proto3', + serialized_options=None, + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x16minitaur_logging.proto\x12-robotics.reinforcement_learning.minitaur.envs\x1a\x0ftimestamp.proto\x1a\x0cvector.proto\"k\n\x0fMinitaurEpisode\x12X\n\x0cstate_action\x18\x01 \x03(\x0b\x32\x42.robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction\"U\n\x12MinitaurMotorState\x12\r\n\x05\x61ngle\x18\x01 \x01(\x01\x12\x10\n\x08velocity\x18\x02 \x01(\x01\x12\x0e\n\x06torque\x18\x03 \x01(\x01\x12\x0e\n\x06\x61\x63tion\x18\x04 \x01(\x01\"\xd0\x02\n\x13MinitaurStateAction\x12\x12\n\ninfo_valid\x18\x06 \x01(\x08\x12*\n\x04time\x18\x01 \x01(\x0b\x32\x1c.robotics.messages.Timestamp\x12\x32\n\rbase_position\x18\x02 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12\x35\n\x10\x62\x61se_orientation\x18\x03 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12\x35\n\x10\x62\x61se_angular_vel\x18\x04 \x01(\x0b\x32\x1b.robotics.messages.Vector3d\x12W\n\x0cmotor_states\x18\x05 \x03(\x0b\x32\x41.robotics.reinforcement_learning.minitaur.envs.MinitaurMotorStateb\x06proto3' + , + dependencies=[timestamp__pb2.DESCRIPTOR,vector__pb2.DESCRIPTOR,]) + + + _MINITAUREPISODE = _descriptor.Descriptor( - name='MinitaurEpisode', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='state_action', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode.state_action', - index=0, - number=1, - type=11, - cpp_type=10, - label=3, - has_default_value=False, - default_value=[], - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - ], - extensions=[], - nested_types=[], - enum_types=[], - options=None, - is_extendable=False, - syntax='proto3', - extension_ranges=[], - oneofs=[], - serialized_start=104, - serialized_end=211, + name='MinitaurEpisode', + full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='state_action', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode.state_action', index=0, + number=1, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=104, + serialized_end=211, ) + _MINITAURMOTORSTATE = _descriptor.Descriptor( - name='MinitaurMotorState', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='angle', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.angle', - index=0, - number=1, - type=1, - cpp_type=5, - label=1, - has_default_value=False, - default_value=float(0), - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='velocity', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.velocity', - index=1, - number=2, - type=1, - cpp_type=5, - label=1, - has_default_value=False, - default_value=float(0), - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='torque', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.torque', - index=2, - number=3, - type=1, - cpp_type=5, - label=1, - has_default_value=False, - default_value=float(0), - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='action', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.action', - index=3, - number=4, - type=1, - cpp_type=5, - label=1, - has_default_value=False, - default_value=float(0), - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - ], - extensions=[], - nested_types=[], - enum_types=[], - options=None, - is_extendable=False, - syntax='proto3', - extension_ranges=[], - oneofs=[], - serialized_start=213, - serialized_end=298, + name='MinitaurMotorState', + full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='angle', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.angle', index=0, + number=1, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='velocity', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.velocity', index=1, + number=2, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='torque', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.torque', index=2, + number=3, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='action', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState.action', index=3, + number=4, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=213, + serialized_end=298, ) + _MINITAURSTATEACTION = _descriptor.Descriptor( - name='MinitaurStateAction', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='info_valid', - full_name= - 'robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.info_valid', - index=0, - number=6, - type=8, - cpp_type=7, - label=1, - has_default_value=False, - default_value=False, - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='time', - full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.time', - index=1, - number=1, - type=11, - cpp_type=10, - label=1, - has_default_value=False, - default_value=None, - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='base_position', - full_name= - 'robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_position', - index=2, - number=2, - type=11, - cpp_type=10, - label=1, - has_default_value=False, - default_value=None, - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='base_orientation', - full_name= - 'robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_orientation', - index=3, - number=3, - type=11, - cpp_type=10, - label=1, - has_default_value=False, - default_value=None, - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='base_angular_vel', - full_name= - 'robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_angular_vel', - index=4, - number=4, - type=11, - cpp_type=10, - label=1, - has_default_value=False, - default_value=None, - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='motor_states', - full_name= - 'robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.motor_states', - index=5, - number=5, - type=11, - cpp_type=10, - label=3, - has_default_value=False, - default_value=[], - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - ], - extensions=[], - nested_types=[], - enum_types=[], - options=None, - is_extendable=False, - syntax='proto3', - extension_ranges=[], - oneofs=[], - serialized_start=301, - serialized_end=635, + name='MinitaurStateAction', + full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='info_valid', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.info_valid', index=0, + number=6, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='time', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.time', index=1, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='base_position', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_position', index=2, + number=2, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='base_orientation', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_orientation', index=3, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='base_angular_vel', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.base_angular_vel', index=4, + number=4, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='motor_states', full_name='robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction.motor_states', index=5, + number=5, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=301, + serialized_end=637, ) _MINITAUREPISODE.fields_by_name['state_action'].message_type = _MINITAURSTATEACTION @@ -296,34 +190,26 @@ DESCRIPTOR.message_types_by_name['MinitaurMotorState'] = _MINITAURMOTORSTATE DESCRIPTOR.message_types_by_name['MinitaurStateAction'] = _MINITAURSTATEACTION _sym_db.RegisterFileDescriptor(DESCRIPTOR) -MinitaurEpisode = _reflection.GeneratedProtocolMessageType( - 'MinitaurEpisode', - (_message.Message,), - dict( - DESCRIPTOR=_MINITAUREPISODE, - __module__='minitaur_logging_pb2' - # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode) - )) +MinitaurEpisode = _reflection.GeneratedProtocolMessageType('MinitaurEpisode', (_message.Message,), { + 'DESCRIPTOR' : _MINITAUREPISODE, + '__module__' : 'minitaur_logging_pb2' + # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurEpisode) + }) _sym_db.RegisterMessage(MinitaurEpisode) -MinitaurMotorState = _reflection.GeneratedProtocolMessageType( - 'MinitaurMotorState', - (_message.Message,), - dict( - DESCRIPTOR=_MINITAURMOTORSTATE, - __module__='minitaur_logging_pb2' - # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState) - )) +MinitaurMotorState = _reflection.GeneratedProtocolMessageType('MinitaurMotorState', (_message.Message,), { + 'DESCRIPTOR' : _MINITAURMOTORSTATE, + '__module__' : 'minitaur_logging_pb2' + # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurMotorState) + }) _sym_db.RegisterMessage(MinitaurMotorState) -MinitaurStateAction = _reflection.GeneratedProtocolMessageType( - 'MinitaurStateAction', - (_message.Message,), - dict( - DESCRIPTOR=_MINITAURSTATEACTION, - __module__='minitaur_logging_pb2' - # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction) - )) +MinitaurStateAction = _reflection.GeneratedProtocolMessageType('MinitaurStateAction', (_message.Message,), { + 'DESCRIPTOR' : _MINITAURSTATEACTION, + '__module__' : 'minitaur_logging_pb2' + # @@protoc_insertion_point(class_scope:robotics.reinforcement_learning.minitaur.envs.MinitaurStateAction) + }) _sym_db.RegisterMessage(MinitaurStateAction) + # @@protoc_insertion_point(module_scope) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py index 8135b7265..446a3156a 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_stand_gym_env.py @@ -167,7 +167,7 @@ class MinitaurStandGymEnv(minitaur_gym_env.MinitaurGymEnv): # Use the one dimensional action to rotate both bottom legs. action_delta = [0, 0, -action, action, 0, 0, action, -action] action_all_legs = map(add, action_all_legs, action_delta) - return action_all_legs + return list(action_all_legs) def _policy_flip(self, time_step, orientation): """Hand coded policy to make the minitaur stand up to its two legs. diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto index b67072deb..93476fc09 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp.proto @@ -1,6 +1,6 @@ syntax = "proto3"; -package google.protobuf; +package robotics.messages; message Timestamp { diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py index bd1bf7703..bed88d0fd 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/timestamp_pb2.py @@ -1,87 +1,77 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: timestamp.proto - -import sys -_b = sys.version_info[0] < 3 and (lambda x: x) or (lambda x: x.encode('latin1')) +"""Generated protocol buffer code.""" from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection from google.protobuf import symbol_database as _symbol_database -from google.protobuf import descriptor_pb2 # @@protoc_insertion_point(imports) _sym_db = _symbol_database.Default() + + + DESCRIPTOR = _descriptor.FileDescriptor( - name='timestamp.proto', - package='google.protobuf', - syntax='proto3', - serialized_pb=_b( - '\n\x0ftimestamp.proto\x12\x0fgoogle.protobuf\"+\n\tTimestamp\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x62\x06proto3' - )) + name='timestamp.proto', + package='robotics.messages', + syntax='proto3', + serialized_options=None, + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x0ftimestamp.proto\x12\x11robotics.messages\"+\n\tTimestamp\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x62\x06proto3' +) + + + _TIMESTAMP = _descriptor.Descriptor( - name='Timestamp', - full_name='google.protobuf.Timestamp', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor(name='seconds', - full_name='google.protobuf.Timestamp.seconds', - index=0, - number=1, - type=3, - cpp_type=2, - label=1, - has_default_value=False, - default_value=0, - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - _descriptor.FieldDescriptor(name='nanos', - full_name='google.protobuf.Timestamp.nanos', - index=1, - number=2, - type=5, - cpp_type=1, - label=1, - has_default_value=False, - default_value=0, - message_type=None, - enum_type=None, - containing_type=None, - is_extension=False, - extension_scope=None, - options=None, - file=DESCRIPTOR), - ], - extensions=[], - nested_types=[], - enum_types=[], - options=None, - is_extendable=False, - syntax='proto3', - extension_ranges=[], - oneofs=[], - serialized_start=36, - serialized_end=79, + name='Timestamp', + full_name='robotics.messages.Timestamp', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='seconds', full_name='robotics.messages.Timestamp.seconds', index=0, + number=1, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='nanos', full_name='robotics.messages.Timestamp.nanos', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=38, + serialized_end=81, ) DESCRIPTOR.message_types_by_name['Timestamp'] = _TIMESTAMP _sym_db.RegisterFileDescriptor(DESCRIPTOR) -Timestamp = _reflection.GeneratedProtocolMessageType( - 'Timestamp', - (_message.Message,), - dict(DESCRIPTOR=_TIMESTAMP, - __module__='timestamp_pb2' - # @@protoc_insertion_point(class_scope:google.protobuf.Timestamp) - )) +Timestamp = _reflection.GeneratedProtocolMessageType('Timestamp', (_message.Message,), { + 'DESCRIPTOR' : _TIMESTAMP, + '__module__' : 'timestamp_pb2' + # @@protoc_insertion_point(class_scope:robotics.messages.Timestamp) + }) _sym_db.RegisterMessage(Timestamp) + # @@protoc_insertion_point(module_scope) diff --git a/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py b/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py index e7ba23040..0d94520cc 100644 --- a/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/prediction/pybullet_sim_gym_env.py @@ -14,7 +14,7 @@ from gym import spaces from gym.utils import seeding import numpy as np import pybullet -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc from pybullet_envs.prediction import boxstack_pybullet_sim diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 51c2791c1..1e22f1bbd 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -130,12 +130,13 @@ class MJCFBasedRobot(XmlBasedRobot): self.objects = self._p.loadMJCF(os.path.join(pybullet_data.getDataPath(), "mjcf", self.model_xml), flags=pybullet.URDF_USE_SELF_COLLISION | - pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) + pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS | + pybullet.URDF_GOOGLEY_UNDEFINED_COLORS ) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self._p, self.objects) else: self.objects = self._p.loadMJCF( - os.path.join(pybullet_data.getDataPath(), "mjcf", self.model_xml)) + os.path.join(pybullet_data.getDataPath(), "mjcf", self.model_xml, flags = pybullet.URDF_GOOGLEY_UNDEFINED_COLORS)) self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self._p, self.objects) self.robot_specific_reset(self._p) @@ -183,14 +184,14 @@ class URDFBasedRobot(XmlBasedRobot): basePosition=self.basePosition, baseOrientation=self.baseOrientation, useFixedBase=self.fixed_base, - flags=pybullet.URDF_USE_SELF_COLLISION)) + flags=pybullet.URDF_USE_SELF_COLLISION | pybullet.URDF_GOOGLEY_UNDEFINED_COLORS)) else: self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( self._p, self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf), basePosition=self.basePosition, baseOrientation=self.baseOrientation, - useFixedBase=self.fixed_base)) + useFixedBase=self.fixed_base, flags = pybullet.URDF_GOOGLEY_UNDEFINED_COLORS)) self.robot_specific_reset(self._p) diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 43ff604ac..993aaa42c 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -44,6 +44,7 @@ class WalkerBase(MJCFBasedRobot): parts_xyz = np.array([p.pose().xyz() for p in self.parts.values()]).flatten() self.body_xyz = (parts_xyz[0::3].mean(), parts_xyz[1::3].mean(), body_pose.xyz()[2] ) # torso z is more informative than mean z + self.body_real_xyz = body_pose.xyz() self.body_rpy = body_pose.rpy() z = self.body_xyz[2] if self.initial_z == None: diff --git a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py index f4baa07b4..30e666d4e 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py @@ -1,5 +1,5 @@ -import pybullet_utils.bullet_client as bc -import pybullet_utils.urdfEditor as ed +from pybullet_utils import bullet_client as bc +from pybullet_utils import urdfEditor as ed import pybullet import pybullet_data import time diff --git a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py index ebd21fcf3..cca972d32 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py @@ -1,6 +1,6 @@ #rudimentary MuJoCo mjcf to ROS URDF converter using the UrdfEditor -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc import pybullet_data as pd import pybullet_utils.urdfEditor as ed diff --git a/examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py b/examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py index a2858ee87..cbf1adec4 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py +++ b/examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py @@ -1,4 +1,4 @@ -import pybullet_utils.bullet_client as bc +from pybullet_utils import bullet_client as bc import pybullet import pybullet_data diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d6beded74..1f8d006f9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1371,11 +1371,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO PyObject* anisotropicFrictionObj = 0; double maxJointVelocity = -1; + double jointLowerLimit = 1; + double jointUpperLimit = -1; + double jointLimitForce = -1; + b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "collisionMargin", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "collisionMargin", "jointLowerLimit","jointUpperLimit", "jointLimitForce", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdddddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &jointLowerLimit , &jointUpperLimit , &jointLimitForce , &physicsClientId)) { return NULL; } @@ -1397,6 +1401,16 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm); b3SharedMemoryStatusHandle statusHandle; + if (jointLimitForce >= 0) + { + b3ChangeDynamicsInfoSetJointLimitForce(command, bodyUniqueId, linkIndex, jointLimitForce); + } + + if (jointLowerLimit <= jointUpperLimit) + { + b3ChangeDynamicsInfoSetJointLimit(command, bodyUniqueId, linkIndex, jointLowerLimit, jointUpperLimit); + } + if (mass >= 0) { b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass); @@ -1603,14 +1617,15 @@ static PyObject* pybullet_getPhysicsEngineParameters(PyObject* self, PyObject* a b3GetStatusPhysicsSimulationParameters(statusHandle, ¶ms); //for now, return a subset, expose more/all on request - val = Py_BuildValue("{s:d,s:i,s:i,s:i,s:d,s:d,s:d}", + val = Py_BuildValue("{s:d,s:i,s:i,s:i,s:d,s:d,s:d, s:i}", "fixedTimeStep", params.m_deltaTime, "numSubSteps", params.m_numSimulationSubSteps, "numSolverIterations", params.m_numSolverIterations, "useRealTimeSimulation", params.m_useRealTimeSimulation, "gravityAccelerationX", params.m_gravityAcceleration[0], "gravityAccelerationY", params.m_gravityAcceleration[1], - "gravityAccelerationZ", params.m_gravityAcceleration[2]); + "gravityAccelerationZ", params.m_gravityAcceleration[2], + "numNonContactInnerIterations", params.m_numNonContactInnerIterations); return val; } //"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", @@ -1649,6 +1664,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double warmStartingFactor = -1; double sparseSdfVoxelSize = -1; + int numNonContactInnerIterations = -1; int physicsClientId = 0; @@ -1678,11 +1694,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar "reportSolverAnalytics", "warmStartingFactor", "sparseSdfVoxelSize", + "numNonContactInnerIterations", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiiddii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, - &reportSolverAnalytics, &warmStartingFactor, &sparseSdfVoxelSize, &physicsClientId)) + &reportSolverAnalytics, &warmStartingFactor, &sparseSdfVoxelSize, &numNonContactInnerIterations, &physicsClientId)) { return NULL; } @@ -1811,6 +1828,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParameterSetSparseSdfVoxelSize(command, sparseSdfVoxelSize); } + if (numNonContactInnerIterations >= 1) + { + b3PhysicsParamSetNumNonContactInnerIterations(command, numNonContactInnerIterations); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -2038,7 +2059,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* int physicsClientId = 0; int flags = 0; - static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", NULL}; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springDampingAllDirections", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL}; int bodyUniqueId = -1; const char* fileName = ""; @@ -2050,6 +2071,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* int useNeoHookean = 0; double springElasticStiffness = 1; double springDampingStiffness = 0.1; + int springDampingAllDirections = 0; double springBendingStiffness = 0.1; double NeoHookeanMu = 1; double NeoHookeanLambda = 1; @@ -2057,7 +2079,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* double frictionCoeff = 0; int useFaceContact = 0; int useSelfCollision = 0; - + double repulsionStiffness = 0.5; b3PhysicsClientHandle sm = 0; @@ -2068,7 +2090,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* PyObject* basePosObj = 0; PyObject* baseOrnObj = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiidddddddii", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiddidddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springDampingAllDirections, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId)) { return NULL; } @@ -2125,6 +2147,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* { b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness); b3LoadSoftBodyUseBendingSprings(command, useBendingSprings, springBendingStiffness); + b3LoadSoftBodyUseAllDirectionDampingSprings(command, springDampingAllDirections); } if (useNeoHookean) { @@ -2134,6 +2157,10 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* { b3LoadSoftBodySetSelfCollision(command, useSelfCollision); } + if (repulsionStiffness > 0) + { + b3LoadSoftBodySetRepulsionStiffness(command, repulsionStiffness); + } b3LoadSoftBodySetFrictionCoefficient(command, frictionCoeff); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -4887,6 +4914,67 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, return Py_None; } +static PyObject* pybullet_changeScaling(PyObject* self, + PyObject* args, PyObject* keywds) +{ + { + int bodyUniqueId; + PyObject* scalingObj; + double scaling[3]; + + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "scaling", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist, &bodyUniqueId, &scalingObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(scalingObj, "expected a sequence"); + len = PySequence_Size(scalingObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + scaling[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "scaling needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + + + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + b3CreatePoseCommandSetBaseScaling(commandHandle, scaling); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + } + Py_INCREF(Py_None); + return Py_None; +} + + // Get the a single joint info for a specific bodyUniqueId // // Args: @@ -6464,11 +6552,13 @@ static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObje double from[3]; double to[3]; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"rayFromPosition", "rayToPosition", "physicsClientId", NULL}; + int reportHitNumber = -1; + static char* kwlist[] = {"rayFromPosition", "rayToPosition", "collisionFilterMask", "reportHitNumber", "physicsClientId", NULL}; int physicsClientId = 0; + int collisionFilterMask = -1; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, - &rayFromObj, &rayToObj, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iii", kwlist, + &rayFromObj, &rayToObj, &collisionFilterMask, &reportHitNumber, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -6481,9 +6571,17 @@ static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObje pybullet_internalSetVectord(rayFromObj, from); pybullet_internalSetVectord(rayToObj, to); + commandHandle = b3CreateRaycastCommandInit(sm, from[0], from[1], from[2], to[0], to[1], to[2]); + + b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); + + if (reportHitNumber >= 0) + { + b3RaycastBatchSetReportHitNumber(commandHandle, reportHitNumber); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) @@ -6546,17 +6644,20 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* PyObject* rayFromObjList = 0; PyObject* rayToObjList = 0; int numThreads = 1; + int reportHitNumber = -1; b3PhysicsClientHandle sm = 0; int sizeFrom = 0; int sizeTo = 0; int parentObjectUniqueId = -1; int parentLinkIndex = -1; + int collisionFilterMask = -1; + double fractionEpsilon = -1; - static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "reportHitNumber", "collisionFilterMask","fractionEpsilon","physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiii", kwlist, - &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiiiidi", kwlist, + &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &reportHitNumber, &collisionFilterMask , &fractionEpsilon, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -6566,11 +6667,44 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* return NULL; } + if (!rayFromObjList || !rayToObjList) + { + PyErr_SetString(SpamError, "rayFromPositions and rayToPositions must be not None."); + return NULL; + } + commandHandle = b3CreateRaycastBatchCommandInit(sm); b3RaycastBatchSetNumThreads(commandHandle, numThreads); - if (rayFromObjList) + + int raysAdded = 0; +#ifdef PYBULLET_USE_NUMPY + // Faster approach if both inputs can be converted into ndarray. + if (PyArray_Check(rayFromObjList) && PyArray_Check(rayToObjList)) { + b3PushProfileTiming(sm, "extractPythonFromToNumpy"); + PyArrayObject* rayFromPyArrayObj = (PyArrayObject*)PyArray_FROMANY(rayFromObjList, NPY_DOUBLE, 1, 2, NPY_ARRAY_CARRAY_RO); + PyArrayObject* rayToPyArrayObj = (PyArrayObject*)PyArray_FROMANY(rayToObjList, NPY_DOUBLE, 1, 2, NPY_ARRAY_CARRAY_RO); + + // If there is error, this will fall back to default method and error messages will be reported there. + if (rayFromPyArrayObj && rayToPyArrayObj + && PyArray_SAMESHAPE(rayFromPyArrayObj, rayToPyArrayObj) + && PyArray_DIMS(rayFromPyArrayObj)[PyArray_NDIM(rayFromPyArrayObj) - 1] == 3) + { + int len = (PyArray_NDIM(rayFromPyArrayObj) == 2) ? PyArray_DIMS(rayFromPyArrayObj)[0] : 1; + if (len <= MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING) + { + b3RaycastBatchAddRays(sm, commandHandle, PyArray_DATA(rayFromPyArrayObj), PyArray_DATA(rayToPyArrayObj), len); + raysAdded = 1; + } + } + if (rayFromPyArrayObj) Py_DECREF(rayFromPyArrayObj); + if (rayToPyArrayObj) Py_DECREF(rayToPyArrayObj); + b3PopProfileTiming(sm); + } +#endif + if (!raysAdded) { + // go back to default method. PyObject* seqRayFromObj = PySequence_Fast(rayFromObjList, "expected a sequence of rayFrom positions"); PyObject* seqRayToObj = PySequence_Fast(rayToObjList, "expected a sequence of 'rayTo' positions"); @@ -6644,7 +6778,17 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* { b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId, parentLinkIndex); } + if (reportHitNumber >= 0) + { + b3RaycastBatchSetReportHitNumber(commandHandle, reportHitNumber); + } + b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); + if (fractionEpsilon >= 0) + { + b3RaycastBatchSetFractionEpsilon(commandHandle, fractionEpsilon); + + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) @@ -6976,7 +7120,27 @@ static PyObject* pybullet_getDebugVisualizerCamera(PyObject* self, PyObject* arg int hasCamInfo; b3SharedMemoryStatusHandle statusHandle; struct b3OpenGLVisualizerCameraInfo camera; - PyObject* pyCameraList = 0; + int i; + camera.m_width=0; + camera.m_height=0; + camera.m_dist=0; + camera.m_yaw=0; + camera.m_pitch=0; + + for (i=0;i<16;i++) + { + camera.m_viewMatrix[i]=0; + camera.m_projectionMatrix[i]=0; + } + for (i=0;i<3;i++) + { + camera.m_camUp[i]=0; + camera.m_camForward[i]=0; + camera.m_horizontal[i]=0; + camera.m_vertical[i]=0; + camera.m_target[i]=0; + } + PyObject* pyCameraList = 0; sm = getPhysicsClient(physicsClientId); if (sm == 0) @@ -6989,7 +7153,7 @@ static PyObject* pybullet_getDebugVisualizerCamera(PyObject* self, PyObject* arg statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); hasCamInfo = b3GetStatusOpenGLVisualizerCamera(statusHandle, &camera); - if (hasCamInfo) + if (1) { PyObject* item = 0; pyCameraList = PyTuple_New(12); @@ -7889,7 +8053,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &collisionShapeA, &collisionShapeB, &collisionShapePositionAObj, &collisionShapePositionBObj, - &collisionShapeOrientationA, &collisionShapeOrientationBObj, + &collisionShapeOrientationAObj, &collisionShapeOrientationBObj, &physicsClientId)) return NULL; @@ -11794,8 +11958,8 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb if (dofCount) { int byteSizeDofCount = sizeof(double) * dofCount; - double* linearJacobian = (double*)malloc(3 * byteSizeDofCount); - double* angularJacobian = (double*)malloc(3 * byteSizeDofCount); + linearJacobian = (double*)malloc(3 * byteSizeDofCount); + angularJacobian = (double*)malloc(3 * byteSizeDofCount); b3GetStatusJacobian(statusHandle, NULL, linearJacobian, @@ -12171,6 +12335,12 @@ static PyMethodDef SpamMethods[] = { "Reset the world position and orientation of the base of the object " "instantaneously, not through physics simulation. (x,y,z) position vector " "and (x,y,z,w) quaternion orientation."}, + + { "unsupportedChangeScaling", + (PyCFunction)pybullet_changeScaling, METH_VARARGS | METH_KEYWORDS, + "Change the scaling of the base of an object." + "Warning: unsupported rudimentary feature that has many limitations." + }, {"getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, METH_VARARGS | METH_KEYWORDS, @@ -12745,6 +12915,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_IGNORE_VISUAL_SHAPES", URDF_IGNORE_VISUAL_SHAPES); PyModule_AddIntConstant(m, "URDF_IGNORE_COLLISION_SHAPES",URDF_IGNORE_COLLISION_SHAPES); PyModule_AddIntConstant(m, "URDF_PRINT_URDF_INFO", URDF_PRINT_URDF_INFO); + PyModule_AddIntConstant(m, "URDF_GOOGLEY_UNDEFINED_COLORS", URDF_GOOGLEY_UNDEFINED_COLORS); PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping); @@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.7.3', + version='2.8.7', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp index 6f2c5251a..4938fa17a 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp @@ -285,7 +285,6 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); curNodeSubPart = nodeSubPart; - b3Assert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, @@ -293,7 +292,13 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: b3Assert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp b/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp index 145de62db..f6c779a91 100644 --- a/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp +++ b/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp @@ -851,12 +851,12 @@ void bFile::swapData(char *data, short type, int arraySize, bool ignoreEndianFla void bFile::safeSwapPtr(char *dst, const char *src) { + if (!src || !dst) + return; + int ptrFile = mFileDNA->getPointerSize(); int ptrMem = mMemoryDNA->getPointerSize(); - if (!src && !dst) - return; - if (ptrFile == ptrMem) { memcpy(dst, src, ptrMem); diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index f4a2d5e36..56011899c 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -61,7 +61,8 @@ public: virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0; virtual int getNumOverlappingPairs() const = 0; - + virtual bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const = 0; + virtual btOverlapFilterCallback* getOverlapFilterCallback() = 0; virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0; virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; @@ -380,6 +381,14 @@ public: { } + bool needsBroadphaseCollision(btBroadphaseProxy*, btBroadphaseProxy*) const + { + return true; + } + btOverlapFilterCallback* getOverlapFilterCallback() + { + return 0; + } virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) { } diff --git a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 4954e773e..19f1737b7 100644 --- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -346,8 +346,6 @@ void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb } } -int maxIterations = 0; - void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const { btAssert(!m_useQuantization); @@ -387,8 +385,6 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, cons curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } /* @@ -529,8 +525,6 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const @@ -654,8 +648,6 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const @@ -718,8 +710,6 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } //This traversal can be called from Playstation 3 SPU diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 85dc488c8..3a1c271af 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -251,6 +251,16 @@ public: m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; } + int getNumObjectsWithoutCollision() const + { + return m_objectsWithoutCollisionCheck.size(); + } + + const btCollisionObject* getObjectWithoutCollision(int index) + { + return m_objectsWithoutCollisionCheck[index]; + } + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); diff --git a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index a4252c296..a71700f58 100644 --- a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -361,7 +361,13 @@ void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangle for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index d663b3d6d..c66ce58e3 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -124,12 +124,17 @@ void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const nodeSubPart); unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { @@ -193,12 +198,17 @@ void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, con nodeSubPart); unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 687399e0a..863ea6d6a 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -286,7 +286,6 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); curNodeSubPart = nodeSubPart; - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, @@ -294,7 +293,13 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/src/BulletCollision/Gimpact/btGImpactShape.h b/src/BulletCollision/Gimpact/btGImpactShape.h index 5b85e8704..cc9107957 100644 --- a/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/src/BulletCollision/Gimpact/btGImpactShape.h @@ -623,13 +623,21 @@ public: i1 = s_indices[1]; i2 = s_indices[2]; } - else + else if (indicestype == PHY_INTEGER) { unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride); i0 = i_indices[0]; i1 = i_indices[1]; i2 = i_indices[2]; } + else + { + btAssert(indicestype == PHY_UCHAR); + unsigned char* i_indices = (unsigned char*)(indexbase + face_index * indexstride); + i0 = i_indices[0]; + i1 = i_indices[1]; + i2 = i_indices[2]; + } } SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 782d8b12b..3316403a8 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -47,6 +47,8 @@ struct btContactSolverInfoData btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_cfm; //constraint force mixing for deformable constraints + btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -83,7 +85,9 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.3); + m_deformable_erp = btScalar(0.06); + m_deformable_cfm = btScalar(0.01); + m_deformable_maxErrorReduction = btScalar(0.1); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index a3c9f42eb..fb15ae31e 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -800,6 +800,14 @@ public: ///don't do CCD when the collision filters are not matching if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; + if (m_pairCache->getOverlapFilterCallback()) { + btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle(); + bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1); + if (!collides) + { + return false; + } + } btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index f1b50b39c..27fdead76 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -384,6 +384,9 @@ void btRigidBody::integrateVelocities(btScalar step) { m_angularVelocity *= (MAX_ANGVEL / step) / angvel; } + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } btQuaternion btRigidBody::getOrientation() const diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 39d47cbbd..0a597d90c 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -305,6 +305,9 @@ public: void applyTorque(const btVector3& torque) { m_totalTorque += torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_totalTorque); + #endif } void applyForce(const btVector3& force, const btVector3& rel_pos) @@ -316,11 +319,17 @@ public: void applyCentralImpulse(const btVector3& impulse) { m_linearVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif } void applyTorqueImpulse(const btVector3& torque) { m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) @@ -361,20 +370,46 @@ public: { m_pushVelocity = v; } - + + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + void clampVelocity(btVector3& v) const { + v.setX( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getX())) + ); + v.setY( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getY())) + ); + v.setZ( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getZ())) + ); + } + #endif + void setTurnVelocity(const btVector3& v) { m_turnVelocity = v; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_turnVelocity); + #endif } void applyCentralPushImpulse(const btVector3& impulse) { m_pushVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_pushVelocity); + #endif } void applyTorqueTurnImpulse(const btVector3& torque) { m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_turnVelocity); + #endif } void clearForces() @@ -408,12 +443,18 @@ public: { m_updateRevision++; m_linearVelocity = lin_vel; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif } inline void setAngularVelocity(const btVector3& ang_vel) { m_updateRevision++; m_angularVelocity = ang_vel; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const @@ -424,6 +465,12 @@ public: //for kinematic objects, we could also use use: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } + + btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const + { + //we also calculate lin/ang velocity for kinematic objects + return m_pushVelocity + m_turnVelocity.cross(rel_pos); + } void translate(const btVector3& v) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a1d5bb9ca..9862bd2e2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -344,6 +344,8 @@ void btMultiBody::finalizeMultiDof() { m_deltaV.resize(0); m_deltaV.resize(6 + m_dofCount); + m_splitV.resize(0); + m_splitV.resize(6 + m_dofCount); m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) m_matrixBuf.resize(m_links.size() + 1); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index be795633f..f2acfab9b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -278,6 +278,11 @@ public: { return &m_deltaV[0]; } + + const btScalar *getSplitVelocityVector() const + { + return &m_splitV[0]; + } /* btScalar * getVelocityVector() { return &real_buf[0]; @@ -397,6 +402,26 @@ public: m_deltaV[dof] += delta_vee[dof] * multiplier; } } + void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] += delta_vee[dof] * multiplier; + } + } + void addSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], 1); + } + void substractSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], -1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] = 0.f; + } + } void processDeltaVeeMultiDof2() { applyDeltaVeeMultiDof(&m_deltaV[0], 1); @@ -711,6 +736,7 @@ private: // offset size array // 0 num_links+1 rot_from_parent // + btAlignedObjectArray<btScalar> m_splitV; btAlignedObjectArray<btScalar> m_deltaV; btAlignedObjectArray<btScalar> m_realBuf; btAlignedObjectArray<btVector3> m_vectorBuf; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d7ed05ce5..1ba586114 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -2,11 +2,12 @@ #include "BulletDynamics/Dynamics/btRigidBody.h" #include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral) +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type) : m_bodyA(bodyA), m_bodyB(bodyB), m_linkA(linkA), m_linkB(linkB), + m_type(type), m_numRows(numRows), m_jacSizeA(0), m_jacSizeBoth(0), diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 5c15f3e85..4a6007ee3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -20,6 +20,21 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "btMultiBody.h" + +//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility +enum btTypedMultiBodyConstraintType +{ + MULTIBODY_CONSTRAINT_LIMIT=3, + MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR, + MULTIBODY_CONSTRAINT_GEAR, + MULTIBODY_CONSTRAINT_POINT_TO_POINT, + MULTIBODY_CONSTRAINT_SLIDER, + MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR, + MULTIBODY_CONSTRAINT_FIXED, + + MAX_MULTIBODY_CONSTRAINT_TYPE, +}; + class btMultiBody; struct btSolverInfo; @@ -46,6 +61,8 @@ protected: int m_linkA; int m_linkB; + int m_type; //btTypedMultiBodyConstraintType + int m_numRows; int m_jacSizeA; int m_jacSizeBoth; @@ -82,12 +99,16 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral); + btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type); virtual ~btMultiBodyConstraint(); void updateJacobianSizes(); void allocateJacobiansMultiDof(); + int getConstraintType() const + { + return m_type; + } //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. virtual void setFrameInB(const btMatrix3x3& frameInB) {} virtual void setPivotInB(const btVector3& pivotInB) {} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index cd1bad089..f599c9ccb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links @@ -610,6 +611,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->substractSplitV(); } else { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp index 5ef9444c2..df2abbe97 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -24,7 +24,7 @@ subject to the following restrictions: #define BTMBFIXEDCONSTRAINT_DIM 6 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -36,7 +36,7 @@ btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int li } btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index bf6b811d2..ee02cf9b0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR), m_gearRatio(1), m_gearAuxLink(-1), m_erp(0), diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 8791ad286..94b36ac10 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) //:btMultiBodyConstraint(body,0,link,-1,2,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT), m_lowerBound(lower), m_upperBound(upper) { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 6716ba490..b810692b4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -42,6 +42,22 @@ public: { //todo(erwincoumans) } + btScalar getLowerBound() const + { + return m_lowerBound; + } + btScalar getUpperBound() const + { + return m_upperBound; + } + void setLowerBound(btScalar lower) + { + m_lowerBound = lower; + } + void setUpperBound(btScalar upper) + { + m_upperBound = upper; + } }; #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 5c816c498..fec9b0321 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), @@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof() btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 37d3aede3..f51e69deb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -37,7 +37,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index e025302ce..48ec1d5af 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: #define EPSILON 0.000001 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -38,7 +38,7 @@ btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int } btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp index 3e5aa30f2..25ddd539b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -23,7 +23,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR), m_desiredVelocity(0, 0, 0), m_desiredPosition(0,0,0,1), m_kd(1.), diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 26f75e5da..945276801 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -44,6 +44,7 @@ SET(BulletSoftBody_HDRS btCGProjection.h btConjugateGradient.h + btConjugateResidual.h btDeformableGravityForce.h btDeformableMassSpringForce.h btDeformableCorotatedForce.h @@ -58,6 +59,7 @@ SET(BulletSoftBody_HDRS btDeformableContactProjection.h btDeformableMultiBodyDynamicsWorld.h btDeformableContactConstraint.h + btKrylovSolver.h poly34.h btSoftBodySolverVertexBuffer.h diff --git a/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h b/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h index 7b225701f..01c7e93a1 100644 --- a/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h +++ b/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h @@ -13,13 +13,12 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver btDeformableMultiBodyConstraintSolver* m_deformableSolver; DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) + btDispatcher* dispatcher) + : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) { } - - virtual void processConstraints(int islandId=-1) + virtual void processConstraints(int islandId = -1) { btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0; @@ -30,7 +29,7 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1)) { m_deformableSolver->m_analyticsData.m_islandId = islandId; m_islandAnalyticsData.push_back(m_solver->m_analyticsData); diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index d047e6d3d..e05970664 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -22,85 +22,83 @@ struct DeformableContactConstraint { - const btSoftBody::Node* m_node; - btAlignedObjectArray<const btSoftBody::RContact*> m_contact; - btAlignedObjectArray<btVector3> m_total_normal_dv; - btAlignedObjectArray<btVector3> m_total_tangent_dv; - btAlignedObjectArray<bool> m_static; - btAlignedObjectArray<bool> m_can_be_dynamic; - - DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node) - { - append(rcontact); - } - - DeformableContactConstraint(): m_node(NULL) - { - m_contact.push_back(NULL); - } - - void append(const btSoftBody::RContact& rcontact) - { - m_contact.push_back(&rcontact); - m_total_normal_dv.push_back(btVector3(0,0,0)); - m_total_tangent_dv.push_back(btVector3(0,0,0)); - m_static.push_back(false); - m_can_be_dynamic.push_back(true); - } - - void replace(const btSoftBody::RContact& rcontact) - { - m_contact.clear(); - m_total_normal_dv.clear(); - m_total_tangent_dv.clear(); - m_static.clear(); - m_can_be_dynamic.clear(); - append(rcontact); - } - - ~DeformableContactConstraint() - { - } + const btSoftBody::Node* m_node; + btAlignedObjectArray<const btSoftBody::RContact*> m_contact; + btAlignedObjectArray<btVector3> m_total_normal_dv; + btAlignedObjectArray<btVector3> m_total_tangent_dv; + btAlignedObjectArray<bool> m_static; + btAlignedObjectArray<bool> m_can_be_dynamic; + + DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node) + { + append(rcontact); + } + + DeformableContactConstraint() : m_node(NULL) + { + m_contact.push_back(NULL); + } + + void append(const btSoftBody::RContact& rcontact) + { + m_contact.push_back(&rcontact); + m_total_normal_dv.push_back(btVector3(0, 0, 0)); + m_total_tangent_dv.push_back(btVector3(0, 0, 0)); + m_static.push_back(false); + m_can_be_dynamic.push_back(true); + } + + void replace(const btSoftBody::RContact& rcontact) + { + m_contact.clear(); + m_total_normal_dv.clear(); + m_total_tangent_dv.clear(); + m_static.clear(); + m_can_be_dynamic.clear(); + append(rcontact); + } + + ~DeformableContactConstraint() + { + } }; class btCGProjection { public: - typedef btAlignedObjectArray<btVector3> TVStack; - typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; - typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - const btScalar& m_dt; - // map from node indices to node pointers - const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; - - btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) - : m_softBodies(softBodies) - , m_dt(dt) - { - } - - virtual ~btCGProjection() - { - } - - // apply the constraints - virtual void project(TVStack& x) = 0; - - virtual void setConstraints() = 0; - - // update the constraints - virtual btScalar update() = 0; - - virtual void reinitialize(bool nodeUpdated) - { - } - - virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) - { - m_nodes = nodes; - } -}; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btScalar& m_dt; + // map from node indices to node pointers + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt) + : m_softBodies(softBodies), m_dt(dt) + { + } + virtual ~btCGProjection() + { + } + + // apply the constraints + virtual void project(TVStack& x) = 0; + + virtual void setConstraints() = 0; + + // update the constraints + virtual btScalar update() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } +}; #endif /* btCGProjection_h */ diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index bd51e584b..bcd5e6b51 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -15,144 +15,103 @@ #ifndef BT_CONJUGATE_GRADIENT_H #define BT_CONJUGATE_GRADIENT_H -#include <iostream> -#include <cmath> -#include <limits> -#include <LinearMath/btAlignedObjectArray.h> -#include <LinearMath/btVector3.h> -#include "LinearMath/btQuickprof.h" +#include "btKrylovSolver.h" template <class MatrixX> -class btConjugateGradient +class btConjugateGradient : public btKrylovSolver<MatrixX> { - typedef btAlignedObjectArray<btVector3> TVStack; - TVStack r,p,z,temp; - int max_iterations; - btScalar tolerance_squared; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp; + public: - btConjugateGradient(const int max_it_in) - : max_iterations(max_it_in) - { - tolerance_squared = 1e-5; - } - - virtual ~btConjugateGradient(){} - - // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) - { - BT_PROFILE("CGSolve"); - btAssert(x.size() == b.size()); - reinitialize(b); - // r = b - A * x --with assigned dof zeroed out - A.multiply(x, temp); - r = sub(b, temp); - A.project(r); - // z = M^(-1) * r - A.precondition(r, z); - A.project(z); - btScalar r_dot_z = dot(z,r); - if (r_dot_z <= tolerance_squared) { - if (verbose) - { - std::cout << "Iteration = 0" << std::endl; - std::cout << "Two norm of the residual = " << r_dot_z << std::endl; - } - return 0; - } - p = z; - btScalar r_dot_z_new = r_dot_z; - for (int k = 1; k <= max_iterations; k++) { - // temp = A*p - A.multiply(p, temp); - A.project(temp); - if (dot(p,temp) < SIMD_EPSILON) - { - if (verbose) - std::cout << "Encountered negative direction in CG!" << std::endl; - if (k == 1) - { - x = b; - } - return k; - } - // alpha = r^T * z / (p^T * A * p) - btScalar alpha = r_dot_z_new / dot(p, temp); - // x += alpha * p; - multAndAddTo(alpha, p, x); - // r -= alpha * temp; - multAndAddTo(-alpha, temp, r); - // z = M^(-1) * r - A.precondition(r, z); - r_dot_z = r_dot_z_new; - r_dot_z_new = dot(r,z); - if (r_dot_z_new < tolerance_squared) { - if (verbose) - { - std::cout << "ConjugateGradient iterations " << k << std::endl; - } - return k; - } + btConjugateGradient(const int max_it_in) + : btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON) + { + } + + virtual ~btConjugateGradient() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CGSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + temp = b; + A.project(temp); + p = temp; + A.precondition(p, z); + btScalar d0 = this->dot(z, temp); + d0 = btMin(btScalar(1), d0); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp); + r = this->sub(b, temp); + A.project(r); + // z = M^(-1) * r + A.precondition(r, z); + A.project(z); + btScalar r_dot_z = this->dot(z, r); + if (r_dot_z <= Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "Iteration = 0" << std::endl; + std::cout << "Two norm of the residual = " << r_dot_z << std::endl; + } + return 0; + } + p = z; + btScalar r_dot_z_new = r_dot_z; + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // temp = A*p + A.multiply(p, temp); + A.project(temp); + if (this->dot(p, temp) < 0) + { + if (verbose) + std::cout << "Encountered negative direction in CG!" << std::endl; + if (k == 1) + { + x = b; + } + return k; + } + // alpha = r^T * z / (p^T * A * p) + btScalar alpha = r_dot_z_new / this->dot(p, temp); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * temp; + this->multAndAddTo(-alpha, temp, r); + // z = M^(-1) * r + A.precondition(r, z); + r_dot_z = r_dot_z_new; + r_dot_z_new = this->dot(r, z); + if (r_dot_z_new < Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl; + } + return k; + } + + btScalar beta = r_dot_z_new / r_dot_z; + p = this->multAndAdd(beta, p, z); + } + if (verbose) + { + std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl; + } + return Base::m_maxIterations; + } - btScalar beta = r_dot_z_new/r_dot_z; - p = multAndAdd(beta, p, z); - } - if (verbose) - { - std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl; - } - return max_iterations; - } - - void reinitialize(const TVStack& b) - { - r.resize(b.size()); - p.resize(b.size()); - z.resize(b.size()); - temp.resize(b.size()); - } - - TVStack sub(const TVStack& a, const TVStack& b) - { - // c = a-b - btAssert(a.size() == b.size()); - TVStack c; - c.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - { - c[i] = a[i] - b[i]; - } - return c; - } - - btScalar squaredNorm(const TVStack& a) - { - return dot(a,a); - } - - btScalar dot(const TVStack& a, const TVStack& b) - { - btScalar ans(0); - for (int i = 0; i < a.size(); ++i) - ans += a[i].dot(b[i]); - return ans; - } - - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) - { -// result += s*a - btAssert(a.size() == result.size()); - for (int i = 0; i < a.size(); ++i) - result[i] += s * a[i]; - } - - TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) - { - // result = a*s + b - TVStack result; - result.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - result[i] = s * a[i] + b[i]; - return result; - } + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp.resize(b.size()); + } }; #endif /* btConjugateGradient_h */ diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h new file mode 100644 index 000000000..e3bca6e12 --- /dev/null +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -0,0 +1,112 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. 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: + 1. 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. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_CONJUGATE_RESIDUAL_H +#define BT_CONJUGATE_RESIDUAL_H +#include "btKrylovSolver.h" + +template <class MatrixX> +class btConjugateResidual : public btKrylovSolver<MatrixX> +{ + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp_p, temp_r, best_x; + // temp_r = A*r + // temp_p = A*p + // z = M^(-1) * temp_p = M^(-1) * A * p + btScalar best_r; + +public: + btConjugateResidual(const int max_it_in) + : Base(max_it_in, 1e-4) + { + } + + virtual ~btConjugateResidual() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CRSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp_r); // borrow temp_r here to store A*x + r = this->sub(b, temp_r); + // z = M^(-1) * r + A.precondition(r, z); // borrow z to store preconditioned r + r = z; + btScalar residual_norm = this->norm(r); + if (residual_norm <= Base::m_tolerance) + { + return 0; + } + p = r; + btScalar r_dot_Ar, r_dot_Ar_new; + // temp_p = A*p + A.multiply(p, temp_p); + // temp_r = A*r + temp_r = temp_p; + r_dot_Ar = this->dot(r, temp_r); + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // z = M^(-1) * Ap + A.precondition(temp_p, z); + // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) + btScalar alpha = r_dot_Ar / this->dot(temp_p, z); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * z; + this->multAndAddTo(-alpha, z, r); + btScalar norm_r = this->norm(r); + if (norm_r < best_r) + { + best_x = x; + best_r = norm_r; + if (norm_r < Base::m_tolerance) + { + return k; + } + } + // temp_r = A * r; + A.multiply(r, temp_r); + r_dot_Ar_new = this->dot(r, temp_r); + btScalar beta = r_dot_Ar_new / r_dot_Ar; + r_dot_Ar = r_dot_Ar_new; + // p = beta*p + r; + p = this->multAndAdd(beta, p, r); + // temp_p = beta*temp_p + temp_r; + temp_p = this->multAndAdd(beta, temp_p, temp_r); + } + if (verbose) + { + std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl; + } + x = best_x; + return Base::m_maxIterations; + } + + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp_p.resize(b.size()); + temp_r.resize(b.size()); + best_x.resize(b.size()); + best_r = SIMD_INFINITY; + } +}; +#endif /* btConjugateResidual_h */ diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index eb50f5a56..0c3e0b5eb 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -17,181 +17,280 @@ #include "btPreconditioner.h" #include "LinearMath/btQuickprof.h" -btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v) -: m_softBodies(softBodies) -, m_projection(softBodies) -, m_backupVelocity(backup_v) -, m_implicit(false) +btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v) + : m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false) { - m_preconditioner = new MassPreconditioner(m_softBodies); + m_massPreconditioner = new MassPreconditioner(m_softBodies); + m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); + m_preconditioner = m_KKTPreconditioner; } btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() { - delete m_preconditioner; + delete m_KKTPreconditioner; + delete m_massPreconditioner; } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) { - BT_PROFILE("reinitialize"); - if (dt > 0) - { - setDt(dt); - } - if(nodeUpdated) - { - updateId(); - } - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->reinitialize(nodeUpdated); - } - m_projection.reinitialize(nodeUpdated); - m_preconditioner->reinitialize(nodeUpdated); + BT_PROFILE("reinitialize"); + if (dt > 0) + { + setDt(dt); + } + if (nodeUpdated) + { + updateId(); + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + } + btMatrix3x3 I; + I.setIdentity(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im); + } + } + m_projection.reinitialize(nodeUpdated); + // m_preconditioner->reinitialize(nodeUpdated); } void btDeformableBackwardEulerObjective::setDt(btScalar dt) { - m_dt = dt; + m_dt = dt; } void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const { - BT_PROFILE("multiply"); - // add in the mass term - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; - ++counter; - } - } - - for (int i = 0; i < m_lf.size(); ++i) - { - // add damping matrix - m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); - if (m_implicit) - { - m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); - } - } + BT_PROFILE("multiply"); + // add in the mass term + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im; + ++counter; + } + } + + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b); + } + } + int offset = m_nodes.size(); + for (int i = offset; i < b.size(); ++i) + { + b[i].setZero(); + } + // add in the lagrange multiplier terms + + for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } } void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - node.m_v = m_backupVelocity[node.index] + dv[node.index]; - } - } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + node.m_v = m_backupVelocity[node.index] + dv[node.index]; + } + } } void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero) { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; - psb->m_nodes[j].m_v += one_over_mass * force[counter++]; - } - } - if (setZero) - { - for (int i = 0; i < force.size(); ++i) - force[i].setZero(); - } + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + if (m_implicit) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im != 0) + { + psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + } + } + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } + } + } + if (setZero) + { + for (int i = 0; i < force.size(); ++i) + force[i].setZero(); + } } -void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) +void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual) { - BT_PROFILE("computeResidual"); - // add implicit force - for (int i = 0; i < m_lf.size(); ++i) - { - if (m_implicit) - { - m_lf[i]->addScaledForces(dt, residual); - } - else - { - m_lf[i]->addScaledDampingForce(dt, residual); - } - } - m_projection.project(residual); + BT_PROFILE("computeResidual"); + // add implicit force + for (int i = 0; i < m_lf.size(); ++i) + { + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledForces(dt, residual); + } + else + { + m_lf[i]->addScaledDampingForce(dt, residual); + } + } + // m_projection.project(residual); } btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const { - btScalar mag = 0; - for (int i = 0; i < residual.size(); ++i) - { - mag += residual[i].length2(); - } - return std::sqrt(mag); + btScalar mag = 0; + for (int i = 0; i < residual.size(); ++i) + { + mag += residual[i].length2(); + } + return std::sqrt(mag); } btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt) { - btScalar e = 0; - for (int i = 0; i < m_lf.size(); ++i) - { - e += m_lf[i]->totalEnergy(dt); - } - return e; + btScalar e = 0; + for (int i = 0; i < m_lf.size(); ++i) + { + e += m_lf[i]->totalEnergy(dt); + } + return e; } void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) { - for (int i = 0; i < m_softBodies.size(); ++i) - { - m_softBodies[i]->advanceDeformation(); - } - - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->addScaledExplicitForce(m_dt, force); - } - applyForce(force, true); + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->advanceDeformation(); + } + if (m_implicit) + { + // apply forces except gravity force + btVector3 gravity; + for (int i = 0; i < m_lf.size(); ++i) + { + if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE) + { + gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity; + } + else + { + m_lf[i]->addScaledForces(m_dt, force); + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledHessian(m_dt); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // add gravity explicitly + psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity; + } + } + } + } + else + { + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledExplicitForce(m_dt, force); + } + } + // calculate inverse mass matrix for all nodes + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse(); + } + } + } + applyForce(force, true); } void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - dv[counter] = psb->m_nodes[j].m_im * residual[counter]; - ++counter; - } - } + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter]; + ++counter; + } + } } //set constraints as projections void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) { - m_projection.setConstraints(infoGlobal); + m_projection.setConstraints(infoGlobal); } void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) { - m_projection.applyDynamicFriction(r); + m_projection.applyDynamicFriction(r); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 7bd03bd03..eb05b9f01 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -15,7 +15,7 @@ #ifndef BT_BACKWARD_EULER_OBJECTIVE_H #define BT_BACKWARD_EULER_OBJECTIVE_H -#include "btConjugateGradient.h" +//#include "btConjugateGradient.h" #include "btDeformableLagrangianForce.h" #include "btDeformableMassSpringForce.h" #include "btDeformableGravityForce.h" @@ -31,105 +31,168 @@ class btDeformableBackwardEulerObjective { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_dt; - btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - Preconditioner* m_preconditioner; - btDeformableContactProjection m_projection; - const TVStack& m_backupVelocity; - btAlignedObjectArray<btSoftBody::Node* > m_nodes; - bool m_implicit; - - btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v); - - virtual ~btDeformableBackwardEulerObjective(); - - void initialize(){} - - // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual - void computeResidual(btScalar dt, TVStack& residual); - - // add explicit force to the velocity - void applyExplicitForce(TVStack& force); - - // apply force to velocity and optionally reset the force to zero - void applyForce(TVStack& force, bool setZero); - - // compute the norm of the residual - btScalar computeNorm(const TVStack& residual) const; - - // compute one step of the solve (there is only one solve if the system is linear) - void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); - - // perform A*x = b - void multiply(const TVStack& x, TVStack& b) const; - - // set initial guess for CG solve - void initialGuess(TVStack& dv, const TVStack& residual); - - // reset data structure and reset dt - void reinitialize(bool nodeUpdated, btScalar dt); - - void setDt(btScalar dt); - - // add friction force to residual - void applyDynamicFriction(TVStack& r); - - // add dv to velocity - void updateVelocity(const TVStack& dv); - - //set constraints as projections - void setConstraints(const btContactSolverInfo& infoGlobal); - - // update the projections and project the residual - void project(TVStack& r) - { - BT_PROFILE("project"); - m_projection.project(r); - } - - // perform precondition M^(-1) x = b - void precondition(const TVStack& x, TVStack& b) - { - m_preconditioner->operator()(x,b); - } - - // reindex all the vertices - virtual void updateId() - { - size_t node_id = 0; - size_t face_id = 0; - m_nodes.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].index = node_id; - m_nodes.push_back(&psb->m_nodes[j]); - ++node_id; - } - for (int j = 0; j < psb->m_faces.size(); ++j) - { - psb->m_faces[j].m_index = face_id; - ++face_id; - } - } - } - - const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const - { - return &m_nodes; - } - - void setImplicit(bool implicit) - { - m_implicit = implicit; - } - - // Calculate the total potential energy in the system - btScalar totalEnergy(btScalar dt); + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_dt; + btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + Preconditioner* m_preconditioner; + btDeformableContactProjection m_projection; + const TVStack& m_backupVelocity; + btAlignedObjectArray<btSoftBody::Node*> m_nodes; + bool m_implicit; + MassPreconditioner* m_massPreconditioner; + KKTPreconditioner* m_KKTPreconditioner; + + btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v); + + virtual ~btDeformableBackwardEulerObjective(); + + void initialize() {} + + // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual + void computeResidual(btScalar dt, TVStack& residual); + + // add explicit force to the velocity + void applyExplicitForce(TVStack& force); + + // apply force to velocity and optionally reset the force to zero + void applyForce(TVStack& force, bool setZero); + + // compute the norm of the residual + btScalar computeNorm(const TVStack& residual) const; + + // compute one step of the solve (there is only one solve if the system is linear) + void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); + + // perform A*x = b + void multiply(const TVStack& x, TVStack& b) const; + + // set initial guess for CG solve + void initialGuess(TVStack& dv, const TVStack& residual); + + // reset data structure and reset dt + void reinitialize(bool nodeUpdated, btScalar dt); + + void setDt(btScalar dt); + + // add friction force to residual + void applyDynamicFriction(TVStack& r); + + // add dv to velocity + void updateVelocity(const TVStack& dv); + + //set constraints as projections + void setConstraints(const btContactSolverInfo& infoGlobal); + + // update the projections and project the residual + void project(TVStack& r) + { + BT_PROFILE("project"); + m_projection.project(r); + } + + // perform precondition M^(-1) x = b + void precondition(const TVStack& x, TVStack& b) + { + m_preconditioner->operator()(x, b); + } + + // reindex all the vertices + virtual void updateId() + { + size_t node_id = 0; + size_t face_id = 0; + m_nodes.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].index = node_id; + m_nodes.push_back(&psb->m_nodes[j]); + ++node_id; + } + for (int j = 0; j < psb->m_faces.size(); ++j) + { + psb->m_faces[j].m_index = face_id; + ++face_id; + } + } + } + + const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const + { + return &m_nodes; + } + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + // Calculate the total potential energy in the system + btScalar totalEnergy(btScalar dt); + + void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec) + { + extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < vec.size(); ++i) + { + extended_vec[i] = vec[i]; + } + int offset = vec.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + extended_vec[offset + i].setZero(); + } + } + + void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual) + { + extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < residual.size(); ++i) + { + extended_residual[i] = residual[i]; + } + int offset = residual.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i]; + extended_residual[offset + i].setZero(); + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int n = 0; n < lm.m_num_nodes; ++n) + { + extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); + } + } + } + } + + void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f) + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im; + ++counter; + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f); + } + counter = 0; + for (; counter < f.size(); ++counter) + { + f[counter] = rhs[counter] - f[counter]; + } + } }; #endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index a334dd443..4916848fe 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -18,452 +18,486 @@ #include "btDeformableBodySolver.h" #include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" -static const int kMaxConjugateGradientIterations = 50; +static const int kMaxConjugateGradientIterations = 300; btDeformableBodySolver::btDeformableBodySolver() -: m_numNodes(0) -, m_cg(kMaxConjugateGradientIterations) -, m_maxNewtonIterations(5) -, m_newtonTolerance(1e-4) -, m_lineSearch(false) + : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) { - m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); + m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } btDeformableBodySolver::~btDeformableBodySolver() { - delete m_objective; + delete m_objective; } void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { - BT_PROFILE("solveDeformableConstraints"); - if (!m_implicit) - { - m_objective->computeResidual(solverdt, m_residual); - m_objective->applyDynamicFriction(m_residual); - computeStep(m_dv, m_residual); - updateVelocity(); - } - else - { - for (int i = 0; i < m_maxNewtonIterations; ++i) - { - updateState(); - // add the inertia term in the residual - int counter = 0; - for (int k = 0; k < m_softBodies.size(); ++k) - { - btSoftBody* psb = m_softBodies[k]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (psb->m_nodes[j].m_im > 0) - { - m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter]; - } - ++counter; - } - } - - m_objective->computeResidual(solverdt, m_residual); - if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) - { - break; - } - // todo xuchenhan@: this really only needs to be calculated once - m_objective->applyDynamicFriction(m_residual); - if (m_lineSearch) - { - btScalar inner_product = computeDescentStep(m_ddv,m_residual); - btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 - btScalar scale = 2; - btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2; - backupDv(); - do { - scale *= beta; - if (scale < 1e-8) { - return; - } - updateEnergy(scale); - f1 = m_objective->totalEnergy(solverdt)+kineticEnergy(); - f2 = f0 - alpha * scale * inner_product; - } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues - revertDv(); - updateDv(scale); - } - else - { - computeStep(m_ddv, m_residual); - updateDv(); - } - for (int j = 0; j < m_numNodes; ++j) - { - m_ddv[j].setZero(); - m_residual[j].setZero(); - } - } - updateVelocity(); - } + BT_PROFILE("solveDeformableConstraints"); + if (!m_implicit) + { + m_objective->computeResidual(solverdt, m_residual); + m_objective->applyDynamicFriction(m_residual); + if (m_useProjection) + { + computeStep(m_dv, m_residual); + } + else + { + TVStack rhs, x; + m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); + m_objective->addLagrangeMultiplier(m_dv, x); + m_objective->m_preconditioner->reinitialize(true); + computeStep(x, rhs); + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = x[i]; + } + } + updateVelocity(); + } + else + { + for (int i = 0; i < m_maxNewtonIterations; ++i) + { + updateState(); + // add the inertia term in the residual + int counter = 0; + for (int k = 0; k < m_softBodies.size(); ++k) + { + btSoftBody* psb = m_softBodies[k]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + { + m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter]; + } + ++counter; + } + } + + m_objective->computeResidual(solverdt, m_residual); + if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) + { + break; + } + // todo xuchenhan@: this really only needs to be calculated once + m_objective->applyDynamicFriction(m_residual); + if (m_lineSearch) + { + btScalar inner_product = computeDescentStep(m_ddv, m_residual); + btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 + btScalar scale = 2; + btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2; + backupDv(); + do + { + scale *= beta; + if (scale < 1e-8) + { + return; + } + updateEnergy(scale); + f1 = m_objective->totalEnergy(solverdt) + kineticEnergy(); + f2 = f0 - alpha * scale * inner_product; + } while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues + revertDv(); + updateDv(scale); + } + else + { + computeStep(m_ddv, m_residual); + updateDv(); + } + for (int j = 0; j < m_numNodes; ++j) + { + m_ddv[j].setZero(); + m_residual[j].setZero(); + } + } + updateVelocity(); + } } btScalar btDeformableBodySolver::kineticEnergy() { - btScalar ke = 0; - for (int i = 0; i < m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size();++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - if (node.m_im > 0) - { - ke += m_dv[node.index].length2() * 0.5 / node.m_im; - } - } - } - return ke; + btScalar ke = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + ke += m_dv[node.index].length2() * 0.5 / node.m_im; + } + } + } + return ke; } void btDeformableBodySolver::backupDv() { - m_backup_dv.resize(m_dv.size()); - for (int i = 0; i<m_backup_dv.size(); ++i) - { - m_backup_dv[i] = m_dv[i]; - } + m_backup_dv.resize(m_dv.size()); + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_backup_dv[i] = m_dv[i]; + } } void btDeformableBodySolver::revertDv() { - for (int i = 0; i<m_backup_dv.size(); ++i) - { - m_dv[i] = m_backup_dv[i]; - } + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i]; + } } void btDeformableBodySolver::updateEnergy(btScalar scale) { - for (int i = 0; i<m_dv.size(); ++i) - { - m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; - } - updateState(); + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; + } + updateState(); } - btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose) { - m_cg.solve(*m_objective, ddv, residual, false); - btScalar inner_product = m_cg.dot(residual, m_ddv); - btScalar res_norm = m_objective->computeNorm(residual); - btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); - if (inner_product < -tol) - { - if (verbose) - { - std::cout << "Looking backwards!" << std::endl; - } - for (int i = 0; i < m_ddv.size();++i) - { - m_ddv[i] = -m_ddv[i]; - } - inner_product = -inner_product; - } - else if (std::abs(inner_product) < tol) - { - if (verbose) - { - std::cout << "Gradient Descent!" << std::endl; - } - btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; - for (int i = 0; i < m_ddv.size();++i) - { - m_ddv[i] = scale * residual[i]; - } - inner_product = scale * res_norm * res_norm; - } - return inner_product; + m_cg.solve(*m_objective, ddv, residual, false); + btScalar inner_product = m_cg.dot(residual, m_ddv); + btScalar res_norm = m_objective->computeNorm(residual); + btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); + if (inner_product < -tol) + { + if (verbose) + { + std::cout << "Looking backwards!" << std::endl; + } + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = -m_ddv[i]; + } + inner_product = -inner_product; + } + else if (std::abs(inner_product) < tol) + { + if (verbose) + { + std::cout << "Gradient Descent!" << std::endl; + } + btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = scale * residual[i]; + } + inner_product = scale * res_norm * res_norm; + } + return inner_product; } void btDeformableBodySolver::updateState() { - updateVelocity(); - updateTempPosition(); + updateVelocity(); + updateTempPosition(); } void btDeformableBodySolver::updateDv(btScalar scale) { - for (int i = 0; i < m_numNodes; ++i) - { - m_dv[i] += scale * m_ddv[i]; - } + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i] += scale * m_ddv[i]; + } } void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - m_cg.solve(*m_objective, ddv, residual); + if (m_useProjection) + m_cg.solve(*m_objective, ddv, residual, false); + else + m_cr.solve(*m_objective, ddv, residual, false); } -void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt) +void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt) { - m_softBodies.copyFromArray(softBodies); - bool nodeUpdated = updateNodes(); - - if (nodeUpdated) - { - m_dv.resize(m_numNodes, btVector3(0,0,0)); - m_ddv.resize(m_numNodes, btVector3(0,0,0)); - m_residual.resize(m_numNodes, btVector3(0,0,0)); - m_backupVelocity.resize(m_numNodes, btVector3(0,0,0)); - } - - // need to setZero here as resize only set value for newly allocated items - for (int i = 0; i < m_numNodes; ++i) - { - m_dv[i].setZero(); - m_ddv[i].setZero(); - m_residual[i].setZero(); - } - - m_dt = dt; - m_objective->reinitialize(nodeUpdated, dt); - updateSoftBodies(); -} + m_softBodies.copyFromArray(softBodies); + bool nodeUpdated = updateNodes(); -void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) -{ - BT_PROFILE("setConstraint"); - m_objective->setConstraints(infoGlobal); -} + if (nodeUpdated) + { + m_dv.resize(m_numNodes, btVector3(0, 0, 0)); + m_ddv.resize(m_numNodes, btVector3(0, 0, 0)); + m_residual.resize(m_numNodes, btVector3(0, 0, 0)); + m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0)); + } -btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) -{ - BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal); - return maxSquaredResidual; + // need to setZero here as resize only set value for newly allocated items + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i].setZero(); + m_ddv[i].setZero(); + m_residual[i].setZero(); + } + + m_dt = dt; + m_objective->reinitialize(nodeUpdated, dt); + updateSoftBodies(); } -btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) { - BT_PROFILE("solveSplitImpulse"); - return m_objective->m_projection.solveSplitImpulse(infoGlobal); + BT_PROFILE("setConstraint"); + m_objective->setConstraints(infoGlobal); } -void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { - m_objective->m_projection.splitImpulseSetup(infoGlobal); + BT_PROFILE("solveContactConstraints"); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal); + return maxSquaredResidual; } void btDeformableBodySolver::updateVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - psb->m_maxSpeedSquared = 0; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - // set NaN to zero; - if (m_dv[counter] != m_dv[counter]) - { - m_dv[counter].setZero(); - } - psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; - psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); - ++counter; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + psb->m_maxSpeedSquared = 0; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // set NaN to zero; + if (m_dv[counter] != m_dv[counter]) + { + m_dv[counter].setZero(); + } + if (m_implicit) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + } + else + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; + } + psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); + ++counter; + } + } } void btDeformableBodySolver::updateTempPosition() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v; - ++counter; - } - psb->updateDeformation(); - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv); + ++counter; + } + psb->updateDeformation(); + } } void btDeformableBodySolver::backupVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - m_backupVelocity[counter++] = psb->m_nodes[j].m_v; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_backupVelocity[counter++] = psb->m_nodes[j].m_v; + } + } } void btDeformableBodySolver::setupDeformableSolve(bool implicit) { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (implicit) - { - if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON) - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - else - m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; - m_backupVelocity[counter] = psb->m_nodes[j].m_vn; - } - else - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit; - ++counter; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (implicit) + { + // setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint. + if (psb->m_nodes[j].m_v == m_backupVelocity[counter]) + m_dv[counter].setZero(); + else + m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; + m_backupVelocity[counter] = psb->m_nodes[j].m_vn; + } + else + { + m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter]; + } + psb->m_nodes[j].m_v = m_backupVelocity[counter]; + ++counter; + } + } } void btDeformableBodySolver::revertVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_v = m_backupVelocity[counter++]; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter++]; + } + } } bool btDeformableBodySolver::updateNodes() { - int numNodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - numNodes += m_softBodies[i]->m_nodes.size(); - if (numNodes != m_numNodes) - { - m_numNodes = numNodes; - return true; - } - return false; + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + numNodes += m_softBodies[i]->m_nodes.size(); + if (numNodes != m_numNodes) + { + m_numNodes = numNodes; + return true; + } + return false; } - void btDeformableBodySolver::predictMotion(btScalar solverdt) { - // apply explicit forces to velocity - m_objective->applyExplicitForce(m_residual); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody *psb = m_softBodies[i]; - - if (psb->isActive()) - { - // predict motion for collision detection - predictDeformableMotion(psb, solverdt); - } - } + // apply explicit forces to velocity + if (m_implicit) + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt; + } + } + } + } + m_objective->applyExplicitForce(m_residual); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + + if (psb->isActive()) + { + // predict motion for collision detection + predictDeformableMotion(psb, solverdt); + } + } } void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) { - BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); - int i, ni; - - /* Update */ - if (psb->m_bUpdateRtCst) - { - psb->m_bUpdateRtCst = false; - psb->updateConstants(); - psb->m_fdbvt.clear(); - if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) - { - psb->initializeFaceTree(); - } - } - - /* Prepare */ - psb->m_sst.sdt = dt * psb->m_cfg.timescale; - psb->m_sst.isdt = 1 / psb->m_sst.sdt; - psb->m_sst.velmrg = psb->m_sst.sdt * 3; - psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); - psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; - /* Bounds */ - psb->updateBounds(); - - /* Integrate */ - // do not allow particles to move more than the bounding box size - btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt; - for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) - { - btSoftBody::Node& n = psb->m_nodes[i]; - // apply drag - n.m_v *= (1 - psb->m_cfg.drag); - // scale velocity back - if (n.m_v.norm() > max_v) - { - n.m_v.safeNormalize(); - n.m_v *= max_v; - } - n.m_q = n.m_x + n.m_v * dt; - n.m_constrained = false; - } - - /* Nodes */ - psb->updateNodeTree(true, true); - if (!psb->m_fdbvt.empty()) - { - psb->updateFaceTree(true, true); - } - /* Clear contacts */ - psb->m_nodeRigidContacts.resize(0); - psb->m_faceRigidContacts.resize(0); - psb->m_faceNodeContacts.resize(0); - /* Optimize dbvt's */ -// psb->m_ndbvt.optimizeIncremental(1); -// psb->m_fdbvt.optimizeIncremental(1); -} + BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); + int i, ni; + /* Update */ + if (psb->m_bUpdateRtCst) + { + psb->m_bUpdateRtCst = false; + psb->updateConstants(); + psb->m_fdbvt.clear(); + if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) + { + psb->initializeFaceTree(); + } + } + + /* Prepare */ + psb->m_sst.sdt = dt * psb->m_cfg.timescale; + psb->m_sst.isdt = 1 / psb->m_sst.sdt; + psb->m_sst.velmrg = psb->m_sst.sdt * 3; + psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); + psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; + /* Bounds */ + psb->updateBounds(); + + /* Integrate */ + // do not allow particles to move more than the bounding box size + btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt; + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + // apply drag + n.m_v *= (1 - psb->m_cfg.drag); + // scale velocity back + if (m_implicit) + { + n.m_q = n.m_x; + } + else + { + if (n.m_v.norm() > max_v) + { + n.m_v.safeNormalize(); + n.m_v *= max_v; + } + n.m_q = n.m_x + n.m_v * dt; + } + n.m_splitv.setZero(); + n.m_constrained = false; + } + + /* Nodes */ + psb->updateNodeTree(true, true); + if (!psb->m_fdbvt.empty()) + { + psb->updateFaceTree(true, true); + } + /* Clear contacts */ + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); + /* Optimize dbvt's */ + // psb->m_ndbvt.optimizeIncremental(1); + // psb->m_fdbvt.optimizeIncremental(1); +} void btDeformableBodySolver::updateSoftBodies() { - BT_PROFILE("updateSoftBodies"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody *psb = (btSoftBody *)m_softBodies[i]; - if (psb->isActive()) - { - psb->updateNormals(); - } - } + BT_PROFILE("updateSoftBodies"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + if (psb->isActive()) + { + psb->updateNormals(); + } + } } void btDeformableBodySolver::setImplicit(bool implicit) { - m_implicit = implicit; - m_objective->setImplicit(implicit); + m_implicit = implicit; + m_objective->setImplicit(implicit); } void btDeformableBodySolver::setLineSearch(bool lineSearch) { - m_lineSearch = lineSearch; + m_lineSearch = lineSearch; } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index bce0d30b3..ae674d6e8 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -16,149 +16,145 @@ #ifndef BT_DEFORMABLE_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H - #include "btSoftBodySolvers.h" #include "btDeformableBackwardEulerObjective.h" #include "btDeformableMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" - +#include "btConjugateResidual.h" +#include "btConjugateGradient.h" struct btCollisionObjectWrapper; class btDeformableBackwardEulerObjective; class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { - typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btVector3> TVStack; + protected: - int m_numNodes; // total number of deformable body nodes - TVStack m_dv; // v_{n+1} - v_n - TVStack m_backup_dv; // backed up dv - TVStack m_ddv; // incremental dv - TVStack m_residual; // rhs of the linear solve - btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies - TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit - btScalar m_dt; // dt - btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver - bool m_implicit; // use implicit scheme if true, explicit scheme if false - int m_maxNewtonIterations; // max number of newton iterations - btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance - bool m_lineSearch; // If true, use newton's method with line search under implicit scheme - + int m_numNodes; // total number of deformable body nodes + TVStack m_dv; // v_{n+1} - v_n + TVStack m_backup_dv; // backed up dv + TVStack m_ddv; // incremental dv + TVStack m_residual; // rhs of the linear solve + btAlignedObjectArray<btSoftBody*> m_softBodies; // all deformable bodies + TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit + btScalar m_dt; // dt + btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver + btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver + bool m_implicit; // use implicit scheme if true, explicit scheme if false + int m_maxNewtonIterations; // max number of newton iterations + btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance + bool m_lineSearch; // If true, use newton's method with line search under implicit scheme public: - // handles data related to objective function - btDeformableBackwardEulerObjective* m_objective; - - btDeformableBodySolver(); - - virtual ~btDeformableBodySolver(); - - virtual SolverTypes getSolverType() const - { - return DEFORMABLE_SOLVER; - } - - // update soft body normals - virtual void updateSoftBodies(); - - // solve the momentum equation - virtual void solveDeformableConstraints(btScalar solverdt); - - // solve the contact between deformable and rigid as well as among deformables - btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); - - // solve the position error between deformable and rigid as well as among deformables; - btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); - - // set up the position error in split impulse - void splitImpulseSetup(const btContactSolverInfo& infoGlobal); - - // resize/clear data structures - void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt); - - // set up contact constraints - void setConstraints(const btContactSolverInfo& infoGlobal); - - // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion - virtual void predictMotion(btScalar solverdt); - - // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* - // x_{n+1}^* is stored in m_q - void predictDeformableMotion(btSoftBody* psb, btScalar dt); - - // save the current velocity to m_backupVelocity - void backupVelocity(); - - // set m_dv and m_backupVelocity to desired value to prepare for momentum solve - void setupDeformableSolve(bool implicit); - - // set the current velocity to that backed up in m_backupVelocity - void revertVelocity(); - - // set velocity to m_dv + m_backupVelocity - void updateVelocity(); - - // update the node count - bool updateNodes(); - - // calculate the change in dv resulting from the momentum solve - void computeStep(TVStack& ddv, const TVStack& residual); - - // calculate the change in dv resulting from the momentum solve when line search is turned on - btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false); - - virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} - - // process collision between deformable and rigid - virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) - { - softBody->defaultCollisionHandler(collisionObjectWrap); - } - - // process collision between deformable and deformable - virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { - softBody->defaultCollisionHandler(otherSoftBody); - } - - // If true, implicit time stepping scheme is used. - // Otherwise, explicit time stepping scheme is used - void setImplicit(bool implicit); - - // If true, newton's method with line search is used when implicit time stepping scheme is turned on - void setLineSearch(bool lineSearch); - - // set temporary position x^* = x_n + dt * v - // update the deformation gradient at position x^* - void updateState(); - - // set dv = dv + scale * ddv - void updateDv(btScalar scale = 1); - - // set temporary position x^* = x_n + dt * v^* - void updateTempPosition(); - - // save the current dv to m_backup_dv; - void backupDv(); - - // set dv to the backed-up value - void revertDv(); - - // set dv = dv + scale * ddv - // set v^* = v_n + dv - // set temporary position x^* = x_n + dt * v^* - // update the deformation gradient at position x^* - void updateEnergy(btScalar scale); - - // calculates the appropriately scaled kinetic energy in the system, which is - // 1/2 * dv^T * M * dv - // used in line search - btScalar kineticEnergy(); - - // unused functions - virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){} - virtual void solveConstraints(btScalar dt){} - virtual bool checkInitialized(){return true;} - virtual void copyBackToSoftBodies(bool bMove = true) {} + // handles data related to objective function + btDeformableBackwardEulerObjective* m_objective; + bool m_useProjection; + + btDeformableBodySolver(); + + virtual ~btDeformableBodySolver(); + + virtual SolverTypes getSolverType() const + { + return DEFORMABLE_SOLVER; + } + + // update soft body normals + virtual void updateSoftBodies(); + + virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // solve the momentum equation + virtual void solveDeformableConstraints(btScalar solverdt); + + // resize/clear data structures + void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt); + + // set up contact constraints + void setConstraints(const btContactSolverInfo& infoGlobal); + + // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion + virtual void predictMotion(btScalar solverdt); + + // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* + // x_{n+1}^* is stored in m_q + void predictDeformableMotion(btSoftBody* psb, btScalar dt); + + // save the current velocity to m_backupVelocity + void backupVelocity(); + + // set m_dv and m_backupVelocity to desired value to prepare for momentum solve + void setupDeformableSolve(bool implicit); + + // set the current velocity to that backed up in m_backupVelocity + void revertVelocity(); + + // set velocity to m_dv + m_backupVelocity + void updateVelocity(); + + // update the node count + bool updateNodes(); + + // calculate the change in dv resulting from the momentum solve + void computeStep(TVStack& ddv, const TVStack& residual); + + // calculate the change in dv resulting from the momentum solve when line search is turned on + btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false); + + virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {} + + // process collision between deformable and rigid + virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap) + { + softBody->defaultCollisionHandler(collisionObjectWrap); + } + + // process collision between deformable and deformable + virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody) + { + softBody->defaultCollisionHandler(otherSoftBody); + } + + // If true, implicit time stepping scheme is used. + // Otherwise, explicit time stepping scheme is used + void setImplicit(bool implicit); + + // If true, newton's method with line search is used when implicit time stepping scheme is turned on + void setLineSearch(bool lineSearch); + + // set temporary position x^* = x_n + dt * v + // update the deformation gradient at position x^* + void updateState(); + + // set dv = dv + scale * ddv + void updateDv(btScalar scale = 1); + + // set temporary position x^* = x_n + dt * v^* + void updateTempPosition(); + + // save the current dv to m_backup_dv; + void backupDv(); + + // set dv to the backed-up value + void revertDv(); + + // set dv = dv + scale * ddv + // set v^* = v_n + dv + // set temporary position x^* = x_n + dt * v^* + // update the deformation gradient at position x^* + void updateEnergy(btScalar scale); + + // calculates the appropriately scaled kinetic energy in the system, which is + // 1/2 * dv^T * M * dv + // used in line search + btScalar kineticEnergy(); + + // unused functions + virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {} + virtual void solveConstraints(btScalar dt) {} + virtual bool checkInitialized() { return true; } + virtual void copyBackToSoftBodies(bool bMove = true) {} }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 389411def..09398d79a 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -16,608 +16,705 @@ #include "btDeformableContactConstraint.h" /* ================ Deformable Node Anchor =================== */ btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal) -: m_anchor(&a) -, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) + : m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) { } btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other) -: m_anchor(other.m_anchor) -, btDeformableContactConstraint(other) + : m_anchor(other.m_anchor), btDeformableContactConstraint(other) { } btVector3 btDeformableNodeAnchorConstraint::getVa() const { - const btSoftBody::sCti& cti = m_anchor->m_cti; - btVector3 va(0, 0, 0); - if (cti.m_colObj->hasContactResponse()) - { - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_n[k]; - } - va = cti.m_normal * vel; - // add in the tangential components of the va - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t1[k]; - } - va += m_anchor->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t2[k]; - } - va += m_anchor->t2 * vel; - } - } - } - return va; + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_anchor->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_anchor->t2 * vel; + } + } + } + return va; } btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - const btSoftBody::sCti& cti = m_anchor->m_cti; - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = (vb - va); - // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 - const btScalar dn = btDot(vr, cti.m_normal); - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_anchor->m_c0 * vr; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - - // apply impulse to the rigid/multibodies involved and change their velocities - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) - { - rigidCol->applyImpulse(impulse, m_anchor->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); - const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); - } - } - return residualSquare; + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = (vb - va); + // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 + const btScalar dn = btDot(vr, vr); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_anchor->m_c0 * vr; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_anchor->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); + const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); + } + } + return residualSquare; } btVector3 btDeformableNodeAnchorConstraint::getVb() const { - return m_anchor->m_node->m_v; + return m_anchor->m_node->m_v; } void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) { - btVector3 dv = impulse * m_anchor->m_c2; - m_anchor->m_node->m_v -= dv; + btVector3 dv = impulse * m_anchor->m_c2; + m_anchor->m_node->m_v -= dv; } /* ================ Deformable vs. Rigid =================== */ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal) -: m_contact(&c) -, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) + : m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) { - m_total_normal_dv.setZero(); - m_total_tangent_dv.setZero(); - // The magnitude of penetration is the depth of penetration. - m_penetration = btMin(btScalar(0),c.m_cti.m_offset); + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); + // The magnitude of penetration is the depth of penetration. + m_penetration = c.m_cti.m_offset; + m_total_split_impulse = 0; + m_binding = false; } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) -: m_contact(other.m_contact) -, btDeformableContactConstraint(other) -, m_penetration(other.m_penetration) + : m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding) { - m_total_normal_dv = other.m_total_normal_dv; - m_total_tangent_dv = other.m_total_tangent_dv; + m_total_normal_dv = other.m_total_normal_dv; + m_total_tangent_dv = other.m_total_tangent_dv; } - btVector3 btDeformableRigidContactConstraint::getVa() const { - const btSoftBody::sCti& cti = m_contact->m_cti; - btVector3 va(0, 0, 0); - if (cti.m_colObj->hasContactResponse()) - { - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_n[k]; - } - va = cti.m_normal * vel; - // add in the tangential components of the va - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t1[k]; - } - va += m_contact->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t2[k]; - } - va += m_contact->t2 * vel; - } - } - } - return va; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + +btVector3 btDeformableRigidContactConstraint::getSplitVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; } btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - const btSoftBody::sCti& cti = m_contact->m_cti; - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; - const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - btVector3 old_total_tangent_dv = m_total_tangent_dv; - // m_c2 is the inverse mass of the deformable node/face - m_total_normal_dv -= impulse_normal * m_contact->m_c2; - m_total_tangent_dv -= impulse_tangent * m_contact->m_c2; - - if (m_total_normal_dv.dot(cti.m_normal) < 0) - { - // separating in the normal direction - m_static = false; - m_total_tangent_dv = btVector3(0,0,0); - impulse_tangent.setZero(); - } - else - { - if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) - { - // dynamic friction - // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - m_static = false; - if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) - { - m_total_tangent_dv = btVector3(0,0,0); - } - else - { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; - } - impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); - } - else - { - // static friction - m_static = true; - } - } - impulse = impulse_normal + impulse_tangent; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - if (residualSquare < 1e-7) - return residualSquare; - // apply impulse to the rigid/multibodies involved and change their velocities - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) - { - rigidCol->applyImpulse(impulse, m_contact->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - if (impulse_tangent.norm() > SIMD_EPSILON) - { - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); - const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); - } - } - } - return residualSquare; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm; + if (m_penetration > 0) + { + dn += m_penetration / infoGlobal.m_timeStep; + } + if (!infoGlobal.m_splitImpulse) + { + dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + } + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); + if (!infoGlobal.m_splitImpulse) + { + impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); + } + btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + if (dn > 0) + { + return 0; + } + m_binding = true; + btScalar residualSquare = dn * dn; + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c5 is the inverse mass of the deformable node/face + m_total_normal_dv -= m_contact->m_c5 * impulse_normal; + m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent; + + if (m_total_normal_dv.dot(cti.m_normal) < 0) + { + // separating in the normal direction + m_binding = false; + m_static = false; + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; + } + // impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + if (impulse_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + } + } + } + return residualSquare; } btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { - const btSoftBody::sCti& cti = m_contact->m_cti; - const btScalar dn = m_penetration; - if (dn != 0) - { - const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep)); - // one iteration of the position impulse corrects all the position error at this timestep - m_penetration -= dn; - // apply impulse to deformable nodes involved and change their position - applySplitImpulse(impulse); - // apply impulse to the rigid/multibodies involved and change their position - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) - { - rigidCol->applyPushImpulse(impulse, m_contact->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - // todo xuchenhan@ - } - return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep); - } - return 0; -} + btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 vb = getSplitVb(); + btVector3 va = getSplitVa(); + btScalar p = m_penetration; + if (p > 0) + { + return 0; + } + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + if (dn > 0) + { + return 0; + } + if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION) + { + dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION) + { + dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + m_total_split_impulse += dn; + + btScalar residualSquare = dn * dn; + const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn); + applySplitImpulse(impulse); + // apply split impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyPushImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); + } + } + return residualSquare; +} /* ================ Node vs. Rigid =================== */ btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) - : m_node(contact.m_node) - , btDeformableRigidContactConstraint(contact, infoGlobal) - { - } + : m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal) +{ +} btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other) -: m_node(other.m_node) -, btDeformableRigidContactConstraint(other) + : m_node(other.m_node), btDeformableRigidContactConstraint(other) { } btVector3 btDeformableNodeRigidContactConstraint::getVb() const { - return m_node->m_v; + return m_node->m_v; } +btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const +{ + return m_node->m_splitv; +} btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const { - return m_total_normal_dv + m_total_tangent_dv; + return m_total_normal_dv + m_total_tangent_dv; } void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableNodeRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - contact->m_node->m_v -= dv; + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_v -= dv; } void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse) { - const btSoftBody::DeformableNodeRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - contact->m_node->m_vsplit -= dv; -}; + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_splitv -= dv; +} /* ================ Face vs. Rigid =================== */ -btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal) -: m_face(contact.m_face) -, btDeformableRigidContactConstraint(contact, infoGlobal) +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting) + : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal) { } btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other) -: m_face(other.m_face) -, btDeformableRigidContactConstraint(other) + : m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other) { } btVector3 btDeformableFaceRigidContactConstraint::getVb() const { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; - return vb; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; } - btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const { - btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - if (m_face->m_n[0] == node) - { - return face_dv * contact->m_weights[0]; - } - if (m_face->m_n[1] == node) - { - return face_dv * contact->m_weights[1]; - } - btAssert(node == m_face->m_n[2]); - return face_dv * contact->m_weights[2]; + btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + if (m_face->m_n[0] == node) + { + return face_dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == node) + { + return face_dv * contact->m_weights[1]; + } + btAssert(node == m_face->m_n[2]); + return face_dv * contact->m_weights[2]; } void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - btSoftBody::Face* face = contact->m_face; - - btVector3& v0 = face->m_n[0]->m_v; - btVector3& v1 = face->m_n[1]->m_v; - btVector3& v2 = face->m_n[2]->m_v; - const btScalar& im0 = face->m_n[0]->m_im; - const btScalar& im1 = face->m_n[1]->m_im; - const btScalar& im2 = face->m_n[2]->m_im; - if (im0 > 0) - v0 -= dv * contact->m_weights[0]; - if (im1 > 0) - v1 -= dv * contact->m_weights[1]; - if (im2 > 0) - v2 -= dv * contact->m_weights[2]; - - btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations); - btScalar m01 = (relaxation/(im0 + im1)); - btScalar m02 = (relaxation/(im0 + im2)); - btScalar m12 = (relaxation/(im1 + im2)); - #ifdef USE_STRAIN_RATE_LIMITING - // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. - btScalar p = 0.01; - btVector3& x0 = face->m_n[0]->m_x; - btVector3& x1 = face->m_n[1]->m_x; - btVector3& x2 = face->m_n[2]->m_x; - const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; - const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; - btVector3 u[3]; - btScalar x_diff_dot_u, dn[3]; - btScalar dt = m_infoGlobal->m_timeStep; - for (int i = 0; i < 3; ++i) - { - btScalar x_diff_norm = x_diff[i].safeNorm(); - btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); - btScalar strainRate = x_diff_norm_new/x_diff_norm; - u[i] = v_diff[i]; - u[i].safeNormalize(); - if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) - { - dn[i] = 0; - continue; - } - x_diff_dot_u = btDot(x_diff[i], u[i]); - btScalar s; - if (1-p > strainRate) - { - s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); - } - else + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + v0 -= dv * contact->m_weights[0]; + if (im1 > 0) + v1 -= dv * contact->m_weights[1]; + if (im2 > 0) + v2 -= dv * contact->m_weights[2]; + if (m_useStrainLimiting) + { + btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations); + btScalar m01 = (relaxation / (im0 + im1)); + btScalar m02 = (relaxation / (im0 + im2)); + btScalar m12 = (relaxation / (im1 + im2)); +#ifdef USE_STRAIN_RATE_LIMITING + // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. + btScalar p = 0.01; + btVector3& x0 = face->m_n[0]->m_x; + btVector3& x1 = face->m_n[1]->m_x; + btVector3& x2 = face->m_n[2]->m_x; + const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1}; + const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1}; + btVector3 u[3]; + btScalar x_diff_dot_u, dn[3]; + btScalar dt = m_infoGlobal->m_timeStep; + for (int i = 0; i < 3; ++i) { - s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); + btScalar x_diff_norm = x_diff[i].safeNorm(); + btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); + btScalar strainRate = x_diff_norm_new / x_diff_norm; + u[i] = v_diff[i]; + u[i].safeNormalize(); + if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p)) + { + dn[i] = 0; + continue; + } + x_diff_dot_u = btDot(x_diff[i], u[i]); + btScalar s; + if (1 - p > strainRate) + { + s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm)); + } + else + { + s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm)); + } + // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); + // strainRate = x_diff_norm_new/x_diff_norm; + dn[i] = s - v_diff[i].safeNorm(); } - // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); - // strainRate = x_diff_norm_new/x_diff_norm; - dn[i] = s - v_diff[i].safeNorm(); - } - btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); - btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); - btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); + btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1])); #else - // apply strain limiting to prevent undamped modes - btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); - btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); - btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); + // apply strain limiting to prevent undamped modes + btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0)); + btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1)); + btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2)); #endif - v0 += dv0; - v1 += dv1; - v2 += dv2; + v0 += dv0; + v1 += dv1; + v2 += dv2; + } } -void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - btSoftBody::Face* face = contact->m_face; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2]; + return vb; +} - btVector3& v0 = face->m_n[0]->m_vsplit; - btVector3& v1 = face->m_n[1]->m_vsplit; - btVector3& v2 = face->m_n[2]->m_vsplit; - const btScalar& im0 = face->m_n[0]->m_im; - const btScalar& im1 = face->m_n[1]->m_im; - const btScalar& im2 = face->m_n[2]->m_im; - if (im0 > 0) - v0 -= dv * contact->m_weights[0]; - if (im1 > 0) - v1 -= dv * contact->m_weights[1]; - if (im2 > 0) - v2 -= dv * contact->m_weights[2]; +void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_splitv; + btVector3& v1 = face->m_n[1]->m_splitv; + btVector3& v2 = face->m_n[2]->m_splitv; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dv * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dv * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dv * contact->m_weights[2]; + } } /* ================ Face vs. Node =================== */ btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) -: m_node(contact.m_node) -, m_face(contact.m_face) -, m_contact(&contact) -, btDeformableContactConstraint(contact.m_normal, infoGlobal) + : m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal) { - m_total_normal_dv.setZero(); - m_total_tangent_dv.setZero(); + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); } btVector3 btDeformableFaceNodeContactConstraint::getVa() const { - return m_node->m_v; + return m_node->m_v; } btVector3 btDeformableFaceNodeContactConstraint::getVb() const { - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; - return vb; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; } btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const { - btVector3 dv = m_total_normal_dv + m_total_tangent_dv; - if (n == m_node) - return dv; - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - if (m_face->m_n[0] == n) - { - return dv * contact->m_weights[0]; - } - if (m_face->m_n[1] == n) - { - return dv * contact->m_weights[1]; - } - btAssert(n == m_face->m_n[2]); - return dv * contact->m_weights[2]; + btVector3 dv = m_total_normal_dv + m_total_tangent_dv; + if (n == m_node) + return dv; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + if (m_face->m_n[0] == n) + { + return dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == n) + { + return dv * contact->m_weights[1]; + } + btAssert(n == m_face->m_n[2]); + return dv * contact->m_weights[2]; } btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = vb - va; - const btScalar dn = btDot(vr, m_contact->m_normal); - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * vr; - const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - - btVector3 old_total_tangent_dv = m_total_tangent_dv; - // m_c2 is the inverse mass of the deformable node/face - if (m_node->m_im > 0) - { - m_total_normal_dv -= impulse_normal * m_node->m_im; - m_total_tangent_dv -= impulse_tangent * m_node->m_im; - } - else - { - m_total_normal_dv -= impulse_normal * m_contact->m_imf; - m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; - } - - if (m_total_normal_dv.dot(m_contact->m_normal) > 0) - { - // separating in the normal direction - m_static = false; - m_total_tangent_dv = btVector3(0,0,0); - impulse_tangent.setZero(); - } - else - { - if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) - { - // dynamic friction - // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - m_static = false; - if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) - { - m_total_tangent_dv = btVector3(0,0,0); - } - else - { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; - } - impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); - } - else - { - // static friction - m_static = true; - } - } - impulse = impulse_normal + impulse_tangent; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - return residualSquare; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, m_contact->m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_contact->m_c0 * vr; + const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c2 is the inverse mass of the deformable node/face + if (m_node->m_im > 0) + { + m_total_normal_dv -= impulse_normal * m_node->m_im; + m_total_tangent_dv -= impulse_tangent * m_node->m_im; + } + else + { + m_total_normal_dv -= impulse_normal * m_contact->m_imf; + m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; + } + + if (m_total_normal_dv.dot(m_contact->m_normal) > 0) + { + // separating in the normal direction + m_static = false; + m_total_tangent_dv = btVector3(0, 0, 0); + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; + } + impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + return residualSquare; } void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - btVector3 dva = impulse * contact->m_node->m_im; - btVector3 dvb = impulse * contact->m_imf; - if (contact->m_node->m_im > 0) - { - contact->m_node->m_v += dva; - } - - btSoftBody::Face* face = contact->m_face; - btVector3& v0 = face->m_n[0]->m_v; - btVector3& v1 = face->m_n[1]->m_v; - btVector3& v2 = face->m_n[2]->m_v; - const btScalar& im0 = face->m_n[0]->m_im; - const btScalar& im1 = face->m_n[1]->m_im; - const btScalar& im2 = face->m_n[2]->m_im; - if (im0 > 0) - { - v0 -= dvb * contact->m_weights[0]; - } - if (im1 > 0) - { - v1 -= dvb * contact->m_weights[1]; - } - if (im2 > 0) - { - v2 -= dvb * contact->m_weights[2]; - } + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 dva = impulse * contact->m_node->m_im; + btVector3 dvb = impulse * contact->m_imf; + if (contact->m_node->m_im > 0) + { + contact->m_node->m_v += dva; + } + + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dvb * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dvb * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dvb * contact->m_weights[2]; + } } diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index cc2d37da8..dc7077377 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -21,57 +21,49 @@ class btDeformableContactConstraint { public: - // True if the friction is static - // False if the friction is dynamic - bool m_static; + // True if the friction is static + // False if the friction is dynamic + bool m_static; const btContactSolverInfo* m_infoGlobal; // normal of the contact btVector3 m_normal; - btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) + btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) { } - btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) + btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) { } - - btDeformableContactConstraint(){} + + btDeformableContactConstraint() {} btDeformableContactConstraint(const btDeformableContactConstraint& other) - : m_static(other.m_static) - , m_normal(other.m_normal) - , m_infoGlobal(other.m_infoGlobal) + : m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal) { } - virtual ~btDeformableContactConstraint(){} - - // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence - // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; - - // solve the position error by applying an inelastic impulse that changes only the position (not velocity) - virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0; - - // get the velocity of the object A in the contact - virtual btVector3 getVa() const = 0; - - // get the velocity of the object B in the contact - virtual btVector3 getVb() const = 0; - - // get the velocity change of the soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const = 0; - - // apply impulse to the soft body node and/or face involved - virtual void applyImpulse(const btVector3& impulse) = 0; - - // apply position based impulse to the soft body node and/or face involved - virtual void applySplitImpulse(const btVector3& impulse) = 0; - - // scale the penetration depth by erp - virtual void setPenetrationScale(btScalar scale) = 0; + virtual ~btDeformableContactConstraint() {} + + // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence + // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const = 0; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const = 0; + + // get the velocity change of the soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const = 0; + + // apply impulse to the soft body node and/or face involved + virtual void applyImpulse(const btVector3& impulse) = 0; + + // scale the penetration depth by erp + virtual void setPenetrationScale(btScalar scale) = 0; }; // @@ -79,48 +71,41 @@ public: class btDeformableStaticConstraint : public btDeformableContactConstraint { public: - btSoftBody::Node* m_node; - - btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) - { - } - btDeformableStaticConstraint(){} - btDeformableStaticConstraint(const btDeformableStaticConstraint& other) - : m_node(other.m_node) - , btDeformableContactConstraint(other) - { - } - - virtual ~btDeformableStaticConstraint(){} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) - { - return 0; - } - - virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) - { - return 0; - } - - virtual btVector3 getVa() const - { - return btVector3(0,0,0); - } - - virtual btVector3 getVb() const - { - return btVector3(0,0,0); - } - - virtual btVector3 getDv(const btSoftBody::Node* n) const - { - return btVector3(0,0,0); - } - - virtual void applyImpulse(const btVector3& impulse){} - virtual void applySplitImpulse(const btVector3& impulse){} - virtual void setPenetrationScale(btScalar scale){} + btSoftBody::Node* m_node; + + btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal) + { + } + btDeformableStaticConstraint() {} + btDeformableStaticConstraint(const btDeformableStaticConstraint& other) + : m_node(other.m_node), btDeformableContactConstraint(other) + { + } + + virtual ~btDeformableStaticConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) + { + return 0; + } + + virtual btVector3 getVa() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getVb() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + + virtual void applyImpulse(const btVector3& impulse) {} + virtual void setPenetrationScale(btScalar scale) {} }; // @@ -128,65 +113,67 @@ public: class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint { public: - const btSoftBody::DeformableNodeRigidAnchor* m_anchor; - - btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); - btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); - btDeformableNodeAnchorConstraint(){} - virtual ~btDeformableNodeAnchorConstraint() - { - } - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) - { - // todo xuchenhan@ - return 0; - } - // object A is the rigid/multi body, and object B is the deformable node/face - virtual btVector3 getVa() const; - // get the velocity of the deformable node in contact - virtual btVector3 getVb() const; - virtual btVector3 getDv(const btSoftBody::Node* n) const - { - return btVector3(0,0,0); - } - virtual void applyImpulse(const btVector3& impulse); - virtual void applySplitImpulse(const btVector3& impulse) - { - // todo xuchenhan@ - }; - virtual void setPenetrationScale(btScalar scale){} -}; + const btSoftBody::DeformableNodeRigidAnchor* m_anchor; + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); + btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + btDeformableNodeAnchorConstraint() {} + virtual ~btDeformableNodeAnchorConstraint() + { + } + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} +}; // // Constraint between rigid/multi body and deformable objects class btDeformableRigidContactConstraint : public btDeformableContactConstraint { public: - btVector3 m_total_normal_dv; - btVector3 m_total_tangent_dv; - btScalar m_penetration; - const btSoftBody::DeformableRigidContact* m_contact; - - btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); - btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); - btDeformableRigidContactConstraint(){} - virtual ~btDeformableRigidContactConstraint() - { - } - - // object A is the rigid/multi body, and object B is the deformable node/face - virtual btVector3 getVa() const; - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); - - virtual void setPenetrationScale(btScalar scale) - { - m_penetration *= scale; - } + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + btScalar m_penetration; + btScalar m_total_split_impulse; + bool m_binding; + const btSoftBody::DeformableRigidContact* m_contact; + + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); + btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + btDeformableRigidContactConstraint() {} + virtual ~btDeformableRigidContactConstraint() + { + } + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const = 0; + + // get the split impulse velocity of the rigid/multibdoy at the contaft + virtual btVector3 getSplitVa() const; + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + virtual void setPenetrationScale(btScalar scale) + { + m_penetration *= scale; + } + + btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + + virtual void applySplitImpulse(const btVector3& impulse) = 0; }; // @@ -194,30 +181,34 @@ public: class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint { public: - // the deformable node in contact - btSoftBody::Node* m_node; - - btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); - btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); - btDeformableNodeRigidContactConstraint(){} - virtual ~btDeformableNodeRigidContactConstraint() - { - } - - // get the velocity of the deformable node in contact - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableNodeRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); - virtual void applySplitImpulse(const btVector3& impulse); + // the deformable node in contact + btSoftBody::Node* m_node; + + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); + btDeformableNodeRigidContactConstraint() {} + virtual ~btDeformableNodeRigidContactConstraint() + { + } + + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableNodeRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -225,28 +216,33 @@ public: class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint { public: - const btSoftBody::Face* m_face; - btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal); - btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); - btDeformableFaceRigidContactConstraint(){} - virtual ~btDeformableFaceRigidContactConstraint() - { - } - - // get the velocity of the deformable face at the contact point - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableFaceRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); - virtual void applySplitImpulse(const btVector3& impulse); + const btSoftBody::Face* m_face; + bool m_useStrainLimiting; + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); + btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); + btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {} + virtual ~btDeformableFaceRigidContactConstraint() + { + } + + // get the velocity of the deformable face at the contact point + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -254,44 +250,35 @@ public: class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint { public: - btSoftBody::Node* m_node; - btSoftBody::Face* m_face; - const btSoftBody::DeformableFaceNodeContact* m_contact; - btVector3 m_total_normal_dv; - btVector3 m_total_tangent_dv; - - btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); - btDeformableFaceNodeContactConstraint(){} - virtual ~btDeformableFaceNodeContactConstraint(){} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) - { - // todo: xuchenhan@ - return 0; - } - - // get the velocity of the object A in the contact - virtual btVector3 getVa() const; - - // get the velocity of the object B in the contact - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableFaceNodeContact* getContact() const - { - return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); - virtual void applySplitImpulse(const btVector3& impulse) - { - // todo xuchenhan@ - } - virtual void setPenetrationScale(btScalar scale){} + btSoftBody::Node* m_node; + btSoftBody::Face* m_face; + const btSoftBody::DeformableFaceNodeContact* m_contact; + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableFaceNodeContactConstraint() {} + virtual ~btDeformableFaceNodeContactConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceNodeContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} }; #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 509378c6d..584b9d6ec 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include <algorithm> #include <cmath> -btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; for (int i = 0; i < numDeformableBodies; ++i) @@ -58,52 +58,31 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod return residualSquare; } -void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) -{ - for (int i = 0; i < m_softBodies.size(); ++i) - { - // node constraints - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); - } - // face constraints - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); - } - } -} - -btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; - for (int i = 0; i < m_softBodies.size(); ++i) + for (int i = 0; i < numDeformableBodies; ++i) { - // node constraints - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; - btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - // anchor constraints - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j]; - btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - // face constraints - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + for (int j = 0; j < m_softBodies.size(); ++j) { - btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; - btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } } - } return residualSquare; } @@ -128,7 +107,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in m_staticConstraints[i].push_back(static_constraint); } } - + // set up deformable anchors for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) { @@ -142,7 +121,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); m_nodeAnchorConstraints[i].push_back(constraint); } - + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { @@ -153,17 +132,9 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_nodeRigidConstraints[i].push_back(constraint); - } + m_nodeRigidConstraints[i].push_back(constraint); } - + // set Deformable Face vs. Rigid constraint for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) { @@ -173,40 +144,15 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in { continue; } - btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_faceRigidConstraints[i].push_back(constraint); - } + btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); + m_faceRigidConstraints[i].push_back(constraint); } -// skip deformable constraints as they are done separately now -#if 0 - // set Deformable Face vs. Deformable Node constraint - for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) - { - const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; - - btDeformableFaceNodeContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, contact.m_normal); - if (dn > -SIMD_EPSILON) - { - m_deformableConstraints[i].push_back(constraint); - } - } -#endif } } void btDeformableContactProjection::project(TVStack& x) { +#ifndef USE_MGS const int dim = 3; for (int index = 0; index < m_projectionsDict.size(); ++index) { @@ -226,7 +172,6 @@ void btDeformableContactProjection::project(TVStack& x) if (free_dir.safeNorm() < SIMD_EPSILON) { x[i] -= x[i].dot(dir0) * dir0; - x[i] -= x[i].dot(dir1) * dir1; } else { @@ -241,15 +186,27 @@ void btDeformableContactProjection::project(TVStack& x) x[i] -= x[i].dot(dir0) * dir0; } } +#else + btReducedVector p(x.size()); + for (int i = 0; i < m_projections.size(); ++i) + { + p += (m_projections[i].dot(x) * m_projections[i]); + } + for (int i = 0; i < p.m_indices.size(); ++i) + { + x[p.m_indices[i]] -= p.m_vecs[i]; + } +#endif } void btDeformableContactProjection::setProjection() { +#ifndef USE_MGS BT_PROFILE("btDeformableContactProjection::setProjection"); btAlignedObjectArray<btVector3> units; - units.push_back(btVector3(1,0,0)); - units.push_back(btVector3(0,1,0)); - units.push_back(btVector3(0,0,1)); + units.push_back(btVector3(1, 0, 0)); + units.push_back(btVector3(0, 1, 0)); + units.push_back(btVector3(0, 0, 1)); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -295,45 +252,9 @@ void btDeformableContactProjection::setProjection() { int index = m_nodeRigidConstraints[i][j].m_node->index; m_nodeRigidConstraints[i][j].m_node->m_constrained = true; - if (m_nodeRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - } - } - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - for (int k = 0; k < 3; ++k) + if (m_nodeRigidConstraints[i][j].m_binding) { - btSoftBody::Node* node = face->m_n[k]; - node->m_constrained = true; - int index = node->index; - if (m_faceRigidConstraints[i][j].m_static) + if (m_nodeRigidConstraints[i][j].m_static) { if (m_projectionsDict.find(index) == NULL) { @@ -353,25 +274,32 @@ void btDeformableContactProjection::setProjection() if (m_projectionsDict.find(index) == NULL) { btAlignedObjectArray<btVector3> projections; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); m_projectionsDict.insert(index, projections); } else { btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); } } } } - for (int j = 0; j < m_deformableConstraints[i].size(); ++j) + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) { - const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face; + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + if (m_faceRigidConstraints[i][j].m_binding) + { + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + } + } for (int k = 0; k < 3; ++k) { - const btSoftBody::Node* node = face->m_n[k]; + btSoftBody::Node* node = face->m_n[k]; int index = node->index; - if (m_deformableConstraints[i][j].m_static) + if (m_faceRigidConstraints[i][j].m_static) { if (m_projectionsDict.find(index) == NULL) { @@ -380,9 +308,9 @@ void btDeformableContactProjection::setProjection() else { btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) + for (int l = 0; l < 3; ++l) { - projections.push_back(units[k]); + projections.push_back(units[l]); } } } @@ -391,53 +319,241 @@ void btDeformableContactProjection::setProjection() if (m_projectionsDict.find(index) == NULL) { btAlignedObjectArray<btVector3> projections; - projections.push_back(m_deformableConstraints[i][j].m_normal); + projections.push_back(m_faceRigidConstraints[i][j].m_normal); m_projectionsDict.insert(index, projections); } else { btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_deformableConstraints[i][j].m_normal); + projections.push_back(m_faceRigidConstraints[i][j].m_normal); } } } - - const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node; - int index = node->index; - if (m_deformableConstraints[i][j].m_static) + } + } +#else + int dof = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + dof += m_softBodies[i]->m_nodes.size(); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + btAlignedObjectArray<int> indices; + indices.push_back(index); + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + if (m_nodeRigidConstraints[i][j].m_static) + { + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + else { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else + vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + } + if (m_faceRigidConstraints[i][j].m_static) + { + for (int l = 0; l < 3; ++l) { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + btReducedVector rv(dof); for (int k = 0; k < 3; ++k) { - projections.push_back(units[k]); + rv.m_indices.push_back(face->m_n[k]->index); + btVector3 v(0, 0, 0); + v[l] = bary[k]; + rv.m_vecs.push_back(v); + rv.sort(); } + m_projections.push_back(rv); } } else { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_deformableConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else + btReducedVector rv(dof); + for (int k = 0; k < 3; ++k) { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_deformableConstraints[i][j].m_normal); + rv.m_indices.push_back(face->m_n[k]->index); + rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal); + rv.sort(); } + m_projections.push_back(rv); } } } + btModifiedGramSchmidt<btReducedVector> mgs(m_projections); + mgs.solve(); + m_projections = mgs.m_out; +#endif } +void btDeformableContactProjection::checkConstraints(const TVStack& x) +{ + for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) + { + btVector3 d(0, 0, 0); + const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int k = 0; k < lm.m_num_nodes; ++k) + { + d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); + } + } + printf("d = %f, %f, %f\n", d[0], d[1], d[2]); + } +} + +void btDeformableContactProjection::setLagrangeMultiplier() +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + if (!m_nodeRigidConstraints[i][j].m_binding) + { + continue; + } + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + if (m_nodeRigidConstraints[i][j].m_static) + { + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + } + else + { + lm.m_num_constraints = 1; + lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; + } + m_lagrangeMultipliers.push_back(lm); + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + if (!m_faceRigidConstraints[i][j].m_binding) + { + continue; + } + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; + LagrangeMultiplier lm; + lm.m_num_nodes = 3; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + lm.m_indices[k] = face->m_n[k]->index; + lm.m_weights[k] = bary[k]; + } + if (m_faceRigidConstraints[i][j].m_static) + { + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + } + else + { + lm.m_num_constraints = 1; + lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; + } + m_lagrangeMultipliers.push_back(lm); + } + } +} +// void btDeformableContactProjection::applyDynamicFriction(TVStack& f) { for (int i = 0; i < m_softBodies.size(); ++i) @@ -449,7 +565,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) @@ -462,7 +578,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -474,7 +590,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } for (int k = 0; k < 3; ++k) { @@ -482,7 +598,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -499,9 +615,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_nodeRigidConstraints.resize(N); m_faceRigidConstraints.resize(N); m_deformableConstraints.resize(N); - } - for (int i = 0 ; i < N; ++i) + for (int i = 0; i < N; ++i) { m_staticConstraints[i].clear(); m_nodeAnchorConstraints[i].clear(); @@ -509,8 +624,10 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_faceRigidConstraints[i].clear(); m_deformableConstraints[i].clear(); } +#ifndef USE_MGS m_projectionsDict.clear(); +#else + m_projections.clear(); +#endif + m_lagrangeMultipliers.clear(); } - - - diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 32d6c4d75..4964eaf99 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -21,30 +21,36 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include "btDeformableContactConstraint.h" #include "LinearMath/btHashMap.h" +#include "LinearMath/btReducedVector.h" +#include "LinearMath/btModifiedGramSchmidt.h" #include <vector> + +struct LagrangeMultiplier +{ + int m_num_constraints; // Number of constraints + int m_num_nodes; // Number of nodes in these constraints + btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes + btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; + int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; +}; + class btDeformableContactProjection { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - -// // map from node index to static constraint -// btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints; -// // map from node index to node rigid constraint -// btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints; -// // map from node index to face rigid constraint -// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints; -// // map from node index to deformable constraint -// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints; -// // map from node index to node anchor constraint -// btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints; - - // all constraints involving face - btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; - - // map from node index to projection directions - btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; - + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + + // all constraints involving face + btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; +#ifndef USE_MGS + // map from node index to projection directions + btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; +#else + btAlignedObjectArray<btReducedVector> m_projections; +#endif + + btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers; + // map from node index to static constraint btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints; // map from node index to node rigid constraint @@ -55,36 +61,39 @@ public: btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints; // map from node index to node anchor constraint btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints; - - btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies) - : m_softBodies(softBodies) - { - } - - virtual ~btDeformableContactProjection() - { - } - - // apply the constraints to the rhs of the linear solve - virtual void project(TVStack& x); - - // add friction force to the rhs of the linear solve - virtual void applyDynamicFriction(TVStack& f); - - // update and solve the constraints - virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); - - // solve the position error using split impulse - virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); - - // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. - virtual void setConstraints(const btContactSolverInfo& infoGlobal); - - // Set up projections for each vertex by adding the projection direction to - virtual void setProjection(); - - virtual void reinitialize(bool nodeUpdated); - - virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal); + + bool m_useStrainLimiting; + + btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual ~btDeformableContactProjection() + { + } + + // apply the constraints to the rhs of the linear solve + virtual void project(TVStack& x); + + // add friction force to the rhs of the linear solve + virtual void applyDynamicFriction(TVStack& f); + + // update and solve the constraints + virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. + virtual void setConstraints(const btContactSolverInfo& infoGlobal); + + // Set up projections for each vertex by adding the projection direction to + virtual void setProjection(); + + virtual void reinitialize(bool nodeUpdated); + + btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + virtual void setLagrangeMultiplier(); + + void checkConstraints(const TVStack& x); }; #endif /* btDeformableContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h index c2a26338e..dfd85523b 100644 --- a/src/BulletSoftBody/btDeformableCorotatedForce.h +++ b/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -21,105 +21,104 @@ static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { - static const btPolarDecomposition polar; - return polar.decompose(m, q, s); + static const btPolarDecomposition polar; + return polar.decompose(m, q, s); } class btDeformableCorotatedForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; - btDeformableCorotatedForce(): m_mu(1), m_lambda(1) - { - - } - - btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(tetra.m_F,P); - btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - // explicit elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) - { - // btMatrix3x3 JFinvT = F.adjoint(); - btScalar J = F.determinant(); - P = F.adjoint().transpose() * (m_lambda * (J-1)); - if (m_mu > SIMD_EPSILON) - { - btMatrix3x3 R,S; - if (J < 1024 * SIMD_EPSILON) - R.setIdentity(); - else - PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd - /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ - P += (F-R) * 2 * m_mu; - } - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_COROTATED_FORCE; - } - -}; + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btDeformableCorotatedForce() : m_mu(1), m_lambda(1) + { + } + + btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(tetra.m_F, P); + btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + // elastic force + // explicit elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) + { + // btMatrix3x3 JFinvT = F.adjoint(); + btScalar J = F.determinant(); + P = F.adjoint().transpose() * (m_lambda * (J - 1)); + if (m_mu > SIMD_EPSILON) + { + btMatrix3x3 R, S; + if (J < 1024 * SIMD_EPSILON) + R.setIdentity(); + else + PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd + /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ + P += (F - R) * 2 * m_mu; + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_COROTATED_FORCE; + } +}; #endif /* btCorotated_h */ diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 33e5a8564..d91867f45 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -21,85 +21,85 @@ class btDeformableGravityForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btVector3 m_gravity; - - btDeformableGravityForce(const btVector3& g) : m_gravity(g) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledGravityForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledGravityForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - } - - virtual void addScaledGravityForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& n = psb->m_nodes[j]; - size_t id = n.index; - btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; - btVector3 scaled_force = scale * m_gravity * mass; - force[id] += scaled_force; - } - } - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_GRAVITY_FORCE; - } + typedef btAlignedObjectArray<btVector3> TVStack; + btVector3 m_gravity; - // the gravitational potential energy - virtual double totalEnergy(btScalar dt) - { - double e = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - if (node.m_im > 0) - { - e -= m_gravity.dot(node.m_q)/node.m_im; - } - } - } - return e; - } - - + btDeformableGravityForce(const btVector3& g) : m_gravity(g) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledGravityForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& n = psb->m_nodes[j]; + size_t id = n.index; + btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; + btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor; + force[id] += scaled_force; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_GRAVITY_FORCE; + } + + // the gravitational potential energy + virtual double totalEnergy(btScalar dt) + { + double e = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + e -= m_gravity.dot(node.m_q) / node.m_im; + } + } + } + return e; + } }; #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */ diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index cdd31c7fd..d58d825d1 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -22,349 +22,351 @@ enum btDeformableLagrangianForceType { - BT_GRAVITY_FORCE = 1, - BT_MASSSPRING_FORCE = 2, - BT_COROTATED_FORCE = 3, - BT_NEOHOOKEAN_FORCE = 4, - BT_LINEAR_ELASTICITY_FORCE = 5, - BT_MOUSE_PICKING_FORCE = 6 + BT_GRAVITY_FORCE = 1, + BT_MASSSPRING_FORCE = 2, + BT_COROTATED_FORCE = 3, + BT_NEOHOOKEAN_FORCE = 4, + BT_LINEAR_ELASTICITY_FORCE = 5, + BT_MOUSE_PICKING_FORCE = 6 }; static inline double randomDouble(double low, double high) { - return low + static_cast<double>(rand()) / RAND_MAX * (high - low); + return low + static_cast<double>(rand()) / RAND_MAX * (high - low); } class btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btAlignedObjectArray<btSoftBody *> m_softBodies; - const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; - - btDeformableLagrangianForce() - { - } - - virtual ~btDeformableLagrangianForce(){} - - // add all forces - virtual void addScaledForces(btScalar scale, TVStack& force) = 0; - - // add damping df - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; - - // add elastic df - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; - - // add all forces that are explicit in explicit solve - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; - - // add all damping forces - virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; - - virtual btDeformableLagrangianForceType getForceType() = 0; - - virtual void reinitialize(bool nodeUpdated) - { - } - - // get number of nodes that have the force - virtual int getNumNodes() - { - int numNodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - numNodes += m_softBodies[i]->m_nodes.size(); - } - return numNodes; - } - - // add a soft body to be affected by the particular lagrangian force - virtual void addSoftBody(btSoftBody* psb) - { - m_softBodies.push_back(psb); - } - - virtual void removeSoftBody(btSoftBody* psb) - { - m_softBodies.remove(psb); - } - - virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) - { - m_nodes = nodes; - } - - // Calculate the incremental deformable generated from the input dx - virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) - { - btVector3 c1 = dx[id1] - dx[id0]; - btVector3 c2 = dx[id2] - dx[id0]; - btVector3 c3 = dx[id3] - dx[id0]; - return btMatrix3x3(c1,c2,c3).transpose(); - } - - // Calculate the incremental deformable generated from the current velocity - virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) - { - btVector3 c1 = n1->m_v - n0->m_v; - btVector3 c2 = n2->m_v - n0->m_v; - btVector3 c3 = n3->m_v - n0->m_v; - return btMatrix3x3(c1,c2,c3).transpose(); - } - - // test for addScaledElasticForce function - virtual void testDerivative() - { - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); - } - psb->updateDeformation(); - } - - TVStack dx; - dx.resize(getNumNodes()); - TVStack dphi_dx; - dphi_dx.resize(dx.size()); - for (int i =0; i < dphi_dx.size();++i) - { - dphi_dx[i].setZero(); - } - addScaledForces(-1, dphi_dx); - - // write down the current position - TVStack x; - x.resize(dx.size()); - int counter = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - x[counter] = psb->m_nodes[j].m_q; - counter++; - } - } - counter = 0; - - // populate dx with random vectors - for (int i = 0; i < dx.size(); ++i) - { - dx[i].setX(randomDouble(-1, 1)); - dx[i].setY(randomDouble(-1, 1)); - dx[i].setZ(randomDouble(-1, 1)); - } - - btAlignedObjectArray<double> errors; - for (int it = 0; it < 10; ++it) - { - for (int i = 0; i < dx.size(); ++i) - { - dx[i] *= 0.5; - } - - // get dphi/dx * dx - double dphi = 0; - for (int i = 0; i < dx.size(); ++i) - { - dphi += dphi_dx[i].dot(dx[i]); - } - - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] + dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double f1 = totalElasticEnergy(0); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] - dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - double f2 = totalElasticEnergy(0); - - //restore m_q - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double error = f1-f2-2*dphi; - errors.push_back(error); - std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; - } - for (int i = 1; i < errors.size(); ++i) - { - std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; - } - } - - // test for addScaledElasticForce function - virtual void testHessian() - { - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); - } - psb->updateDeformation(); - } - - - TVStack dx; - dx.resize(getNumNodes()); - TVStack df; - df.resize(dx.size()); - TVStack f1; - f1.resize(dx.size()); - TVStack f2; - f2.resize(dx.size()); - - - // write down the current position - TVStack x; - x.resize(dx.size()); - int counter = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - x[counter] = psb->m_nodes[j].m_q; - counter++; - } - } - counter = 0; - - // populate dx with random vectors - for (int i = 0; i < dx.size(); ++i) - { - dx[i].setX(randomDouble(-1, 1)); - dx[i].setY(randomDouble(-1, 1)); - dx[i].setZ(randomDouble(-1, 1)); - } - - btAlignedObjectArray<double> errors; - for (int it = 0; it < 10; ++it) - { - for (int i = 0; i < dx.size(); ++i) - { - dx[i] *= 0.5; - } - - // get df - for (int i =0; i < df.size();++i) - { - df[i].setZero(); - f1[i].setZero(); - f2[i].setZero(); - } - - //set df - addScaledElasticForceDifferential(-1, dx, df); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] + dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - //set f1 - addScaledForces(-1, f1); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] - dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - //set f2 - addScaledForces(-1, f2); - - //restore m_q - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double error = 0; - for (int i = 0; i < df.size();++i) - { - btVector3 error_vector = f1[i]-f2[i]-2*df[i]; - error += error_vector.length2(); - } - error = btSqrt(error); - errors.push_back(error); - std::cout << "Iteration = " << it << ", error = " << error << std::endl; - } - for (int i = 1; i < errors.size(); ++i) - { - std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; - } - } - - // - virtual double totalElasticEnergy(btScalar dt) - { - return 0; - } - - // - virtual double totalDampingEnergy(btScalar dt) - { - return 0; - } - - // total Energy takes dt as input because certain energies depend on dt - virtual double totalEnergy(btScalar dt) - { - return totalElasticEnergy(dt) + totalDampingEnergy(dt); - } + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*> m_softBodies; + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btDeformableLagrangianForce() + { + } + + virtual ~btDeformableLagrangianForce() {} + + // add all forces + virtual void addScaledForces(btScalar scale, TVStack& force) = 0; + + // add damping df + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + + // build diagonal of A matrix + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0; + + // add elastic df + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; + + // add all forces that are explicit in explicit solve + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + + // add all damping forces + virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; + + virtual void addScaledHessian(btScalar scale) {} + + virtual btDeformableLagrangianForceType getForceType() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + // get number of nodes that have the force + virtual int getNumNodes() + { + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + numNodes += m_softBodies[i]->m_nodes.size(); + } + return numNodes; + } + + // add a soft body to be affected by the particular lagrangian force + virtual void addSoftBody(btSoftBody* psb) + { + m_softBodies.push_back(psb); + } + + virtual void removeSoftBody(btSoftBody* psb) + { + m_softBodies.remove(psb); + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } + + // Calculate the incremental deformable generated from the input dx + virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) + { + btVector3 c1 = dx[id1] - dx[id0]; + btVector3 c2 = dx[id2] - dx[id0]; + btVector3 c3 = dx[id3] - dx[id0]; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // Calculate the incremental deformable generated from the current velocity + virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) + { + btVector3 c1 = n1->m_v - n0->m_v; + btVector3 c2 = n2->m_v - n0->m_v; + btVector3 c3 = n3->m_v - n0->m_v; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // test for addScaledElasticForce function + virtual void testDerivative() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack dphi_dx; + dphi_dx.resize(dx.size()); + for (int i = 0; i < dphi_dx.size(); ++i) + { + dphi_dx[i].setZero(); + } + addScaledForces(-1, dphi_dx); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get dphi/dx * dx + double dphi = 0; + for (int i = 0; i < dx.size(); ++i) + { + dphi += dphi_dx[i].dot(dx[i]); + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double f1 = totalElasticEnergy(0); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + double f2 = totalElasticEnergy(0); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = f1 - f2 - 2 * dphi; + errors.push_back(error); + std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // test for addScaledElasticForce function + virtual void testHessian() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack df; + df.resize(dx.size()); + TVStack f1; + f1.resize(dx.size()); + TVStack f2; + f2.resize(dx.size()); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get df + for (int i = 0; i < df.size(); ++i) + { + df[i].setZero(); + f1[i].setZero(); + f2[i].setZero(); + } + + //set df + addScaledElasticForceDifferential(-1, dx, df); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f1 + addScaledForces(-1, f1); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f2 + addScaledForces(-1, f2); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = 0; + for (int i = 0; i < df.size(); ++i) + { + btVector3 error_vector = f1[i] - f2[i] - 2 * df[i]; + error += error_vector.length2(); + } + error = btSqrt(error); + errors.push_back(error); + std::cout << "Iteration = " << it << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // + virtual double totalElasticEnergy(btScalar dt) + { + return 0; + } + + // + virtual double totalDampingEnergy(btScalar dt) + { + return 0; + } + + // total Energy takes dt as input because certain energies depend on dt + virtual double totalEnergy(btScalar dt) + { + return totalElasticEnergy(dt) + totalDampingEnergy(dt); + } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableLinearElasticityForce.h b/src/BulletSoftBody/btDeformableLinearElasticityForce.h index 106dc10ad..971192050 100644 --- a/src/BulletSoftBody/btDeformableLinearElasticityForce.h +++ b/src/BulletSoftBody/btDeformableLinearElasticityForce.h @@ -18,323 +18,445 @@ #include "btDeformableLagrangianForce.h" #include "LinearMath/btQuickprof.h" +#include "btSoftBodyInternals.h" +#define TETRA_FLAT_THRESHOLD 0.01 class btDeformableLinearElasticityForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; - btScalar m_mu_damp, m_lambda_damp; - btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1) - { - btScalar damping = 0.05; - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; - } - - btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) - { - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * df_on_node0; - force[id1] -= scale1 * df_on_node123.getColumn(0); - force[id2] -= scale1 * df_on_node123.getColumn(1); - force[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetraScratches.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(s); - } - } - return energy; - } - - // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - double elasticEnergyDensity(const btSoftBody::TetraScratch& s) - { - double density = 0; - btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); - btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; - density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); - density += m_lambda * trace * trace * 0.5; - return density; - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar max_p = psb->m_cfg.m_maxStress; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(psb->m_tetraScratches[j],P); + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_damping_alpha, m_damping_beta; + btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01) + { + updateYoungsModulusAndPoissonRatio(); + } + + btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta) + { + updateYoungsModulusAndPoissonRatio(); + } + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping_alpha, btScalar damping_beta) + { + m_damping_alpha = damping_alpha; + m_damping_beta = damping_beta; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + force[id] -= scale * node.m_v / node.m_im * m_damping_alpha; + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); + density += m_lambda * trace * trace * 0.5; + return density; + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); #if USE_SVD - if (max_p > 0) - { - // since we want to clamp the principal stress to max_p, we only need to - // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p - btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); - if (trPTP > max_p * max_p) - { - btMatrix3x3 U, V; - btVector3 sigma; - singularValueDecomposition(P, U, sigma, V); - sigma[0] = btMin(sigma[0], max_p); - sigma[1] = btMin(sigma[1], max_p); - sigma[2] = btMin(sigma[2], max_p); - sigma[0] = btMax(sigma[0], -max_p); - sigma[1] = btMax(sigma[1], -max_p); - sigma[2] = btMax(sigma[2], -max_p); - btMatrix3x3 Sigma; - Sigma.setIdentity(); - Sigma[0][0] = sigma[0]; - Sigma[1][1] = sigma[1]; - Sigma[2][2] = sigma[2]; - P = U * Sigma * V.transpose(); - } - } + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } #endif - // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; - btMatrix3x3 dP; - firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); - // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // elastic force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) - { - btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); - btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; - P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; - } - - // Let P be the first piola stress. - // This function calculates the dP = dP/dF * dF - void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); - dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; - } - - // Let Q be the damping stress. - // This function calculates the dP = dQ/dF * dF - void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); - dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace; - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_LINEAR_ELASTICITY_FORCE; - } - + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + df[id] -= scale * dv[id] / node.m_im * m_damping_alpha; + } + } + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F; + + btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace; + } + + virtual void addScaledHessian(btScalar scale) + { + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); // make sure scratch is evaluated at x_n + dt * vn + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms; + node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1; + node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1; + node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1; + node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + btMatrix3x3 I; + I.setIdentity(); + node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha); + } + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_LINEAR_ELASTICITY_FORCE; + } }; #endif /* BT_LINEAR_ELASTICITY_H */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 54b4e4481..8c97bd1ba 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -20,236 +20,282 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce { - // If true, the damping force will be in the direction of the spring - // If false, the damping force will be in the direction of the velocity - bool m_momentum_conserving; - btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + bool m_momentum_conserving; + btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; + public: - typedef btAlignedObjectArray<btVector3> TVStack; - btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) - { - } - btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) - { - if (m_bendingStiffness < btScalar(0)) - { - m_bendingStiffness = m_elasticStiffness; - } - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - - // damping force - btVector3 v_diff = (node2->m_v - node1->m_v); - btVector3 scaled_force = scale * m_dampingStiffness * v_diff; - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; - } - } - force[id1] += scaled_force; - force[id2] -= scaled_force; - } - } - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - btScalar r = link.m_rl; - size_t id1 = node1->index; - size_t id2 = node2->index; - - // elastic force - btVector3 dir = (node2->m_q - node1->m_q); - btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); - btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); - force[id1] += scaled_force; - force[id2] -= scaled_force; - } - } - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - - btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; - } - } - df[id1] += local_scaled_df; - df[id2] -= local_scaled_df; - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - btScalar r = link.m_rl; - - // elastic force - btVector3 dir = (node2->m_q - node1->m_q); - energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r); - } - } - return energy; - } - - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - btScalar r = link.m_rl; - - btVector3 dir = (node1->m_q - node2->m_q); - btScalar dir_norm = dir.norm(); - btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btVector3 dx_diff = dx[id1] - dx[id2]; - btVector3 scaled_df = btVector3(0,0,0); - btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); - if (dir_norm > SIMD_EPSILON) - { - scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; - scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized; - scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff; - } - - df[id1] += scaled_df; - df[id2] -= scaled_df; - } - } - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_MASSSPRING_FORCE; - } - + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) + { + } + btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) + { + if (m_bendingStiffness < btScalar(0)) + { + m_bendingStiffness = m_elasticStiffness; + } + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + } + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; + } + } + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d]; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d]; + } + } + } + else + { + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp; + } + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r); + } + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + btScalar r = link.m_rl; + + btVector3 dir = (node1->m_q - node2->m_q); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btVector3 dx_diff = dx[id1] - dx[id2]; + btVector3 scaled_df = btVector3(0, 0, 0); + btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff; + } + + df[id1] += scaled_df; + df[id2] -= scaled_df; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MASSSPRING_FORCE; + } }; #endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableMousePickingForce.h b/src/BulletSoftBody/btDeformableMousePickingForce.h index 0e2ca064f..d218d9621 100644 --- a/src/BulletSoftBody/btDeformableMousePickingForce.h +++ b/src/BulletSoftBody/btDeformableMousePickingForce.h @@ -20,124 +20,143 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce { - // If true, the damping force will be in the direction of the spring - // If false, the damping force will be in the direction of the velocity - btScalar m_elasticStiffness, m_dampingStiffness; - const btSoftBody::Face& m_face; - btVector3 m_mouse_pos; - btScalar m_maxForce; + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + btScalar m_elasticStiffness, m_dampingStiffness; + const btSoftBody::Face& m_face; + btVector3 m_mouse_pos; + btScalar m_maxForce; + public: - typedef btAlignedObjectArray<btVector3> TVStack; - btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - for (int i = 0; i < 3; ++i) - { - btVector3 v_diff = m_face.m_n[i]->m_v; - btVector3 scaled_force = scale * m_dampingStiffness * v_diff; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; - } - force[m_face.m_n[i]->index] -= scaled_force; - } - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - btScalar scaled_stiffness = scale * m_elasticStiffness; - for (int i = 0; i < 3; ++i) - { - btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); - btVector3 scaled_force = scaled_stiffness * dir; - if (scaled_force.safeNorm() > m_maxForce) - { - scaled_force.safeNormalize(); - scaled_force *= m_maxForce; - } - force[m_face.m_n[i]->index] -= scaled_force; - } - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int i = 0; i < 3; ++i) - { - btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; - } - df[m_face.m_n[i]->index] -= local_scaled_df; - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < 3; ++i) - { - btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); - btVector3 scaled_force = m_elasticStiffness * dir; - if (scaled_force.safeNorm() > m_maxForce) - { - scaled_force.safeNormalize(); - scaled_force *= m_maxForce; - } - energy += 0.5 * scaled_force.dot(dir); - } - return energy; - } - - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < 3; ++i) - { - btVector3 v_diff = m_face.m_n[i]->m_v; - btVector3 scaled_force = m_dampingStiffness * v_diff; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; - } - energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; - } - return energy; - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - //TODO - } - - void setMousePos(const btVector3& p) - { - m_mouse_pos = p; - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_MOUSE_PICKING_FORCE; - } - + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = scaled_stiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int i = 0; i < 3; ++i) + { + btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; + } + df[m_face.m_n[i]->index] -= local_scaled_df; + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = m_elasticStiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + energy += 0.5 * scaled_force.dot(dir); + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; + } + energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + int id = m_face.m_n[i]->index; + btVector3 dx_diff = dx[id]; + btScalar r = 0; // rest length is 0 for picking spring + btVector3 scaled_df = btVector3(0, 0, 0); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff; + } + df[id] += scaled_df; + } + } + + void setMousePos(const btVector3& p) + { + m_mouse_pos = p; + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MOUSE_PICKING_FORCE; + } }; #endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index c8cc47923..631fd5fbe 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -13,131 +13,132 @@ 3. This notice may not be removed or altered from any source distribution. */ - #include "btDeformableMultiBodyConstraintSolver.h" #include <iostream> // override the iterations method to include deformable/multibody contact -btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - { - ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + { + ///this is a special step to resolve penetrations (just for contacts) + solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + for (int iteration = 0; iteration < maxIterations; iteration++) + { + // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body - int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - for (int iteration = 0; iteration < maxIterations; iteration++) - { - // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body - - // solve rigid/rigid in solver body - m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - // solver body velocity -> rigid body velocity - solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal); - // update rigid body velocity in rigid/deformable contact - m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); - // solver body velocity <- rigid body velocity - writeToSolverBody(bodies, numBodies, infoGlobal); - - if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) - { + // solve rigid/rigid in solver body + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // solver body velocity -> rigid body velocity + solverBodyWriteBack(infoGlobal); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal); + // update rigid body velocity in rigid/deformable contact + m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); + // solver body velocity <- rigid body velocity + writeToSolverBody(bodies, numBodies, infoGlobal); + + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) + { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); + if (iteration >= (maxIterations - 1)) + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif - m_analyticsData.m_numSolverCalls++; - m_analyticsData.m_numIterationsUsed = iteration+1; - m_analyticsData.m_islandId = -2; - if (numBodies>0) - m_analyticsData.m_islandId = bodies[0]->getCompanionId(); - m_analyticsData.m_numBodies = numBodies; - m_analyticsData.m_numContactManifolds = numManifolds; - m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; - break; - } - } - } - return 0.f; + m_analyticsData.m_numSolverCalls++; + m_analyticsData.m_numIterationsUsed = iteration + 1; + m_analyticsData.m_islandId = -2; + if (numBodies > 0) + m_analyticsData.m_islandId = bodies[0]->getCompanionId(); + m_analyticsData.m_numBodies = numBodies; + m_analyticsData.m_numContactManifolds = numManifolds; + m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; + break; + } + } + } + return 0.f; } -void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // overriden - solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlyFinish(bodies, numBodies, info); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // overriden + solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlyFinish(bodies, numBodies, info); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; } void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < numBodies; i++) - { - int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + for (int i = 0; i < numBodies; i++) + { + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body && body->getInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; - solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; - } - } + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; + solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; + } + } } void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; - if (body) - { - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); - } - } + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) + { + btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + if (body) + { + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity); + } + } } -void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); - int iteration; - if (infoGlobal.m_splitImpulse) - { - { -// m_deformableSolver->splitImpulseSetup(infoGlobal); - for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) - { - btScalar leastSquaresResidual = 0.f; - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j = 0; j < numPoolConstraints; j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); - } - // solve the position correction between deformable and rigid/multibody -// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); -// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); - } - if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) - { + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); + int iteration; + if (infoGlobal.m_splitImpulse) + { + { + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + { + btScalar leastSquaresResidual = 0.f; + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j = 0; j < numPoolConstraints; j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + + btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + // solve the position correction between deformable and rigid/multibody + // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); + btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) + { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); + if (iteration >= (infoGlobal.m_numIterations - 1)) + printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration); #endif - break; - } - } - } - } + break; + } + } + } + } } diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 0c7cc26a8..94aabce83 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -13,7 +13,6 @@ 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H #define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H @@ -32,30 +31,31 @@ class btDeformableBodySolver; ATTRIBUTE_ALIGNED16(class) btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver { - btDeformableBodySolver* m_deformableSolver; - + btDeformableBodySolver* m_deformableSolver; + protected: - // override the iterations method to include deformable/multibody contact -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - // write the velocity of the the solver body to the underlying rigid body - void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); - - // write the velocity of the underlying rigid body to the the the solver body - void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - - virtual btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + // override the iterations method to include deformable/multibody contact + // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + // write the velocity of the the solver body to the underlying rigid body + void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); + + // write the velocity of the underlying rigid body to the the the solver body + void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - void setDeformableSolver(btDeformableBodySolver* deformableSolver) - { - m_deformableSolver = deformableSolver; - } - - virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + BT_DECLARE_ALIGNED_ALLOCATOR(); + + void setDeformableSolver(btDeformableBodySolver * deformableSolver) + { + m_deformableSolver = deformableSolver; + } + + virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 93d52272b..dbde78944 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -22,7 +22,6 @@ Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz). 2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*. 3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable. - TODO: add option for positional drift correction (using vel_target += erp * pos_error/dt 3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass, @@ -41,8 +40,9 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed #include "LinearMath/btQuickprof.h" #include "btSoftBodyInternals.h" btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver) -: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), -m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) + : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), + m_deformableBodySolver(deformableBodySolver), + m_solverCallback(0) { m_drawFlags = fDrawFlags::Std; m_drawNodeTree = true; @@ -53,7 +53,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_sbi.m_sparsesdf.Initialize(); m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005); m_sbi.m_sparsesdf.Reset(); - + m_sbi.air_density = (btScalar)1.2; m_sbi.water_density = 0; m_sbi.water_offset = 0; @@ -62,102 +62,121 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_internalTime = 0.0; m_implicit = false; m_lineSearch = false; - m_selfCollision = true; + m_useProjection = false; m_ccdIterations = 5; m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); } btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld() { - delete m_solverDeformableBodyIslandCallback; + delete m_solverDeformableBodyIslandCallback; } void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - BT_PROFILE("internalSingleStepSimulation"); - if (0 != m_internalPreTickCallback) - { - (*m_internalPreTickCallback)(this, timeStep); - } - reinitialize(timeStep); - // add gravity to velocity of rigid and multi bodys - applyRigidBodyGravity(timeStep); - - ///apply gravity and explicit force to velocity, predict motion - predictUnconstraintMotion(timeStep); - - ///perform collision detection - btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); - - if (m_selfCollision) - { - softBodySelfCollision(); - } - - btMultiBodyDynamicsWorld::calculateSimulationIslands(); - - beforeSolverCallbacks(timeStep); - - ///solve contact constraints and then deformable bodies momemtum equation - solveConstraints(timeStep); - - afterSolverCallbacks(timeStep); - - applyRepulsionForce(timeStep); - - performGeometricCollisions(timeStep); - - integrateTransforms(timeStep); - - ///update vehicle simulation - btMultiBodyDynamicsWorld::updateActions(timeStep); - - updateActivationState(timeStep); - // End solver-wise simulation step - // /////////////////////////////// + BT_PROFILE("internalSingleStepSimulation"); + if (0 != m_internalPreTickCallback) + { + (*m_internalPreTickCallback)(this, timeStep); + } + reinitialize(timeStep); + + // add gravity to velocity of rigid and multi bodys + applyRigidBodyGravity(timeStep); + + ///apply gravity and explicit force to velocity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection that involves rigid/multi bodies + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + + btMultiBodyDynamicsWorld::calculateSimulationIslands(); + + beforeSolverCallbacks(timeStep); + + ///solve contact constraints and then deformable bodies momemtum equation + solveConstraints(timeStep); + + afterSolverCallbacks(timeStep); + + performDeformableCollisionDetection(); + + applyRepulsionForce(timeStep); + + performGeometricCollisions(timeStep); + + integrateTransforms(timeStep); + + ///update vehicle simulation + btMultiBodyDynamicsWorld::updateActions(timeStep); + + updateActivationState(timeStep); + // End solver-wise simulation step + // /////////////////////////////// +} + +void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->m_softSoftCollision = true; + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + for (int j = i; j < m_softBodies.size(); ++j) + { + m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]); + } + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->m_softSoftCollision = false; + } } void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) { - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - psb->updateDeactivation(timeStep); - if (psb->wantsSleeping()) - { - if (psb->getActivationState() == ACTIVE_TAG) - psb->setActivationState(WANTS_DEACTIVATION); - if (psb->getActivationState() == ISLAND_SLEEPING) - { - psb->setZeroVelocity(); - } - } - else - { - if (psb->getActivationState() != DISABLE_DEACTIVATION) - psb->setActivationState(ACTIVE_TAG); - } - } - btMultiBodyDynamicsWorld::updateActivationState(timeStep); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + psb->updateDeactivation(timeStep); + if (psb->wantsSleeping()) + { + if (psb->getActivationState() == ACTIVE_TAG) + psb->setActivationState(WANTS_DEACTIVATION); + if (psb->getActivationState() == ISLAND_SLEEPING) + { + psb->setZeroVelocity(); + } + } + else + { + if (psb->getActivationState() != DISABLE_DEACTIVATION) + psb->setActivationState(ACTIVE_TAG); + } + } + btMultiBodyDynamicsWorld::updateActivationState(timeStep); } void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep) { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->isActive()) - { + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { psb->applyRepulsionForce(timeStep, true); - } - } + } + } } void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep) { BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions"); - // refit the BVH tree for CCD + // refit the BVH tree for CCD for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -196,7 +215,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep; } } - } + } // apply CCD to register new contact points for (int i = 0; i < m_softBodies.size(); ++i) @@ -210,7 +229,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]); } } - } + } int penetration_count = 0; for (int i = 0; i < m_softBodies.size(); ++i) @@ -236,301 +255,296 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim } } } +} - for (int i = 0; i < m_softBodies.size(); ++i) +void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() +{ + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); + for (int i = 0; i < m_softBodies.size(); i++) { btSoftBody* psb = m_softBodies[i]; - if (psb->isActive() && psb->m_usePostCollisionDamping) + if (psb->isActive()) { - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (!psb->m_nodes[j].m_constrained) - { - psb->m_nodes[j].m_v *= psb->m_dampingCoefficient; - } - } + psb->defaultCollisionHandler(psb); } } } -void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() -{ - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->isActive()) - { - psb->defaultCollisionHandler(psb); - } - } -} - void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) { - // correct the position of rigid bodies with temporary velocity generated from split impulse - btContactSolverInfo infoGlobal; - btVector3 zero(0,0,0); - for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) - { - btRigidBody* rb = m_nonStaticRigidBodies[i]; - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btVector3 pushVelocity = rb->getPushVelocity(); - btVector3 turnVelocity = rb->getTurnVelocity(); - if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) - { - btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); - rb->setWorldTransform(newTransform); - rb->setPushVelocity(zero); - rb->setTurnVelocity(zero); - } - } + // correct the position of rigid bodies with temporary velocity generated from split impulse + btContactSolverInfo infoGlobal; + btVector3 zero(0, 0, 0); + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btVector3 pushVelocity = rb->getPushVelocity(); + btVector3 turnVelocity = rb->getTurnVelocity(); + if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) + { + btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); + rb->setWorldTransform(newTransform); + rb->setPushVelocity(zero); + rb->setTurnVelocity(zero); + } + } } void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { - BT_PROFILE("integrateTransforms"); - positionCorrection(timeStep); - btMultiBodyDynamicsWorld::integrateTransforms(timeStep); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / timeStep; - for (int c = 0; c < 3; c++) - { - if (node.m_v[c] > clampDeltaV) - { - node.m_v[c] = clampDeltaV; - } - if (node.m_v[c] < -clampDeltaV) - { - node.m_v[c] = -clampDeltaV; - } - } - node.m_x = node.m_x + timeStep * node.m_v; - node.m_v -= node.m_vsplit; - node.m_vsplit.setZero(); - node.m_q = node.m_x; - node.m_vn = node.m_v; - } - // enforce anchor constraints - for (int j = 0; j < psb->m_deformableAnchors.size();++j) - { - btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; - btSoftBody::Node* n = a.m_node; - n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; - - // update multibody anchor info - if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 nrm; - const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); - const btTransform& wtr = multibodyLinkCol->getWorldTransform(); - psb->m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(n->m_x), - shp, - nrm, - 0); - a.m_cti.m_normal = wtr.getBasis() * nrm; - btVector3 normal = a.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - a.m_c0 = rot.transpose() * local_impulse_matrix * rot; - a.jacobianData_normal = jacobianData_normal; - a.jacobianData_t1 = jacobianData_t1; - a.jacobianData_t2 = jacobianData_t2; - a.t1 = t1; - a.t2 = t2; - } - } - } - psb->interpolateRenderMesh(); - } + BT_PROFILE("integrateTransforms"); + positionCorrection(timeStep); + btMultiBodyDynamicsWorld::integrateTransforms(timeStep); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) + { + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } + } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; + } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } + } + psb->interpolateRenderMesh(); + } } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); - - // set up constraints among multibodies and between multibodies and deformable bodies - setupConstraints(); - - // solve contact constraints - solveContactConstraints(); - - // set up the directions in which the velocity does not change in the momentum solve - m_deformableBodySolver->m_objective->m_projection.setProjection(); - - // for explicit scheme, m_backupVelocity = v_{n+1}^* - // for implicit scheme, m_backupVelocity = v_n - // Here, set dv = v_{n+1} - v_n for nodes in contact - m_deformableBodySolver->setupDeformableSolve(m_implicit); - - // At this point, dv should be golden for nodes in contact - // proceed to solve deformable momentum equation - m_deformableBodySolver->solveDeformableConstraints(timeStep); + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + + // set up constraints among multibodies and between multibodies and deformable bodies + setupConstraints(); + + // solve contact constraints + solveContactConstraints(); + + // set up the directions in which the velocity does not change in the momentum solve + if (m_useProjection) + m_deformableBodySolver->m_objective->m_projection.setProjection(); + else + m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); + + // for explicit scheme, m_backupVelocity = v_{n+1}^* + // for implicit scheme, m_backupVelocity = v_n + // Here, set dv = v_{n+1} - v_n for nodes in contact + m_deformableBodySolver->setupDeformableSolve(m_implicit); + + // At this point, dv should be golden for nodes in contact + // proceed to solve deformable momentum equation + m_deformableBodySolver->solveDeformableConstraints(timeStep); } void btDeformableMultiBodyDynamicsWorld::setupConstraints() { - // set up constraints between multibody and deformable bodies - m_deformableBodySolver->setConstraints(m_solverInfo); - - // set up constraints among multibodies - { - sortConstraints(); - // setup the solver callback - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - - // build islands - m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); - } + // set up constraints between multibody and deformable bodies + m_deformableBodySolver->setConstraints(m_solverInfo); + + // set up constraints among multibodies + { + sortConstraints(); + // setup the solver callback + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + + // build islands + m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); + } } void btDeformableMultiBodyDynamicsWorld::sortConstraints() { - m_sortedConstraints.resize(m_constraints.size()); - int i; - for (i = 0; i < getNumConstraints(); i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i = 0; i < m_multiBodyConstraints.size(); i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); } - - + void btDeformableMultiBodyDynamicsWorld::solveContactConstraints() { - // process constraints on each island - m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); - - // process deferred - m_solverDeformableBodyIslandCallback->processConstraints(); - m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); - - // write joint feedback - { - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - if (bod->internalNeedsJointFeedback()) - { - if (!bod->isUsingRK4Integration()) - { - if (bod->internalNeedsJointFeedback()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - } - } - } - - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } + // process constraints on each island + m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); + + // process deferred + m_solverDeformableBodyIslandCallback->processConstraints(); + m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); + + // write joint feedback + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + if (bod->internalNeedsJointFeedback()) + { + if (!bod->isUsingRK4Integration()) + { + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + } + } + } + + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } } void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) { - m_softBodies.push_back(body); - - // Set the soft body solver that will deal with this body - // to be the world's solver - body->setSoftBodySolver(m_deformableBodySolver); - - btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver(m_deformableBodySolver); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); } void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - BT_PROFILE("predictUnconstraintMotion"); - btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); - m_deformableBodySolver->predictMotion(timeStep); + BT_PROFILE("predictUnconstraintMotion"); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(timeStep); } void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { - m_internalTime += timeStep; - m_deformableBodySolver->setImplicit(m_implicit); - m_deformableBodySolver->setLineSearch(m_lineSearch); - m_deformableBodySolver->reinitialize(m_softBodies, timeStep); - btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); - btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + m_internalTime += timeStep; + m_deformableBodySolver->setImplicit(m_implicit); + m_deformableBodySolver->setLineSearch(m_lineSearch); + m_deformableBodySolver->reinitialize(m_softBodies, timeStep); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + if (m_useProjection) + { + m_deformableBodySolver->m_useProjection = true; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; + } + else + { + m_deformableBodySolver->m_useProjection = false; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; + } } - void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() { - btMultiBodyDynamicsWorld::debugDrawWorld(); for (int i = 0; i < getSoftBodyArray().size(); i++) @@ -541,253 +555,250 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags()); } } - - } void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) { - // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again - // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep - // when there are multiple substeps - btMultiBodyDynamicsWorld::applyGravity(); - // integrate rigid body gravity - for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) - { - btRigidBody* rb = m_nonStaticRigidBodies[i]; - rb->integrateVelocities(timeStep); - } - - // integrate multibody gravity - { - forwardKinematics(); - clearMultiBodyConstraintForces(); - { - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - m_scratch_r.resize(bod->getNumLinks() + 1); - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - bool isConstraintPass = false; - { - if (!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, - m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - else - { - btAssert(" RK4Integration is not supported" ); - } - } - } - } - } - } - clearGravity(); + // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep + // when there are multiple substeps + btMultiBodyDynamicsWorld::applyGravity(); + // integrate rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + + // integrate multibody gravity + { + forwardKinematics(); + clearMultiBodyConstraintForces(); + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + m_scratch_r.resize(bod->getNumLinks() + 1); + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + else + { + btAssert(" RK4Integration is not supported"); + } + } + } + } + } + } + clearGravity(); } void btDeformableMultiBodyDynamicsWorld::clearGravity() { - BT_PROFILE("btMultiBody clearGravity"); - // clear rigid body gravity - for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive()) - { - body->clearGravity(); - } - } - // clear multibody gravity - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - bod->addBaseForce(-m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); - } - } - } + BT_PROFILE("btMultiBody clearGravity"); + // clear rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + { + body->clearGravity(); + } + } + // clear multibody gravity + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + bod->addBaseForce(-m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); + } + } + } } void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) { - if (0 != m_internalTickCallback) - { - (*m_internalTickCallback)(this, timeStep); - } - - if (0 != m_solverCallback) - { - (*m_solverCallback)(m_internalTime, this); - } + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } } void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) { - if (0 != m_solverCallback) - { - (*m_solverCallback)(m_internalTime, this); - } + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } } void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; - bool added = false; - for (int i = 0; i < forces.size(); ++i) - { - if (forces[i]->getForceType() == force->getForceType()) - { - forces[i]->addSoftBody(psb); - added = true; - break; - } - } - if (!added) - { - force->addSoftBody(psb); - force->setIndices(m_deformableBodySolver->m_objective->getIndices()); - forces.push_back(force); - } + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } } void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; - int removed_index = -1; - for (int i = 0; i < forces.size(); ++i) - { - if (forces[i]->getForceType() == force->getForceType()) - { - forces[i]->removeSoftBody(psb); - if (forces[i]->m_softBodies.size() == 0) - removed_index = i; - break; - } - } - if (removed_index >= 0) - forces.removeAtIndex(removed_index); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + int removed_index = -1; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->removeSoftBody(psb); + if (forces[i]->m_softBodies.size() == 0) + removed_index = i; + break; + } + } + if (removed_index >= 0) + forces.removeAtIndex(removed_index); } void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) { - m_softBodies.remove(body); - btCollisionWorld::removeCollisionObject(body); - // force a reinitialize so that node indices get updated. - m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); + m_softBodies.remove(body); + btCollisionWorld::removeCollisionObject(body); + // force a reinitialize so that node indices get updated. + m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); } void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { - btSoftBody* body = btSoftBody::upcast(collisionObject); - if (body) - removeSoftBody(body); - else - btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); } - int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { - startProfiling(timeStep); - - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_fixedTimeStep = fixedTimeStep; - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int(m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } - else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; - m_fixedTimeStep = 0; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } - else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (getDebugDrawer()) - { - btIDebugDraw* debugDrawer = getDebugDrawer(); - gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; - } - if (numSimulationSubSteps) - { - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; - - saveKinematicState(fixedTimeStep * clampedSimulationSteps); - - for (int i = 0; i < clampedSimulationSteps; i++) - { - internalSingleStepSimulation(fixedTimeStep); - synchronizeMotionStates(); - } - } - else - { - synchronizeMotionStates(); - } - - clearForces(); - + startProfiling(timeStep); + + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_fixedTimeStep = fixedTimeStep; + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int(m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } + else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; + m_fixedTimeStep = 0; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } + else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + btIDebugDraw* debugDrawer = getDebugDrawer(); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; + + saveKinematicState(fixedTimeStep * clampedSimulationSteps); + + for (int i = 0; i < clampedSimulationSteps; i++) + { + internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); + } + } + else + { + synchronizeMotionStates(); + } + + clearForces(); + #ifndef BT_NO_PROFILE - CProfileManager::Increment_Frame_Counter(); + CProfileManager::Increment_Frame_Counter(); #endif //BT_NO_PROFILE - - return numSimulationSubSteps; + + return numSimulationSubSteps; } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 17512c8db..32bae3aa6 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -36,183 +36,190 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld { - typedef btAlignedObjectArray<btVector3> TVStack; - ///Solver classes that encapsulate multiple deformable bodies for solving - btDeformableBodySolver* m_deformableBodySolver; - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; - btSoftBodyWorldInfo m_sbi; - btScalar m_internalTime; - int m_ccdIterations; - bool m_implicit; - bool m_lineSearch; - bool m_selfCollision; - DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; - - typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); - btSolverCallback m_solverCallback; - + typedef btAlignedObjectArray<btVector3> TVStack; + ///Solver classes that encapsulate multiple deformable bodies for solving + btDeformableBodySolver* m_deformableBodySolver; + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + btScalar m_internalTime; + int m_ccdIterations; + bool m_implicit; + bool m_lineSearch; + bool m_useProjection; + DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; + + typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); + btSolverCallback m_solverCallback; + protected: - virtual void internalSingleStepSimulation(btScalar timeStep); - - virtual void integrateTransforms(btScalar timeStep); - - void positionCorrection(btScalar timeStep); - - void solveConstraints(btScalar timeStep); - - void updateActivationState(btScalar timeStep); - - void clearGravity(); - + virtual void internalSingleStepSimulation(btScalar timeStep); + + virtual void integrateTransforms(btScalar timeStep); + + void positionCorrection(btScalar timeStep); + + void solveConstraints(btScalar timeStep); + + void updateActivationState(btScalar timeStep); + + void clearGravity(); + public: btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0); - virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); virtual void debugDrawWorld(); - void setSolverCallback(btSolverCallback cb) - { - m_solverCallback = cb; - } - - virtual ~btDeformableMultiBodyDynamicsWorld(); - - virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() - { - return (btMultiBodyDynamicsWorld*)(this); - } - - virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const - { - return (const btMultiBodyDynamicsWorld*)(this); - } - - virtual btDynamicsWorldType getWorldType() const - { - return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; - } - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - - btSoftBodyArray& getSoftBodyArray() - { - return m_softBodies; - } - - const btSoftBodyArray& getSoftBodyArray() const - { - return m_softBodies; - } - - btSoftBodyWorldInfo& getWorldInfo() - { - return m_sbi; - } - - const btSoftBodyWorldInfo& getWorldInfo() const - { - return m_sbi; - } - - void reinitialize(btScalar timeStep); - - void applyRigidBodyGravity(btScalar timeStep); - - void beforeSolverCallbacks(btScalar timeStep); - - void afterSolverCallbacks(btScalar timeStep); - - void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); - - void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); - - void removeSoftBody(btSoftBody* body); - - void removeCollisionObject(btCollisionObject* collisionObject); - - int getDrawFlags() const { return (m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags = f; } - - void setupConstraints(); - - void solveMultiBodyConstraints(); - - void solveContactConstraints(); - - void sortConstraints(); - - void softBodySelfCollision(); - - void setImplicit(bool implicit) - { - m_implicit = implicit; - } - - void setLineSearch(bool lineSearch) - { - m_lineSearch = lineSearch; - } - - void applyRepulsionForce(btScalar timeStep); - - void performGeometricCollisions(btScalar timeStep); - - struct btDeformableSingleRayCallback : public btBroadphaseRayCallback - { - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btDeformableMultiBodyDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) - : m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld - rayFromWorld); - - 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]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); - } - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + void setSolverCallback(btSolverCallback cb) + { + m_solverCallback = cb; + } + + virtual ~btDeformableMultiBodyDynamicsWorld(); + + virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() + { + return (btMultiBodyDynamicsWorld*)(this); + } + + virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const + { + return (const btMultiBodyDynamicsWorld*)(this); + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; + } + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + void reinitialize(btScalar timeStep); + + void applyRigidBodyGravity(btScalar timeStep); + + void beforeSolverCallbacks(btScalar timeStep); + + void afterSolverCallbacks(btScalar timeStep); + + void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeSoftBody(btSoftBody* body); + + void removeCollisionObject(btCollisionObject* collisionObject); + + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } + + void setupConstraints(); + + void performDeformableCollisionDetection(); + + void solveMultiBodyConstraints(); + + void solveContactConstraints(); + + void sortConstraints(); + + void softBodySelfCollision(); + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + void setLineSearch(bool lineSearch) + { + m_lineSearch = lineSearch; + } + + void setUseProjection(bool useProjection) + { + m_useProjection = useProjection; + } + + void applyRepulsionForce(btScalar timeStep); + + void performGeometricCollisions(btScalar timeStep); + + struct btDeformableSingleRayCallback : public btBroadphaseRayCallback + { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btDeformableMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); + + btVector3 rayDir = (rayToWorld - rayFromWorld); + + 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]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; #if 0 #ifdef RECALCULATE_AABB btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -223,87 +230,85 @@ public: const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; #endif #endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } - }; - - - - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const - { - 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 - btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); - + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) + { + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); + } + } + return true; + } + }; + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const + { + 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 + btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); + #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i = 0; i < this->getNumCollisionObjects(); i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } + for (int i = 0; i < this->getNumCollisionObjects(); i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } #endif //USE_BRUTEFORCE_RAYBROADPHASE - } - - void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) const - { - if (collisionShape->isSoftBody()) - { - btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) - { - btSoftBody::sRayCast softResult; - if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) - { - if (softResult.fraction <= resultCallback.m_closestHitFraction) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = 0; - shapeInfo.m_triangleIndex = softResult.index; - // get the normal - btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - btVector3 normal = -rayDir; - normal.normalize(); - { - normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) - { - // normal always point toward origin of the ray - normal = -normal; - } - } - - btCollisionWorld::LocalRayResult rayResult(collisionObject, - &shapeInfo, - normal, - softResult.fraction); - bool normalInWorldSpace = true; - resultCallback.addSingleResult(rayResult, normalInWorldSpace); - } - } - } - } - else - { - btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); - } - } + } + + void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) const + { + if (collisionShape->isSoftBody()) + { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) + { + btSoftBody::sRayCast softResult; + if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + if (softResult.fraction <= resultCallback.m_closestHitFraction) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal = -rayDir; + normal.normalize(); + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) + { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); + } + } + } + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); + } + } }; #endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 2fb1c0b99..60798c5bc 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -23,30 +23,30 @@ subject to the following restrictions: class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; // Lame Parameters - btScalar m_E, m_nu; // Young's modulus and Poisson ratio - btScalar m_mu_damp, m_lambda_damp; - btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) - { - btScalar damping = 0.05; - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; // Lame Parameters + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_mu_damp, m_lambda_damp; + btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1) + { + btScalar damping = 0.05; + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; updateYoungsModulusAndPoissonRatio(); - } - - btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) - { - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + } + + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; updateYoungsModulusAndPoissonRatio(); - } + } void updateYoungsModulusAndPoissonRatio() { // conversion from Lame Parameters to Young's modulus and Poisson ratio // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters - m_E = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu); + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); m_nu = m_lambda * 0.5 / (m_mu + m_lambda); } @@ -55,21 +55,21 @@ public: // conversion from Young's modulus and Poisson ratio to Lame Parameters // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters m_mu = m_E * 0.5 / (1 + m_nu); - m_lambda = m_E * m_nu / ((1 + m_nu) * (1- 2*m_nu)); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); } - void setYoungsModulus(btScalar E) - { + void setYoungsModulus(btScalar E) + { m_E = E; updateLameParameters(); - } + } void setPoissonRatio(btScalar nu) { m_nu = nu; updateLameParameters(); } - + void setDamping(btScalar damping) { m_mu_damp = damping * m_mu; @@ -83,337 +83,338 @@ public: updateYoungsModulusAndPoissonRatio(); } - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + density += m_mu * 0.5 * (s.m_trace - 3.); + density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda); + density -= m_mu * 0.5 * log(s.m_trace + 1); + return density; + } - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * df_on_node0; - force[id1] -= scale1 * df_on_node123.getColumn(0); - force[id2] -= scale1 * df_on_node123.getColumn(1); - force[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetraScratches.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(s); - } - } - return energy; - } - - // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - double elasticEnergyDensity(const btSoftBody::TetraScratch& s) - { - double density = 0; - density += m_mu * 0.5 * (s.m_trace - 3.); - density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda); - density -= m_mu * 0.5 * log(s.m_trace+1); - return density; - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar max_p = psb->m_cfg.m_maxStress; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(psb->m_tetraScratches[j],P); + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); #ifdef USE_SVD - if (max_p > 0) - { - // since we want to clamp the principal stress to max_p, we only need to - // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p - btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); - if (trPTP > max_p * max_p) - { - btMatrix3x3 U, V; - btVector3 sigma; - singularValueDecomposition(P, U, sigma, V); - sigma[0] = btMin(sigma[0], max_p); - sigma[1] = btMin(sigma[1], max_p); - sigma[2] = btMin(sigma[2], max_p); - sigma[0] = btMax(sigma[0], -max_p); - sigma[1] = btMax(sigma[1], -max_p); - sigma[2] = btMax(sigma[2], -max_p); - btMatrix3x3 Sigma; - Sigma.setIdentity(); - Sigma[0][0] = sigma[0]; - Sigma[1][1] = sigma[1]; - Sigma[2][2] = sigma[2]; - P = U * Sigma * V.transpose(); - } - } + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } #endif -// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); -// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); + P = s.m_F * c1 + s.m_cofF * c2; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.)); + btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))); + btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP); + dP += s.m_cofF * c3; + } - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; - btMatrix3x3 dP; - firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); -// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // elastic force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) - { - btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.))); - btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); - P = s.m_F * c1 + s.m_cofF * c2; - } - - // Let P be the first piola stress. - // This function calculates the dP = dP/dF * dF - void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.)); - btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); - btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); - dP = dF * c1 + s.m_F * c2; - addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP); - dP += s.m_cofF * c3; - } - - // Let Q be the damping stress. - // This function calculates the dP = dQ/dF * dF - void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.))); - btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace)))); - btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); - dP = dF * c1 + s.m_F * c2; - addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP); - dP += s.m_cofF * c3; - } - - btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) - { - btScalar ans = 0; - for (int i = 0; i < 3; ++i) - { - ans += A[i].dot(B[i]); - } - return ans; - } - - // Let C(A) be the cofactor of the matrix A - // Let H = the derivative of C(A) with respect to A evaluated at F = A - // This function calculates H*dF - void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) - { - M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); - M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); - M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); - M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); - M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); - M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); - M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); - M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); - M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_NEOHOOKEAN_FORCE; - } - + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)))); + btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP); + dP += s.m_cofF * c3; + } + + btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) + { + btScalar ans = 0; + for (int i = 0; i < 3; ++i) + { + ans += A[i].dot(B[i]); + } + return ans; + } + + // Let C(A) be the cofactor of the matrix A + // Let H = the derivative of C(A) with respect to A evaluated at F = A + // This function calculates H*dF + void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) + { + M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); + M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); + M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); + M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); + M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); + M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); + M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); + M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); + M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_NEOHOOKEAN_FORCE; + } }; #endif /* BT_NEOHOOKEAN_H */ diff --git a/src/BulletSoftBody/btKrylovSolver.h b/src/BulletSoftBody/btKrylovSolver.h new file mode 100644 index 000000000..59126b47a --- /dev/null +++ b/src/BulletSoftBody/btKrylovSolver.h @@ -0,0 +1,107 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. 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: + 1. 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. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_KRYLOV_SOLVER_H +#define BT_KRYLOV_SOLVER_H +#include <iostream> +#include <cmath> +#include <limits> +#include <LinearMath/btAlignedObjectArray.h> +#include <LinearMath/btVector3.h> +#include <LinearMath/btScalar.h> +#include "LinearMath/btQuickprof.h" + +template <class MatrixX> +class btKrylovSolver +{ + typedef btAlignedObjectArray<btVector3> TVStack; + +public: + int m_maxIterations; + btScalar m_tolerance; + btKrylovSolver(int maxIterations, btScalar tolerance) + : m_maxIterations(maxIterations), m_tolerance(tolerance) + { + } + + virtual ~btKrylovSolver() {} + + virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0; + + virtual void reinitialize(const TVStack& b) = 0; + + virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b) + { + // c = a-b + btAssert(a.size() == b.size()); + TVStack c; + c.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + { + c[i] = a[i] - b[i]; + } + return c; + } + + virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a) + { + return dot(a, a); + } + + virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a) + { + btScalar ret = 0; + for (int i = 0; i < a.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + ret = btMax(ret, btFabs(a[i][d])); + } + } + return ret; + } + + virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b) + { + btScalar ans(0); + for (int i = 0; i < a.size(); ++i) + ans += a[i].dot(b[i]); + return ans; + } + + virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) + { + // result += s*a + btAssert(a.size() == result.size()); + for (int i = 0; i < a.size(); ++i) + result[i] += s * a[i]; + } + + virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) + { + // result = a*s + b + TVStack result; + result.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + result[i] = s * a[i] + b[i]; + return result; + } + + virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance) + { + m_tolerance = tolerance; + } +}; +#endif /* BT_KRYLOV_SOLVER_H */ diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index d71242038..21c1106a4 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -19,61 +19,267 @@ class Preconditioner { public: - typedef btAlignedObjectArray<btVector3> TVStack; - virtual void operator()(const TVStack& x, TVStack& b) = 0; - virtual void reinitialize(bool nodeUpdated) = 0; - virtual ~Preconditioner(){} + typedef btAlignedObjectArray<btVector3> TVStack; + virtual void operator()(const TVStack& x, TVStack& b) = 0; + virtual void reinitialize(bool nodeUpdated) = 0; + virtual ~Preconditioner() {} }; class DefaultPreconditioner : public Preconditioner { public: - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - for (int i = 0; i < b.size(); ++i) - b[i] = x[i]; - } - virtual void reinitialize(bool nodeUpdated) - { - } - - virtual ~DefaultPreconditioner(){} + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i]; + } + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual ~DefaultPreconditioner() {} }; class MassPreconditioner : public Preconditioner { - btAlignedObjectArray<btScalar> m_inv_mass; - const btAlignedObjectArray<btSoftBody *>& m_softBodies; + btAlignedObjectArray<btScalar> m_inv_mass; + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + public: - MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies) - : m_softBodies(softBodies) - { - } - - virtual void reinitialize(bool nodeUpdated) - { - if (nodeUpdated) - { - m_inv_mass.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - m_inv_mass.push_back(psb->m_nodes[j].m_im); - } - } - } - - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - btAssert(m_inv_mass.size() == x.size()); - for (int i = 0; i < b.size(); ++i) - { - b[i] = x[i] * m_inv_mass[i]; - } - } + MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + m_inv_mass.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + m_inv_mass.push_back(psb->m_nodes[j].m_im); + } + } + } + + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + btAssert(m_inv_mass.size() <= x.size()); + for (int i = 0; i < m_inv_mass.size(); ++i) + { + b[i] = x[i] * m_inv_mass[i]; + } + for (int i = m_inv_mass.size(); i < b.size(); ++i) + { + b[i] = x[i]; + } + } +}; + +class KKTPreconditioner : public Preconditioner +{ + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btDeformableContactProjection& m_projections; + const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf; + TVStack m_inv_A, m_inv_S; + const btScalar& m_dt; + const bool& m_implicit; + +public: + KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit) + : m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + int num_nodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + num_nodes += psb->m_nodes.size(); + } + m_inv_A.resize(num_nodes); + } + buildDiagonalA(m_inv_A); + for (int i = 0; i < m_inv_A.size(); ++i) + { + // printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d]; + } + } + m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); + // printf("S.size() = %d \n", m_inv_S.size()); + buildDiagonalS(m_inv_A, m_inv_S); + for (int i = 0; i < m_inv_S.size(); ++i) + { + // printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d]; + } + } + } + + void buildDiagonalA(TVStack& diagA) const + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im); + ++counter; + } + } + if (m_implicit) + { + printf("implicit not implemented\n"); + btAssert(false); + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA); + } + } + + void buildDiagonalS(const TVStack& inv_A, TVStack& diagS) + { + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // S[k,k] = e_k^T * C A_d^-1 C^T * e_k + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + btVector3& t = diagS[c]; + t.setZero(); + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int d = 0; d < 3; ++d) + { + t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i]; + } + } + } + } + } +//#define USE_FULL_PRECONDITIONER +#ifndef USE_FULL_PRECONDITIONER + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } + int offset = m_inv_A.size(); + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = x[i + offset] * m_inv_S[i]; + } + } +#else + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + int offset = m_inv_A.size(); + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset].setZero(); + } + + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = b[i + offset] * m_inv_S[i]; + } + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i].setZero(); + } + + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = (x[i] - b[i]) * m_inv_A[i]; + } + + TVStack t; + t.resize(b.size()); + for (int i = 0; i < m_inv_S.size(); ++i) + { + t[i + offset] = x[i + offset] * m_inv_S[i]; + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + t[i].setZero(); + } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] += t[i] * m_inv_A[i]; + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] -= x[i + offset] * m_inv_S[i]; + } + } +#endif }; #endif /* BT_PRECONDITIONER_H */ diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 883adc99d..427c14954 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -37,12 +37,12 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l { btAlignedObjectArray<bool> marked; btAlignedObjectArray<btDbvtNode*> newLeafNodes; - btAlignedObjectArray<std::pair<int,int> > childIds; + btAlignedObjectArray<std::pair<int, int> > childIds; btAlignedObjectArray<btAlignedObjectArray<int> > newAdj; marked.resize(N); for (int i = 0; i < N; ++i) marked[i] = false; - + // pair adjacent nodes into new(parent) node for (int i = 0; i < N; ++i) { @@ -61,7 +61,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l leafNodes[i]->parent = node; leafNodes[n]->parent = node; newLeafNodes.push_back(node); - childIds.push_back(std::make_pair(i,n)); + childIds.push_back(std::make_pair(i, n)); merged = true; marked[n] = true; break; @@ -70,7 +70,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l if (!merged) { newLeafNodes.push_back(leafNodes[i]); - childIds.push_back(std::make_pair(i,-1)); + childIds.push_back(std::make_pair(i, -1)); } marked[i] = true; } @@ -78,7 +78,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l newAdj.resize(newLeafNodes.size()); for (int i = 0; i < newLeafNodes.size(); ++i) { - for (int j = i+1; j < newLeafNodes.size(); ++j) + for (int j = i + 1; j < newLeafNodes.size(); ++j) { bool neighbor = false; const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first]; @@ -143,7 +143,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV /* Nodes */ const btScalar margin = getCollisionShape()->getMargin(); m_nodes.resize(node_count); - m_X.resize(node_count); + m_X.resize(node_count); for (int i = 0, ni = node_count; i < ni; ++i) { Node& n = m_nodes[i]; @@ -154,7 +154,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV n.m_im = n.m_im > 0 ? 1 / n.m_im : 0; n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); n.m_material = pm; - m_X[i] = n.m_x; + m_X[i] = n.m_x; } updateBounds(); setCollisionQuadrature(3); @@ -195,8 +195,8 @@ void btSoftBody::initDefaults() m_cfg.piterations = 1; m_cfg.diterations = 0; m_cfg.citerations = 4; - m_cfg.drag = 0; - m_cfg.m_maxStress = 0; + m_cfg.drag = 0; + m_cfg.m_maxStress = 0; m_cfg.collisions = fCollision::Default; m_pose.m_bvolume = false; m_pose.m_bframe = false; @@ -224,10 +224,11 @@ void btSoftBody::initDefaults() m_dampingCoefficient = 1.0; m_sleepingThreshold = .4; m_useSelfCollision = false; - m_usePostCollisionDamping = false; m_collisionFlags = 0; + m_softSoftCollision = false; m_maxSpeedSquared = 0; m_repulsionStiffness = 0.5; + m_gravityFactor = 1; m_fdbvnt = 0; } @@ -436,7 +437,7 @@ void btSoftBody::appendFace(int model, Material* mat) ZeroInitialize(f); f.m_material = mat ? mat : m_materials[0]; } - m_faces.push_back(f); + m_faces.push_back(f); } // @@ -525,94 +526,111 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca // void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) { - DeformableNodeRigidAnchor c; - btSoftBody::Node& n = m_nodes[node]; - const btScalar ima = n.m_im; - const btScalar imb = body->getInvMass(); - btVector3 nrm; - const btCollisionShape* shp = body->getCollisionShape(); - const btTransform& wtr = body->getWorldTransform(); - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(m_nodes[node].m_x), - shp, - nrm, - 0); - - c.m_cti.m_colObj = body; - c.m_cti.m_normal = wtr.getBasis() * nrm; - c.m_cti.m_offset = dst; - c.m_node = &m_nodes[node]; - const btScalar fc = m_cfg.kDF * body->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); - const btVector3 ra = n.m_x - wtr.getOrigin(); - - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; - c.m_node->m_battach = 1; - m_deformableAnchors.push_back(c); + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + const btScalar imb = body->getInvMass(); + btVector3 nrm; + const btCollisionShape* shp = body->getCollisionShape(); + const btTransform& wtr = body->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + + c.m_cti.m_colObj = body; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * body->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} + +void btSoftBody::removeAnchor(int node) +{ + const btSoftBody::Node& n = m_nodes[node]; + for (int i = 0; i < m_deformableAnchors.size();) + { + const DeformableNodeRigidAnchor& c = m_deformableAnchors[i]; + if (c.m_node == &n) + { + m_deformableAnchors.removeAtIndex(i); + } + else + { + i++; + } + } } // void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link) { - DeformableNodeRigidAnchor c; - btSoftBody::Node& n = m_nodes[node]; - const btScalar ima = n.m_im; - btVector3 nrm; - const btCollisionShape* shp = link->getCollisionShape(); - const btTransform& wtr = link->getWorldTransform(); - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(m_nodes[node].m_x), - shp, - nrm, - 0); - c.m_cti.m_colObj = link; - c.m_cti.m_normal = wtr.getBasis() * nrm; - c.m_cti.m_offset = dst; - c.m_node = &m_nodes[node]; - const btScalar fc = m_cfg.kDF * link->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; - btVector3 normal = c.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); - findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); - findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = link->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - const btVector3 ra = n.m_x - wtr.getOrigin(); - c.m_c1 = ra; - c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; - c.m_node->m_battach = 1; - m_deformableAnchors.push_back(c); + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + btVector3 nrm; + const btCollisionShape* shp = link->getCollisionShape(); + const btTransform& wtr = link->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + c.m_cti.m_colObj = link; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * link->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + btVector3 normal = c.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = link->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + const btVector3 ra = n.m_x - wtr.getOrigin(); + c.m_c1 = ra; + c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) @@ -882,6 +900,7 @@ void btSoftBody::setVelocity(const btVector3& velocity) if (n.m_im > 0) { n.m_v = velocity; + n.m_vn = velocity; } } } @@ -1010,66 +1029,70 @@ void btSoftBody::setVolumeDensity(btScalar density) // btVector3 btSoftBody::getLinearVelocity() { - btVector3 total_momentum = btVector3(0,0,0); - for (int i = 0; i < m_nodes.size(); ++i) - { - btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0/m_nodes[i].m_im; - total_momentum += mass * m_nodes[i].m_v; - } - btScalar total_mass = getTotalMass(); - return total_mass == 0 ? total_momentum : total_momentum / total_mass; + btVector3 total_momentum = btVector3(0, 0, 0); + for (int i = 0; i < m_nodes.size(); ++i) + { + btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im; + total_momentum += mass * m_nodes[i].m_v; + } + btScalar total_mass = getTotalMass(); + return total_mass == 0 ? total_momentum : total_momentum / total_mass; } // void btSoftBody::setLinearVelocity(const btVector3& linVel) { - btVector3 old_vel = getLinearVelocity(); - btVector3 diff = linVel - old_vel; - for (int i = 0; i < m_nodes.size(); ++i) - m_nodes[i].m_v += diff; + btVector3 old_vel = getLinearVelocity(); + btVector3 diff = linVel - old_vel; + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].m_v += diff; } // void btSoftBody::setAngularVelocity(const btVector3& angVel) { - btVector3 old_vel = getLinearVelocity(); - btVector3 com = getCenterOfMass(); - for (int i = 0; i < m_nodes.size(); ++i) - { - m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; - } + btVector3 old_vel = getLinearVelocity(); + btVector3 com = getCenterOfMass(); + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; + } } // btTransform btSoftBody::getRigidTransform() { - btVector3 t = getCenterOfMass(); - btMatrix3x3 S; - S.setZero(); - // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || - for (int i = 0; i < m_nodes.size(); ++i) - { - S += OuterProduct(m_X[i], t-m_nodes[i].m_x); - } - btVector3 sigma; - btMatrix3x3 U,V; - singularValueDecomposition(S,U,sigma,V); - btMatrix3x3 R = V * U.transpose(); - btTransform trs; - trs.setIdentity(); - trs.setOrigin(t); - trs.setBasis(R); - return trs; + btVector3 t = getCenterOfMass(); + btMatrix3x3 S; + S.setZero(); + // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || + // It's important to make sure that S has the correct signs. + // SVD is only unique up to the ordering of singular values. + // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular + // vaues are negative, SVD will permute colums of U to make two of them positive. + for (int i = 0; i < m_nodes.size(); ++i) + { + S -= OuterProduct(m_X[i], t - m_nodes[i].m_x); + } + btVector3 sigma; + btMatrix3x3 U, V; + singularValueDecomposition(S, U, sigma, V); + btMatrix3x3 R = V * U.transpose(); + btTransform trs; + trs.setIdentity(); + trs.setOrigin(t); + trs.setBasis(R); + return trs; } // void btSoftBody::transformTo(const btTransform& trs) { - // get the current best rigid fit - btTransform current_transform = getRigidTransform(); - // apply transform in material space - btTransform new_transform = trs * current_transform.inverse(); - transform(new_transform); + // get the current best rigid fit + btTransform current_transform = getRigidTransform(); + // apply transform in material space + btTransform new_transform = trs * current_transform.inverse(); + transform(new_transform); } // @@ -1130,7 +1153,7 @@ void btSoftBody::scale(const btVector3& scl) updateNormals(); updateBounds(); updateConstants(); - initializeDmInverse(); + initializeDmInverse(); } // @@ -2010,22 +2033,22 @@ bool btSoftBody::rayTest(const btVector3& rayFrom, } bool btSoftBody::rayFaceTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) + const btVector3& rayTo, + sRayCast& results) { if (m_faces.size() == 0) return false; else { - if (m_fdbvt.empty()) - initializeFaceTree(); + if (m_fdbvt.empty()) + initializeFaceTree(); } - - results.body = this; - results.fraction = 1.f; - results.index = -1; - - return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); + + results.body = this; + results.fraction = 1.f; + results.index = -1; + + return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); } // @@ -2056,112 +2079,111 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) void btSoftBody::predictMotion(btScalar dt) { - int i, ni; - - /* Update */ - if (m_bUpdateRtCst) - { - m_bUpdateRtCst = false; - updateConstants(); - m_fdbvt.clear(); - if (m_cfg.collisions & fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt * m_cfg.timescale; - m_sst.isdt = 1 / m_sst.sdt; - m_sst.velmrg = m_sst.sdt * 3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity * m_sst.sdt); - applyForces(); - /* Integrate */ - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; - { - btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / m_sst.sdt; - for (int c = 0; c < 3; c++) - { - if (deltaV[c] > clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (deltaV[c] < -clampDeltaV) - { - deltaV[c] = -clampDeltaV; - } - } - } - n.m_v += deltaV; - n.m_x += n.m_v * m_sst.sdt; - n.m_f = btVector3(0, 0, 0); - } - /* Clusters */ - updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) - vol; - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); - m_ndbvt.update(n.m_leaf, - vol, - n.m_v * m_sst.velmrg, - m_sst.updmrg); - } - /* Faces */ - if (!m_fdbvt.empty()) - { - for (int i = 0; i < m_faces.size(); ++i) - { - Face& f = m_faces[i]; - const btVector3 v = (f.m_n[0]->m_v + - f.m_n[1]->m_v + - f.m_n[2]->m_v) / - 3; - vol = VolumeOf(f, m_sst.radmrg); - m_fdbvt.update(f.m_leaf, - vol, - v * m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ - updatePose(); - /* Match */ - if (m_pose.m_bframe && (m_cfg.kMT > 0)) - { - const btMatrix3x3 posetrs = m_pose.m_rot; - for (int i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - if (n.m_im > 0) - { - const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; - n.m_x = Lerp(n.m_x, x, m_cfg.kMT); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); -} + int i, ni; + /* Update */ + if (m_bUpdateRtCst) + { + m_bUpdateRtCst = false; + updateConstants(); + m_fdbvt.clear(); + if (m_cfg.collisions & fCollision::VF_SS) + { + initializeFaceTree(); + } + } + + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + applyForces(); + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; + { + btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) + { + if (deltaV[c] > clampDeltaV) + { + deltaV[c] = clampDeltaV; + } + if (deltaV[c] < -clampDeltaV) + { + deltaV[c] = -clampDeltaV; + } + } + } + n.m_v += deltaV; + n.m_x += n.m_v * m_sst.sdt; + n.m_f = btVector3(0, 0, 0); + } + /* Clusters */ + updateClusters(); + /* Bounds */ + updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + m_ndbvt.update(n.m_leaf, + vol, + n.m_v * m_sst.velmrg, + m_sst.updmrg); + } + /* Faces */ + if (!m_fdbvt.empty()) + { + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ + updatePose(); + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) + { + const btMatrix3x3 posetrs = m_pose.m_rot; + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + if (n.m_im > 0) + { + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); + } + } + } + /* Clear contacts */ + m_rcontacts.resize(0); + m_scontacts.resize(0); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); +} // void btSoftBody::solveConstraints() @@ -2534,12 +2556,12 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, } int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, - btScalar& mint, int& index) const + btScalar& mint, int& index) const { int cnt = 0; { /* Use dbvt */ RayFromToCaster collider(rayFrom, rayTo, mint); - + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); if (collider.m_face) { @@ -2551,7 +2573,6 @@ int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, return (cnt); } - // static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) { @@ -2580,7 +2601,7 @@ static inline void calculateNormalCone(btDbvntNode* root) } else { - btVector3 n0(0,0,0), n1(0,0,0); + btVector3 n0(0, 0, 0), n1(0, 0, 0); btScalar a0 = 0, a1 = 0; if (root->childs[0]) { @@ -2594,8 +2615,8 @@ static inline void calculateNormalCone(btDbvntNode* root) n1 = root->childs[1]->normal; a1 = root->childs[1]->angle; } - root->normal = (n0+n1).safeNormalize(); - root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5; + root->normal = (n0 + n1).safeNormalize(); + root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5; } } @@ -2609,7 +2630,8 @@ void btSoftBody::initializeFaceTree() for (int i = 0; i < m_faces.size(); ++i) { Face& f = m_faces[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = VolumeOf(f, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &f; @@ -2623,7 +2645,7 @@ void btSoftBody::initializeFaceTree() // construct the adjacency list for triangles for (int i = 0; i < adj.size(); ++i) { - for (int j = i+1; j < adj.size(); ++j) + for (int j = i + 1; j < adj.size(); ++j) { int dup = 0; for (int k = 0; k < 3; ++k) @@ -2661,7 +2683,8 @@ void btSoftBody::rebuildNodeTree() for (int i = 0; i < m_nodes.size(); ++i) { Node& n = m_nodes[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = btDbvtVolume::FromCR(n.m_x, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &n; @@ -2704,61 +2727,61 @@ btVector3 btSoftBody::evaluateCom() const } bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti) const -{ - btVector3 nrm; - const btCollisionShape* shp = colObjWrap->getCollisionShape(); - // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); - //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform& wtr = colObjWrap->getWorldTransform(); - //todo: check which transform is needed here - - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(x), - shp, - nrm, - margin); - if (dst < 0) - { - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); - return (true); - } - return (false); + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); + //todo: check which transform is needed here + + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + if (dst < 0) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); + } + return (false); } // bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti, bool predict) const + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); - // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect - // but resolve contact at x_n - btTransform wtr = (predict) ? - (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - : colObjWrap->getWorldTransform(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), shp, nrm, margin); + if (!predict) { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = dst; + cti.m_offset = dst; } - if (dst < 0) - return true; + if (dst < 0) + return true; return (false); } @@ -2767,146 +2790,134 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr // point p with respect to triangle (a, b, c) static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary) { - btVector3 v0 = b - a, v1 = c - a, v2 = p - a; - btScalar d00 = v0.dot(v0); - btScalar d01 = v0.dot(v1); - btScalar d11 = v1.dot(v1); - btScalar d20 = v2.dot(v0); - btScalar d21 = v2.dot(v1); - btScalar denom = d00 * d11 - d01 * d01; - bary.setY((d11 * d20 - d01 * d21) / denom); - bary.setZ((d00 * d21 - d01 * d20) / denom); - bary.setX(btScalar(1) - bary.getY() - bary.getZ()); + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = v0.dot(v0); + btScalar d01 = v0.dot(v1); + btScalar d11 = v1.dot(v1); + btScalar d20 = v2.dot(v0); + btScalar d21 = v2.dot(v1); + btScalar denom = d00 * d11 - d01 * d01; + bary.setY((d11 * d20 - d01 * d21) / denom); + bary.setZ((d00 * d21 - d01 * d20) / denom); + bary.setX(btScalar(1) - bary.getY() - bary.getZ()); } // bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, - Face& f, - btVector3& contact_point, - btVector3& bary, - btScalar margin, - btSoftBody::sCti& cti, bool predict) const -{ - btVector3 nrm; - const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); - // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect - // but resolve contact at x_n - btTransform wtr = (predict) ? - (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - : colObjWrap->getWorldTransform(); - btScalar dst; - -//#define USE_QUADRATURE 1 -//#define CACHE_PREV_COLLISION - - // use the contact position of the previous collision + Face& f, + btVector3& contact_point, + btVector3& bary, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + btScalar dst; + + //#define USE_QUADRATURE 1 + //#define CACHE_PREV_COLLISION + + // use the contact position of the previous collision #ifdef CACHE_PREV_COLLISION - if (f.m_pcontact[3] != 0) - { - for (int i = 0; i < 3; ++i) - bary[i] = f.m_pcontact[i]; - contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - dst = m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(contact_point), - shp, - nrm, - margin); - nrm = wtr.getBasis() * nrm; - cti.m_colObj = colObjWrap->getCollisionObject(); - // use cached contact point - } - else - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_x); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); - btVector3 guess(0,0,0); - const btConvexShape* csh = static_cast<const btConvexShape*>(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance - margin; - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - nrm = results.normal; - cti.m_colObj = colObjWrap->getCollisionObject(); - for (int i = 0; i < 3; ++i) - f.m_pcontact[i] = bary[i]; - } - return (dst < 0); + if (f.m_pcontact[3] != 0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(contact_point), + shp, + nrm, + margin); + nrm = wtr.getBasis() * nrm; + cti.m_colObj = colObjWrap->getCollisionObject(); + // use cached contact point + } + else + { + btGjkEpaSolver2::sResults results; + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + btVector3 guess(0, 0, 0); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + dst = results.distance - margin; + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + nrm = results.normal; + cti.m_colObj = colObjWrap->getCollisionObject(); + for (int i = 0; i < 3; ++i) + f.m_pcontact[i] = bary[i]; + } + return (dst < 0); #endif - // use collision quadrature point + // use collision quadrature point #ifdef USE_QUADRATURE - { - dst = SIMD_INFINITY; - btVector3 local_nrm; - for (int q = 0; q < m_quads.size(); ++q) - { - btVector3 p; - if (predict) - p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); - else - p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); - btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(p), - shp, - local_nrm, - margin); - if (local_dst < dst) - { - if (local_dst < 0 && predict) - return true; - dst = local_dst; - contact_point = p; - bary = m_quads[q]; - nrm = local_nrm; - } - if (!predict) - { - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = dst; - } - } - return (dst < 0); - } + { + dst = SIMD_INFINITY; + btVector3 local_nrm; + for (int q = 0; q < m_quads.size(); ++q) + { + btVector3 p; + if (predict) + p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); + else + p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); + btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(p), + shp, + local_nrm, + margin); + if (local_dst < dst) + { + if (local_dst < 0 && predict) + return true; + dst = local_dst; + contact_point = p; + bary = m_quads[q]; + nrm = local_nrm; + } + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; + } + } + return (dst < 0); + } #endif - - // regular face contact - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_x); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); - btVector3 guess(0,0,0); - if (predict) - { - triangle_transform.setOrigin(f.m_n[0]->m_q); - triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); - } - const btConvexShape* csh = static_cast<const btConvexShape*>(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance - margin; - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - nrm = results.normal; - for (int i = 0; i < 3; ++i) - f.m_pcontact[i] = bary[i]; - } - - if (!predict) - { - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = nrm; - cti.m_offset = dst; - } - - return (dst < 0); + btGjkEpaSolver2::sResults results; + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_q); + btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q); + btVector3 guess(0, 0, 0); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + dst = results.distance - 2.0 * csh->getMargin() - margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable + if (dst >= 0) + return false; + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + dst = results.distance - csh->getMargin() - margin; + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = results.normal; + cti.m_offset = dst; + return true; } - // void btSoftBody::updateNormals() { @@ -2951,63 +2962,63 @@ void btSoftBody::updateBounds() m_bounds[1] = btVector3(1000, 1000, 1000); } else {*/ -// if (m_ndbvt.m_root) -// { -// const btVector3& mins = m_ndbvt.m_root->volume.Mins(); -// const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); -// const btScalar csm = getCollisionShape()->getMargin(); -// const btVector3 mrg = btVector3(csm, -// csm, -// csm) * -// 1; // ??? to investigate... -// m_bounds[0] = mins - mrg; -// m_bounds[1] = maxs + mrg; -// if (0 != getBroadphaseHandle()) -// { -// m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), -// m_bounds[0], -// m_bounds[1], -// m_worldInfo->m_dispatcher); -// } -// } -// else -// { -// m_bounds[0] = -// m_bounds[1] = btVector3(0, 0, 0); -// } - if (m_nodes.size()) - { - btVector3 mins = m_nodes[0].m_x; - btVector3 maxs = m_nodes[0].m_x; - for (int i = 1; i < m_nodes.size(); ++i) - { - for (int d = 0; d < 3; ++d) - { - if (m_nodes[i].m_x[d] > maxs[d]) - maxs[d] = m_nodes[i].m_x[d]; - if (m_nodes[i].m_x[d] < mins[d]) - mins[d] = m_nodes[i].m_x[d]; - } - } - const btScalar csm = getCollisionShape()->getMargin(); - const btVector3 mrg = btVector3(csm, - csm, - csm); - m_bounds[0] = mins - mrg; - m_bounds[1] = maxs + mrg; - if (0 != getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } - } - else - { - m_bounds[0] = - m_bounds[1] = btVector3(0, 0, 0); - } + // if (m_ndbvt.m_root) + // { + // const btVector3& mins = m_ndbvt.m_root->volume.Mins(); + // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); + // const btScalar csm = getCollisionShape()->getMargin(); + // const btVector3 mrg = btVector3(csm, + // csm, + // csm) * + // 1; // ??? to investigate... + // m_bounds[0] = mins - mrg; + // m_bounds[1] = maxs + mrg; + // if (0 != getBroadphaseHandle()) + // { + // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + // m_bounds[0], + // m_bounds[1], + // m_worldInfo->m_dispatcher); + // } + // } + // else + // { + // m_bounds[0] = + // m_bounds[1] = btVector3(0, 0, 0); + // } + if (m_nodes.size()) + { + btVector3 mins = m_nodes[0].m_x; + btVector3 maxs = m_nodes[0].m_x; + for (int i = 1; i < m_nodes.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + if (m_nodes[i].m_x[d] > maxs[d]) + maxs[d] = m_nodes[i].m_x[d]; + if (m_nodes[i].m_x[d] < mins[d]) + mins[d] = m_nodes[i].m_x[d]; + } + } + const btScalar csm = getCollisionShape()->getMargin(); + const btVector3 mrg = btVector3(csm, + csm, + csm); + m_bounds[0] = mins - mrg; + m_bounds[1] = maxs + mrg; + if (0 != getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0] = + m_bounds[1] = btVector3(0, 0, 0); + } } // @@ -3426,60 +3437,115 @@ void btSoftBody::dampClusters() void btSoftBody::setSpringStiffness(btScalar k) { - for (int i = 0; i < m_links.size(); ++i) - { - m_links[i].Feature::m_material->m_kLST = k; - } - m_repulsionStiffness = k; + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } + m_repulsionStiffness = k; +} + +void btSoftBody::setGravityFactor(btScalar gravFactor) +{ + m_gravityFactor = gravFactor; } void btSoftBody::initializeDmInverse() { - btScalar unit_simplex_measure = 1./6.; - - for (int i = 0; i < m_tetras.size(); ++i) - { - Tetra &t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; - btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; - btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; - btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - t.m_element_measure = Dm.determinant() * unit_simplex_measure; - t.m_Dm_inverse = Dm.inverse(); - } + btScalar unit_simplex_measure = 1. / 6.; + + for (int i = 0; i < m_tetras.size(); ++i) + { + Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; + btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; + btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; + btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_element_measure = Dm.determinant() * unit_simplex_measure; + t.m_Dm_inverse = Dm.inverse(); + + // calculate the first three columns of P^{-1} + btVector3 a = t.m_n[0]->m_x; + btVector3 b = t.m_n[1]->m_x; + btVector3 c = t.m_n[2]->m_x; + btVector3 d = t.m_n[3]->m_x; + + btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]); + + btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2]; + btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2]; + btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1]; + btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2]; + btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2]; + btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1]; + btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2]; + btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2]; + btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1]; + btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2]; + btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2]; + btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1]; + + btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det); + btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det); + btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det); + + t.m_P_inv[0] = p1; + t.m_P_inv[1] = p2; + t.m_P_inv[2] = p3; + } +} + +static btScalar Dot4(const btVector4& a, const btVector4& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; } void btSoftBody::updateDeformation() { - for (int i = 0; i < m_tetras.size(); ++i) - { - btSoftBody::Tetra& t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; - btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; - btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; - btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - t.m_F = Ds * t.m_Dm_inverse; - - btSoftBody::TetraScratch& s = m_tetraScratches[i]; - s.m_F = t.m_F; - s.m_J = t.m_F.determinant(); - btMatrix3x3 C = t.m_F.transpose()*t.m_F; - s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); - s.m_cofF = t.m_F.adjoint().transpose(); - } + btQuaternion q; + for (int i = 0; i < m_tetras.size(); ++i) + { + btSoftBody::Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_F = Ds * t.m_Dm_inverse; + + btSoftBody::TetraScratch& s = m_tetraScratches[i]; + s.m_F = t.m_F; + s.m_J = t.m_F.determinant(); + btMatrix3x3 C = t.m_F.transpose() * t.m_F; + s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); + s.m_cofF = t.m_F.adjoint().transpose(); + + btVector3 a = t.m_n[0]->m_q; + btVector3 b = t.m_n[1]->m_q; + btVector3 c = t.m_n[2]->m_q; + btVector3 d = t.m_n[3]->m_q; + btVector4 q1(a[0], b[0], c[0], d[0]); + btVector4 q2(a[1], b[1], c[1], d[1]); + btVector4 q3(a[2], b[2], c[2], d[2]); + btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]), + Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]), + Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2])); + q.setRotation(btVector3(0, 0, 1), 0); + B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness. + btMatrix3x3 Q(q); + s.m_corotation = Q; + } } void btSoftBody::advanceDeformation() { - updateDeformation(); - for (int i = 0; i < m_tetras.size(); ++i) - { - m_tetraScratchesTn[i] = m_tetraScratches[i]; - } + updateDeformation(); + for (int i = 0; i < m_tetras.size(); ++i) + { + m_tetraScratchesTn[i] = m_tetraScratches[i]; + } } // void btSoftBody::Joint::Prepare(btScalar dt, int) @@ -3722,7 +3788,7 @@ void btSoftBody::applyForces() // void btSoftBody::setMaxStress(btScalar maxStress) { - m_cfg.m_maxStress = maxStress; + m_cfg.m_maxStress = maxStress; } // @@ -3765,13 +3831,13 @@ void btSoftBody::interpolateRenderMesh() void btSoftBody::setCollisionQuadrature(int N) { - for (int i = 0; i <= N; ++i) - { - for (int j = 0; i+j <= N; ++j) - { - m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N))); - } - } + for (int i = 0; i <= N; ++i) + { + for (int j = 0; i + j <= N; ++j) + { + m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N))); + } + } } // @@ -3978,12 +4044,12 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) void btSoftBody::setSelfCollision(bool useSelfCollision) { - m_useSelfCollision = useSelfCollision; + m_useSelfCollision = useSelfCollision; } bool btSoftBody::useSelfCollision() { - return m_useSelfCollision; + return m_useSelfCollision; } // @@ -4024,54 +4090,54 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap collider.ProcessColObj(this, pcoWrap); } break; - case fCollision::SDF_RD: - { - - btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); - if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) - { - const btTransform wtr = pcoWrap->getWorldTransform(); -// const btTransform ctr = pcoWrap->getWorldTransform(); -// const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); - const btScalar timemargin = 0; - const btScalar basemargin = getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) - volume; - pcoWrap->getCollisionShape()->getAabb(wtr, - mins, - maxs); - volume = btDbvtVolume::FromMM(mins, maxs); - volume.Expand(btVector3(basemargin, basemargin, basemargin)); - btSoftColliders::CollideSDF_RD docollideNode; - docollideNode.psb = this; - docollideNode.m_colObj1Wrap = pcoWrap; - docollideNode.m_rigidBody = prb1; - docollideNode.dynmargin = basemargin + timemargin; - docollideNode.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); - - if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) - { - btSoftColliders::CollideSDF_RDF docollideFace; - docollideFace.psb = this; - docollideFace.m_colObj1Wrap = pcoWrap; - docollideFace.m_rigidBody = prb1; + case fCollision::SDF_RD: + { + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) + { + const btTransform wtr = pcoWrap->getWorldTransform(); + const btScalar timemargin = 0; + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(wtr, + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + if (m_cfg.collisions & fCollision::SDF_RDN) + { + btSoftColliders::CollideSDF_RD docollideNode; + docollideNode.psb = this; + docollideNode.m_colObj1Wrap = pcoWrap; + docollideNode.m_rigidBody = prb1; + docollideNode.dynmargin = basemargin + timemargin; + docollideNode.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); + } + + if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) + { + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; docollideFace.dynmargin = basemargin + timemargin; docollideFace.stamargin = basemargin; - m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); - } - } - } - break; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + } + } + } + break; } } // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { - BT_PROFILE("Deformable Collision"); + BT_PROFILE("Deformable Collision"); const int cf = m_cfg.collisions & psb->m_cfg.collisions; switch (cf & fCollision::SVSmask) { @@ -4109,58 +4175,60 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } break; - case fCollision::VF_DD: - { - if (psb->isActive() || this->isActive()) - { - if (this != psb) - { - btSoftColliders::CollideVF_DD docollide; - /* common */ - docollide.mrg = getCollisionShape()->getMargin() + - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - if (psb->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - docollide.psb[0] = this; - docollide.psb[1] = psb; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - - /* psb1 nodes vs psb0 faces */ - if (this->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - docollide.psb[0] = psb; - docollide.psb[1] = this; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - } - else - { - if (psb->useSelfCollision()) - { - btSoftColliders::CollideFF_DD docollide; - docollide.mrg = getCollisionShape()->getMargin(); - docollide.psb[0] = this; - docollide.psb[1] = psb; - if (this->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - /* psb0 faces vs psb0 faces */ - calculateNormalCone(this->m_fdbvnt); - this->m_fdbvt.selfCollideT(m_fdbvnt,docollide); - } - } - } - } - break; + case fCollision::VF_DD: + { + if (!psb->m_softSoftCollision) + return; + if (psb->isActive() || this->isActive()) + { + if (this != psb) + { + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + + /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = 2 * getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + /* psb0 faces vs psb0 faces */ + calculateNormalCone(this->m_fdbvnt); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); + } + } + } + } + break; default: { } @@ -4175,7 +4243,7 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) { btSoftColliders::CollideCCD docollide; /* common */ - docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin + docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin docollide.dt = psb->m_sst.sdt; /* psb0 nodes vs psb1 faces */ if (psb->m_tetras.size() > 0) @@ -4185,8 +4253,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.psb[0] = this; docollide.psb[1] = psb; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + docollide.psb[1]->m_fdbvt.m_root, + docollide); /* psb1 nodes vs psb0 faces */ if (this->m_tetras.size() > 0) docollide.useFaceNormal = true; @@ -4195,8 +4263,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.psb[0] = psb; docollide.psb[1] = this; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + docollide.psb[1]->m_fdbvt.m_root, + docollide); } else { @@ -4206,14 +4274,14 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.mrg = SAFE_EPSILON; docollide.psb[0] = this; docollide.psb[1] = psb; - docollide.dt = psb->m_sst.sdt; + docollide.dt = psb->m_sst.sdt; if (this->m_tetras.size() > 0) docollide.useFaceNormal = true; else docollide.useFaceNormal = false; /* psb0 faces vs psb0 faces */ calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope - this->m_fdbvt.selfCollideT(m_fdbvnt,docollide); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); } } } @@ -4618,44 +4686,43 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ void btSoftBody::updateDeactivation(btScalar timeStep) { - if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) - return; + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + return; - if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) - { - m_deactivationTime += timeStep; - } - else - { - m_deactivationTime = btScalar(0.); - setActivationState(0); - } + if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) + { + m_deactivationTime += timeStep; + } + else + { + m_deactivationTime = btScalar(0.); + setActivationState(0); + } } - void btSoftBody::setZeroVelocity() { - for (int i = 0; i < m_nodes.size(); ++i) - { - m_nodes[i].m_v.setZero(); - } + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v.setZero(); + } } bool btSoftBody::wantsSleeping() { - if (getActivationState() == DISABLE_DEACTIVATION) - return false; + if (getActivationState() == DISABLE_DEACTIVATION) + return false; - //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) - return false; + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) + return false; - if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) - return true; + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + return true; - if (m_deactivationTime > gDeactivationTime) - { - return true; - } - return false; + if (m_deactivationTime > gDeactivationTime) + { + return true; + } + return false; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 7b7374a73..efe3f5f3c 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -35,7 +35,7 @@ subject to the following restrictions: //#else #define btSoftBodyData btSoftBodyFloatData #define btSoftBodyDataName "btSoftBodyFloatData" -static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; +static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; static unsigned long seed = 243703; //#endif //BT_USE_DOUBLE_PRECISION @@ -163,7 +163,7 @@ public: RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - SDF_RD = 0x0004, ///SDF based rigid vs deformable + SDF_RD = 0x0004, ///rigid vs deformable SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling @@ -171,9 +171,10 @@ public: CL_SELF = 0x0040, ///Cluster soft body self collision VF_DD = 0x0080, ///Vertex vs face soft vs soft handling - RVDFmask = 0x0f00, /// Rigid versus deformable face mask - SDF_RDF = 0x0100, /// SDF based Rigid vs. deformable face - SDF_MDF = 0x0200, /// SDF based Multibody vs. deformable face + RVDFmask = 0x0f00, /// Rigid versus deformable face mask + SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face + SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face + SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node /* presets */ Default = SDF_RS, END @@ -225,7 +226,7 @@ public: const btCollisionObject* m_colObj; /* Rigid body */ btVector3 m_normal; /* Outward normal */ btScalar m_offset; /* Offset from origin */ - btVector3 m_bary; /* Barycentric weights for faces */ + btVector3 m_bary; /* Barycentric weights for faces */ }; /* sMedium */ @@ -262,16 +263,18 @@ public: btVector3 m_x; // Position btVector3 m_q; // Previous step position/Test position btVector3 m_v; // Velocity - btVector3 m_vsplit; // Temporary Velocity in addintion to velocity used in split impulse - btVector3 m_vn; // Previous step velocity + btVector3 m_vn; // Previous step velocity btVector3 m_f; // Force accumulator btVector3 m_n; // Normal btScalar m_im; // 1/mass btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data - bool m_constrained; // constrained node + int m_constrained; // depth of penetration int m_battach : 1; // Attached - int index; + int index; + btVector3 m_splitv; // velocity associated with split impulse + btMatrix3x3 m_effectiveMass; // effective mass in contact + btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass }; /* Link */ ATTRIBUTE_ALIGNED16(struct) @@ -290,37 +293,39 @@ public: /* Face */ struct Face : Feature { - Node* m_n[3]; // Node pointers - btVector3 m_normal; // Normal - btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data - btVector4 m_pcontact; // barycentric weights of the persistent contact - btVector3 m_n0, m_n1, m_vn; - int m_index; + Node* m_n[3]; // Node pointers + btVector3 m_normal; // Normal + btScalar m_ra; // Rest area + btDbvtNode* m_leaf; // Leaf data + btVector4 m_pcontact; // barycentric weights of the persistent contact + btVector3 m_n0, m_n1, m_vn; + int m_index; }; /* Tetra */ struct Tetra : Feature { - Node* m_n[4]; // Node pointers - btScalar m_rv; // Rest volume - btDbvtNode* m_leaf; // Leaf data - btVector3 m_c0[4]; // gradients - btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) - btScalar m_c2; // m_c1/sum(|g0..3|^2) - btMatrix3x3 m_Dm_inverse; // rest Dm^-1 - btMatrix3x3 m_F; - btScalar m_element_measure; + Node* m_n[4]; // Node pointers + btScalar m_rv; // Rest volume + btDbvtNode* m_leaf; // Leaf data + btVector3 m_c0[4]; // gradients + btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) + btScalar m_c2; // m_c1/sum(|g0..3|^2) + btMatrix3x3 m_Dm_inverse; // rest Dm^-1 + btMatrix3x3 m_F; + btScalar m_element_measure; + btVector4 m_P_inv[3]; // first three columns of P_inv matrix + }; + + /* TetraScratch */ + struct TetraScratch + { + btMatrix3x3 m_F; // deformation gradient F + btScalar m_trace; // trace of F^T * F + btScalar m_J; // det(F) + btMatrix3x3 m_cofF; // cofactor of F + btMatrix3x3 m_corotation; // corotatio of the tetra }; - - /* TetraScratch */ - struct TetraScratch - { - btMatrix3x3 m_F; // deformation gradient F - btScalar m_trace; // trace of F^T * F - btScalar m_J; // det(F) - btMatrix3x3 m_cofF; // cofactor of F - }; - + /* RContact */ struct RContact { @@ -331,67 +336,68 @@ public: btScalar m_c2; // ima*dt btScalar m_c3; // Friction btScalar m_c4; // Hardness - - // jacobians and unit impulse responses for multibody - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_t1; - btMultiBodyJacobianData jacobianData_t2; - btVector3 t1; - btVector3 t2; + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; + }; + + class DeformableRigidContact + { + public: + sCti m_cti; // Contact infos + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // inverse mass of node/face + btScalar m_c3; // Friction + btScalar m_c4; // Hardness + btMatrix3x3 m_c5; // inverse effective mass + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; }; - - class DeformableRigidContact - { - public: - sCti m_cti; // Contact infos - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // inverse mass of node/face - btScalar m_c3; // Friction - btScalar m_c4; // Hardness - - // jacobians and unit impulse responses for multibody - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_t1; - btMultiBodyJacobianData jacobianData_t2; - btVector3 t1; - btVector3 t2; - }; - - class DeformableNodeRigidContact : public DeformableRigidContact - { - public: - Node* m_node; // Owner node - }; - - class DeformableNodeRigidAnchor : public DeformableNodeRigidContact - { - public: - btVector3 m_local; // Anchor position in body space - }; - - class DeformableFaceRigidContact : public DeformableRigidContact - { - public: - Face* m_face; // Owner face - btVector3 m_contactPoint; // Contact point - btVector3 m_bary; // Barycentric weights - btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; - }; - - struct DeformableFaceNodeContact - { - Node* m_node; // Node - Face* m_face; // Face - btVector3 m_bary; // Barycentric weights - btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; - btVector3 m_normal; // Normal - btScalar m_margin; // Margin - btScalar m_friction; // Friction - btScalar m_imf; // inverse mass of the face at contact point - btScalar m_c0; // scale of the impulse matrix; - }; - + + class DeformableNodeRigidContact : public DeformableRigidContact + { + public: + Node* m_node; // Owner node + }; + + class DeformableNodeRigidAnchor : public DeformableNodeRigidContact + { + public: + btVector3 m_local; // Anchor position in body space + }; + + class DeformableFaceRigidContact : public DeformableRigidContact + { + public: + Face* m_face; // Owner face + btVector3 m_contactPoint; // Contact point + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + }; + + struct DeformableFaceNodeContact + { + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_imf; // inverse mass of the face at contact point + btScalar m_c0; // scale of the impulse matrix; + }; + /* SContact */ struct SContact { @@ -718,19 +724,19 @@ public: tVSolverArray m_vsequence; // Velocity solvers sequence tPSolverArray m_psequence; // Position solvers sequence tPSolverArray m_dsequence; // Drift solvers sequence - btScalar drag; // deformable air drag - btScalar m_maxStress; // Maximum principle first Piola stress + btScalar drag; // deformable air drag + btScalar m_maxStress; // Maximum principle first Piola stress }; /* SolverState */ struct SolverState { //if you add new variables, always initialize them! SolverState() - :sdt(0), - isdt(0), - velmrg(0), - radmrg(0), - updmrg(0) + : sdt(0), + isdt(0), + velmrg(0), + radmrg(0), + updmrg(0) { } btScalar sdt; // dt*timescale @@ -791,42 +797,43 @@ public: btSoftBodyWorldInfo* m_worldInfo; // World info tNoteArray m_notes; // Notes tNodeArray m_nodes; // Nodes - tNodeArray m_renderNodes; // Nodes + tNodeArray m_renderNodes; // Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces - tFaceArray m_renderFaces; // Faces + tFaceArray m_renderFaces; // Faces tTetraArray m_tetras; // Tetras - btAlignedObjectArray<TetraScratch> m_tetraScratches; - btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; - tAnchorArray m_anchors; // Anchors - btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; - tRContactArray m_rcontacts; // Rigid contacts - btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; - btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; - btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; - tSContactArray m_scontacts; // Soft contacts - tJointArray m_joints; // Joints - tMaterialArray m_materials; // Materials - btScalar m_timeacc; // Time accumulator - btVector3 m_bounds[2]; // Spatial bounds - bool m_bUpdateRtCst; // Update runtime constants - btDbvt m_ndbvt; // Nodes tree - btDbvt m_fdbvt; // Faces tree - btDbvntNode* m_fdbvnt; // Faces tree with normals - btDbvt m_cdbvt; // Clusters tree - tClusterArray m_clusters; // Clusters - btScalar m_dampingCoefficient; // Damping Coefficient + btAlignedObjectArray<TetraScratch> m_tetraScratches; + btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; + tAnchorArray m_anchors; // Anchors + btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; + tRContactArray m_rcontacts; // Rigid contacts + btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; + btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; + btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; + tSContactArray m_scontacts; // Soft contacts + tJointArray m_joints; // Joints + tMaterialArray m_materials; // Materials + btScalar m_timeacc; // Time accumulator + btVector3 m_bounds[2]; // Spatial bounds + bool m_bUpdateRtCst; // Update runtime constants + btDbvt m_ndbvt; // Nodes tree + btDbvt m_fdbvt; // Faces tree + btDbvntNode* m_fdbvnt; // Faces tree with normals + btDbvt m_cdbvt; // Clusters tree + tClusterArray m_clusters; // Clusters + btScalar m_dampingCoefficient; // Damping Coefficient btScalar m_sleepingThreshold; btScalar m_maxSpeedSquared; - btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection + btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection btScalar m_repulsionStiffness; - btAlignedObjectArray<btVector3> m_X; // initial positions + btScalar m_gravityFactor; + btAlignedObjectArray<btVector3> m_X; // initial positions btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights; btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents; - btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation + btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation bool m_useSelfCollision; - bool m_usePostCollisionDamping; + bool m_softSoftCollision; btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision @@ -856,11 +863,11 @@ public: { return m_worldInfo; } - - void setDampingCoefficient(btScalar damping_coeff) - { - m_dampingCoefficient = damping_coeff; - } + + void setDampingCoefficient(btScalar damping_coeff) + { + m_dampingCoefficient = damping_coeff; + } ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) @@ -921,11 +928,12 @@ public: Material* mat = 0); /* Append anchor */ - void appendDeformableAnchor(int node, btRigidBody* body); - void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); - void appendAnchor(int node, + void appendDeformableAnchor(int node, btRigidBody* body); + void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); + void appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); + void removeAnchor(int node); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1); void appendLinearJoint(const LJoint::Specs& specs, Body body = Body()); @@ -976,10 +984,10 @@ public: void setLinearVelocity(const btVector3& linVel); /* Set the angular velocity of the center of mass */ void setAngularVelocity(const btVector3& angVel); - /* Get best fit rigid transform */ - btTransform getRigidTransform(); - /* Transform to given pose */ - void transformTo(const btTransform& trs); + /* Get best fit rigid transform */ + btTransform getRigidTransform(); + /* Transform to given pose */ + void transformTo(const btTransform& trs); /* Transform */ void transform(const btTransform& trs); /* Translate */ @@ -1068,11 +1076,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); void defaultCollisionHandler(btSoftBody* psb); - void setSelfCollision(bool useSelfCollision); - bool useSelfCollision(); - void updateDeactivation(btScalar timeStep); - void setZeroVelocity(); - bool wantsSleeping(); + void setSelfCollision(bool useSelfCollision); + bool useSelfCollision(); + void updateDeactivation(btScalar timeStep); + void setZeroVelocity(); + bool wantsSleeping(); // // Functionality to deal with new accelerated solvers. @@ -1151,8 +1159,8 @@ public: void rebuildNodeTree(); btVector3 evaluateCom() const; bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; - bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; + bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); void updatePose(); @@ -1166,14 +1174,15 @@ public: void solveClusters(btScalar sor); void applyClusters(bool drift); void dampClusters(); - void setSpringStiffness(btScalar k); - void initializeDmInverse(); - void updateDeformation(); - void advanceDeformation(); + void setSpringStiffness(btScalar k); + void setGravityFactor(btScalar gravFactor); + void initializeDmInverse(); + void updateDeformation(); + void advanceDeformation(); void applyForces(); - void setMaxStress(btScalar maxStress); - void interpolateRenderMesh(); - void setCollisionQuadrature(int N); + void setMaxStress(btScalar maxStress); + void interpolateRenderMesh(); + void setCollisionQuadrature(int N); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti); @@ -1182,14 +1191,15 @@ public: static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); void geometricCollisionHandler(btSoftBody* psb); -#define SAFE_EPSILON SIMD_EPSILON*100.0 +#define SAFE_EPSILON SIMD_EPSILON * 100.0 void updateNode(btDbvtNode* node, bool use_velocity, bool margin) { if (node->isleaf()) { btSoftBody::Node* n = (btSoftBody::Node*)(node->data); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision if (use_velocity) { btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v}; @@ -1207,38 +1217,40 @@ public: { updateNode(node->childs[0], use_velocity, margin); updateNode(node->childs[1], use_velocity, margin); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; Merge(node->childs[0]->volume, node->childs[1]->volume, vol); node->volume = vol; } } - - void updateNodeTree(bool use_velocity, bool margin) + + void updateNodeTree(bool use_velocity, bool margin) { if (m_ndbvt.m_root) updateNode(m_ndbvt.m_root, use_velocity, margin); } - template <class DBVTNODE> // btDbvtNode or btDbvntNode + template <class DBVTNODE> // btDbvtNode or btDbvntNode void updateFace(DBVTNODE* node, bool use_velocity, bool margin) { if (node->isleaf()) { btSoftBody::Face* f = (btSoftBody::Face*)(node->data); - btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; if (use_velocity) { btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v, - f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, - f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; + f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, + f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; vol = btDbvtVolume::FromPoints(points, 6); } else { btVector3 points[3] = {f->m_n[0]->m_x, - f->m_n[1]->m_x, - f->m_n[2]->m_x}; + f->m_n[1]->m_x, + f->m_n[2]->m_x}; vol = btDbvtVolume::FromPoints(points, 3); } vol.Expand(btVector3(pad, pad, pad)); @@ -1249,7 +1261,8 @@ public: { updateFace(node->childs[0], use_velocity, margin); updateFace(node->childs[1], use_velocity, margin); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; Merge(node->childs[0]->volume, node->childs[1]->volume, vol); node->volume = vol; } @@ -1271,7 +1284,7 @@ public: return (a * coord.x() + b * coord.y() + c * coord.z()); } - void applyRepulsionForce(btScalar timeStep, bool applySpringForce) + void applyRepulsionForce(btScalar timeStep, bool applySpringForce) { btAlignedObjectArray<int> indices; { @@ -1279,7 +1292,6 @@ public: indices.resize(m_faceNodeContacts.size()); for (int i = 0; i < m_faceNodeContacts.size(); ++i) indices[i] = i; -// static unsigned long seed = 243703; #define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff) int i, ni; @@ -1298,66 +1310,68 @@ public: const btVector3& n = c.m_normal; btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w); btScalar d = c.m_margin - n.dot(l); - d = btMax(btScalar(0),d); - + d = btMax(btScalar(0), d); + const btVector3& va = node->m_v; btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w); btVector3 vr = va - vb; - const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing + const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep) continue; - btVector3 vt = vr - vn*n; + btVector3 vt = vr - vn * n; btScalar I = 0; - btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im; + btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im; if (applySpringForce) I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); if (vn < 0) I += 0.5 * mass * vn; - bool face_constrained = false, node_constrained = node->m_constrained; + int face_penetration = 0, node_penetration = node->m_constrained; for (int i = 0; i < 3; ++i) - face_constrained |= face->m_n[i]->m_constrained; - btScalar I_tilde = .5 *I /(1.0+w.length2()); - - // double the impulse if node or face is constrained. - if (face_constrained || node_constrained) + face_penetration |= face->m_n[i]->m_constrained; + btScalar I_tilde = 2.0 * I / (1.0 + w.length2()); + + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + { I_tilde *= 2.0; - if (!face_constrained) + } + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) - face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; + face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im; } - if (!node_constrained) + if (node_penetration <= 0) { - node->m_v -= I_tilde*node->m_im*n; + node->m_v -= I_tilde * node->m_im * n; } - + // apply frictional impulse btScalar vt_norm = vt.safeNorm(); if (vt_norm > SIMD_EPSILON) { btScalar delta_vn = -2 * I * node->m_im; btScalar mu = c.m_friction; - btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm; - I = 0.5 * mass * (vt_norm-vt_new); + btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm; + I = 0.5 * mass * (vt_norm - vt_new); vt.safeNormalize(); - I_tilde = .5 *I /(1.0+w.length2()); - // double the impulse if node or face is constrained. - if (face_constrained || node_constrained) + I_tilde = 2.0 * I / (1.0 + w.length2()); + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) I_tilde *= 2.0; - if (!face_constrained) + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) - face->m_n[j]->m_v += w[j]*vt*I_tilde*node->m_im; + face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; } - if (!node_constrained) + if (node_penetration <= 0) { - node->m_v -= I_tilde*node->m_im*vt; + node->m_v -= I_tilde * node->m_im * vt; } } } } 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; }; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 9645fbccd..c30b65e89 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -727,7 +727,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const int resy, int fixeds, bool gendiags, - btScalar perturbation) + btScalar perturbation) { #define IDX(_x_, _y_) ((_y_)*rx + (_x_)) /* Create nodes */ @@ -747,12 +747,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const for (int ix = 0; ix < rx; ++ix) { const btScalar tx = ix / (btScalar)(rx - 1); - btScalar pert = perturbation * btScalar(rand())/RAND_MAX; - btVector3 temp1 = py1; - temp1.setY(py1.getY() + pert); - btVector3 temp = py0; - pert = perturbation * btScalar(rand())/RAND_MAX; - temp.setY(py0.getY() + pert); + btScalar pert = perturbation * btScalar(rand()) / RAND_MAX; + btVector3 temp1 = py1; + temp1.setY(py1.getY() + pert); + btVector3 temp = py0; + pert = perturbation * btScalar(rand()) / RAND_MAX; + temp.setY(py0.getY() + pert); x[IDX(ix, iy)] = lerp(temp, temp1, tx); m[IDX(ix, iy)] = 1; } @@ -1233,9 +1233,9 @@ if(face&&face[0]) } } } - psb->initializeDmInverse(); - psb->m_tetraScratches.resize(psb->m_tetras.size()); - psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size()); @@ -1245,61 +1245,61 @@ if(face&&face[0]) btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) { - std::ifstream fs; - fs.open(vtk_file); - btAssert(fs); - - typedef btAlignedObjectArray<int> Index; - std::string line; - btAlignedObjectArray<btVector3> X; - btVector3 position; - btAlignedObjectArray<Index> indices; - bool reading_points = false; - bool reading_tets = false; - size_t n_points = 0; - size_t n_tets = 0; - size_t x_count = 0; - size_t indices_count = 0; - while (std::getline(fs, line)) - { - std::stringstream ss(line); - if (line.size() == (size_t)(0)) - { - } - else if (line.substr(0, 6) == "POINTS") - { - reading_points = true; - reading_tets = false; - ss.ignore(128, ' '); // ignore "POINTS" - ss >> n_points; - X.resize(n_points); - } - else if (line.substr(0, 5) == "CELLS") - { - reading_points = false; - reading_tets = true; - ss.ignore(128, ' '); // ignore "CELLS" - ss >> n_tets; - indices.resize(n_tets); - } - else if (line.substr(0, 10) == "CELL_TYPES") - { - reading_points = false; - reading_tets = false; - } - else if (reading_points) - { - btScalar p; - ss >> p; - position.setX(p); - ss >> p; - position.setY(p); - ss >> p; - position.setZ(p); - X[x_count++] = position; - } - else if (reading_tets) - { + std::ifstream fs; + fs.open(vtk_file); + btAssert(fs); + + typedef btAlignedObjectArray<int> Index; + std::string line; + btAlignedObjectArray<btVector3> X; + btVector3 position; + btAlignedObjectArray<Index> indices; + bool reading_points = false; + bool reading_tets = false; + size_t n_points = 0; + size_t n_tets = 0; + size_t x_count = 0; + size_t indices_count = 0; + while (std::getline(fs, line)) + { + std::stringstream ss(line); + if (line.size() == (size_t)(0)) + { + } + else if (line.substr(0, 6) == "POINTS") + { + reading_points = true; + reading_tets = false; + ss.ignore(128, ' '); // ignore "POINTS" + ss >> n_points; + X.resize(n_points); + } + else if (line.substr(0, 5) == "CELLS") + { + reading_points = false; + reading_tets = true; + ss.ignore(128, ' '); // ignore "CELLS" + ss >> n_tets; + indices.resize(n_tets); + } + else if (line.substr(0, 10) == "CELL_TYPES") + { + reading_points = false; + reading_tets = false; + } + else if (reading_points) + { + btScalar p; + ss >> p; + position.setX(p); + ss >> p; + position.setY(p); + ss >> p; + position.setZ(p); + X[x_count++] = position; + } + else if (reading_tets) + { int d; ss >> d; if (d != 4) @@ -1308,315 +1308,354 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, fs.close(); return 0; } - ss.ignore(128, ' '); // ignore "4" - Index tet; - tet.resize(4); - for (size_t i = 0; i < 4; i++) - { - ss >> tet[i]; - } - indices[indices_count++] = tet; - } - } - btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); - - for (int i = 0; i < n_tets; ++i) - { - const Index& ni = indices[i]; - psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); - { - psb->appendLink(ni[0], ni[1], 0, true); - psb->appendLink(ni[1], ni[2], 0, true); - psb->appendLink(ni[2], ni[0], 0, true); - psb->appendLink(ni[0], ni[3], 0, true); - psb->appendLink(ni[1], ni[3], 0, true); - psb->appendLink(ni[2], ni[3], 0, true); - } - } - - - generateBoundaryFaces(psb); - psb->initializeDmInverse(); - psb->m_tetraScratches.resize(psb->m_tetras.size()); - psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); - printf("Nodes: %u\r\n", psb->m_nodes.size()); - printf("Links: %u\r\n", psb->m_links.size()); - printf("Faces: %u\r\n", psb->m_faces.size()); - printf("Tetras: %u\r\n", psb->m_tetras.size()); - - fs.close(); - return psb; + ss.ignore(128, ' '); // ignore "4" + Index tet; + tet.resize(4); + for (size_t i = 0; i < 4; i++) + { + ss >> tet[i]; + printf("%d ", tet[i]); + } + printf("\n"); + indices[indices_count++] = tet; + } + } + btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); + + for (int i = 0; i < n_tets; ++i) + { + const Index& ni = indices[i]; + psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); + { + psb->appendLink(ni[0], ni[1], 0, true); + psb->appendLink(ni[1], ni[2], 0, true); + psb->appendLink(ni[2], ni[0], 0, true); + psb->appendLink(ni[0], ni[3], 0, true); + psb->appendLink(ni[1], ni[3], 0, true); + psb->appendLink(ni[2], ni[3], 0, true); + } + } + + generateBoundaryFaces(psb); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + printf("Nodes: %u\r\n", psb->m_nodes.size()); + printf("Links: %u\r\n", psb->m_links.size()); + printf("Faces: %u\r\n", psb->m_faces.size()); + printf("Tetras: %u\r\n", psb->m_tetras.size()); + + fs.close(); + return psb; } void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) { - int counter = 0; - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - psb->m_nodes[i].index = counter++; - } - typedef btAlignedObjectArray<int> Index; - btAlignedObjectArray<Index> indices; - indices.resize(psb->m_tetras.size()); - for (int i = 0; i < indices.size(); ++i) - { - Index index; - index.push_back(psb->m_tetras[i].m_n[0]->index); - index.push_back(psb->m_tetras[i].m_n[1]->index); - index.push_back(psb->m_tetras[i].m_n[2]->index); - index.push_back(psb->m_tetras[i].m_n[3]->index); - indices[i] = index; - } - - std::map<std::vector<int>, std::vector<int> > dict; - for (int i = 0; i < indices.size(); ++i) - { - for (int j = 0; j < 4; ++j) - { - std::vector<int> f; - if (j == 0) - { - f.push_back(indices[i][1]); - f.push_back(indices[i][0]); - f.push_back(indices[i][2]); - } - if (j == 1) - { - f.push_back(indices[i][3]); - f.push_back(indices[i][0]); - f.push_back(indices[i][1]); - } - if (j == 2) - { - f.push_back(indices[i][3]); - f.push_back(indices[i][1]); - f.push_back(indices[i][2]); - } - if (j == 3) - { - f.push_back(indices[i][2]); - f.push_back(indices[i][0]); - f.push_back(indices[i][3]); - } - std::vector<int> f_sorted = f; - std::sort(f_sorted.begin(), f_sorted.end()); - if (dict.find(f_sorted) != dict.end()) - { - dict.erase(f_sorted); - } - else - { - dict.insert(std::make_pair(f_sorted, f)); - } - } - } - - for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) - { - std::vector<int> f = it->second; - psb->appendFace(f[0], f[1], f[2]); - } + int counter = 0; + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + psb->m_nodes[i].index = counter++; + } + typedef btAlignedObjectArray<int> Index; + btAlignedObjectArray<Index> indices; + indices.resize(psb->m_tetras.size()); + for (int i = 0; i < indices.size(); ++i) + { + Index index; + index.push_back(psb->m_tetras[i].m_n[0]->index); + index.push_back(psb->m_tetras[i].m_n[1]->index); + index.push_back(psb->m_tetras[i].m_n[2]->index); + index.push_back(psb->m_tetras[i].m_n[3]->index); + indices[i] = index; + } + + std::map<std::vector<int>, std::vector<int> > dict; + for (int i = 0; i < indices.size(); ++i) + { + for (int j = 0; j < 4; ++j) + { + std::vector<int> f; + if (j == 0) + { + f.push_back(indices[i][1]); + f.push_back(indices[i][0]); + f.push_back(indices[i][2]); + } + if (j == 1) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][0]); + f.push_back(indices[i][1]); + } + if (j == 2) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][1]); + f.push_back(indices[i][2]); + } + if (j == 3) + { + f.push_back(indices[i][2]); + f.push_back(indices[i][0]); + f.push_back(indices[i][3]); + } + std::vector<int> f_sorted = f; + std::sort(f_sorted.begin(), f_sorted.end()); + if (dict.find(f_sorted) != dict.end()) + { + dict.erase(f_sorted); + } + else + { + dict.insert(std::make_pair(f_sorted, f)); + } + } + } + + for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) + { + std::vector<int> f = it->second; + psb->appendFace(f[0], f[1], f[2]); + } } +//Write the surface mesh to an obj file. void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) { - std::ofstream fs; - fs.open(filename); - btAssert(fs); - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - fs << "v"; - for (int d = 0; d < 3; d++) - { - fs << " " << psb->m_nodes[i].m_x[d]; - } - fs << "\n"; - } - - for (int i = 0; i < psb->m_faces.size(); ++i) - { - fs << "f"; - for (int n = 0; n < 3; n++) - { - fs << " " << psb->m_faces[i].m_n[n]->index + 1; - } - fs << "\n"; - } - fs.close(); + std::ofstream fs; + fs.open(filename); + btAssert(fs); + + if (psb->m_tetras.size() > 0) + { + // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/ + std::map<int, int> dict; + for (int i = 0; i < psb->m_faces.size(); i++) + { + for (int d = 0; d < 3; d++) + { + int index = psb->m_faces[i].m_n[d]->index; + if (dict.find(index) == dict.end()) + { + int dict_size = dict.size(); + dict[index] = dict_size; + fs << "v"; + for (int k = 0; k < 3; k++) + { + fs << " " << psb->m_nodes[index].m_x[k]; + } + fs << "\n"; + } + } + } + // Write surface mesh. + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1; + } + fs << "\n"; + } + } + else + { + // For trimesh, directly write out all the nodes and faces.xs + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_x[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << psb->m_faces[i].m_n[n]->index + 1; + } + fs << "\n"; + } + } + fs.close(); } void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) { - std::ifstream fs_read; - fs_read.open(filename); - std::string line; - btVector3 pos; - btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; - while (std::getline(fs_read, line)) - { - std::stringstream ss(line); - if (line[0] == 'v') - { - } - else if (line[0] == 'f') - { - ss.ignore(); - int id0, id1, id2; - ss >> id0; - ss >> id1; - ss >> id2; - btAlignedObjectArray<int> new_face; - new_face.push_back(id1); - new_face.push_back(id0); - new_face.push_back(id2); - additional_faces.push_back(new_face); - } - } - fs_read.close(); - - std::ofstream fs_write; - fs_write.open(filename, std::ios_base::app); - for (int i = 0; i < additional_faces.size(); ++i) - { - fs_write << "f"; - for (int n = 0; n < 3; n++) - { - fs_write << " " << additional_faces[i][n]; - } - fs_write << "\n"; - } - fs_write.close(); + std::ifstream fs_read; + fs_read.open(filename); + std::string line; + btVector3 pos; + btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; + while (std::getline(fs_read, line)) + { + std::stringstream ss(line); + if (line[0] == 'v') + { + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btAlignedObjectArray<int> new_face; + new_face.push_back(id1); + new_face.push_back(id0); + new_face.push_back(id2); + additional_faces.push_back(new_face); + } + } + fs_read.close(); + + std::ofstream fs_write; + fs_write.open(filename, std::ios_base::app); + for (int i = 0; i < additional_faces.size(); ++i) + { + fs_write << "f"; + for (int n = 0; n < 3; n++) + { + fs_write << " " << additional_faces[i][n]; + } + fs_write << "\n"; + } + fs_write.close(); } // Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) { - btVector3 vap = p - a; - btVector3 vbp = p - b; - - btVector3 vab = b - a; - btVector3 vac = c - a; - btVector3 vad = d - a; - - btVector3 vbc = c - b; - btVector3 vbd = d - b; - btScalar va6 = (vbp.cross(vbd)).dot(vbc); - btScalar vb6 = (vap.cross(vac)).dot(vad); - btScalar vc6 = (vap.cross(vad)).dot(vab); - btScalar vd6 = (vap.cross(vab)).dot(vac); - btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); - bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); + btVector3 vap = p - a; + btVector3 vbp = p - b; + + btVector3 vab = b - a; + btVector3 vac = c - a; + btVector3 vad = d - a; + + btVector3 vbc = c - b; + btVector3 vbd = d - b; + btScalar va6 = (vbp.cross(vbd)).dot(vbc); + btScalar vb6 = (vap.cross(vac)).dot(vad); + btScalar vc6 = (vap.cross(vad)).dot(vab); + btScalar vd6 = (vap.cross(vab)).dot(vac); + btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); + bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); } // Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0. void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary) { - btVector3 v0 = b - a, v1 = c - a, v2 = p - a; - btScalar d00 = btDot(v0, v0); - btScalar d01 = btDot(v0, v1); - btScalar d11 = btDot(v1, v1); - btScalar d20 = btDot(v2, v0); - btScalar d21 = btDot(v2, v1); - btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); - bary[1] = (d11 * d20 - d01 * d21) * invDenom; - bary[2] = (d00 * d21 - d01 * d20) * invDenom; - bary[0] = 1.0 - bary[1] - bary[2]; - bary[3] = 0; + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = btDot(v0, v0); + btScalar d01 = btDot(v0, v1); + btScalar d11 = btDot(v1, v1); + btScalar d20 = btDot(v2, v0); + btScalar d21 = btDot(v2, v1); + btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); + bary[1] = (d11 * d20 - d01 * d21) * invDenom; + bary[2] = (d00 * d21 - d01 * d20) * invDenom; + bary[0] = 1.0 - bary[1] - bary[2]; + bary[3] = 0; } // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) { - psb->m_z.resize(0); - psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); - psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); - for (int i = 0; i < psb->m_renderNodes.size(); ++i) - { - const btVector3& p = psb->m_renderNodes[i].m_x; - btVector4 bary; - btVector4 optimal_bary; - btScalar min_bary_weight = -1e3; - btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - const btSoftBody::Tetra& t = psb->m_tetras[j]; - getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); - btScalar new_min_bary_weight = bary[0]; - for (int k = 1; k < 4; ++k) - { - new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); - } - if (new_min_bary_weight > min_bary_weight) - { - btAlignedObjectArray<const btSoftBody::Node*> parents; - parents.push_back(t.m_n[0]); - parents.push_back(t.m_n[1]); - parents.push_back(t.m_n[2]); - parents.push_back(t.m_n[3]); - optimal_parents = parents; - optimal_bary = bary; - min_bary_weight = new_min_bary_weight; - // stop searching if p is inside the tetrahedron at hand - if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.) - { - break; - } - } - } - psb->m_renderNodesInterpolationWeights[i] = optimal_bary; - psb->m_renderNodesParents[i] = optimal_parents; - } + psb->m_z.resize(0); + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -1e3; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + const btSoftBody::Tetra& t = psb->m_tetras[j]; + getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 4; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + if (new_min_bary_weight > min_bary_weight) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(t.m_n[0]); + parents.push_back(t.m_n[1]); + parents.push_back(t.m_n[2]); + parents.push_back(t.m_n[3]); + optimal_parents = parents; + optimal_bary = bary; + min_bary_weight = new_min_bary_weight; + // stop searching if p is inside the tetrahedron at hand + if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.) + { + break; + } + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + } } - // Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense. void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) { - psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); - psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); - psb->m_z.resize(psb->m_renderNodes.size()); - for (int i = 0; i < psb->m_renderNodes.size(); ++i) - { - const btVector3& p = psb->m_renderNodes[i].m_x; - btVector4 bary; - btVector4 optimal_bary; - btScalar min_bary_weight = -SIMD_INFINITY; - btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; - btScalar dist = 0, optimal_dist = 0; - for (int j = 0; j < psb->m_faces.size(); ++j) - { - const btSoftBody::Face& f = psb->m_faces[j]; - btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); - btVector3 unit_n = n.normalized(); - dist = (p-f.m_n[0]->m_x).dot(unit_n); - btVector3 proj_p = p - dist*unit_n; - getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); - btScalar new_min_bary_weight = bary[0]; - for (int k = 1; k < 3; ++k) - { - new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); - } - - // p is out of the current best triangle, we found a traingle that's better - bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.); - // p is inside of the current best triangle, we found a triangle that's better - bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist)); - - if (better_than_closest_outisde || better_than_best_inside) - { - btAlignedObjectArray<const btSoftBody::Node*> parents; - parents.push_back(f.m_n[0]); - parents.push_back(f.m_n[1]); - parents.push_back(f.m_n[2]); - optimal_parents = parents; - optimal_bary = bary; - optimal_dist = dist; - min_bary_weight = new_min_bary_weight; - } - } - psb->m_renderNodesInterpolationWeights[i] = optimal_bary; - psb->m_renderNodesParents[i] = optimal_parents; - psb->m_z[i] = optimal_dist; - } + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + psb->m_z.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -SIMD_INFINITY; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + btScalar dist = 0, optimal_dist = 0; + for (int j = 0; j < psb->m_faces.size(); ++j) + { + const btSoftBody::Face& f = psb->m_faces[j]; + btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + btVector3 unit_n = n.normalized(); + dist = (p - f.m_n[0]->m_x).dot(unit_n); + btVector3 proj_p = p - dist * unit_n; + getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 3; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + + // p is out of the current best triangle, we found a traingle that's better + bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.); + // p is inside of the current best triangle, we found a triangle that's better + bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist)); + + if (better_than_closest_outisde || better_than_best_inside) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(f.m_n[0]); + parents.push_back(f.m_n[1]); + parents.push_back(f.m_n[2]); + optimal_parents = parents; + optimal_bary = bary; + optimal_dist = dist; + min_bary_weight = new_min_bary_weight; + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + psb->m_z[i] = optimal_dist; + } } diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index abe187089..237d29761 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -93,7 +93,7 @@ struct btSoftBodyHelpers int resy, int fixeds, bool gendiags, - btScalar perturbation = 0.); + btScalar perturbation = 0.); /* Create a patch with UV Texture Coordinates */ static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00, @@ -142,21 +142,21 @@ struct btSoftBodyHelpers bool bfacelinks, bool btetralinks, bool bfacesfromtetras); - static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); - static void writeObj(const char* file, const btSoftBody* psb); - - static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); - - static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); - - static void interpolateBarycentricWeights(btSoftBody* psb); - - static void extrapolateBarycentricWeights(btSoftBody* psb); - - static void generateBoundaryFaces(btSoftBody* psb); - - static void duplicateFaces(const char* filename, const btSoftBody* psb); + static void writeObj(const char* file, const btSoftBody* psb); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); + + static void interpolateBarycentricWeights(btSoftBody* psb); + + static void extrapolateBarycentricWeights(btSoftBody* psb); + + static void generateBoundaryFaces(btSoftBody* psb); + + static void duplicateFaces(const char* filename, const btSoftBody* psb); /// Sort the list of links to move link calculations that are dependent upon earlier /// ones as far as possible away from the calculation of those values /// This tends to make adjacent loop iterations not dependent upon one another, diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index c256d9918..8d13b39bd 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -32,86 +32,85 @@ subject to the following restrictions: // Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, - btMultiBodyJacobianData& jacobianData, - const btVector3& contact_point, - const btVector3& dir) -{ - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - jacobianData.m_jacobians.resize(ndof); - jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); - btScalar* jac = &jacobianData.m_jacobians[0]; - - multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); - multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); } static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u) { - btScalar ux = u.getX(); - btScalar uy = u.getY(); - btScalar uz = u.getZ(); - btScalar ax = std::abs(ux); - btScalar ay = std::abs(uy); - btScalar az = std::abs(uz); - btVector3 v; - if (ax <= ay && ax <= az) - v = btVector3(0, -uz, uy); - else if (ay <= ax && ay <= az) - v = btVector3(-uz, 0, ux); - else - v = btVector3(-uy, ux, 0); - v.normalize(); - return v; + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; } static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary) { - btVector3 x43 = x4-x3; - if (std::abs(x43.dot(normal)) > mrg) - return false; - btVector3 x13 = x1-x3; - btVector3 x23 = x2-x3; - btScalar a11 = x13.length2(); - btScalar a22 = x23.length2(); - btScalar a12 = x13.dot(x23); - btScalar b1 = x13.dot(x43); - btScalar b2 = x23.dot(x43); - btScalar det = a11*a22 - a12*a12; - if (det < SIMD_EPSILON) - return false; - btScalar w1 = (b1*a22-b2*a12)/det; - btScalar w2 = (b2*a11-b1*a12)/det; - btScalar w3 = 1-w1-w2; - btScalar delta = mrg / std::sqrt(0.5*std::abs(x13.cross(x23).safeNorm())); - bary = btVector3(w1,w2,w3); - for (int i = 0; i < 3; ++i) - { - if (bary[i] < -delta || bary[i] > 1+delta) - return false; - } - return true; + btVector3 x43 = x4 - x3; + if (std::abs(x43.dot(normal)) > mrg) + return false; + btVector3 x13 = x1 - x3; + btVector3 x23 = x2 - x3; + btScalar a11 = x13.length2(); + btScalar a22 = x23.length2(); + btScalar a12 = x13.dot(x23); + btScalar b1 = x13.dot(x43); + btScalar b2 = x23.dot(x43); + btScalar det = a11 * a22 - a12 * a12; + if (det < SIMD_EPSILON) + return false; + btScalar w1 = (b1 * a22 - b2 * a12) / det; + btScalar w2 = (b2 * a11 - b1 * a12) / det; + btScalar w3 = 1 - w1 - w2; + btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm())); + bary = btVector3(w1, w2, w3); + for (int i = 0; i < 3; ++i) + { + if (bary[i] < -delta || bary[i] > 1 + delta) + return false; + } + return true; } static const int KDOP_COUNT = 13; -static btVector3 dop[KDOP_COUNT]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1), - btVector3(1,1,0), - btVector3(1,0,1), - btVector3(0,1,1), - btVector3(1,-1,0), - btVector3(1,0,-1), - btVector3(0,1,-1), - btVector3(1,1,1), - btVector3(1,-1,1), - btVector3(1,1,-1), - btVector3(1,-1,-1) -}; +static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1), + btVector3(1, 1, 0), + btVector3(1, 0, 1), + btVector3(0, 1, 1), + btVector3(1, -1, 0), + btVector3(1, 0, -1), + btVector3(0, 1, -1), + btVector3(1, 1, 1), + btVector3(1, -1, 1), + btVector3(1, 1, -1), + btVector3(1, -1, -1)}; static inline int getSign(const btVector3& n, const btVector3& x) { btScalar d = n.dot(x); - if (d>SIMD_EPSILON) + if (d > SIMD_EPSILON) return 1; - if (d<-SIMD_EPSILON) + if (d < -SIMD_EPSILON) return -1; return 0; } @@ -119,13 +118,12 @@ static inline int getSign(const btVector3& n, const btVector3& x) static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x, - face->m_n[1]->m_x - node->m_x, - face->m_n[2]->m_x - node->m_x, - face->m_n[0]->m_x + dt*face->m_n[0]->m_v - node->m_x, - face->m_n[1]->m_x + dt*face->m_n[1]->m_v - node->m_x, - face->m_n[2]->m_x + dt*face->m_n[2]->m_v - node->m_x - }; - btVector3 segment = dt*node->m_v; + face->m_n[1]->m_x - node->m_x, + face->m_n[2]->m_x - node->m_x, + face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x, + face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x, + face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x}; + btVector3 segment = dt * node->m_v; for (int i = 0; i < KDOP_COUNT; ++i) { int s = getSign(dop[i], segment); @@ -143,488 +141,494 @@ static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, c static SIMD_FORCE_INLINE bool nearZero(const btScalar& a) { - return (a>-SAFE_EPSILON && a<SAFE_EPSILON); + return (a > -SAFE_EPSILON && a < SAFE_EPSILON); } static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b) { - return (nearZero(a) || nearZero(b) || (a>SAFE_EPSILON && b>SAFE_EPSILON) || (a<-SAFE_EPSILON && b<-SAFE_EPSILON)); + return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON)); } static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b) { - return !sameSign(a, b); -} -inline btScalar evaluateBezier2(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &t, const btScalar &s) -{ - btScalar s2 = s*s; - btScalar t2 = t*t; - - return p0*s2+p1*btScalar(2.0)*s*t+p2*t2; -} -inline btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &p3, const btScalar &t, const btScalar &s) -{ - btScalar s2 = s*s; - btScalar s3 = s2*s; - btScalar t2 = t*t; - btScalar t3 = t2*t; - - return p0*s3+p1*btScalar(3.0)*s2*t+p2*btScalar(3.0)*s*t2+p3*t3; -} -static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar <0, btScalar <1) -{ - if (sameSign(t0, t1)) { - lt0 = t0; - lt1 = t0; - return true; - } - - if (type_c || diffSign(k0, k3)) { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0<-0) - ft = -ft; - - if (sameSign(ft, k0)) { - lt0 = t1; - lt1 = t1; - } - else { - lt0 = t0; - lt1 = t0; - } - return true; - } - - if (!type_c) { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0<-0) - ft = -ft; - - if (diffSign(ft, k0)) { - lt0 = t0; - lt1 = t1; - return true; - } - - btScalar fk = evaluateBezier2(k1-k0, k2-k1, k3-k2, t0, -t1); - - if (sameSign(fk, k1-k0)) - lt0 = lt1 = t1; - else - lt0 = lt1 = t0; - - return true; - } - return false; + return !sameSign(a, b); +} +inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar t2 = t * t; + + return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2; +} +inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar s3 = s2 * s; + btScalar t2 = t * t; + btScalar t3 = t2 * t; + + return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3; +} +static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1) +{ + if (sameSign(t0, t1)) + { + lt0 = t0; + lt1 = t0; + return true; + } + + if (type_c || diffSign(k0, k3)) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (sameSign(ft, k0)) + { + lt0 = t1; + lt1 = t1; + } + else + { + lt0 = t0; + lt1 = t0; + } + return true; + } + + if (!type_c) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (diffSign(ft, k0)) + { + lt0 = t0; + lt1 = t1; + return true; + } + + btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1); + + if (sameSign(fk, k1 - k0)) + lt0 = lt1 = t1; + else + lt0 = lt1 = t0; + + return true; + } + return false; } static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3) { - const btVector3& n0 = face->m_n0; - const btVector3& n1 = face->m_n1; - btVector3 n_hat = n0 + n1 - face->m_vn; - btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; - btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; - k0 = (p0ma0).dot(n0) * 3.0; - k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); - k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); - k3 = (p1ma1).dot(n1) * 3.0; + const btVector3& n0 = face->m_n0; + const btVector3& n1 = face->m_n1; + btVector3 n_hat = n0 + n1 - face->m_vn; + btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; + btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; + k0 = (p0ma0).dot(n0) * 3.0; + k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); + k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); + k3 = (p1ma1).dot(n1) * 3.0; } static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1) { - btScalar denom = 4.0 * (j1-j2) * (j1-j0) + (j2-j0) * (j2-j0); - u0 = (2.0*(j1-j2)*(3.0*k1-2.0*k0-k3) - (j0-j2)*(3.0*k2-2.0*k3-k0)) / denom; - u1 = (2.0*(j1-j0)*(3.0*k2-2.0*k3-k0) - (j2-j0)*(3.0*k1-2.0*k0-k3)) / denom; - v0 = k0-u0*j0; - v1 = k3-u1*j2; + btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0); + u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom; + u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom; + v0 = k0 - u0 * j0; + v1 = k3 - u1 * j2; } static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3) { - btScalar u0, u1, v0, v1; - btScalar j0 = 3.0*(k1-k0); - btScalar j1 = 3.0*(k2-k1); - btScalar j2 = 3.0*(k3-k2); - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (sameSign(v0, v1)) - { - btScalar Ypa = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*v0*(1.0-v0) + j2*v0*v0; // Y'(v0) - if (sameSign(Ypa, j0)) - { - return (diffSign(k0,v1)); - } - } - return diffSign(k0,v0); -} - -static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) -{ - const btVector3& a0 = a->m_x; - const btVector3& b0 = b->m_x; - const btVector3& c0 = c->m_x; - const btVector3& va = a->m_v; - const btVector3& vb = b->m_v; - const btVector3& vc = c->m_v; - const btVector3 a1 = a0 + dt*va; - const btVector3 b1 = b0 + dt*vb; - const btVector3 c1 = c0 + dt*vc; - btVector3 n0 = (b0-a0).cross(c0-a0); - btVector3 n1 = (b1-a1).cross(c1-a1); - btVector3 n_hat = n0+n1 - dt*dt*(vb-va).cross(vc-va); - const btVector3& p0 = p->m_x; - const btVector3& vp = p->m_v; - btVector3 p1 = p0 + dt*vp; - btVector3 m0 = (b0-p0).cross(c0-p0); - btVector3 m1 = (b1-p1).cross(c1-p1); - btVector3 m_hat = m0+m1 - dt*dt*(vb-vp).cross(vc-vp); - btScalar l0 = m0.dot(n0); - btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); - btScalar l2 = btScalar(1)/btScalar(6)*(m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); - btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); - btScalar l4 = m1.dot(n1); - - btScalar k1p = 0.25 * k0 + 0.75 * k1; - btScalar k2p = 0.5 * k1 + 0.5 * k2; - btScalar k3p = 0.75 * k2 + 0.25 * k3; - - btScalar s0 = (l1 * k0 - l0 * k1p)*4.0; - btScalar s1 = (l2 * k0 - l0 * k2p)*2.0; - btScalar s2 = (l3 * k0 - l0 * k3p)*btScalar(4)/btScalar(3); - btScalar s3 = l4 * k0 - l0 * k3; - - j0 = (s1*k0 - s0*k1) * 3.0; - j1 = (s2*k0 - s0*k2) * 1.5; - j2 = (s3*k0 - s0*k3); + btScalar u0, u1, v0, v1; + btScalar j0 = 3.0 * (k1 - k0); + btScalar j1 = 3.0 * (k2 - k1); + btScalar j2 = 3.0 * (k3 - k2); + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (sameSign(v0, v1)) + { + btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0; // Y'(v0) + if (sameSign(Ypa, j0)) + { + return (diffSign(k0, v1)); + } + } + return diffSign(k0, v0); +} + +static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) +{ + const btVector3& a0 = a->m_x; + const btVector3& b0 = b->m_x; + const btVector3& c0 = c->m_x; + const btVector3& va = a->m_v; + const btVector3& vb = b->m_v; + const btVector3& vc = c->m_v; + const btVector3 a1 = a0 + dt * va; + const btVector3 b1 = b0 + dt * vb; + const btVector3 c1 = c0 + dt * vc; + btVector3 n0 = (b0 - a0).cross(c0 - a0); + btVector3 n1 = (b1 - a1).cross(c1 - a1); + btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va); + const btVector3& p0 = p->m_x; + const btVector3& vp = p->m_v; + btVector3 p1 = p0 + dt * vp; + btVector3 m0 = (b0 - p0).cross(c0 - p0); + btVector3 m1 = (b1 - p1).cross(c1 - p1); + btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp); + btScalar l0 = m0.dot(n0); + btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); + btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); + btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); + btScalar l4 = m1.dot(n1); + + btScalar k1p = 0.25 * k0 + 0.75 * k1; + btScalar k2p = 0.5 * k1 + 0.5 * k2; + btScalar k3p = 0.75 * k2 + 0.25 * k3; + + btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0; + btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0; + btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3); + btScalar s3 = l4 * k0 - l0 * k3; + + j0 = (s1 * k0 - s0 * k1) * 3.0; + j1 = (s2 * k0 - s0 * k2) * 1.5; + j2 = (s3 * k0 - s0 * k3); } static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) { - btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0) - btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(v0) + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(v0) - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; - btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K = (sameSign(sign_Ytp,k0)) ? v1 : v0; - return diffSign(L,K); + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; + btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0; + return diffSign(L, K); } static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) { - btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0) - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; - if (diffSign(sign_Ytp,k0)) - { - L1 = u0; - L2 = u1; - } - else - { - btScalar Yp_u0 = j0*(1.0-u0)*(1.0-u0) + 2.0*j1*(1.0-u0)*u0 + j2*u0*u0; - if (sameSign(Yp_u0,j0)) - { - L1 = u1; - L2 = u1; - } - else - { - L1 = u0; - L2 = u0; - } - } - btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(uv0) - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K1, K2; - if (diffSign(sign_Ytp,k0)) - { - K1 = v0; - K2 = v1; - } - else - { - btScalar Yp_v0 = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*(1.0-v0)*v0 + j2*v0*v0; - if (sameSign(Yp_v0,j0)) - { - K1 = v1; - K2 = v1; - } - else - { - K1 = v0; - K2 = v0; - } - } - return (diffSign(K1, L1) || diffSign(L2, K2)); + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; + if (diffSign(sign_Ytp, k0)) + { + L1 = u0; + L2 = u1; + } + else + { + btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0; + if (sameSign(Yp_u0, j0)) + { + L1 = u1; + L2 = u1; + } + else + { + L1 = u0; + L2 = u0; + } + } + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(uv0) + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K1, K2; + if (diffSign(sign_Ytp, k0)) + { + K1 = v0; + K2 = v1; + } + else + { + btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0; + if (sameSign(Yp_v0, j0)) + { + K1 = v1; + K2 = v1; + } + else + { + K1 = v0; + K2 = v0; + } + } + return (diffSign(K1, L1) || diffSign(L2, K2)); } static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - // p2 - getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - // p3 - getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - return true; + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + return true; } static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0 = true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - // p2 - getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0=true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - // p3 - getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0=true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - return true; + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + return true; } static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - // Coplanar test - if (diffSign(k1-k0, k3-k2)) - { - // Case b: - if (sameSign(k0, k3) && !rootFindingLemma(k0,k1,k2,k3)) - return false; - // inside test - return signDetermination2(k0, k1, k2, k3, face, node, dt); - } - else - { - // Case c: - if (sameSign(k0, k3)) - return false; - // inside test - return signDetermination1(k0, k1, k2, k3, face, node, dt); - } - return false; + // Coplanar test + if (diffSign(k1 - k0, k3 - k2)) + { + // Case b: + if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3)) + return false; + // inside test + return signDetermination2(k0, k1, k2, k3, face, node, dt); + } + else + { + // Case c: + if (sameSign(k0, k3)) + return false; + // inside test + return signDetermination1(k0, k1, k2, k3, face, node, dt); + } + return false; } static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg) { - if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) - return true; - if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) - return true; - return false; + if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) + return true; + if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) + return true; + return false; } static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); } static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12) { - k10 = k0*(1.0-t0) + k1*t0; - btScalar k11 = k1*(1.0-t0) + k2*t0; - k12 = k2*(1.0-t0) + k3*t0; - k20 = k10*(1.0-t0) + k11*t0; - k21 = k11*(1.0-t0) + k12*t0; - k30 = k20*(1.0-t0) + k21*t0; + k10 = k0 * (1.0 - t0) + k1 * t0; + btScalar k11 = k1 * (1.0 - t0) + k2 * t0; + k12 = k2 * (1.0 - t0) + k3 * t0; + k20 = k10 * (1.0 - t0) + k11 * t0; + k21 = k11 * (1.0 - t0) + k12 * t0; + k30 = k20 * (1.0 - t0) + k21 * t0; } static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg) { - btScalar k0, k1, k2, k3; - getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return true; - if (diffSign(k2-2.0*k1+k0, k3-2.0*k2+k1)) - { - btScalar k10, k20, k30, k21, k12; - btScalar t0 = (k2-2.0*k1+k0)/(k0-3.0*k1+3.0*k2-k3); - deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); - return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); - } - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); + btScalar k0, k1, k2, k3; + getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return true; + if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1)) + { + btScalar k10, k20, k30, k21, k12; + btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3); + deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); + return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); + } + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); } static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) { - if (hasSeparatingPlane(face, node, dt)) - return false; - btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; - btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; - btVector3 x41 = node->m_x - face->m_n[0]->m_x; - btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; - btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; - btVector3 v41 = node->m_v - face->m_n[0]->m_v; - btVector3 a = x21.cross(x31); - btVector3 b = x21.cross(v31) + v21.cross(x31); - btVector3 c = v21.cross(v31); - btVector3 d = x41; - btVector3 e = v41; - btScalar a0 = a.dot(d); - btScalar a1 = a.dot(e) + b.dot(d); - btScalar a2 = c.dot(d) + b.dot(e); - btScalar a3 = c.dot(e); - btScalar eps = SAFE_EPSILON; - int num_roots = 0; - btScalar roots[3]; - if (std::abs(a3) < eps) - { - // cubic term is zero - if (std::abs(a2) < eps) - { - if (std::abs(a1) < eps) - { - if (std::abs(a0) < eps) - { - num_roots = 2; - roots[0] = 0; - roots[1] = dt; - } - } - else - { - num_roots = 1; - roots[0] = -a0/a1; - } - } - else - { - num_roots = SolveP2(roots, a1/a2, a0/a2); - } - } - else - { - num_roots = SolveP3(roots, a2/a3, a1/a3, a0/a3); - } -// std::sort(roots, roots+num_roots); - if (num_roots > 1) - { - if (roots[0] > roots[1]) - btSwap(roots[0], roots[1]); - } - if (num_roots > 2) - { - if (roots[0] > roots[2]) - btSwap(roots[0], roots[2]); - if (roots[1] > roots[2]) - btSwap(roots[1], roots[2]); - } - for (int r = 0; r < num_roots; ++r) - { - double root = roots[r]; - if (root <= 0) - continue; - if (root > dt + SIMD_EPSILON) - return false; - btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; - btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; - btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; - btVector3 x4 = node->m_x + root * node->m_v; - btVector3 normal = (x2-x1).cross(x3-x1); - normal.safeNormalize(); - if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) - return true; - } - return false; + if (hasSeparatingPlane(face, node, dt)) + return false; + btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; + btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; + btVector3 x41 = node->m_x - face->m_n[0]->m_x; + btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; + btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; + btVector3 v41 = node->m_v - face->m_n[0]->m_v; + btVector3 a = x21.cross(x31); + btVector3 b = x21.cross(v31) + v21.cross(x31); + btVector3 c = v21.cross(v31); + btVector3 d = x41; + btVector3 e = v41; + btScalar a0 = a.dot(d); + btScalar a1 = a.dot(e) + b.dot(d); + btScalar a2 = c.dot(d) + b.dot(e); + btScalar a3 = c.dot(e); + btScalar eps = SAFE_EPSILON; + int num_roots = 0; + btScalar roots[3]; + if (std::abs(a3) < eps) + { + // cubic term is zero + if (std::abs(a2) < eps) + { + if (std::abs(a1) < eps) + { + if (std::abs(a0) < eps) + { + num_roots = 2; + roots[0] = 0; + roots[1] = dt; + } + } + else + { + num_roots = 1; + roots[0] = -a0 / a1; + } + } + else + { + num_roots = SolveP2(roots, a1 / a2, a0 / a2); + } + } + else + { + num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3); + } + // std::sort(roots, roots+num_roots); + if (num_roots > 1) + { + if (roots[0] > roots[1]) + btSwap(roots[0], roots[1]); + } + if (num_roots > 2) + { + if (roots[0] > roots[2]) + btSwap(roots[0], roots[2]); + if (roots[1] > roots[2]) + btSwap(roots[1], roots[2]); + } + for (int r = 0; r < num_roots; ++r) + { + double root = roots[r]; + if (root <= 0) + continue; + if (root > dt + SIMD_EPSILON) + return false; + btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; + btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; + btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; + btVector3 x4 = node->m_x + root * node->m_v; + btVector3 normal = (x2 - x1).cross(x3 - x1); + normal.safeNormalize(); + if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) + return true; + } + return false; } static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) { - if (!bernsteinVFTest(face, node, dt, mrg)) - return false; - if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) - return false; - return true; + if (!bernsteinVFTest(face, node, dt, mrg)) + return false; + if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) + return false; + return true; } // @@ -902,62 +906,61 @@ static inline btMatrix3x3 Diagonal(btScalar x) static inline btMatrix3x3 Diagonal(const btVector3& v) { - btMatrix3x3 m; - m[0] = btVector3(v.getX(), 0, 0); - m[1] = btVector3(0, v.getY(), 0); - m[2] = btVector3(0, 0, v.getZ()); - return (m); -} - -static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof) -{ - btScalar result = 0; - for (int i = 0; i < ndof; ++i) - result += a[i] * b[i]; - return result; -} - -static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3, - const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) -{ - btMatrix3x3 m; - btScalar a11 = Dot(v1,u1,ndof); - btScalar a12 = Dot(v1,u2,ndof); - btScalar a13 = Dot(v1,u3,ndof); - - btScalar a21 = Dot(v2,u1,ndof); - btScalar a22 = Dot(v2,u2,ndof); - btScalar a23 = Dot(v2,u3,ndof); - - btScalar a31 = Dot(v3,u1,ndof); - btScalar a32 = Dot(v3,u2,ndof); - btScalar a33 = Dot(v3,u3,ndof); - m[0] = btVector3(a11, a12, a13); - m[1] = btVector3(a21, a22, a23); - m[2] = btVector3(a31, a32, a33); - return (m); -} - -static inline btMatrix3x3 OuterProduct(const btVector3& v1,const btVector3& v2) -{ - btMatrix3x3 m; - btScalar a11 = v1[0] * v2[0]; - btScalar a12 = v1[0] * v2[1]; - btScalar a13 = v1[0] * v2[2]; - - btScalar a21 = v1[1] * v2[0]; - btScalar a22 = v1[1] * v2[1]; - btScalar a23 = v1[1] * v2[2]; - - btScalar a31 = v1[2] * v2[0]; - btScalar a32 = v1[2] * v2[1]; - btScalar a33 = v1[2] * v2[2]; - m[0] = btVector3(a11, a12, a13); - m[1] = btVector3(a21, a22, a23); - m[2] = btVector3(a31, a32, a33); - return (m); + btMatrix3x3 m; + m[0] = btVector3(v.getX(), 0, 0); + m[1] = btVector3(0, v.getY(), 0); + m[2] = btVector3(0, 0, v.getZ()); + return (m); +} + +static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof) +{ + btScalar result = 0; + for (int i = 0; i < ndof; ++i) + result += a[i] * b[i]; + return result; } +static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3, + const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) +{ + btMatrix3x3 m; + btScalar a11 = Dot(v1, u1, ndof); + btScalar a12 = Dot(v1, u2, ndof); + btScalar a13 = Dot(v1, u3, ndof); + + btScalar a21 = Dot(v2, u1, ndof); + btScalar a22 = Dot(v2, u2, ndof); + btScalar a23 = Dot(v2, u3, ndof); + + btScalar a31 = Dot(v3, u1, ndof); + btScalar a32 = Dot(v3, u2, ndof); + btScalar a33 = Dot(v3, u3, ndof); + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} + +static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2) +{ + btMatrix3x3 m; + btScalar a11 = v1[0] * v2[0]; + btScalar a12 = v1[0] * v2[1]; + btScalar a13 = v1[0] * v2[2]; + + btScalar a21 = v1[1] * v2[0]; + btScalar a22 = v1[1] * v2[1]; + btScalar a23 = v1[1] * v2[2]; + + btScalar a31 = v1[2] * v2[0]; + btScalar a32 = v1[2] * v2[1]; + btScalar a33 = v1[2] * v2[2]; + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} // static inline btMatrix3x3 Add(const btMatrix3x3& a, @@ -1008,6 +1011,20 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt, } // +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + const btMatrix3x3& effective_mass_inv, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) +{ + return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse()); + // btMatrix3x3 iimb = MassMatrix(imb, iwi, r); + // if (iimb.determinant() == 0) + // return effective_mass_inv.inverse(); + // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse(); +} + +// static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra, btScalar imb, const btMatrix3x3& iib, const btVector3& rb) { @@ -1091,73 +1108,70 @@ static inline void ProjectOrigin(const btVector3& a, // static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t) { - btScalar a, f, u, v; - - btVector3 e1 = v1 - v0; - btVector3 e2 = v2 - v0; - btVector3 h = dir.cross(e2); - a = e1.dot(h); - - if (a > -0.00001 && a < 0.00001) - return (false); - - f = btScalar(1) / a; - btVector3 s = origin - v0; - u = f * s.dot(h); - - if (u < 0.0 || u > 1.0) - return (false); - - btVector3 q = s.cross(e1); - v = f * dir.dot(q); - if (v < 0.0 || u + v > 1.0) - return (false); - // at this stage we can compute t to find out where - // the intersection point is on the line - t = f * e2.dot(q); - if (t > 0) // ray intersection - return (true); - else // this means that there is a line intersection - // but not a ray intersection - return (false); + btScalar a, f, u, v; + + btVector3 e1 = v1 - v0; + btVector3 e2 = v2 - v0; + btVector3 h = dir.cross(e2); + a = e1.dot(h); + + if (a > -0.00001 && a < 0.00001) + return (false); + + f = btScalar(1) / a; + btVector3 s = origin - v0; + u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) + return (false); + + btVector3 q = s.cross(e1); + v = f * dir.dot(q); + if (v < 0.0 || u + v > 1.0) + return (false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * e2.dot(q); + if (t > 0) // ray intersection + return (true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); } static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal) { - btVector3 dir = rayEnd - rayStart; - btScalar dir_norm = dir.norm(); - if (dir_norm < SIMD_EPSILON) - return false; - dir.normalize(); - - btScalar t; - - bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); - - if (ret) - { - if (t <= dir_norm) - { - sect = rayStart + dir * t; - } - else - { - ret = false; - } - } - - if (ret) - { - btVector3 n = (p3-p1).cross(p2-p1); - n.safeNormalize(); - if (n.dot(dir) < 0) - normal = n; - else - normal = -n; - } - return ret; -} + btVector3 dir = rayEnd - rayStart; + btScalar dir_norm = dir.norm(); + if (dir_norm < SIMD_EPSILON) + return false; + dir.normalize(); + btScalar t; + bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); + if (ret) + { + if (t <= dir_norm) + { + sect = rayStart + dir * t; + } + else + { + ret = false; + } + } + + if (ret) + { + btVector3 n = (p3 - p1).cross(p2 - p1); + n.safeNormalize(); + if (n.dot(dir) < 0) + normal = n; + else + normal = -n; + } + return ret; +} // template <typename T> @@ -1586,57 +1600,57 @@ struct btSoftColliders psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); } }; - // - // CollideSDF_RS - // - struct CollideSDF_RS : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; - DoNode(*node); - } - void DoNode(btSoftBody::Node& n) const - { - const btScalar m = n.m_im > 0 ? dynmargin : stamargin; - btSoftBody::RContact c; - - if ((!n.m_battach) && - psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) - { - const btScalar ima = n.m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_c2 = ima * psb->m_sst.sdt; - c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } - } - } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; + + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; // // CollideSDF_RD @@ -1654,73 +1668,74 @@ struct btSoftColliders btSoftBody::DeformableNodeRigidContact c; if (!n.m_battach) - { + { // check for collision at x_{n+1}^* if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) - { - const btScalar ima = n.m_im; - // todo: collision between multibody and fixed deformable node will be missed. - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - n.m_constrained = true; - // resolve contact at x_n - psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); - btSoftBody::sCti& cti = c.m_cti; - c.m_node = &n; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 normal = cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - } - } - psb->m_nodeRigidContacts.push_back(c); - } - } + { + const btScalar ima = n.m_im; + // todo: collision between multibody and fixed deformable node will be missed. + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_node = &n; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); + c.m_c5 = n.m_effectiveMass_inv; + // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_nodeRigidContacts.push_back(c); + } + } } } btSoftBody* psb; @@ -1729,115 +1744,113 @@ struct btSoftColliders btScalar dynmargin; btScalar stamargin; }; - - // - // CollideSDF_RDF - // - struct CollideSDF_RDF : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; - DoNode(*face); - } - void DoNode(btSoftBody::Face& f) const - { - btSoftBody::Node* n0 = f.m_n[0]; - btSoftBody::Node* n1 = f.m_n[1]; - btSoftBody::Node* n2 = f.m_n[2]; - if (n0->m_constrained && n1->m_constrained && n2->m_constrained) - return; - const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; - btSoftBody::DeformableFaceRigidContact c; - btVector3 contact_point; - btVector3 bary; - if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) - { - f.m_pcontact[3] = 1; - btScalar ima = n0->m_im + n1->m_im + n2->m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - // todo: collision between multibody and fixed deformable face will be missed. - const btScalar ms = ima + imb; - if (ms > 0) - { - // resolve contact at x_n - psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); - btSoftBody::sCti& cti = c.m_cti; - c.m_contactPoint = contact_point; - c.m_bary = bary; - // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices - c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary; - c.m_face = &f; + + // + // CollideSDF_RDF + // + struct CollideSDF_RDF : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; + DoNode(*face); + } + void DoNode(btSoftBody::Face& f) const + { + btSoftBody::Node* n0 = f.m_n[0]; + btSoftBody::Node* n1 = f.m_n[1]; + btSoftBody::Node* n2 = f.m_n[2]; + const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin; + btSoftBody::DeformableFaceRigidContact c; + btVector3 contact_point; + btVector3 bary; + if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) + { + f.m_pcontact[3] = 1; + btScalar ima = n0->m_im + n1->m_im + n2->m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + // todo: collision between multibody and fixed deformable face will be missed. + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + // psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_contactPoint = contact_point; + c.m_bary = bary; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary; + c.m_face = &f; // friction is handled by the nodes to prevent sticking -// const btScalar fc = 0; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - - // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf - ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im; - - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = contact_point - wtr.getOrigin(); - - // we do not scale the impulse matrix by dt - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 normal = cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - } - } - psb->m_faceRigidContacts.push_back(c); - } - } - else - { - f.m_pcontact[3] = 0; - } - } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; - + // const btScalar fc = 0; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im; + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = Diagonal(ima); + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = contact_point - wtr.getOrigin(); + + // we do not scale the impulse matrix by dt + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_faceRigidContacts.push_back(c); + } + } + else + { + f.m_pcontact[3] = 0; + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // // CollideVF_SS // @@ -1848,12 +1861,12 @@ struct btSoftColliders { btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - for (int i = 0; i < 3; ++i) - { - if (face->m_n[i] == node) - continue; - } - + for (int i = 0; i < 3; ++i) + { + if (face->m_n[i] == node) + continue; + } + btVector3 o = node->m_x; btVector3 p; btScalar d = SIMD_INFINITY; @@ -1883,7 +1896,7 @@ struct btSoftColliders c.m_node = node; c.m_face = face; c.m_weights = w; - c.m_friction = btMax (psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR; c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR; psb[0]->m_scontacts.push_back(c); @@ -1893,206 +1906,205 @@ struct btSoftColliders btSoftBody* psb[2]; btScalar mrg; }; - - - // - // CollideVF_DD - // - struct CollideVF_DD : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - btVector3 bary; - if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - { - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - const btVector3 w = bary; - const btScalar ma = node->m_im; - btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); - if ((n[0]->m_im <= 0) || - (n[1]->m_im <= 0) || - (n[2]->m_im <= 0)) - { - mb = 0; - } - const btScalar ms = ma + mb; - if (ms > 0) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = w; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar mrg; - bool useFaceNormal; - }; - - // - // CollideFF_DD - // - struct CollideFF_DD : btDbvt::ICollide - { - void Process(const btDbvntNode* lface1, - const btDbvntNode* lface2) - { - btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 + + // + // CollideVF_DD + // + struct CollideVF_DD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 bary; + if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + { + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = bary; + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + if ((n[0]->m_im <= 0) || + (n[1]->m_im <= 0) || + (n[2]->m_im <= 0)) + { + mb = 0; + } + const btScalar ms = ma + mb; + if (ms > 0) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + // + // CollideFF_DD + // + struct CollideFF_DD : btDbvt::ICollide + { + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 #ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } #endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; #ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } #endif - btSoftBody::Face* face = f2; - btVector3 bary; - if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - continue; - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - btSoftBody* psb[2]; - btScalar mrg; - bool useFaceNormal; - }; - - struct CollideCCD : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - btVector3 bary; - if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - void Process(const btDbvntNode* lface1, - const btDbvntNode* lface2) - { - btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 + btSoftBody::Face* face = f2; + btVector3 bary; + if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + continue; + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + struct CollideCCD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 bary; + if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 #ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } #endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; #ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } #endif - btSoftBody::Face* face = f2; - btVector3 bary; + btSoftBody::Face* face = f2; + btVector3 bary; if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar dt, mrg; - bool useFaceNormal; - }; + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar dt, mrg; + bool useFaceNormal; + }; }; #endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/src/BulletSoftBody/btSoftBodySolvers.h b/src/BulletSoftBody/btSoftBodySolvers.h index c4ac4141a..dbb2624ee 100644 --- a/src/BulletSoftBody/btSoftBodySolvers.h +++ b/src/BulletSoftBody/btSoftBodySolvers.h @@ -36,7 +36,7 @@ public: CL_SIMD_SOLVER, DX_SOLVER, DX_SIMD_SOLVER, - DEFORMABLE_SOLVER + DEFORMABLE_SOLVER }; protected: diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index eb290a1db..d611726bc 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -22,36 +22,36 @@ subject to the following restrictions: // Fast Hash -#if !defined (get16bits) -#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\ -+(unsigned int)(((const unsigned char *)(d))[0]) ) +#if !defined(get16bits) +#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0])) #endif // // super hash function by Paul Hsieh // -inline unsigned int HsiehHash (const char * data, int len) { - unsigned int hash = len, tmp; - len>>=2; - - /* Main loop */ - for (;len > 0; len--) { - hash += get16bits (data); - tmp = (get16bits (data+2) << 11) ^ hash; - hash = (hash << 16) ^ tmp; - data += 2*sizeof (unsigned short); - hash += hash >> 11; - } +inline unsigned int HsiehHash(const char* data, int len) +{ + unsigned int hash = len, tmp; + len >>= 2; + /* Main loop */ + for (; len > 0; len--) + { + hash += get16bits(data); + tmp = (get16bits(data + 2) << 11) ^ hash; + hash = (hash << 16) ^ tmp; + data += 2 * sizeof(unsigned short); + hash += hash >> 11; + } - /* Force "avalanching" of final 127 bits */ - hash ^= hash << 3; - hash += hash >> 5; - hash ^= hash << 4; - hash += hash >> 17; - hash ^= hash << 25; - hash += hash >> 6; + /* Force "avalanching" of final 127 bits */ + hash ^= hash << 3; + hash += hash >> 5; + hash ^= hash << 4; + hash += hash >> 17; + hash ^= hash << 25; + hash += hash >> 6; - return hash; + return hash; } template <const int CELLSIZE> @@ -81,7 +81,7 @@ struct btSparseSdf btAlignedObjectArray<Cell*> cells; btScalar voxelsz; - btScalar m_defaultVoxelsz; + btScalar m_defaultVoxelsz; int puid; int ncells; int m_clampCells; @@ -103,16 +103,16 @@ struct btSparseSdf //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) m_clampCells = clampCells; cells.resize(hashsize, 0); - m_defaultVoxelsz = 0.25; + m_defaultVoxelsz = 0.25; Reset(); } // - - void setDefaultVoxelsz(btScalar sz) - { - m_defaultVoxelsz = sz; - } - + + void setDefaultVoxelsz(btScalar sz) + { + m_defaultVoxelsz = sz; + } + void Reset() { for (int i = 0, ni = cells.size(); i < ni; ++i) @@ -162,7 +162,7 @@ struct btSparseSdf nqueries = 1; nprobes = 1; ++puid; ///@todo: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ + /* else setup a priority list... */ } // int RemoveReferences(btCollisionShape* pcs) @@ -221,7 +221,7 @@ struct btSparseSdf else { // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]); - //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); + //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); c = c->next; } @@ -363,7 +363,7 @@ struct btSparseSdf myset.p = (void*)shape; const char* ptr = (const char*)&myset; - unsigned int result = HsiehHash(ptr, sizeof(btS) ); + unsigned int result = HsiehHash(ptr, sizeof(btS)); return result; } diff --git a/src/BulletSoftBody/poly34.cpp b/src/BulletSoftBody/poly34.cpp index 819d0c79f..ec7549c8e 100644 --- a/src/BulletSoftBody/poly34.cpp +++ b/src/BulletSoftBody/poly34.cpp @@ -6,7 +6,7 @@ // #include <math.h> -#include "poly34.h" // solution of cubic and quartic equation +#include "poly34.h" // solution of cubic and quartic equation #define TwoPi 6.28318530717958648 const btScalar eps = SIMD_EPSILON; @@ -15,50 +15,53 @@ const btScalar eps = SIMD_EPSILON; //============================================================================= static SIMD_FORCE_INLINE btScalar _root3(btScalar x) { - btScalar s = 1.; - while (x < 1.) { - x *= 8.; - s *= 0.5; - } - while (x > 8.) { - x *= 0.125; - s *= 2.; - } - btScalar r = 1.5; - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - return r * s; + btScalar s = 1.; + while (x < 1.) + { + x *= 8.; + s *= 0.5; + } + while (x > 8.) + { + x *= 0.125; + s *= 2.; + } + btScalar r = 1.5; + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + return r * s; } btScalar SIMD_FORCE_INLINE root3(btScalar x) { - if (x > 0) - return _root3(x); - else if (x < 0) - return -_root3(-x); - else - return 0.; + if (x > 0) + return _root3(x); + else if (x < 0) + return -_root3(-x); + else + return 0.; } // x - array of size 2 // return 2: 2 real roots x[0], x[1] // return 0: pair of complex roots: x[0]i*x[1] int SolveP2(btScalar* x, btScalar a, btScalar b) -{ // solve equation x^2 + a*x + b = 0 - btScalar D = 0.25 * a * a - b; - if (D >= 0) { - D = sqrt(D); - x[0] = -0.5 * a + D; - x[1] = -0.5 * a - D; - return 2; - } - x[0] = -0.5 * a; - x[1] = sqrt(-D); - return 0; +{ // solve equation x^2 + a*x + b = 0 + btScalar D = 0.25 * a * a - b; + if (D >= 0) + { + D = sqrt(D); + x[0] = -0.5 * a + D; + x[1] = -0.5 * a - D; + return 2; + } + x[0] = -0.5 * a; + x[1] = sqrt(-D); + return 0; } //--------------------------------------------------------------------------- // x - array of size 3 @@ -66,217 +69,228 @@ int SolveP2(btScalar* x, btScalar a, btScalar b) // 2 real roots: x[0], x[1], return 2 // 1 real root : x[0], x[1] i*x[2], return 1 int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c) -{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 - btScalar a2 = a * a; - btScalar q = (a2 - 3 * b) / 9; - if (q < 0) - q = eps; - btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; - // equation x^3 + q*x + r = 0 - btScalar r2 = r * r; - btScalar q3 = q * q * q; - btScalar A, B; - if (r2 <= (q3 + eps)) { //<<-- FIXED! - btScalar t = r / sqrt(q3); - if (t < -1) - t = -1; - if (t > 1) - t = 1; - t = acos(t); - a /= 3; - q = -2 * sqrt(q); - x[0] = q * cos(t / 3) - a; - x[1] = q * cos((t + TwoPi) / 3) - a; - x[2] = q * cos((t - TwoPi) / 3) - a; - return (3); - } - else { - //A =-pow(fabs(r)+sqrt(r2-q3),1./3); - A = -root3(fabs(r) + sqrt(r2 - q3)); - if (r < 0) - A = -A; - B = (A == 0 ? 0 : q / A); - - a /= 3; - x[0] = (A + B) - a; - x[1] = -0.5 * (A + B) - a; - x[2] = 0.5 * sqrt(3.) * (A - B); - if (fabs(x[2]) < eps) { - x[2] = x[1]; - return (2); - } - return (1); - } -} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { +{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 + btScalar a2 = a * a; + btScalar q = (a2 - 3 * b) / 9; + if (q < 0) + q = eps; + btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; + // equation x^3 + q*x + r = 0 + btScalar r2 = r * r; + btScalar q3 = q * q * q; + btScalar A, B; + if (r2 <= (q3 + eps)) + { //<<-- FIXED! + btScalar t = r / sqrt(q3); + if (t < -1) + t = -1; + if (t > 1) + t = 1; + t = acos(t); + a /= 3; + q = -2 * sqrt(q); + x[0] = q * cos(t / 3) - a; + x[1] = q * cos((t + TwoPi) / 3) - a; + x[2] = q * cos((t - TwoPi) / 3) - a; + return (3); + } + else + { + //A =-pow(fabs(r)+sqrt(r2-q3),1./3); + A = -root3(fabs(r) + sqrt(r2 - q3)); + if (r < 0) + A = -A; + B = (A == 0 ? 0 : q / A); + + a /= 3; + x[0] = (A + B) - a; + x[1] = -0.5 * (A + B) - a; + x[2] = 0.5 * sqrt(3.) * (A - B); + if (fabs(x[2]) < eps) + { + x[2] = x[1]; + return (2); + } + return (1); + } +} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { //--------------------------------------------------------------------------- // a>=0! -void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) { - btScalar r = sqrt(x * x + y * y); - if (y == 0) { - r = sqrt(r); - if (x >= 0) { - a = r; - b = 0; - } - else { - a = 0; - b = r; - } - } - else { // y != 0 - a = sqrt(0.5 * (x + r)); - b = 0.5 * y / a; - } + btScalar r = sqrt(x * x + y * y); + if (y == 0) + { + r = sqrt(r); + if (x >= 0) + { + a = r; + b = 0; + } + else + { + a = 0; + b = r; + } + } + else + { // y != 0 + a = sqrt(0.5 * (x + r)); + b = 0.5 * y / a; + } } //--------------------------------------------------------------------------- -int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 +int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 { - btScalar D = b * b - 4 * d; - if (D >= 0) { - btScalar sD = sqrt(D); - btScalar x1 = (-b + sD) / 2; - btScalar x2 = (-b - sD) / 2; // x2 <= x1 - if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots - { - btScalar sx1 = sqrt(x1); - btScalar sx2 = sqrt(x2); - x[0] = -sx1; - x[1] = sx1; - x[2] = -sx2; - x[3] = sx2; - return 4; - } - if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots - { - btScalar sx1 = sqrt(-x1); - btScalar sx2 = sqrt(-x2); - x[0] = 0; - x[1] = sx1; - x[2] = 0; - x[3] = sx2; - return 0; - } - // now x2 < 0 <= x1 , two real roots and one pair of imginary root - btScalar sx1 = sqrt(x1); - btScalar sx2 = sqrt(-x2); - x[0] = -sx1; - x[1] = sx1; - x[2] = 0; - x[3] = sx2; - return 2; - } - else { // if( D < 0 ), two pair of compex roots - btScalar sD2 = 0.5 * sqrt(-D); - CSqrt(-0.5 * b, sD2, x[0], x[1]); - CSqrt(-0.5 * b, -sD2, x[2], x[3]); - return 0; - } // if( D>=0 ) -} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d + btScalar D = b * b - 4 * d; + if (D >= 0) + { + btScalar sD = sqrt(D); + btScalar x1 = (-b + sD) / 2; + btScalar x2 = (-b - sD) / 2; // x2 <= x1 + if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots + { + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = -sx2; + x[3] = sx2; + return 4; + } + if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots + { + btScalar sx1 = sqrt(-x1); + btScalar sx2 = sqrt(-x2); + x[0] = 0; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 0; + } + // now x2 < 0 <= x1 , two real roots and one pair of imginary root + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(-x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 2; + } + else + { // if( D < 0 ), two pair of compex roots + btScalar sD2 = 0.5 * sqrt(-D); + CSqrt(-0.5 * b, sD2, x[0], x[1]); + CSqrt(-0.5 * b, -sD2, x[2], x[3]); + return 0; + } // if( D>=0 ) +} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d //--------------------------------------------------------------------------- #define SWAP(a, b) \ -{ \ -t = b; \ -b = a; \ -a = t; \ -} -static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c + { \ + t = b; \ + b = a; \ + a = t; \ + } +static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c { - btScalar t; - if (a > b) - SWAP(a, b); // now a<=b - if (c < b) { - SWAP(b, c); // now a<=b, b<=c - if (a > b) - SWAP(a, b); // now a<=b - } + btScalar t; + if (a > b) + SWAP(a, b); // now a<=b + if (c < b) + { + SWAP(b, c); // now a<=b, b<=c + if (a > b) + SWAP(a, b); // now a<=b + } } //--------------------------------------------------------------------------- -int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d { - //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 - if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) - return SolveP4Bi(x, b, d); // After that, c!=0 - - int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent - // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 - if (res3 > 1) // 3 real roots, - { - dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] - // Note: x[0]*x[1]*x[2]= c*c > 0 - if (x[0] > 0) // all roots are positive - { - btScalar sz1 = sqrt(x[0]); - btScalar sz2 = sqrt(x[1]); - btScalar sz3 = sqrt(x[2]); - // Note: sz1*sz2*sz3= -c (and not equal to 0) - if (c > 0) { - x[0] = (-sz1 - sz2 - sz3) / 2; - x[1] = (-sz1 + sz2 + sz3) / 2; - x[2] = (+sz1 - sz2 + sz3) / 2; - x[3] = (+sz1 + sz2 - sz3) / 2; - return 4; - } - // now: c<0 - x[0] = (-sz1 - sz2 + sz3) / 2; - x[1] = (-sz1 + sz2 - sz3) / 2; - x[2] = (+sz1 - sz2 - sz3) / 2; - x[3] = (+sz1 + sz2 + sz3) / 2; - return 4; - } // if( x[0] > 0) // all roots are positive - // now x[0] <= x[1] < 0, x[2] > 0 - // two pair of comlex roots - btScalar sz1 = sqrt(-x[0]); - btScalar sz2 = sqrt(-x[1]); - btScalar sz3 = sqrt(x[2]); - - if (c > 0) // sign = -1 - { - x[0] = -sz3 / 2; - x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] - x[2] = sz3 / 2; - x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] - return 0; - } - // now: c<0 , sign = +1 - x[0] = sz3 / 2; - x[1] = (-sz1 + sz2) / 2; - x[2] = -sz3 / 2; - x[3] = (sz1 + sz2) / 2; - return 0; - } // if( res3>1 ) // 3 real roots, - // now resoventa have 1 real and pair of compex roots - // x[0] - real root, and x[0]>0, - // x[1]i*x[2] - complex roots, - // x[0] must be >=0. But one times x[0]=~ 1e-17, so: - if (x[0] < 0) - x[0] = 0; - btScalar sz1 = sqrt(x[0]); - btScalar szr, szi; - CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] - if (c > 0) // sign = -1 - { - x[0] = -sz1 / 2 - szr; // 1st real root - x[1] = -sz1 / 2 + szr; // 2nd real root - x[2] = sz1 / 2; - x[3] = szi; - return 2; - } - // now: c<0 , sign = +1 - x[0] = sz1 / 2 - szr; // 1st real root - x[1] = sz1 / 2 + szr; // 2nd real root - x[2] = -sz1 / 2; - x[3] = szi; - return 2; -} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d + //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 + if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) + return SolveP4Bi(x, b, d); // After that, c!=0 + + int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent + // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 + if (res3 > 1) // 3 real roots, + { + dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] + // Note: x[0]*x[1]*x[2]= c*c > 0 + if (x[0] > 0) // all roots are positive + { + btScalar sz1 = sqrt(x[0]); + btScalar sz2 = sqrt(x[1]); + btScalar sz3 = sqrt(x[2]); + // Note: sz1*sz2*sz3= -c (and not equal to 0) + if (c > 0) + { + x[0] = (-sz1 - sz2 - sz3) / 2; + x[1] = (-sz1 + sz2 + sz3) / 2; + x[2] = (+sz1 - sz2 + sz3) / 2; + x[3] = (+sz1 + sz2 - sz3) / 2; + return 4; + } + // now: c<0 + x[0] = (-sz1 - sz2 + sz3) / 2; + x[1] = (-sz1 + sz2 - sz3) / 2; + x[2] = (+sz1 - sz2 - sz3) / 2; + x[3] = (+sz1 + sz2 + sz3) / 2; + return 4; + } // if( x[0] > 0) // all roots are positive + // now x[0] <= x[1] < 0, x[2] > 0 + // two pair of comlex roots + btScalar sz1 = sqrt(-x[0]); + btScalar sz2 = sqrt(-x[1]); + btScalar sz3 = sqrt(x[2]); + + if (c > 0) // sign = -1 + { + x[0] = -sz3 / 2; + x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] + x[2] = sz3 / 2; + x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] + return 0; + } + // now: c<0 , sign = +1 + x[0] = sz3 / 2; + x[1] = (-sz1 + sz2) / 2; + x[2] = -sz3 / 2; + x[3] = (sz1 + sz2) / 2; + return 0; + } // if( res3>1 ) // 3 real roots, + // now resoventa have 1 real and pair of compex roots + // x[0] - real root, and x[0]>0, + // x[1]i*x[2] - complex roots, + // x[0] must be >=0. But one times x[0]=~ 1e-17, so: + if (x[0] < 0) + x[0] = 0; + btScalar sz1 = sqrt(x[0]); + btScalar szr, szi; + CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] + if (c > 0) // sign = -1 + { + x[0] = -sz1 / 2 - szr; // 1st real root + x[1] = -sz1 / 2 + szr; // 2nd real root + x[2] = sz1 / 2; + x[3] = szi; + return 2; + } + // now: c<0 , sign = +1 + x[0] = sz1 / 2 - szr; // 1st real root + x[1] = sz1 / 2 + szr; // 2nd real root + x[2] = -sz1 / 2; + x[3] = szi; + return 2; +} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d //----------------------------------------------------------------------------- -btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d { - btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) - if (fxs == 0) - return x; //return 1e99; <<-- FIXED! - btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) - return x - fx / fxs; + btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) + if (fxs == 0) + return x; //return 1e99; <<-- FIXED! + btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) + return x - fx / fxs; } //----------------------------------------------------------------------------- // x - array of size 4 @@ -284,136 +298,150 @@ btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // o // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d) -{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method - // move to a=0: - btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); - btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); - btScalar b1 = b - 0.375 * a * a; - int res = SolveP4De(x, b1, c1, d1); - if (res == 4) { - x[0] -= a / 4; - x[1] -= a / 4; - x[2] -= a / 4; - x[3] -= a / 4; - } - else if (res == 2) { - x[0] -= a / 4; - x[1] -= a / 4; - x[2] -= a / 4; - } - else { - x[0] -= a / 4; - x[2] -= a / 4; - } - // one Newton step for each real root: - if (res > 0) { - x[0] = N4Step(x[0], a, b, c, d); - x[1] = N4Step(x[1], a, b, c, d); - } - if (res > 2) { - x[2] = N4Step(x[2], a, b, c, d); - x[3] = N4Step(x[3], a, b, c, d); - } - return res; +{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method + // move to a=0: + btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); + btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); + btScalar b1 = b - 0.375 * a * a; + int res = SolveP4De(x, b1, c1, d1); + if (res == 4) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + x[3] -= a / 4; + } + else if (res == 2) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + } + else + { + x[0] -= a / 4; + x[2] -= a / 4; + } + // one Newton step for each real root: + if (res > 0) + { + x[0] = N4Step(x[0], a, b, c, d); + x[1] = N4Step(x[1], a, b, c, d); + } + if (res > 2) + { + x[2] = N4Step(x[2], a, b, c, d); + x[3] = N4Step(x[3], a, b, c, d); + } + return res; } //----------------------------------------------------------------------------- #define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e) //----------------------------------------------------------------------------- -btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 { - int cnt; - if (fabs(e) < eps) - return 0; - - btScalar brd = fabs(a); // brd - border of real roots - if (fabs(b) > brd) - brd = fabs(b); - if (fabs(c) > brd) - brd = fabs(c); - if (fabs(d) > brd) - brd = fabs(d); - if (fabs(e) > brd) - brd = fabs(e); - brd++; // brd - border of real roots - - btScalar x0, f0; // less than root - btScalar x1, f1; // greater than root - btScalar x2, f2, f2s; // next values, f(x2), f'(x2) - btScalar dx = 0; - - if (e < 0) { - x0 = 0; - x1 = brd; - f0 = e; - f1 = F5(x1); - x2 = 0.01 * brd; - } // positive root - else { - x0 = -brd; - x1 = 0; - f0 = F5(x0); - f1 = e; - x2 = -0.01 * brd; - } // negative root - - if (fabs(f0) < eps) - return x0; - if (fabs(f1) < eps) - return x1; - - // now x0<x1, f(x0)<0, f(x1)>0 - // Firstly 10 bisections - for (cnt = 0; cnt < 10; cnt++) { - x2 = (x0 + x1) / 2; // next point - //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point - f2 = F5(x2); // f(x2) - if (fabs(f2) < eps) - return x2; - if (f2 > 0) { - x1 = x2; - f1 = f2; - } - else { - x0 = x2; - f0 = f2; - } - } - - // At each step: - // x0<x1, f(x0)<0, f(x1)>0. - // x2 - next value - // we hope that x0 < x2 < x1, but not necessarily - do { - if (cnt++ > 50) - break; - if (x2 <= x0 || x2 >= x1) - x2 = (x0 + x1) / 2; // now x0 < x2 < x1 - f2 = F5(x2); // f(x2) - if (fabs(f2) < eps) - return x2; - if (f2 > 0) { - x1 = x2; - f1 = f2; - } - else { - x0 = x2; - f0 = f2; - } - f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) - if (fabs(f2s) < eps) { - x2 = 1e99; - continue; - } - dx = f2 / f2s; - x2 -= dx; - } while (fabs(dx) > eps); - return x2; -} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 + int cnt; + if (fabs(e) < eps) + return 0; + + btScalar brd = fabs(a); // brd - border of real roots + if (fabs(b) > brd) + brd = fabs(b); + if (fabs(c) > brd) + brd = fabs(c); + if (fabs(d) > brd) + brd = fabs(d); + if (fabs(e) > brd) + brd = fabs(e); + brd++; // brd - border of real roots + + btScalar x0, f0; // less than root + btScalar x1, f1; // greater than root + btScalar x2, f2, f2s; // next values, f(x2), f'(x2) + btScalar dx = 0; + + if (e < 0) + { + x0 = 0; + x1 = brd; + f0 = e; + f1 = F5(x1); + x2 = 0.01 * brd; + } // positive root + else + { + x0 = -brd; + x1 = 0; + f0 = F5(x0); + f1 = e; + x2 = -0.01 * brd; + } // negative root + + if (fabs(f0) < eps) + return x0; + if (fabs(f1) < eps) + return x1; + + // now x0<x1, f(x0)<0, f(x1)>0 + // Firstly 10 bisections + for (cnt = 0; cnt < 10; cnt++) + { + x2 = (x0 + x1) / 2; // next point + //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + } + + // At each step: + // x0<x1, f(x0)<0, f(x1)>0. + // x2 - next value + // we hope that x0 < x2 < x1, but not necessarily + do + { + if (cnt++ > 50) + break; + if (x2 <= x0 || x2 >= x1) + x2 = (x0 + x1) / 2; // now x0 < x2 < x1 + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) + if (fabs(f2s) < eps) + { + x2 = 1e99; + continue; + } + dx = f2 / f2s; + x2 -= dx; + } while (fabs(dx) > eps); + return x2; +} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- -int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 { - btScalar r = x[0] = SolveP5_1(a, b, c, d, e); - btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; - return 1 + SolveP4(x + 1, a1, b1, c1, d1); -} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 + btScalar r = x[0] = SolveP5_1(a, b, c, d, e); + btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; + return 1 + SolveP4(x + 1, a1, b1, c1, d1); +} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- diff --git a/src/BulletSoftBody/poly34.h b/src/BulletSoftBody/poly34.h index 32ad5d7da..35a52c5fe 100644 --- a/src/BulletSoftBody/poly34.h +++ b/src/BulletSoftBody/poly34.h @@ -8,31 +8,31 @@ // x - array of size 2 // return 2: 2 real roots x[0], x[1] // return 0: pair of complex roots: x[0]i*x[1] -int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 +int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 // x - array of size 3 // return 3: 3 real roots x[0], x[1], x[2] // return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2] -int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 +int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 // x - array of size 4 // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], -int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method +int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method // x - array of size 5 // return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots // return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4], // return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4], -int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- // And some additional functions for internal use. // Your may remove this definitions from here -int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 -int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 -void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) -btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d -btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 #endif diff --git a/src/LinearMath/CMakeLists.txt b/src/LinearMath/CMakeLists.txt index a5491c325..a0532c76e 100644 --- a/src/LinearMath/CMakeLists.txt +++ b/src/LinearMath/CMakeLists.txt @@ -10,6 +10,7 @@ SET(LinearMath_SRCS btGeometryUtil.cpp btPolarDecomposition.cpp btQuickprof.cpp + btReducedVector.cpp btSerializer.cpp btSerializer64.cpp btThreads.cpp @@ -34,12 +35,14 @@ SET(LinearMath_HDRS btMatrix3x3.h btImplicitQRSVD.h btMinMax.h + btModifiedGramSchmidt.h btMotionState.h btPolarDecomposition.h btPoolAllocator.h btQuadWord.h btQuaternion.h btQuickprof.h + btReducedVector.h btRandom.h btScalar.h btSerializer.h diff --git a/src/LinearMath/btAlignedAllocator.cpp b/src/LinearMath/btAlignedAllocator.cpp index 39b302b60..be8f8aa6d 100644 --- a/src/LinearMath/btAlignedAllocator.cpp +++ b/src/LinearMath/btAlignedAllocator.cpp @@ -138,7 +138,7 @@ struct btDebugPtrMagic }; }; -void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filename) +void *btAlignedAllocInternal(size_t size, int alignment, int line, const char *filename) { if (size == 0) { @@ -195,7 +195,7 @@ void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filenam return (ret); } -void btAlignedFreeInternal(void *ptr, int line, char *filename) +void btAlignedFreeInternal(void *ptr, int line, const char *filename) { void *real; diff --git a/src/LinearMath/btAlignedAllocator.h b/src/LinearMath/btAlignedAllocator.h index ce4d3585f..971f62bfb 100644 --- a/src/LinearMath/btAlignedAllocator.h +++ b/src/LinearMath/btAlignedAllocator.h @@ -35,9 +35,9 @@ int btDumpMemoryLeaks(); #define btAlignedFree(ptr) \ btAlignedFreeInternal(ptr, __LINE__, __FILE__) -void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename); +void* btAlignedAllocInternal(size_t size, int alignment, int line, const char* filename); -void btAlignedFreeInternal(void* ptr, int line, char* filename); +void btAlignedFreeInternal(void* ptr, int line, const char* filename); #else void* btAlignedAllocInternal(size_t size, int alignment); diff --git a/src/LinearMath/btImplicitQRSVD.h b/src/LinearMath/btImplicitQRSVD.h index 1e1a3e7b4..aaedc964f 100644 --- a/src/LinearMath/btImplicitQRSVD.h +++ b/src/LinearMath/btImplicitQRSVD.h @@ -753,7 +753,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A, btMatrix3x3& V, btScalar tol = 128*std::numeric_limits<btScalar>::epsilon()) { - using std::fabs; +// using std::fabs; btMatrix3x3 B = A; U.setIdentity(); V.setIdentity(); diff --git a/src/LinearMath/btModifiedGramSchmidt.h b/src/LinearMath/btModifiedGramSchmidt.h new file mode 100644 index 000000000..33bab8d65 --- /dev/null +++ b/src/LinearMath/btModifiedGramSchmidt.h @@ -0,0 +1,83 @@ +// +// btModifiedGramSchmidt.h +// LinearMath +// +// Created by Xuchen Han on 4/4/20. +// + +#ifndef btModifiedGramSchmidt_h +#define btModifiedGramSchmidt_h + +#include "btReducedVector.h" +#include "btAlignedObjectArray.h" +#include <iostream> +#include <cmath> +template<class TV> +class btModifiedGramSchmidt +{ +public: + btAlignedObjectArray<TV> m_in; + btAlignedObjectArray<TV> m_out; + + btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs) + { + m_out.resize(0); + } + + void solve() + { + m_out.resize(m_in.size()); + for (int i = 0; i < m_in.size(); ++i) + { +// printf("========= starting %d ==========\n", i); + TV v(m_in[i]); +// v.print(); + for (int j = 0; j < i; ++j) + { + v = v - v.proj(m_out[j]); +// v.print(); + } + v.normalize(); + m_out[i] = v; +// v.print(); + } + } + + void test() + { + std::cout << SIMD_EPSILON << std::endl; + printf("=======inputs=========\n"); + for (int i = 0; i < m_out.size(); ++i) + { + m_in[i].print(); + } + printf("=======output=========\n"); + for (int i = 0; i < m_out.size(); ++i) + { + m_out[i].print(); + } + btScalar eps = SIMD_EPSILON; + for (int i = 0; i < m_out.size(); ++i) + { + for (int j = 0; j < m_out.size(); ++j) + { + if (i == j) + { + if (std::abs(1.0-m_out[i].dot(m_out[j])) > eps)// && std::abs(m_out[i].dot(m_out[j])) > eps) + { + printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j])); + } + } + else + { + if (std::abs(m_out[i].dot(m_out[j])) > eps) + { + printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j])); + } + } + } + } + } +}; +template class btModifiedGramSchmidt<btReducedVector>; +#endif /* btModifiedGramSchmidt_h */ diff --git a/src/LinearMath/btReducedVector.cpp b/src/LinearMath/btReducedVector.cpp new file mode 100644 index 000000000..1539584e7 --- /dev/null +++ b/src/LinearMath/btReducedVector.cpp @@ -0,0 +1,170 @@ +// +// btReducedVector.cpp +// LinearMath +// +// Created by Xuchen Han on 4/4/20. +// +#include <stdio.h> +#include "btReducedVector.h" +#include <cmath> + +// returns the projection of this onto other +btReducedVector btReducedVector::proj(const btReducedVector& other) const +{ + btReducedVector ret(m_sz); + btScalar other_length2 = other.length2(); + if (other_length2 < SIMD_EPSILON) + { + return ret; + } + return other*(this->dot(other))/other_length2; +} + +void btReducedVector::normalize() +{ + if (this->length2() < SIMD_EPSILON) + { + m_indices.clear(); + m_vecs.clear(); + return; + } + *this /= std::sqrt(this->length2()); +} + +bool btReducedVector::testAdd() const +{ + int sz = 5; + btAlignedObjectArray<int> id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray<btVector3> v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btAlignedObjectArray<int> id2; + id2.push_back(2); + id2.push_back(3); + id2.push_back(5); + btAlignedObjectArray<btVector3> v2; + v2.push_back(btVector3(2,3,1)); + v2.push_back(btVector3(3,4,9)); + v2.push_back(btVector3(0,4,0)); + btAlignedObjectArray<int> id3; + id3.push_back(1); + id3.push_back(2); + id3.push_back(3); + id3.push_back(5); + btAlignedObjectArray<btVector3> v3; + v3.push_back(btVector3(1,0,1)); + v3.push_back(btVector3(2,3,1)); + v3.push_back(btVector3(6,5,14)); + v3.push_back(btVector3(0,4,0)); + btReducedVector rv1(sz, id1, v1); + btReducedVector rv2(sz, id2, v2); + btReducedVector ans(sz, id3, v3); + bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1)); + if (!ret) + printf("btReducedVector testAdd failed\n"); + return ret; +} + +bool btReducedVector::testMinus() const +{ + int sz = 5; + btAlignedObjectArray<int> id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray<btVector3> v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btAlignedObjectArray<int> id2; + id2.push_back(2); + id2.push_back(3); + id2.push_back(5); + btAlignedObjectArray<btVector3> v2; + v2.push_back(btVector3(2,3,1)); + v2.push_back(btVector3(3,4,9)); + v2.push_back(btVector3(0,4,0)); + btAlignedObjectArray<int> id3; + id3.push_back(1); + id3.push_back(2); + id3.push_back(3); + id3.push_back(5); + btAlignedObjectArray<btVector3> v3; + v3.push_back(btVector3(-1,-0,-1)); + v3.push_back(btVector3(2,3,1)); + v3.push_back(btVector3(0,3,4)); + v3.push_back(btVector3(0,4,0)); + btReducedVector rv1(sz, id1, v1); + btReducedVector rv2(sz, id2, v2); + btReducedVector ans(sz, id3, v3); + bool ret = (ans == rv2-rv1); + if (!ret) + printf("btReducedVector testMinus failed\n"); + return ret; +} + +bool btReducedVector::testDot() const +{ + int sz = 5; + btAlignedObjectArray<int> id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray<btVector3> v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btAlignedObjectArray<int> id2; + id2.push_back(2); + id2.push_back(3); + id2.push_back(5); + btAlignedObjectArray<btVector3> v2; + v2.push_back(btVector3(2,3,1)); + v2.push_back(btVector3(3,4,9)); + v2.push_back(btVector3(0,4,0)); + btReducedVector rv1(sz, id1, v1); + btReducedVector rv2(sz, id2, v2); + btScalar ans = 58; + bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2)); + ans = 14+16+9+16+81; + ret &= (ans==rv2.dot(rv2)); + + if (!ret) + printf("btReducedVector testDot failed\n"); + return ret; +} + +bool btReducedVector::testMultiply() const +{ + int sz = 5; + btAlignedObjectArray<int> id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray<btVector3> v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btScalar s = 2; + btReducedVector rv1(sz, id1, v1); + btAlignedObjectArray<int> id2; + id2.push_back(1); + id2.push_back(3); + btAlignedObjectArray<btVector3> v2; + v2.push_back(btVector3(2,0,2)); + v2.push_back(btVector3(6,2,10)); + btReducedVector ans(sz, id2, v2); + bool ret = (ans == rv1*s); + if (!ret) + printf("btReducedVector testMultiply failed\n"); + return ret; +} + +void btReducedVector::test() const +{ + bool ans = testAdd() && testMinus() && testDot() && testMultiply(); + if (ans) + { + printf("All tests passed\n"); + } + else + { + printf("Tests failed\n"); + } +} diff --git a/src/LinearMath/btReducedVector.h b/src/LinearMath/btReducedVector.h new file mode 100644 index 000000000..83b5e581e --- /dev/null +++ b/src/LinearMath/btReducedVector.h @@ -0,0 +1,320 @@ +// +// btReducedVectors.h +// BulletLinearMath +// +// Created by Xuchen Han on 4/4/20. +// +#ifndef btReducedVectors_h +#define btReducedVectors_h +#include "btVector3.h" +#include "btMatrix3x3.h" +#include "btAlignedObjectArray.h" +#include <stdio.h> +#include <vector> +#include <algorithm> +struct TwoInts +{ + int a,b; +}; +inline bool operator<(const TwoInts& A, const TwoInts& B) +{ + return A.b < B.b; +} + + +// A helper vector type used for CG projections +class btReducedVector +{ +public: + btAlignedObjectArray<int> m_indices; + btAlignedObjectArray<btVector3> m_vecs; + int m_sz; // all m_indices value < m_sz +public: + btReducedVector():m_sz(0) + { + m_indices.resize(0); + m_vecs.resize(0); + m_indices.clear(); + m_vecs.clear(); + } + + btReducedVector(int sz): m_sz(sz) + { + m_indices.resize(0); + m_vecs.resize(0); + m_indices.clear(); + m_vecs.clear(); + } + + btReducedVector(int sz, const btAlignedObjectArray<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs) + { + } + + void simplify() + { + btAlignedObjectArray<int> old_indices(m_indices); + btAlignedObjectArray<btVector3> old_vecs(m_vecs); + m_indices.resize(0); + m_vecs.resize(0); + m_indices.clear(); + m_vecs.clear(); + for (int i = 0; i < old_indices.size(); ++i) + { + if (old_vecs[i].length2() > SIMD_EPSILON) + { + m_indices.push_back(old_indices[i]); + m_vecs.push_back(old_vecs[i]); + } + } + } + + btReducedVector operator+(const btReducedVector& other) + { + btReducedVector ret(m_sz); + int i=0, j=0; + while (i < m_indices.size() && j < other.m_indices.size()) + { + if (m_indices[i] < other.m_indices[j]) + { + ret.m_indices.push_back(m_indices[i]); + ret.m_vecs.push_back(m_vecs[i]); + ++i; + } + else if (m_indices[i] > other.m_indices[j]) + { + ret.m_indices.push_back(other.m_indices[j]); + ret.m_vecs.push_back(other.m_vecs[j]); + ++j; + } + else + { + ret.m_indices.push_back(other.m_indices[j]); + ret.m_vecs.push_back(m_vecs[i] + other.m_vecs[j]); + ++i; ++j; + } + } + while (i < m_indices.size()) + { + ret.m_indices.push_back(m_indices[i]); + ret.m_vecs.push_back(m_vecs[i]); + ++i; + } + while (j < other.m_indices.size()) + { + ret.m_indices.push_back(other.m_indices[j]); + ret.m_vecs.push_back(other.m_vecs[j]); + ++j; + } + ret.simplify(); + return ret; + } + + btReducedVector operator-() + { + btReducedVector ret(m_sz); + for (int i = 0; i < m_indices.size(); ++i) + { + ret.m_indices.push_back(m_indices[i]); + ret.m_vecs.push_back(-m_vecs[i]); + } + ret.simplify(); + return ret; + } + + btReducedVector operator-(const btReducedVector& other) + { + btReducedVector ret(m_sz); + int i=0, j=0; + while (i < m_indices.size() && j < other.m_indices.size()) + { + if (m_indices[i] < other.m_indices[j]) + { + ret.m_indices.push_back(m_indices[i]); + ret.m_vecs.push_back(m_vecs[i]); + ++i; + } + else if (m_indices[i] > other.m_indices[j]) + { + ret.m_indices.push_back(other.m_indices[j]); + ret.m_vecs.push_back(-other.m_vecs[j]); + ++j; + } + else + { + ret.m_indices.push_back(other.m_indices[j]); + ret.m_vecs.push_back(m_vecs[i] - other.m_vecs[j]); + ++i; ++j; + } + } + while (i < m_indices.size()) + { + ret.m_indices.push_back(m_indices[i]); + ret.m_vecs.push_back(m_vecs[i]); + ++i; + } + while (j < other.m_indices.size()) + { + ret.m_indices.push_back(other.m_indices[j]); + ret.m_vecs.push_back(-other.m_vecs[j]); + ++j; + } + ret.simplify(); + return ret; + } + + bool operator==(const btReducedVector& other) const + { + if (m_sz != other.m_sz) + return false; + if (m_indices.size() != other.m_indices.size()) + return false; + for (int i = 0; i < m_indices.size(); ++i) + { + if (m_indices[i] != other.m_indices[i] || m_vecs[i] != other.m_vecs[i]) + { + return false; + } + } + return true; + } + + bool operator!=(const btReducedVector& other) const + { + return !(*this == other); + } + + btReducedVector& operator=(const btReducedVector& other) + { + if (this == &other) + { + return *this; + } + m_sz = other.m_sz; + m_indices.copyFromArray(other.m_indices); + m_vecs.copyFromArray(other.m_vecs); + return *this; + } + + btScalar dot(const btReducedVector& other) const + { + btScalar ret = 0; + int j = 0; + for (int i = 0; i < m_indices.size(); ++i) + { + while (j < other.m_indices.size() && other.m_indices[j] < m_indices[i]) + { + ++j; + } + if (j < other.m_indices.size() && other.m_indices[j] == m_indices[i]) + { + ret += m_vecs[i].dot(other.m_vecs[j]); +// ++j; + } + } + return ret; + } + + btScalar dot(const btAlignedObjectArray<btVector3>& other) const + { + btScalar ret = 0; + for (int i = 0; i < m_indices.size(); ++i) + { + ret += m_vecs[i].dot(other[m_indices[i]]); + } + return ret; + } + + btScalar length2() const + { + return this->dot(*this); + } + + void normalize(); + + // returns the projection of this onto other + btReducedVector proj(const btReducedVector& other) const; + + bool testAdd() const; + + bool testMinus() const; + + bool testDot() const; + + bool testMultiply() const; + + void test() const; + + void print() const + { + for (int i = 0; i < m_indices.size(); ++i) + { + printf("%d: (%f, %f, %f)/", m_indices[i], m_vecs[i][0],m_vecs[i][1],m_vecs[i][2]); + } + printf("\n"); + } + + + void sort() + { + std::vector<TwoInts> tuples; + for (int i = 0; i < m_indices.size(); ++i) + { + TwoInts ti; + ti.a = i; + ti.b = m_indices[i]; + tuples.push_back(ti); + } + std::sort(tuples.begin(), tuples.end()); + btAlignedObjectArray<int> new_indices; + btAlignedObjectArray<btVector3> new_vecs; + for (int i = 0; i < tuples.size(); ++i) + { + new_indices.push_back(tuples[i].b); + new_vecs.push_back(m_vecs[tuples[i].a]); + } + m_indices = new_indices; + m_vecs = new_vecs; + } +}; + +SIMD_FORCE_INLINE btReducedVector operator*(const btReducedVector& v, btScalar s) +{ + btReducedVector ret(v.m_sz); + for (int i = 0; i < v.m_indices.size(); ++i) + { + ret.m_indices.push_back(v.m_indices[i]); + ret.m_vecs.push_back(s*v.m_vecs[i]); + } + ret.simplify(); + return ret; +} + +SIMD_FORCE_INLINE btReducedVector operator*(btScalar s, const btReducedVector& v) +{ + return v*s; +} + +SIMD_FORCE_INLINE btReducedVector operator/(const btReducedVector& v, btScalar s) +{ + return v * (1.0/s); +} + +SIMD_FORCE_INLINE btReducedVector& operator/=(btReducedVector& v, btScalar s) +{ + v = v/s; + return v; +} + +SIMD_FORCE_INLINE btReducedVector& operator+=(btReducedVector& v1, const btReducedVector& v2) +{ + v1 = v1+v2; + return v1; +} + +SIMD_FORCE_INLINE btReducedVector& operator-=(btReducedVector& v1, const btReducedVector& v2) +{ + v1 = v1-v2; + return v1; +} + +#endif /* btReducedVectors_h */ diff --git a/src/btLinearMathAll.cpp b/src/btLinearMathAll.cpp index 808f41280..d05a19e63 100644 --- a/src/btLinearMathAll.cpp +++ b/src/btLinearMathAll.cpp @@ -8,6 +8,7 @@ #include "LinearMath/btConvexHullComputer.cpp" #include "LinearMath/btQuickprof.cpp" #include "LinearMath/btThreads.cpp" +#include "LinearMath/btReducedVector.cpp" #include "LinearMath/TaskScheduler/btTaskScheduler.cpp" #include "LinearMath/TaskScheduler/btThreadSupportPosix.cpp" #include "LinearMath/TaskScheduler/btThreadSupportWin32.cpp" |