mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-19 05:20:06 +00:00
Merge pull request #2265 from akien-mga/dos2unix
Convert DOS (CRLF) source files to Unix (LF) line endings
This commit is contained in:
erwincoumans
GitHub
commit
7a7652fd43
236
README.md
236
README.md
@ -1,118 +1,118 @@
|
|||||||
[](https://travis-ci.org/bulletphysics/bullet3)
|
[](https://travis-ci.org/bulletphysics/bullet3)
|
||||||
[](https://ci.appveyor.com/project/erwincoumans/bullet3)
|
[](https://ci.appveyor.com/project/erwincoumans/bullet3)
|
||||||
|
|
||||||
# Bullet Physics SDK
|
# Bullet Physics SDK
|
||||||
|
|
||||||
This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
|
This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
|
||||||
|
|
||||||
New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
|
New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
|
||||||
|
|
||||||
The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
|
The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
|
||||||
The steps towards a new API is in a nutshell:
|
The steps towards a new API is in a nutshell:
|
||||||
|
|
||||||
1. The old Bullet2 demos are being merged into the examples/ExampleBrowser
|
1. The old Bullet2 demos are being merged into the examples/ExampleBrowser
|
||||||
2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h
|
2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h
|
||||||
3. Python bindings in pybullet are on top of this C-API, see examples/pybullet
|
3. Python bindings in pybullet are on top of this C-API, see examples/pybullet
|
||||||
4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available
|
4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available
|
||||||
5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
|
5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
|
||||||
|
|
||||||
You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk
|
You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk
|
||||||
|
|
||||||
## Requirements for Bullet 2
|
## Requirements for Bullet 2
|
||||||
|
|
||||||
A C++ compiler for C++ 2003. The library is tested on Windows, Linux, Mac OSX, iOS, Android,
|
A C++ compiler for C++ 2003. The library is tested on Windows, Linux, Mac OSX, iOS, Android,
|
||||||
but should likely work on any platform with C++ compiler.
|
but should likely work on any platform with C++ compiler.
|
||||||
Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical demos and unit tests too.
|
Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical demos and unit tests too.
|
||||||
|
|
||||||
## Contributors and Coding Style information
|
## Contributors and Coding Style information
|
||||||
|
|
||||||
https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit
|
https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit
|
||||||
|
|
||||||
## Requirements for experimental OpenCL GPGPU support
|
## Requirements for experimental OpenCL GPGPU support
|
||||||
|
|
||||||
The entire collision detection and rigid body dynamics can be executed on the GPU.
|
The entire collision detection and rigid body dynamics can be executed on the GPU.
|
||||||
|
|
||||||
A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
|
A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
|
||||||
We succesfully tested the software under Windows, Linux and Mac OSX.
|
We succesfully tested the software under Windows, Linux and Mac OSX.
|
||||||
The software currently doesn't work on OpenCL CPU devices. It might run
|
The software currently doesn't work on OpenCL CPU devices. It might run
|
||||||
on a laptop GPU but performance will not likely be very good. Note that
|
on a laptop GPU but performance will not likely be very good. Note that
|
||||||
often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
|
often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
|
||||||
track down the issue, but more work is required to cover all OpenCL kernels.
|
track down the issue, but more work is required to cover all OpenCL kernels.
|
||||||
|
|
||||||
## License
|
## License
|
||||||
|
|
||||||
All source code files are licensed under the permissive zlib license
|
All source code files are licensed under the permissive zlib license
|
||||||
(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.
|
(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.
|
||||||
|
|
||||||
## Build instructions for Bullet using premake. You can also use cmake instead.
|
## Build instructions for Bullet using premake. You can also use cmake instead.
|
||||||
|
|
||||||
**Windows**
|
**Windows**
|
||||||
|
|
||||||
Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln
|
Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln
|
||||||
When asked, convert the projects to a newer version of Visual Studio.
|
When asked, convert the projects to a newer version of Visual Studio.
|
||||||
If you installed Python in the C:\ root directory, the batch file should find it automatically.
|
If you installed Python in the C:\ root directory, the batch file should find it automatically.
|
||||||
Otherwise, edit this batch file to choose where Python include/lib directories are located.
|
Otherwise, edit this batch file to choose where Python include/lib directories are located.
|
||||||
|
|
||||||
**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**
|
**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**
|
||||||
|
|
||||||
Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.
|
Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.
|
||||||
You can connect from Python pybullet to the sandbox using:
|
You can connect from Python pybullet to the sandbox using:
|
||||||
|
|
||||||
```
|
```
|
||||||
import pybullet as p
|
import pybullet as p
|
||||||
p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86.10",1234)
|
p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86.10",1234)
|
||||||
```
|
```
|
||||||
|
|
||||||
**Linux and Mac OSX gnu make**
|
**Linux and Mac OSX gnu make**
|
||||||
|
|
||||||
Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org)
|
Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org)
|
||||||
|
|
||||||
In a terminal type:
|
In a terminal type:
|
||||||
|
|
||||||
./build_cmake_pybullet_double.sh
|
./build_cmake_pybullet_double.sh
|
||||||
|
|
||||||
This script will invoke cmake and build in the build_cmake directory. You can find pybullet in Bullet/examples/pybullet.
|
This script will invoke cmake and build in the build_cmake directory. You can find pybullet in Bullet/examples/pybullet.
|
||||||
The BulletExampleBrowser binary will be in Bullet/examples/ExampleBrowser.
|
The BulletExampleBrowser binary will be in Bullet/examples/ExampleBrowser.
|
||||||
|
|
||||||
You can also build Bullet using premake. There are premake executables in the build3 folder.
|
You can also build Bullet using premake. There are premake executables in the build3 folder.
|
||||||
Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
|
Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
|
||||||
Using premake:
|
Using premake:
|
||||||
```
|
```
|
||||||
cd build3
|
cd build3
|
||||||
./premake4_linux --double gmake
|
./premake4_linux --double gmake
|
||||||
./premake4_linux64 --double gmake
|
./premake4_linux64 --double gmake
|
||||||
./premake4_osx --double --enable_pybullet gmake
|
./premake4_osx --double --enable_pybullet gmake
|
||||||
```
|
```
|
||||||
Then
|
Then
|
||||||
```
|
```
|
||||||
cd gmake
|
cd gmake
|
||||||
make
|
make
|
||||||
```
|
```
|
||||||
|
|
||||||
Note that on Linux, you need to use cmake to build pybullet, since the compiler has issues of mixing shared and static libraries.
|
Note that on Linux, you need to use cmake to build pybullet, since the compiler has issues of mixing shared and static libraries.
|
||||||
|
|
||||||
**Mac OSX Xcode**
|
**Mac OSX Xcode**
|
||||||
|
|
||||||
Click on build3/xcode4.command or in a terminal window execute
|
Click on build3/xcode4.command or in a terminal window execute
|
||||||
|
|
||||||
./premake_osx xcode4
|
./premake_osx xcode4
|
||||||
|
|
||||||
## Usage
|
## Usage
|
||||||
|
|
||||||
The App_ExampleBrowser executables will be located in the bin folder.
|
The App_ExampleBrowser executables will be located in the bin folder.
|
||||||
You can just run it though a terminal/command prompt, or by clicking it.
|
You can just run it though a terminal/command prompt, or by clicking it.
|
||||||
|
|
||||||
|
|
||||||
```
|
```
|
||||||
[--start_demo_name="Demo Name"] Start with a selected demo
|
[--start_demo_name="Demo Name"] Start with a selected demo
|
||||||
[--mp4=moviename.mp4] Create a mp4 movie of the window, requires ffmpeg installed
|
[--mp4=moviename.mp4] Create a mp4 movie of the window, requires ffmpeg installed
|
||||||
[--mouse_move_multiplier=0.400000] Set the mouse move sensitivity
|
[--mouse_move_multiplier=0.400000] Set the mouse move sensitivity
|
||||||
[--mouse_wheel_multiplier=0.01] Set the mouse wheel sensitivity
|
[--mouse_wheel_multiplier=0.01] Set the mouse wheel sensitivity
|
||||||
[--background_color_red= 0.9] Set the red component for background color. Same for green and blue
|
[--background_color_red= 0.9] Set the red component for background color. Same for green and blue
|
||||||
[--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666)
|
[--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666)
|
||||||
```
|
```
|
||||||
|
|
||||||
You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
|
You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
|
||||||
Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
|
Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
|
||||||
|
|
||||||
Check out the docs folder and the Bullet physics forums for further information.
|
Check out the docs folder and the Bullet physics forums for further information.
|
||||||
|
|||||||
@ -1,49 +1,49 @@
|
|||||||
LOCAL_PATH := ../../..
|
LOCAL_PATH := ../../..
|
||||||
|
|
||||||
include $(CLEAR_VARS)
|
include $(CLEAR_VARS)
|
||||||
|
|
||||||
LOCAL_CFLAGS := $(LOCAL_C_INCLUDES:%=-I%) -DUSE_PTHREADS -mfpu=neon -mfloat-abi=softfp -pthread -DSCE_PFX_USE_SIMD_VECTORMATH
|
LOCAL_CFLAGS := $(LOCAL_C_INCLUDES:%=-I%) -DUSE_PTHREADS -mfpu=neon -mfloat-abi=softfp -pthread -DSCE_PFX_USE_SIMD_VECTORMATH
|
||||||
|
|
||||||
# apply these flags if needed
|
# apply these flags if needed
|
||||||
# -ffast-math -funsafe-math-optimizations
|
# -ffast-math -funsafe-math-optimizations
|
||||||
|
|
||||||
# apply this to disable optimization
|
# apply this to disable optimization
|
||||||
# TARGET_CFLAGS := $(TARGET_CFLAGS) -O0
|
# TARGET_CFLAGS := $(TARGET_CFLAGS) -O0
|
||||||
|
|
||||||
# apply these 2 to turn on assembly output (*.c/*.cpp to *.s file)
|
# apply these 2 to turn on assembly output (*.c/*.cpp to *.s file)
|
||||||
#compile-cpp-source = $(eval $(call ev-compile-cpp-source,$1,$(1:%$(LOCAL_CPP_EXTENSION)=%.s)))
|
#compile-cpp-source = $(eval $(call ev-compile-cpp-source,$1,$(1:%$(LOCAL_CPP_EXTENSION)=%.s)))
|
||||||
#TARGET_CFLAGS := $(TARGET_CFLAGS) -S
|
#TARGET_CFLAGS := $(TARGET_CFLAGS) -S
|
||||||
|
|
||||||
# Enable or disable NEON. Don't forget to apply, or not apply, -mfpu=neon and -mfloat-abi=softfp
|
# Enable or disable NEON. Don't forget to apply, or not apply, -mfpu=neon and -mfloat-abi=softfp
|
||||||
# flags in addition, e.g., if this is true both of those need to be included in LOCAL_CFLAGS
|
# flags in addition, e.g., if this is true both of those need to be included in LOCAL_CFLAGS
|
||||||
# to avoid the possibility that ndk-build will "forget" to add them on some files
|
# to avoid the possibility that ndk-build will "forget" to add them on some files
|
||||||
LOCAL_ARM_NEON := true
|
LOCAL_ARM_NEON := true
|
||||||
|
|
||||||
TARGET_CFLAGS := $(filter-out -ffpu=vfp,$(TARGET_CFLAGS))
|
TARGET_CFLAGS := $(filter-out -ffpu=vfp,$(TARGET_CFLAGS))
|
||||||
|
|
||||||
# setup to build static library
|
# setup to build static library
|
||||||
LOCAL_MODULE := libBullet
|
LOCAL_MODULE := libBullet
|
||||||
|
|
||||||
LOCAL_C_INCLUDES := $(LOCAL_PATH)/src
|
LOCAL_C_INCLUDES := $(LOCAL_PATH)/src
|
||||||
|
|
||||||
#find all the file recursively under jni/
|
#find all the file recursively under jni/
|
||||||
FILE_LIST := $(wildcard \
|
FILE_LIST := $(wildcard \
|
||||||
$(LOCAL_PATH)/src/LinearMath/*.cpp \
|
$(LOCAL_PATH)/src/LinearMath/*.cpp \
|
||||||
$(LOCAL_PATH)/src/Bullet3Common/*.cpp \
|
$(LOCAL_PATH)/src/Bullet3Common/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletCollision/BroadphaseCollision/*.cpp \
|
$(LOCAL_PATH)/src/BulletCollision/BroadphaseCollision/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletCollision/CollisionDispatch/*.cpp \
|
$(LOCAL_PATH)/src/BulletCollision/CollisionDispatch/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletCollision/CollisionShapes/*.cpp \
|
$(LOCAL_PATH)/src/BulletCollision/CollisionShapes/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletCollision/NarrowPhaseCollision/*.cpp \
|
$(LOCAL_PATH)/src/BulletCollision/NarrowPhaseCollision/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletDynamics/ConstraintSolver/*.cpp \
|
$(LOCAL_PATH)/src/BulletDynamics/ConstraintSolver/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletDynamics/Dynamics/*.cpp \
|
$(LOCAL_PATH)/src/BulletDynamics/Dynamics/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletDynamics/Featherstone/*.cpp \
|
$(LOCAL_PATH)/src/BulletDynamics/Featherstone/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletDynamics/MLCPSolvers/*.cpp \
|
$(LOCAL_PATH)/src/BulletDynamics/MLCPSolvers/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletDynamics/Vehicle/*.cpp \
|
$(LOCAL_PATH)/src/BulletDynamics/Vehicle/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletDynamics/Character/*.cpp \
|
$(LOCAL_PATH)/src/BulletDynamics/Character/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletSoftBody/*.cpp \
|
$(LOCAL_PATH)/src/BulletSoftBody/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletInverseDynamics/*.cpp \
|
$(LOCAL_PATH)/src/BulletInverseDynamics/*.cpp \
|
||||||
$(LOCAL_PATH)/src/BulletInverseDynamics/details/*.cpp \
|
$(LOCAL_PATH)/src/BulletInverseDynamics/details/*.cpp \
|
||||||
)
|
)
|
||||||
LOCAL_SRC_FILES := $(FILE_LIST:$(LOCAL_PATH)/%=%)
|
LOCAL_SRC_FILES := $(FILE_LIST:$(LOCAL_PATH)/%=%)
|
||||||
|
|
||||||
include $(BUILD_STATIC_LIBRARY)
|
include $(BUILD_STATIC_LIBRARY)
|
||||||
|
|||||||
@ -1,7 +1,7 @@
|
|||||||
APP_MODULES := libBullet
|
APP_MODULES := libBullet
|
||||||
APP_ABI := armeabi-v7a
|
APP_ABI := armeabi-v7a
|
||||||
APP_OPTIM := release
|
APP_OPTIM := release
|
||||||
|
|
||||||
#We only need STL for placement new (#include <new>)
|
#We only need STL for placement new (#include <new>)
|
||||||
#We don't use STL in Bullet
|
#We don't use STL in Bullet
|
||||||
APP_STL := stlport_static
|
APP_STL := stlport_static
|
||||||
|
|||||||
@ -1,21 +1,21 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef ET_INCLINED_PLANE_EXAMPLE_H
|
#ifndef ET_INCLINED_PLANE_EXAMPLE_H
|
||||||
#define ET_INCLINED_PLANE_EXAMPLE_H
|
#define ET_INCLINED_PLANE_EXAMPLE_H
|
||||||
|
|
||||||
class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
|
class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
|
||||||
|
|
||||||
#endif //ET_INCLINED_PLANE_EXAMPLE_H
|
#endif //ET_INCLINED_PLANE_EXAMPLE_H
|
||||||
|
|||||||
@ -1,21 +1,21 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef ET_MULTI_PENDULUM_EXAMPLE_H
|
#ifndef ET_MULTI_PENDULUM_EXAMPLE_H
|
||||||
#define ET_MULTI_PENDULUM_EXAMPLE_H
|
#define ET_MULTI_PENDULUM_EXAMPLE_H
|
||||||
|
|
||||||
class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
|
class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
|
||||||
|
|
||||||
#endif //ET_MULTI_PENDULUM_EXAMPLE_H
|
#endif //ET_MULTI_PENDULUM_EXAMPLE_H
|
||||||
|
|||||||
@ -1,21 +1,21 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H
|
#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||||
#define ET_NEWTONS_CRADLE_EXAMPLE_H
|
#define ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||||
|
|
||||||
class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
|
class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
|
||||||
|
|
||||||
#endif //ET_NEWTONS_CRADLE_EXAMPLE_H
|
#endif //ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||||
|
|||||||
@ -1,21 +1,21 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||||
#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||||
|
|
||||||
class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
|
class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
|
||||||
|
|
||||||
#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||||
|
|||||||
@ -1,128 +1,128 @@
|
|||||||
import pybullet as p
|
import pybullet as p
|
||||||
import time
|
import time
|
||||||
import math
|
import math
|
||||||
|
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
#don't create a ground plane, to allow for gaps etc
|
#don't create a ground plane, to allow for gaps etc
|
||||||
p.resetSimulation()
|
p.resetSimulation()
|
||||||
#p.createCollisionShape(p.GEOM_PLANE)
|
#p.createCollisionShape(p.GEOM_PLANE)
|
||||||
#p.createMultiBody(0,0)
|
#p.createMultiBody(0,0)
|
||||||
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
|
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
|
||||||
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
|
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
|
||||||
|
|
||||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
||||||
|
|
||||||
sphereRadius = 0.05
|
sphereRadius = 0.05
|
||||||
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
|
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
|
||||||
|
|
||||||
#a few different ways to create a mesh:
|
#a few different ways to create a mesh:
|
||||||
|
|
||||||
#convex mesh from obj
|
#convex mesh from obj
|
||||||
stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
|
stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
|
||||||
|
|
||||||
boxHalfLength = 0.5
|
boxHalfLength = 0.5
|
||||||
boxHalfWidth = 2.5
|
boxHalfWidth = 2.5
|
||||||
boxHalfHeight = 0.1
|
boxHalfHeight = 0.1
|
||||||
segmentLength = 5
|
segmentLength = 5
|
||||||
|
|
||||||
colBoxId = p.createCollisionShape(p.GEOM_BOX,
|
colBoxId = p.createCollisionShape(p.GEOM_BOX,
|
||||||
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
|
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
|
||||||
|
|
||||||
mass = 1
|
mass = 1
|
||||||
visualShapeId = -1
|
visualShapeId = -1
|
||||||
|
|
||||||
segmentStart = 0
|
segmentStart = 0
|
||||||
|
|
||||||
for i in range(segmentLength):
|
for i in range(segmentLength):
|
||||||
p.createMultiBody(baseMass=0,
|
p.createMultiBody(baseMass=0,
|
||||||
baseCollisionShapeIndex=colBoxId,
|
baseCollisionShapeIndex=colBoxId,
|
||||||
basePosition=[segmentStart, 0, -0.1])
|
basePosition=[segmentStart, 0, -0.1])
|
||||||
segmentStart = segmentStart - 1
|
segmentStart = segmentStart - 1
|
||||||
|
|
||||||
for i in range(segmentLength):
|
for i in range(segmentLength):
|
||||||
height = 0
|
height = 0
|
||||||
if (i % 2):
|
if (i % 2):
|
||||||
height = 1
|
height = 1
|
||||||
p.createMultiBody(baseMass=0,
|
p.createMultiBody(baseMass=0,
|
||||||
baseCollisionShapeIndex=colBoxId,
|
baseCollisionShapeIndex=colBoxId,
|
||||||
basePosition=[segmentStart, 0, -0.1 + height])
|
basePosition=[segmentStart, 0, -0.1 + height])
|
||||||
segmentStart = segmentStart - 1
|
segmentStart = segmentStart - 1
|
||||||
|
|
||||||
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
|
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
|
||||||
|
|
||||||
for i in range(segmentLength):
|
for i in range(segmentLength):
|
||||||
p.createMultiBody(baseMass=0,
|
p.createMultiBody(baseMass=0,
|
||||||
baseCollisionShapeIndex=colBoxId,
|
baseCollisionShapeIndex=colBoxId,
|
||||||
basePosition=[segmentStart, 0, -0.1])
|
basePosition=[segmentStart, 0, -0.1])
|
||||||
segmentStart = segmentStart - 1
|
segmentStart = segmentStart - 1
|
||||||
if (i % 2):
|
if (i % 2):
|
||||||
p.createMultiBody(baseMass=0,
|
p.createMultiBody(baseMass=0,
|
||||||
baseCollisionShapeIndex=colBoxId,
|
baseCollisionShapeIndex=colBoxId,
|
||||||
basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
|
basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
|
||||||
baseOrientation=baseOrientation)
|
baseOrientation=baseOrientation)
|
||||||
|
|
||||||
for i in range(segmentLength):
|
for i in range(segmentLength):
|
||||||
p.createMultiBody(baseMass=0,
|
p.createMultiBody(baseMass=0,
|
||||||
baseCollisionShapeIndex=colBoxId,
|
baseCollisionShapeIndex=colBoxId,
|
||||||
basePosition=[segmentStart, 0, -0.1])
|
basePosition=[segmentStart, 0, -0.1])
|
||||||
width = 4
|
width = 4
|
||||||
for j in range(width):
|
for j in range(width):
|
||||||
p.createMultiBody(baseMass=0,
|
p.createMultiBody(baseMass=0,
|
||||||
baseCollisionShapeIndex=stoneId,
|
baseCollisionShapeIndex=stoneId,
|
||||||
basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
|
basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
|
||||||
segmentStart = segmentStart - 1
|
segmentStart = segmentStart - 1
|
||||||
|
|
||||||
link_Masses = [1]
|
link_Masses = [1]
|
||||||
linkCollisionShapeIndices = [colBoxId]
|
linkCollisionShapeIndices = [colBoxId]
|
||||||
linkVisualShapeIndices = [-1]
|
linkVisualShapeIndices = [-1]
|
||||||
linkPositions = [[0, 0, 0]]
|
linkPositions = [[0, 0, 0]]
|
||||||
linkOrientations = [[0, 0, 0, 1]]
|
linkOrientations = [[0, 0, 0, 1]]
|
||||||
linkInertialFramePositions = [[0, 0, 0]]
|
linkInertialFramePositions = [[0, 0, 0]]
|
||||||
linkInertialFrameOrientations = [[0, 0, 0, 1]]
|
linkInertialFrameOrientations = [[0, 0, 0, 1]]
|
||||||
indices = [0]
|
indices = [0]
|
||||||
jointTypes = [p.JOINT_REVOLUTE]
|
jointTypes = [p.JOINT_REVOLUTE]
|
||||||
axis = [[1, 0, 0]]
|
axis = [[1, 0, 0]]
|
||||||
|
|
||||||
baseOrientation = [0, 0, 0, 1]
|
baseOrientation = [0, 0, 0, 1]
|
||||||
for i in range(segmentLength):
|
for i in range(segmentLength):
|
||||||
boxId = p.createMultiBody(0,
|
boxId = p.createMultiBody(0,
|
||||||
colSphereId,
|
colSphereId,
|
||||||
-1, [segmentStart, 0, -0.1],
|
-1, [segmentStart, 0, -0.1],
|
||||||
baseOrientation,
|
baseOrientation,
|
||||||
linkMasses=link_Masses,
|
linkMasses=link_Masses,
|
||||||
linkCollisionShapeIndices=linkCollisionShapeIndices,
|
linkCollisionShapeIndices=linkCollisionShapeIndices,
|
||||||
linkVisualShapeIndices=linkVisualShapeIndices,
|
linkVisualShapeIndices=linkVisualShapeIndices,
|
||||||
linkPositions=linkPositions,
|
linkPositions=linkPositions,
|
||||||
linkOrientations=linkOrientations,
|
linkOrientations=linkOrientations,
|
||||||
linkInertialFramePositions=linkInertialFramePositions,
|
linkInertialFramePositions=linkInertialFramePositions,
|
||||||
linkInertialFrameOrientations=linkInertialFrameOrientations,
|
linkInertialFrameOrientations=linkInertialFrameOrientations,
|
||||||
linkParentIndices=indices,
|
linkParentIndices=indices,
|
||||||
linkJointTypes=jointTypes,
|
linkJointTypes=jointTypes,
|
||||||
linkJointAxis=axis)
|
linkJointAxis=axis)
|
||||||
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
|
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
|
||||||
print(p.getNumJoints(boxId))
|
print(p.getNumJoints(boxId))
|
||||||
for joint in range(p.getNumJoints(boxId)):
|
for joint in range(p.getNumJoints(boxId)):
|
||||||
targetVelocity = 10
|
targetVelocity = 10
|
||||||
if (i % 2):
|
if (i % 2):
|
||||||
targetVelocity = -10
|
targetVelocity = -10
|
||||||
p.setJointMotorControl2(boxId,
|
p.setJointMotorControl2(boxId,
|
||||||
joint,
|
joint,
|
||||||
p.VELOCITY_CONTROL,
|
p.VELOCITY_CONTROL,
|
||||||
targetVelocity=targetVelocity,
|
targetVelocity=targetVelocity,
|
||||||
force=100)
|
force=100)
|
||||||
segmentStart = segmentStart - 1.1
|
segmentStart = segmentStart - 1.1
|
||||||
|
|
||||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||||||
while (1):
|
while (1):
|
||||||
camData = p.getDebugVisualizerCamera()
|
camData = p.getDebugVisualizerCamera()
|
||||||
viewMat = camData[2]
|
viewMat = camData[2]
|
||||||
projMat = camData[3]
|
projMat = camData[3]
|
||||||
p.getCameraImage(256,
|
p.getCameraImage(256,
|
||||||
256,
|
256,
|
||||||
viewMatrix=viewMat,
|
viewMatrix=viewMat,
|
||||||
projectionMatrix=projMat,
|
projectionMatrix=projMat,
|
||||||
renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
||||||
keys = p.getKeyboardEvents()
|
keys = p.getKeyboardEvents()
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
#print(keys)
|
#print(keys)
|
||||||
time.sleep(0.01)
|
time.sleep(0.01)
|
||||||
|
|||||||
@ -1,26 +1,26 @@
|
|||||||
import pybullet as p
|
import pybullet as p
|
||||||
import time
|
import time
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
|
|
||||||
p.resetSimulation()
|
p.resetSimulation()
|
||||||
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
|
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
|
||||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
||||||
print("load plane.urdf")
|
print("load plane.urdf")
|
||||||
p.loadURDF("plane.urdf")
|
p.loadURDF("plane.urdf")
|
||||||
print("load r2d2.urdf")
|
print("load r2d2.urdf")
|
||||||
|
|
||||||
p.loadURDF("r2d2.urdf", 0, 0, 1)
|
p.loadURDF("r2d2.urdf", 0, 0, 1)
|
||||||
print("load kitchen/1.sdf")
|
print("load kitchen/1.sdf")
|
||||||
p.loadSDF("kitchens/1.sdf")
|
p.loadSDF("kitchens/1.sdf")
|
||||||
print("load 100 times plate.urdf")
|
print("load 100 times plate.urdf")
|
||||||
for i in range(100):
|
for i in range(100):
|
||||||
p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
|
p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
|
||||||
|
|
||||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||||||
|
|
||||||
p.stopStateLogging(timinglog)
|
p.stopStateLogging(timinglog)
|
||||||
print("stopped state logging")
|
print("stopped state logging")
|
||||||
p.getCameraImage(320, 200)
|
p.getCameraImage(320, 200)
|
||||||
|
|
||||||
while (1):
|
while (1):
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
|
|||||||
@ -1,150 +1,150 @@
|
|||||||
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
|
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
|
||||||
#otherwise use testrender.py (slower but compatible without numpy)
|
#otherwise use testrender.py (slower but compatible without numpy)
|
||||||
#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
|
#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import sys
|
import sys
|
||||||
import time
|
import time
|
||||||
import itertools
|
import itertools
|
||||||
import subprocess
|
import subprocess
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import pybullet
|
import pybullet
|
||||||
from multiprocessing import Process
|
from multiprocessing import Process
|
||||||
|
|
||||||
camTargetPos = [0, 0, 0]
|
camTargetPos = [0, 0, 0]
|
||||||
cameraUp = [0, 0, 1]
|
cameraUp = [0, 0, 1]
|
||||||
cameraPos = [1, 1, 1]
|
cameraPos = [1, 1, 1]
|
||||||
|
|
||||||
pitch = -10.0
|
pitch = -10.0
|
||||||
roll = 0
|
roll = 0
|
||||||
upAxisIndex = 2
|
upAxisIndex = 2
|
||||||
camDistance = 4
|
camDistance = 4
|
||||||
pixelWidth = 84 # 320
|
pixelWidth = 84 # 320
|
||||||
pixelHeight = 84 # 200
|
pixelHeight = 84 # 200
|
||||||
nearPlane = 0.01
|
nearPlane = 0.01
|
||||||
farPlane = 100
|
farPlane = 100
|
||||||
fov = 60
|
fov = 60
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
|
||||||
class BulletSim():
|
class BulletSim():
|
||||||
|
|
||||||
def __init__(self, connection_mode, *argv):
|
def __init__(self, connection_mode, *argv):
|
||||||
self.connection_mode = connection_mode
|
self.connection_mode = connection_mode
|
||||||
self.argv = argv
|
self.argv = argv
|
||||||
|
|
||||||
def __enter__(self):
|
def __enter__(self):
|
||||||
print("connecting")
|
print("connecting")
|
||||||
optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
|
optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
|
||||||
optionstring += ' --window_backend=2 --render_device=0'
|
optionstring += ' --window_backend=2 --render_device=0'
|
||||||
|
|
||||||
print(self.connection_mode, optionstring, *self.argv)
|
print(self.connection_mode, optionstring, *self.argv)
|
||||||
cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
|
cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
|
||||||
if cid < 0:
|
if cid < 0:
|
||||||
raise ValueError
|
raise ValueError
|
||||||
print("connected")
|
print("connected")
|
||||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
|
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
|
||||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
|
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
|
||||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
|
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
|
||||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
|
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
|
||||||
|
|
||||||
pybullet.resetSimulation()
|
pybullet.resetSimulation()
|
||||||
pybullet.loadURDF("plane.urdf", [0, 0, -1])
|
pybullet.loadURDF("plane.urdf", [0, 0, -1])
|
||||||
pybullet.loadURDF("r2d2.urdf")
|
pybullet.loadURDF("r2d2.urdf")
|
||||||
pybullet.loadURDF("duck_vhacd.urdf")
|
pybullet.loadURDF("duck_vhacd.urdf")
|
||||||
pybullet.setGravity(0, 0, -10)
|
pybullet.setGravity(0, 0, -10)
|
||||||
|
|
||||||
def __exit__(self, *_, **__):
|
def __exit__(self, *_, **__):
|
||||||
pybullet.disconnect()
|
pybullet.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test(num_runs=300, shadow=1, log=True, plot=False):
|
def test(num_runs=300, shadow=1, log=True, plot=False):
|
||||||
if log:
|
if log:
|
||||||
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
|
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
|
||||||
|
|
||||||
if plot:
|
if plot:
|
||||||
plt.ion()
|
plt.ion()
|
||||||
|
|
||||||
img = np.random.rand(200, 320)
|
img = np.random.rand(200, 320)
|
||||||
#img = [tandard_normal((50,100))
|
#img = [tandard_normal((50,100))
|
||||||
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
|
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
|
||||||
ax = plt.gca()
|
ax = plt.gca()
|
||||||
|
|
||||||
times = np.zeros(num_runs)
|
times = np.zeros(num_runs)
|
||||||
yaw_gen = itertools.cycle(range(0, 360, 10))
|
yaw_gen = itertools.cycle(range(0, 360, 10))
|
||||||
for i, yaw in zip(range(num_runs), yaw_gen):
|
for i, yaw in zip(range(num_runs), yaw_gen):
|
||||||
pybullet.stepSimulation()
|
pybullet.stepSimulation()
|
||||||
start = time.time()
|
start = time.time()
|
||||||
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
|
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
|
||||||
roll, upAxisIndex)
|
roll, upAxisIndex)
|
||||||
aspect = pixelWidth / pixelHeight
|
aspect = pixelWidth / pixelHeight
|
||||||
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
|
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
|
||||||
img_arr = pybullet.getCameraImage(pixelWidth,
|
img_arr = pybullet.getCameraImage(pixelWidth,
|
||||||
pixelHeight,
|
pixelHeight,
|
||||||
viewMatrix,
|
viewMatrix,
|
||||||
projectionMatrix,
|
projectionMatrix,
|
||||||
shadow=shadow,
|
shadow=shadow,
|
||||||
lightDirection=[1, 1, 1],
|
lightDirection=[1, 1, 1],
|
||||||
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
|
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
|
||||||
#renderer=pybullet.ER_TINY_RENDERER)
|
#renderer=pybullet.ER_TINY_RENDERER)
|
||||||
stop = time.time()
|
stop = time.time()
|
||||||
duration = (stop - start)
|
duration = (stop - start)
|
||||||
if (duration):
|
if (duration):
|
||||||
fps = 1. / duration
|
fps = 1. / duration
|
||||||
#print("fps=",fps)
|
#print("fps=",fps)
|
||||||
else:
|
else:
|
||||||
fps = 0
|
fps = 0
|
||||||
#print("fps=",fps)
|
#print("fps=",fps)
|
||||||
#print("duraction=",duration)
|
#print("duraction=",duration)
|
||||||
#print("fps=",fps)
|
#print("fps=",fps)
|
||||||
times[i] = fps
|
times[i] = fps
|
||||||
|
|
||||||
if plot:
|
if plot:
|
||||||
rgb = img_arr[2]
|
rgb = img_arr[2]
|
||||||
image.set_data(rgb) #np_img_arr)
|
image.set_data(rgb) #np_img_arr)
|
||||||
ax.plot([0])
|
ax.plot([0])
|
||||||
#plt.draw()
|
#plt.draw()
|
||||||
#plt.show()
|
#plt.show()
|
||||||
plt.pause(0.01)
|
plt.pause(0.01)
|
||||||
|
|
||||||
mean_time = float(np.mean(times))
|
mean_time = float(np.mean(times))
|
||||||
print("mean: {0} for {1} runs".format(mean_time, num_runs))
|
print("mean: {0} for {1} runs".format(mean_time, num_runs))
|
||||||
print("")
|
print("")
|
||||||
if log:
|
if log:
|
||||||
pybullet.stopStateLogging(logId)
|
pybullet.stopStateLogging(logId)
|
||||||
return mean_time
|
return mean_time
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|
||||||
res = []
|
res = []
|
||||||
|
|
||||||
with BulletSim(pybullet.DIRECT):
|
with BulletSim(pybullet.DIRECT):
|
||||||
print("\nTesting DIRECT")
|
print("\nTesting DIRECT")
|
||||||
mean_time = test(log=False, plot=True)
|
mean_time = test(log=False, plot=True)
|
||||||
res.append(("tiny", mean_time))
|
res.append(("tiny", mean_time))
|
||||||
|
|
||||||
with BulletSim(pybullet.DIRECT):
|
with BulletSim(pybullet.DIRECT):
|
||||||
plugin_fn = os.path.join(
|
plugin_fn = os.path.join(
|
||||||
pybullet.__file__.split("bullet3")[0],
|
pybullet.__file__.split("bullet3")[0],
|
||||||
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
|
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
|
||||||
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
|
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
|
||||||
if plugin < 0:
|
if plugin < 0:
|
||||||
print("\nPlugin Failed to load!\n")
|
print("\nPlugin Failed to load!\n")
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
print("\nTesting DIRECT+OpenGL")
|
print("\nTesting DIRECT+OpenGL")
|
||||||
mean_time = test(log=True)
|
mean_time = test(log=True)
|
||||||
res.append(("plugin", mean_time))
|
res.append(("plugin", mean_time))
|
||||||
|
|
||||||
with BulletSim(pybullet.GUI):
|
with BulletSim(pybullet.GUI):
|
||||||
print("\nTesting GUI")
|
print("\nTesting GUI")
|
||||||
mean_time = test(log=False)
|
mean_time = test(log=False)
|
||||||
res.append(("egl", mean_time))
|
res.append(("egl", mean_time))
|
||||||
|
|
||||||
print()
|
print()
|
||||||
print("rendertest.py")
|
print("rendertest.py")
|
||||||
print("back nenv fps fps_tot")
|
print("back nenv fps fps_tot")
|
||||||
for r in res:
|
for r in res:
|
||||||
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))
|
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))
|
||||||
|
|||||||
@ -1,110 +1,110 @@
|
|||||||
import pybullet as p
|
import pybullet as p
|
||||||
import time
|
import time
|
||||||
|
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
plane = p.loadURDF("plane.urdf")
|
plane = p.loadURDF("plane.urdf")
|
||||||
p.setGravity(0, 0, -9.8)
|
p.setGravity(0, 0, -9.8)
|
||||||
p.setTimeStep(1. / 500)
|
p.setTimeStep(1. / 500)
|
||||||
#p.setDefaultContactERP(0)
|
#p.setDefaultContactERP(0)
|
||||||
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
||||||
urdfFlags = p.URDF_USE_SELF_COLLISION
|
urdfFlags = p.URDF_USE_SELF_COLLISION
|
||||||
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
||||||
flags=urdfFlags,
|
flags=urdfFlags,
|
||||||
useFixedBase=False)
|
useFixedBase=False)
|
||||||
|
|
||||||
#enable collision between lower legs
|
#enable collision between lower legs
|
||||||
|
|
||||||
for j in range(p.getNumJoints(quadruped)):
|
for j in range(p.getNumJoints(quadruped)):
|
||||||
print(p.getJointInfo(quadruped, j))
|
print(p.getJointInfo(quadruped, j))
|
||||||
|
|
||||||
#2,5,8 and 11 are the lower legs
|
#2,5,8 and 11 are the lower legs
|
||||||
lower_legs = [2, 5, 8, 11]
|
lower_legs = [2, 5, 8, 11]
|
||||||
for l0 in lower_legs:
|
for l0 in lower_legs:
|
||||||
for l1 in lower_legs:
|
for l1 in lower_legs:
|
||||||
if (l1 > l0):
|
if (l1 > l0):
|
||||||
enableCollision = 1
|
enableCollision = 1
|
||||||
print("collision for pair", l0, l1,
|
print("collision for pair", l0, l1,
|
||||||
p.getJointInfo(quadruped, l0)[12],
|
p.getJointInfo(quadruped, l0)[12],
|
||||||
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
||||||
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
||||||
|
|
||||||
jointIds = []
|
jointIds = []
|
||||||
paramIds = []
|
paramIds = []
|
||||||
jointOffsets = []
|
jointOffsets = []
|
||||||
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
||||||
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||||
|
|
||||||
for i in range(4):
|
for i in range(4):
|
||||||
jointOffsets.append(0)
|
jointOffsets.append(0)
|
||||||
jointOffsets.append(-0.7)
|
jointOffsets.append(-0.7)
|
||||||
jointOffsets.append(0.7)
|
jointOffsets.append(0.7)
|
||||||
|
|
||||||
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
||||||
|
|
||||||
for j in range(p.getNumJoints(quadruped)):
|
for j in range(p.getNumJoints(quadruped)):
|
||||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||||
info = p.getJointInfo(quadruped, j)
|
info = p.getJointInfo(quadruped, j)
|
||||||
#print(info)
|
#print(info)
|
||||||
jointName = info[1]
|
jointName = info[1]
|
||||||
jointType = info[2]
|
jointType = info[2]
|
||||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||||
jointIds.append(j)
|
jointIds.append(j)
|
||||||
|
|
||||||
p.getCameraImage(480, 320)
|
p.getCameraImage(480, 320)
|
||||||
p.setRealTimeSimulation(0)
|
p.setRealTimeSimulation(0)
|
||||||
|
|
||||||
joints = []
|
joints = []
|
||||||
|
|
||||||
with open("data1.txt", "r") as filestream:
|
with open("data1.txt", "r") as filestream:
|
||||||
for line in filestream:
|
for line in filestream:
|
||||||
print("line=", line)
|
print("line=", line)
|
||||||
maxForce = p.readUserDebugParameter(maxForceId)
|
maxForce = p.readUserDebugParameter(maxForceId)
|
||||||
currentline = line.split(",")
|
currentline = line.split(",")
|
||||||
#print (currentline)
|
#print (currentline)
|
||||||
#print("-----")
|
#print("-----")
|
||||||
frame = currentline[0]
|
frame = currentline[0]
|
||||||
t = currentline[1]
|
t = currentline[1]
|
||||||
#print("frame[",frame,"]")
|
#print("frame[",frame,"]")
|
||||||
joints = currentline[2:14]
|
joints = currentline[2:14]
|
||||||
#print("joints=",joints)
|
#print("joints=",joints)
|
||||||
for j in range(12):
|
for j in range(12):
|
||||||
targetPos = float(joints[j])
|
targetPos = float(joints[j])
|
||||||
p.setJointMotorControl2(quadruped,
|
p.setJointMotorControl2(quadruped,
|
||||||
jointIds[j],
|
jointIds[j],
|
||||||
p.POSITION_CONTROL,
|
p.POSITION_CONTROL,
|
||||||
jointDirections[j] * targetPos + jointOffsets[j],
|
jointDirections[j] * targetPos + jointOffsets[j],
|
||||||
force=maxForce)
|
force=maxForce)
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
for lower_leg in lower_legs:
|
for lower_leg in lower_legs:
|
||||||
#print("points for ", quadruped, " link: ", lower_leg)
|
#print("points for ", quadruped, " link: ", lower_leg)
|
||||||
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
||||||
#print("num points=",len(pts))
|
#print("num points=",len(pts))
|
||||||
#for pt in pts:
|
#for pt in pts:
|
||||||
# print(pt[9])
|
# print(pt[9])
|
||||||
time.sleep(1. / 500.)
|
time.sleep(1. / 500.)
|
||||||
|
|
||||||
for j in range(p.getNumJoints(quadruped)):
|
for j in range(p.getNumJoints(quadruped)):
|
||||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||||
info = p.getJointInfo(quadruped, j)
|
info = p.getJointInfo(quadruped, j)
|
||||||
js = p.getJointState(quadruped, j)
|
js = p.getJointState(quadruped, j)
|
||||||
#print(info)
|
#print(info)
|
||||||
jointName = info[1]
|
jointName = info[1]
|
||||||
jointType = info[2]
|
jointType = info[2]
|
||||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||||
paramIds.append(
|
paramIds.append(
|
||||||
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
||||||
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
||||||
|
|
||||||
p.setRealTimeSimulation(1)
|
p.setRealTimeSimulation(1)
|
||||||
|
|
||||||
while (1):
|
while (1):
|
||||||
|
|
||||||
for i in range(len(paramIds)):
|
for i in range(len(paramIds)):
|
||||||
c = paramIds[i]
|
c = paramIds[i]
|
||||||
targetPos = p.readUserDebugParameter(c)
|
targetPos = p.readUserDebugParameter(c)
|
||||||
maxForce = p.readUserDebugParameter(maxForceId)
|
maxForce = p.readUserDebugParameter(maxForceId)
|
||||||
p.setJointMotorControl2(quadruped,
|
p.setJointMotorControl2(quadruped,
|
||||||
jointIds[i],
|
jointIds[i],
|
||||||
p.POSITION_CONTROL,
|
p.POSITION_CONTROL,
|
||||||
jointDirections[i] * targetPos + jointOffsets[i],
|
jointDirections[i] * targetPos + jointOffsets[i],
|
||||||
force=maxForce)
|
force=maxForce)
|
||||||
|
|||||||
@ -1 +1 @@
|
|||||||
from . import *
|
from . import *
|
||||||
|
|||||||
@ -1,114 +1,114 @@
|
|||||||
import pybullet as p
|
import pybullet as p
|
||||||
import pybullet_data as pd
|
import pybullet_data as pd
|
||||||
|
|
||||||
import time
|
import time
|
||||||
|
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
p.setAdditionalSearchPath(pd.getDataPath())
|
p.setAdditionalSearchPath(pd.getDataPath())
|
||||||
|
|
||||||
plane = p.loadURDF("plane.urdf")
|
plane = p.loadURDF("plane.urdf")
|
||||||
p.setGravity(0, 0, -9.8)
|
p.setGravity(0, 0, -9.8)
|
||||||
p.setTimeStep(1. / 500)
|
p.setTimeStep(1. / 500)
|
||||||
#p.setDefaultContactERP(0)
|
#p.setDefaultContactERP(0)
|
||||||
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
||||||
urdfFlags = p.URDF_USE_SELF_COLLISION
|
urdfFlags = p.URDF_USE_SELF_COLLISION
|
||||||
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
||||||
flags=urdfFlags,
|
flags=urdfFlags,
|
||||||
useFixedBase=False)
|
useFixedBase=False)
|
||||||
|
|
||||||
#enable collision between lower legs
|
#enable collision between lower legs
|
||||||
|
|
||||||
for j in range(p.getNumJoints(quadruped)):
|
for j in range(p.getNumJoints(quadruped)):
|
||||||
print(p.getJointInfo(quadruped, j))
|
print(p.getJointInfo(quadruped, j))
|
||||||
|
|
||||||
#2,5,8 and 11 are the lower legs
|
#2,5,8 and 11 are the lower legs
|
||||||
lower_legs = [2, 5, 8, 11]
|
lower_legs = [2, 5, 8, 11]
|
||||||
for l0 in lower_legs:
|
for l0 in lower_legs:
|
||||||
for l1 in lower_legs:
|
for l1 in lower_legs:
|
||||||
if (l1 > l0):
|
if (l1 > l0):
|
||||||
enableCollision = 1
|
enableCollision = 1
|
||||||
print("collision for pair", l0, l1,
|
print("collision for pair", l0, l1,
|
||||||
p.getJointInfo(quadruped, l0)[12],
|
p.getJointInfo(quadruped, l0)[12],
|
||||||
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
||||||
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
||||||
|
|
||||||
jointIds = []
|
jointIds = []
|
||||||
paramIds = []
|
paramIds = []
|
||||||
jointOffsets = []
|
jointOffsets = []
|
||||||
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
||||||
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||||
|
|
||||||
for i in range(4):
|
for i in range(4):
|
||||||
jointOffsets.append(0)
|
jointOffsets.append(0)
|
||||||
jointOffsets.append(-0.7)
|
jointOffsets.append(-0.7)
|
||||||
jointOffsets.append(0.7)
|
jointOffsets.append(0.7)
|
||||||
|
|
||||||
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
||||||
|
|
||||||
for j in range(p.getNumJoints(quadruped)):
|
for j in range(p.getNumJoints(quadruped)):
|
||||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||||
info = p.getJointInfo(quadruped, j)
|
info = p.getJointInfo(quadruped, j)
|
||||||
#print(info)
|
#print(info)
|
||||||
jointName = info[1]
|
jointName = info[1]
|
||||||
jointType = info[2]
|
jointType = info[2]
|
||||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||||
jointIds.append(j)
|
jointIds.append(j)
|
||||||
|
|
||||||
p.getCameraImage(480, 320)
|
p.getCameraImage(480, 320)
|
||||||
p.setRealTimeSimulation(0)
|
p.setRealTimeSimulation(0)
|
||||||
|
|
||||||
joints = []
|
joints = []
|
||||||
|
|
||||||
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
|
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
|
||||||
for line in filestream:
|
for line in filestream:
|
||||||
print("line=", line)
|
print("line=", line)
|
||||||
maxForce = p.readUserDebugParameter(maxForceId)
|
maxForce = p.readUserDebugParameter(maxForceId)
|
||||||
currentline = line.split(",")
|
currentline = line.split(",")
|
||||||
#print (currentline)
|
#print (currentline)
|
||||||
#print("-----")
|
#print("-----")
|
||||||
frame = currentline[0]
|
frame = currentline[0]
|
||||||
t = currentline[1]
|
t = currentline[1]
|
||||||
#print("frame[",frame,"]")
|
#print("frame[",frame,"]")
|
||||||
joints = currentline[2:14]
|
joints = currentline[2:14]
|
||||||
#print("joints=",joints)
|
#print("joints=",joints)
|
||||||
for j in range(12):
|
for j in range(12):
|
||||||
targetPos = float(joints[j])
|
targetPos = float(joints[j])
|
||||||
p.setJointMotorControl2(quadruped,
|
p.setJointMotorControl2(quadruped,
|
||||||
jointIds[j],
|
jointIds[j],
|
||||||
p.POSITION_CONTROL,
|
p.POSITION_CONTROL,
|
||||||
jointDirections[j] * targetPos + jointOffsets[j],
|
jointDirections[j] * targetPos + jointOffsets[j],
|
||||||
force=maxForce)
|
force=maxForce)
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
for lower_leg in lower_legs:
|
for lower_leg in lower_legs:
|
||||||
#print("points for ", quadruped, " link: ", lower_leg)
|
#print("points for ", quadruped, " link: ", lower_leg)
|
||||||
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
||||||
#print("num points=",len(pts))
|
#print("num points=",len(pts))
|
||||||
#for pt in pts:
|
#for pt in pts:
|
||||||
# print(pt[9])
|
# print(pt[9])
|
||||||
time.sleep(1. / 500.)
|
time.sleep(1. / 500.)
|
||||||
|
|
||||||
for j in range(p.getNumJoints(quadruped)):
|
for j in range(p.getNumJoints(quadruped)):
|
||||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||||
info = p.getJointInfo(quadruped, j)
|
info = p.getJointInfo(quadruped, j)
|
||||||
js = p.getJointState(quadruped, j)
|
js = p.getJointState(quadruped, j)
|
||||||
#print(info)
|
#print(info)
|
||||||
jointName = info[1]
|
jointName = info[1]
|
||||||
jointType = info[2]
|
jointType = info[2]
|
||||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||||
paramIds.append(
|
paramIds.append(
|
||||||
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
||||||
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
||||||
|
|
||||||
p.setRealTimeSimulation(1)
|
p.setRealTimeSimulation(1)
|
||||||
|
|
||||||
while (1):
|
while (1):
|
||||||
|
|
||||||
for i in range(len(paramIds)):
|
for i in range(len(paramIds)):
|
||||||
c = paramIds[i]
|
c = paramIds[i]
|
||||||
targetPos = p.readUserDebugParameter(c)
|
targetPos = p.readUserDebugParameter(c)
|
||||||
maxForce = p.readUserDebugParameter(maxForceId)
|
maxForce = p.readUserDebugParameter(maxForceId)
|
||||||
p.setJointMotorControl2(quadruped,
|
p.setJointMotorControl2(quadruped,
|
||||||
jointIds[i],
|
jointIds[i],
|
||||||
p.POSITION_CONTROL,
|
p.POSITION_CONTROL,
|
||||||
jointDirections[i] * targetPos + jointOffsets[i],
|
jointDirections[i] * targetPos + jointOffsets[i],
|
||||||
force=maxForce)
|
force=maxForce)
|
||||||
|
|||||||
@ -1,354 +1,354 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
|
Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||||
#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||||
|
|
||||||
#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
|
#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
|
||||||
#include "btGjkEpa3.h"
|
#include "btGjkEpa3.h"
|
||||||
#include "btGjkCollisionDescription.h"
|
#include "btGjkCollisionDescription.h"
|
||||||
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
|
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
|
||||||
|
|
||||||
template <typename btConvexTemplate>
|
template <typename btConvexTemplate>
|
||||||
bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
|
bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
|
||||||
const btGjkCollisionDescription& colDesc,
|
const btGjkCollisionDescription& colDesc,
|
||||||
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
|
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
|
||||||
{
|
{
|
||||||
(void)v;
|
(void)v;
|
||||||
|
|
||||||
// const btScalar radialmargin(btScalar(0.));
|
// const btScalar radialmargin(btScalar(0.));
|
||||||
|
|
||||||
btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input?
|
btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input?
|
||||||
|
|
||||||
btGjkEpaSolver3::sResults results;
|
btGjkEpaSolver3::sResults results;
|
||||||
|
|
||||||
if (btGjkEpaSolver3_Penetration(a, b, guessVector, results))
|
if (btGjkEpaSolver3_Penetration(a, b, guessVector, results))
|
||||||
|
|
||||||
{
|
{
|
||||||
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
|
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
|
||||||
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
|
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
|
||||||
wWitnessOnA = results.witnesses[0];
|
wWitnessOnA = results.witnesses[0];
|
||||||
wWitnessOnB = results.witnesses[1];
|
wWitnessOnB = results.witnesses[1];
|
||||||
v = results.normal;
|
v = results.normal;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (btGjkEpaSolver3_Distance(a, b, guessVector, results))
|
if (btGjkEpaSolver3_Distance(a, b, guessVector, results))
|
||||||
{
|
{
|
||||||
wWitnessOnA = results.witnesses[0];
|
wWitnessOnA = results.witnesses[0];
|
||||||
wWitnessOnB = results.witnesses[1];
|
wWitnessOnB = results.witnesses[1];
|
||||||
v = results.normal;
|
v = results.normal;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename btConvexTemplate, typename btGjkDistanceTemplate>
|
template <typename btConvexTemplate, typename btGjkDistanceTemplate>
|
||||||
int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
|
int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
|
||||||
{
|
{
|
||||||
bool m_catchDegeneracies = true;
|
bool m_catchDegeneracies = true;
|
||||||
btScalar m_cachedSeparatingDistance = 0.f;
|
btScalar m_cachedSeparatingDistance = 0.f;
|
||||||
|
|
||||||
btScalar distance = btScalar(0.);
|
btScalar distance = btScalar(0.);
|
||||||
btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
|
btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
|
||||||
|
|
||||||
btVector3 pointOnA, pointOnB;
|
btVector3 pointOnA, pointOnB;
|
||||||
btTransform localTransA = a.getWorldTransform();
|
btTransform localTransA = a.getWorldTransform();
|
||||||
btTransform localTransB = b.getWorldTransform();
|
btTransform localTransB = b.getWorldTransform();
|
||||||
|
|
||||||
btScalar marginA = a.getMargin();
|
btScalar marginA = a.getMargin();
|
||||||
btScalar marginB = b.getMargin();
|
btScalar marginB = b.getMargin();
|
||||||
|
|
||||||
int m_curIter = 0;
|
int m_curIter = 0;
|
||||||
int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN?
|
int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN?
|
||||||
btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
|
btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
|
||||||
|
|
||||||
bool isValid = false;
|
bool isValid = false;
|
||||||
bool checkSimplex = false;
|
bool checkSimplex = false;
|
||||||
bool checkPenetration = true;
|
bool checkPenetration = true;
|
||||||
int m_degenerateSimplex = 0;
|
int m_degenerateSimplex = 0;
|
||||||
|
|
||||||
int m_lastUsedMethod = -1;
|
int m_lastUsedMethod = -1;
|
||||||
|
|
||||||
{
|
{
|
||||||
btScalar squaredDistance = BT_LARGE_FLOAT;
|
btScalar squaredDistance = BT_LARGE_FLOAT;
|
||||||
btScalar delta = btScalar(0.);
|
btScalar delta = btScalar(0.);
|
||||||
|
|
||||||
btScalar margin = marginA + marginB;
|
btScalar margin = marginA + marginB;
|
||||||
|
|
||||||
simplexSolver.reset();
|
simplexSolver.reset();
|
||||||
|
|
||||||
for (;;)
|
for (;;)
|
||||||
//while (true)
|
//while (true)
|
||||||
{
|
{
|
||||||
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
|
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
|
||||||
btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
|
btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
|
||||||
|
|
||||||
btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
|
btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
|
||||||
btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
|
btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
|
||||||
|
|
||||||
btVector3 pWorld = localTransA(pInA);
|
btVector3 pWorld = localTransA(pInA);
|
||||||
btVector3 qWorld = localTransB(qInB);
|
btVector3 qWorld = localTransB(qInB);
|
||||||
|
|
||||||
btVector3 w = pWorld - qWorld;
|
btVector3 w = pWorld - qWorld;
|
||||||
delta = m_cachedSeparatingAxis.dot(w);
|
delta = m_cachedSeparatingAxis.dot(w);
|
||||||
|
|
||||||
// potential exit, they don't overlap
|
// potential exit, they don't overlap
|
||||||
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
|
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
|
||||||
{
|
{
|
||||||
m_degenerateSimplex = 10;
|
m_degenerateSimplex = 10;
|
||||||
checkSimplex = true;
|
checkSimplex = true;
|
||||||
//checkPenetration = false;
|
//checkPenetration = false;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
||||||
if (simplexSolver.inSimplex(w))
|
if (simplexSolver.inSimplex(w))
|
||||||
{
|
{
|
||||||
m_degenerateSimplex = 1;
|
m_degenerateSimplex = 1;
|
||||||
checkSimplex = true;
|
checkSimplex = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
// are we getting any closer ?
|
// are we getting any closer ?
|
||||||
btScalar f0 = squaredDistance - delta;
|
btScalar f0 = squaredDistance - delta;
|
||||||
btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
|
btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
|
||||||
|
|
||||||
if (f0 <= f1)
|
if (f0 <= f1)
|
||||||
{
|
{
|
||||||
if (f0 <= btScalar(0.))
|
if (f0 <= btScalar(0.))
|
||||||
{
|
{
|
||||||
m_degenerateSimplex = 2;
|
m_degenerateSimplex = 2;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
m_degenerateSimplex = 11;
|
m_degenerateSimplex = 11;
|
||||||
}
|
}
|
||||||
checkSimplex = true;
|
checkSimplex = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
//add current vertex to simplex
|
//add current vertex to simplex
|
||||||
simplexSolver.addVertex(w, pWorld, qWorld);
|
simplexSolver.addVertex(w, pWorld, qWorld);
|
||||||
btVector3 newCachedSeparatingAxis;
|
btVector3 newCachedSeparatingAxis;
|
||||||
|
|
||||||
//calculate the closest point to the origin (update vector v)
|
//calculate the closest point to the origin (update vector v)
|
||||||
if (!simplexSolver.closest(newCachedSeparatingAxis))
|
if (!simplexSolver.closest(newCachedSeparatingAxis))
|
||||||
{
|
{
|
||||||
m_degenerateSimplex = 3;
|
m_degenerateSimplex = 3;
|
||||||
checkSimplex = true;
|
checkSimplex = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2)
|
if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2)
|
||||||
{
|
{
|
||||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||||
m_degenerateSimplex = 6;
|
m_degenerateSimplex = 6;
|
||||||
checkSimplex = true;
|
checkSimplex = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
btScalar previousSquaredDistance = squaredDistance;
|
btScalar previousSquaredDistance = squaredDistance;
|
||||||
squaredDistance = newCachedSeparatingAxis.length2();
|
squaredDistance = newCachedSeparatingAxis.length2();
|
||||||
#if 0
|
#if 0
|
||||||
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
|
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
|
||||||
if (squaredDistance>previousSquaredDistance)
|
if (squaredDistance>previousSquaredDistance)
|
||||||
{
|
{
|
||||||
m_degenerateSimplex = 7;
|
m_degenerateSimplex = 7;
|
||||||
squaredDistance = previousSquaredDistance;
|
squaredDistance = previousSquaredDistance;
|
||||||
checkSimplex = false;
|
checkSimplex = false;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#endif //
|
#endif //
|
||||||
|
|
||||||
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||||
|
|
||||||
//are we getting any closer ?
|
//are we getting any closer ?
|
||||||
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
|
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
|
||||||
{
|
{
|
||||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||||
checkSimplex = true;
|
checkSimplex = true;
|
||||||
m_degenerateSimplex = 12;
|
m_degenerateSimplex = 12;
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||||
|
|
||||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||||
if (m_curIter++ > gGjkMaxIter)
|
if (m_curIter++ > gGjkMaxIter)
|
||||||
{
|
{
|
||||||
#if defined(DEBUG) || defined(_DEBUG)
|
#if defined(DEBUG) || defined(_DEBUG)
|
||||||
|
|
||||||
printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
|
printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
|
||||||
printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
|
printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
|
||||||
m_cachedSeparatingAxis.getX(),
|
m_cachedSeparatingAxis.getX(),
|
||||||
m_cachedSeparatingAxis.getY(),
|
m_cachedSeparatingAxis.getY(),
|
||||||
m_cachedSeparatingAxis.getZ(),
|
m_cachedSeparatingAxis.getZ(),
|
||||||
squaredDistance);
|
squaredDistance);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool check = (!simplexSolver.fullSimplex());
|
bool check = (!simplexSolver.fullSimplex());
|
||||||
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
|
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
|
||||||
|
|
||||||
if (!check)
|
if (!check)
|
||||||
{
|
{
|
||||||
//do we need this backup_closest here ?
|
//do we need this backup_closest here ?
|
||||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||||
m_degenerateSimplex = 13;
|
m_degenerateSimplex = 13;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (checkSimplex)
|
if (checkSimplex)
|
||||||
{
|
{
|
||||||
simplexSolver.compute_points(pointOnA, pointOnB);
|
simplexSolver.compute_points(pointOnA, pointOnB);
|
||||||
normalInB = m_cachedSeparatingAxis;
|
normalInB = m_cachedSeparatingAxis;
|
||||||
|
|
||||||
btScalar lenSqr = m_cachedSeparatingAxis.length2();
|
btScalar lenSqr = m_cachedSeparatingAxis.length2();
|
||||||
|
|
||||||
//valid normal
|
//valid normal
|
||||||
if (lenSqr < 0.0001)
|
if (lenSqr < 0.0001)
|
||||||
{
|
{
|
||||||
m_degenerateSimplex = 5;
|
m_degenerateSimplex = 5;
|
||||||
}
|
}
|
||||||
if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
|
if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
|
||||||
{
|
{
|
||||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
|
btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
|
||||||
normalInB *= rlen; //normalize
|
normalInB *= rlen; //normalize
|
||||||
|
|
||||||
btScalar s = btSqrt(squaredDistance);
|
btScalar s = btSqrt(squaredDistance);
|
||||||
|
|
||||||
btAssert(s > btScalar(0.0));
|
btAssert(s > btScalar(0.0));
|
||||||
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
||||||
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
||||||
distance = ((btScalar(1.) / rlen) - margin);
|
distance = ((btScalar(1.) / rlen) - margin);
|
||||||
isValid = true;
|
isValid = true;
|
||||||
|
|
||||||
m_lastUsedMethod = 1;
|
m_lastUsedMethod = 1;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
m_lastUsedMethod = 2;
|
m_lastUsedMethod = 2;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool catchDegeneratePenetrationCase =
|
bool catchDegeneratePenetrationCase =
|
||||||
(m_catchDegeneracies && m_degenerateSimplex && ((distance + margin) < 0.01));
|
(m_catchDegeneracies && m_degenerateSimplex && ((distance + margin) < 0.01));
|
||||||
|
|
||||||
//if (checkPenetration && !isValid)
|
//if (checkPenetration && !isValid)
|
||||||
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
|
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
|
||||||
{
|
{
|
||||||
//penetration case
|
//penetration case
|
||||||
|
|
||||||
//if there is no way to handle penetrations, bail out
|
//if there is no way to handle penetrations, bail out
|
||||||
|
|
||||||
// Penetration depth case.
|
// Penetration depth case.
|
||||||
btVector3 tmpPointOnA, tmpPointOnB;
|
btVector3 tmpPointOnA, tmpPointOnB;
|
||||||
|
|
||||||
m_cachedSeparatingAxis.setZero();
|
m_cachedSeparatingAxis.setZero();
|
||||||
|
|
||||||
bool isValid2 = btGjkEpaCalcPenDepth(a, b,
|
bool isValid2 = btGjkEpaCalcPenDepth(a, b,
|
||||||
colDesc,
|
colDesc,
|
||||||
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
|
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
|
||||||
|
|
||||||
if (isValid2)
|
if (isValid2)
|
||||||
{
|
{
|
||||||
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
|
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
|
||||||
btScalar lenSqr = tmpNormalInB.length2();
|
btScalar lenSqr = tmpNormalInB.length2();
|
||||||
if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
|
if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
|
||||||
{
|
{
|
||||||
tmpNormalInB = m_cachedSeparatingAxis;
|
tmpNormalInB = m_cachedSeparatingAxis;
|
||||||
lenSqr = m_cachedSeparatingAxis.length2();
|
lenSqr = m_cachedSeparatingAxis.length2();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
|
if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
|
||||||
{
|
{
|
||||||
tmpNormalInB /= btSqrt(lenSqr);
|
tmpNormalInB /= btSqrt(lenSqr);
|
||||||
btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
|
btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
|
||||||
//only replace valid penetrations when the result is deeper (check)
|
//only replace valid penetrations when the result is deeper (check)
|
||||||
if (!isValid || (distance2 < distance))
|
if (!isValid || (distance2 < distance))
|
||||||
{
|
{
|
||||||
distance = distance2;
|
distance = distance2;
|
||||||
pointOnA = tmpPointOnA;
|
pointOnA = tmpPointOnA;
|
||||||
pointOnB = tmpPointOnB;
|
pointOnB = tmpPointOnB;
|
||||||
normalInB = tmpNormalInB;
|
normalInB = tmpNormalInB;
|
||||||
|
|
||||||
isValid = true;
|
isValid = true;
|
||||||
m_lastUsedMethod = 3;
|
m_lastUsedMethod = 3;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
m_lastUsedMethod = 8;
|
m_lastUsedMethod = 8;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
m_lastUsedMethod = 9;
|
m_lastUsedMethod = 9;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
|
|
||||||
{
|
{
|
||||||
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
|
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
|
||||||
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
|
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
|
||||||
///reports a valid positive distance. Use the results of the second GJK instead of failing.
|
///reports a valid positive distance. Use the results of the second GJK instead of failing.
|
||||||
///thanks to Jacob.Langford for the reproduction case
|
///thanks to Jacob.Langford for the reproduction case
|
||||||
///http://code.google.com/p/bullet/issues/detail?id=250
|
///http://code.google.com/p/bullet/issues/detail?id=250
|
||||||
|
|
||||||
if (m_cachedSeparatingAxis.length2() > btScalar(0.))
|
if (m_cachedSeparatingAxis.length2() > btScalar(0.))
|
||||||
{
|
{
|
||||||
btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
|
btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
|
||||||
//only replace valid distances when the distance is less
|
//only replace valid distances when the distance is less
|
||||||
if (!isValid || (distance2 < distance))
|
if (!isValid || (distance2 < distance))
|
||||||
{
|
{
|
||||||
distance = distance2;
|
distance = distance2;
|
||||||
pointOnA = tmpPointOnA;
|
pointOnA = tmpPointOnA;
|
||||||
pointOnB = tmpPointOnB;
|
pointOnB = tmpPointOnB;
|
||||||
pointOnA -= m_cachedSeparatingAxis * marginA;
|
pointOnA -= m_cachedSeparatingAxis * marginA;
|
||||||
pointOnB += m_cachedSeparatingAxis * marginB;
|
pointOnB += m_cachedSeparatingAxis * marginB;
|
||||||
normalInB = m_cachedSeparatingAxis;
|
normalInB = m_cachedSeparatingAxis;
|
||||||
normalInB.normalize();
|
normalInB.normalize();
|
||||||
|
|
||||||
isValid = true;
|
isValid = true;
|
||||||
m_lastUsedMethod = 6;
|
m_lastUsedMethod = 6;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
m_lastUsedMethod = 5;
|
m_lastUsedMethod = 5;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (isValid && ((distance < 0) || (distance * distance < colDesc.m_maximumDistanceSquared)))
|
if (isValid && ((distance < 0) || (distance * distance < colDesc.m_maximumDistanceSquared)))
|
||||||
{
|
{
|
||||||
m_cachedSeparatingAxis = normalInB;
|
m_cachedSeparatingAxis = normalInB;
|
||||||
m_cachedSeparatingDistance = distance;
|
m_cachedSeparatingDistance = distance;
|
||||||
distInfo->m_distance = distance;
|
distInfo->m_distance = distance;
|
||||||
distInfo->m_normalBtoA = normalInB;
|
distInfo->m_normalBtoA = normalInB;
|
||||||
distInfo->m_pointOnB = pointOnB;
|
distInfo->m_pointOnB = pointOnB;
|
||||||
distInfo->m_pointOnA = pointOnB + normalInB * distance;
|
distInfo->m_pointOnA = pointOnB + normalInB * distance;
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
return -m_lastUsedMethod;
|
return -m_lastUsedMethod;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -1,4 +1,4 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||||
|
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -1,23 +1,23 @@
|
|||||||
project "BulletCollision"
|
project "BulletCollision"
|
||||||
|
|
||||||
kind "StaticLib"
|
kind "StaticLib"
|
||||||
if os.is("Linux") then
|
if os.is("Linux") then
|
||||||
buildoptions{"-fPIC"}
|
buildoptions{"-fPIC"}
|
||||||
end
|
end
|
||||||
includedirs {
|
includedirs {
|
||||||
"..",
|
"..",
|
||||||
}
|
}
|
||||||
files {
|
files {
|
||||||
"*.cpp",
|
"*.cpp",
|
||||||
"*.h",
|
"*.h",
|
||||||
"BroadphaseCollision/*.cpp",
|
"BroadphaseCollision/*.cpp",
|
||||||
"BroadphaseCollision/*.h",
|
"BroadphaseCollision/*.h",
|
||||||
"CollisionDispatch/*.cpp",
|
"CollisionDispatch/*.cpp",
|
||||||
"CollisionDispatch/*.h",
|
"CollisionDispatch/*.h",
|
||||||
"CollisionShapes/*.cpp",
|
"CollisionShapes/*.cpp",
|
||||||
"CollisionShapes/*.h",
|
"CollisionShapes/*.h",
|
||||||
"Gimpact/*.cpp",
|
"Gimpact/*.cpp",
|
||||||
"Gimpact/*.h",
|
"Gimpact/*.h",
|
||||||
"NarrowPhaseCollision/*.cpp",
|
"NarrowPhaseCollision/*.cpp",
|
||||||
"NarrowPhaseCollision/*.h",
|
"NarrowPhaseCollision/*.h",
|
||||||
}
|
}
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -1,215 +1,215 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
///This file was written by Erwin Coumans
|
///This file was written by Erwin Coumans
|
||||||
|
|
||||||
#include "btMultiBodyFixedConstraint.h"
|
#include "btMultiBodyFixedConstraint.h"
|
||||||
#include "btMultiBodyLinkCollider.h"
|
#include "btMultiBodyLinkCollider.h"
|
||||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||||
#include "LinearMath/btIDebugDraw.h"
|
#include "LinearMath/btIDebugDraw.h"
|
||||||
|
|
||||||
#define BTMBFIXEDCONSTRAINT_DIM 6
|
#define BTMBFIXEDCONSTRAINT_DIM 6
|
||||||
|
|
||||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
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),
|
||||||
m_rigidBodyA(0),
|
m_rigidBodyA(0),
|
||||||
m_rigidBodyB(bodyB),
|
m_rigidBodyB(bodyB),
|
||||||
m_pivotInA(pivotInA),
|
m_pivotInA(pivotInA),
|
||||||
m_pivotInB(pivotInB),
|
m_pivotInB(pivotInB),
|
||||||
m_frameInA(frameInA),
|
m_frameInA(frameInA),
|
||||||
m_frameInB(frameInB)
|
m_frameInB(frameInB)
|
||||||
{
|
{
|
||||||
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
||||||
}
|
}
|
||||||
|
|
||||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
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),
|
||||||
m_rigidBodyA(0),
|
m_rigidBodyA(0),
|
||||||
m_rigidBodyB(0),
|
m_rigidBodyB(0),
|
||||||
m_pivotInA(pivotInA),
|
m_pivotInA(pivotInA),
|
||||||
m_pivotInB(pivotInB),
|
m_pivotInB(pivotInB),
|
||||||
m_frameInA(frameInA),
|
m_frameInA(frameInA),
|
||||||
m_frameInB(frameInB)
|
m_frameInB(frameInB)
|
||||||
{
|
{
|
||||||
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyFixedConstraint::finalizeMultiDof()
|
void btMultiBodyFixedConstraint::finalizeMultiDof()
|
||||||
{
|
{
|
||||||
//not implemented yet
|
//not implemented yet
|
||||||
btAssert(0);
|
btAssert(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
|
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodyFixedConstraint::getIslandIdA() const
|
int btMultiBodyFixedConstraint::getIslandIdA() const
|
||||||
{
|
{
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
return m_rigidBodyA->getIslandTag();
|
return m_rigidBodyA->getIslandTag();
|
||||||
|
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
{
|
{
|
||||||
if (m_linkA < 0)
|
if (m_linkA < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodyFixedConstraint::getIslandIdB() const
|
int btMultiBodyFixedConstraint::getIslandIdB() const
|
||||||
{
|
{
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
return m_rigidBodyB->getIslandTag();
|
return m_rigidBodyB->getIslandTag();
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
{
|
{
|
||||||
if (m_linkB < 0)
|
if (m_linkB < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
||||||
{
|
{
|
||||||
int numDim = BTMBFIXEDCONSTRAINT_DIM;
|
int numDim = BTMBFIXEDCONSTRAINT_DIM;
|
||||||
for (int i = 0; i < numDim; i++)
|
for (int i = 0; i < numDim; i++)
|
||||||
{
|
{
|
||||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||||
constraintRow.m_orgConstraint = this;
|
constraintRow.m_orgConstraint = this;
|
||||||
constraintRow.m_orgDofIndex = i;
|
constraintRow.m_orgDofIndex = i;
|
||||||
constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
|
constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
|
||||||
constraintRow.m_contactNormal1.setValue(0, 0, 0);
|
constraintRow.m_contactNormal1.setValue(0, 0, 0);
|
||||||
constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
|
constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
|
||||||
constraintRow.m_contactNormal2.setValue(0, 0, 0);
|
constraintRow.m_contactNormal2.setValue(0, 0, 0);
|
||||||
constraintRow.m_angularComponentA.setValue(0, 0, 0);
|
constraintRow.m_angularComponentA.setValue(0, 0, 0);
|
||||||
constraintRow.m_angularComponentB.setValue(0, 0, 0);
|
constraintRow.m_angularComponentB.setValue(0, 0, 0);
|
||||||
|
|
||||||
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
||||||
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
||||||
|
|
||||||
// Convert local points back to world
|
// Convert local points back to world
|
||||||
btVector3 pivotAworld = m_pivotInA;
|
btVector3 pivotAworld = m_pivotInA;
|
||||||
btMatrix3x3 frameAworld = m_frameInA;
|
btMatrix3x3 frameAworld = m_frameInA;
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
{
|
{
|
||||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||||
frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
{
|
{
|
||||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||||
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
|
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
btVector3 pivotBworld = m_pivotInB;
|
btVector3 pivotBworld = m_pivotInB;
|
||||||
btMatrix3x3 frameBworld = m_frameInB;
|
btMatrix3x3 frameBworld = m_frameInB;
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
{
|
{
|
||||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||||
frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
{
|
{
|
||||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||||
frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
|
frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
||||||
btVector3 angleDiff;
|
btVector3 angleDiff;
|
||||||
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
||||||
|
|
||||||
btVector3 constraintNormalLin(0, 0, 0);
|
btVector3 constraintNormalLin(0, 0, 0);
|
||||||
btVector3 constraintNormalAng(0, 0, 0);
|
btVector3 constraintNormalAng(0, 0, 0);
|
||||||
btScalar posError = 0.0;
|
btScalar posError = 0.0;
|
||||||
if (i < 3)
|
if (i < 3)
|
||||||
{
|
{
|
||||||
constraintNormalLin[i] = 1;
|
constraintNormalLin[i] = 1;
|
||||||
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
||||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||||
constraintNormalLin, pivotAworld, pivotBworld,
|
constraintNormalLin, pivotAworld, pivotBworld,
|
||||||
posError,
|
posError,
|
||||||
infoGlobal,
|
infoGlobal,
|
||||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{ //i>=3
|
{ //i>=3
|
||||||
constraintNormalAng = frameAworld.getColumn(i % 3);
|
constraintNormalAng = frameAworld.getColumn(i % 3);
|
||||||
posError = angleDiff[i % 3];
|
posError = angleDiff[i % 3];
|
||||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||||
constraintNormalLin, pivotAworld, pivotBworld,
|
constraintNormalLin, pivotAworld, pivotBworld,
|
||||||
posError,
|
posError,
|
||||||
infoGlobal,
|
infoGlobal,
|
||||||
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
|
void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
|
||||||
{
|
{
|
||||||
btTransform tr;
|
btTransform tr;
|
||||||
tr.setIdentity();
|
tr.setIdentity();
|
||||||
|
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
{
|
{
|
||||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||||
tr.setOrigin(pivot);
|
tr.setOrigin(pivot);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
{
|
{
|
||||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||||
tr.setOrigin(pivotAworld);
|
tr.setOrigin(pivotAworld);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
{
|
{
|
||||||
// that ideally should draw the same frame
|
// that ideally should draw the same frame
|
||||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||||
tr.setOrigin(pivot);
|
tr.setOrigin(pivot);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
{
|
{
|
||||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||||
tr.setOrigin(pivotBworld);
|
tr.setOrigin(pivotBworld);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,183 +1,183 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
///This file was written by Erwin Coumans
|
///This file was written by Erwin Coumans
|
||||||
|
|
||||||
#include "btMultiBodyJointMotor.h"
|
#include "btMultiBodyJointMotor.h"
|
||||||
#include "btMultiBody.h"
|
#include "btMultiBody.h"
|
||||||
#include "btMultiBodyLinkCollider.h"
|
#include "btMultiBodyLinkCollider.h"
|
||||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||||
|
|
||||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
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),
|
||||||
m_desiredVelocity(desiredVelocity),
|
m_desiredVelocity(desiredVelocity),
|
||||||
m_desiredPosition(0),
|
m_desiredPosition(0),
|
||||||
m_kd(1.),
|
m_kd(1.),
|
||||||
m_kp(0),
|
m_kp(0),
|
||||||
m_erp(1),
|
m_erp(1),
|
||||||
m_rhsClamp(SIMD_INFINITY)
|
m_rhsClamp(SIMD_INFINITY)
|
||||||
{
|
{
|
||||||
m_maxAppliedImpulse = maxMotorImpulse;
|
m_maxAppliedImpulse = maxMotorImpulse;
|
||||||
// the data.m_jacobians never change, so may as well
|
// the data.m_jacobians never change, so may as well
|
||||||
// initialize them here
|
// initialize them here
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyJointMotor::finalizeMultiDof()
|
void btMultiBodyJointMotor::finalizeMultiDof()
|
||||||
{
|
{
|
||||||
allocateJacobiansMultiDof();
|
allocateJacobiansMultiDof();
|
||||||
// note: we rely on the fact that data.m_jacobians are
|
// note: we rely on the fact that data.m_jacobians are
|
||||||
// always initialized to zero by the Constraint ctor
|
// always initialized to zero by the Constraint ctor
|
||||||
int linkDoF = 0;
|
int linkDoF = 0;
|
||||||
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
|
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
|
||||||
|
|
||||||
// row 0: the lower bound
|
// row 0: the lower bound
|
||||||
// row 0: the lower bound
|
// row 0: the lower bound
|
||||||
jacobianA(0)[offset] = 1;
|
jacobianA(0)[offset] = 1;
|
||||||
|
|
||||||
m_numDofsFinalized = m_jacSizeBoth;
|
m_numDofsFinalized = m_jacSizeBoth;
|
||||||
}
|
}
|
||||||
|
|
||||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||||
//:btMultiBodyConstraint(body,0,link,-1,1,true),
|
//: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),
|
||||||
m_desiredVelocity(desiredVelocity),
|
m_desiredVelocity(desiredVelocity),
|
||||||
m_desiredPosition(0),
|
m_desiredPosition(0),
|
||||||
m_kd(1.),
|
m_kd(1.),
|
||||||
m_kp(0),
|
m_kp(0),
|
||||||
m_erp(1),
|
m_erp(1),
|
||||||
m_rhsClamp(SIMD_INFINITY)
|
m_rhsClamp(SIMD_INFINITY)
|
||||||
{
|
{
|
||||||
btAssert(linkDoF < body->getLink(link).m_dofCount);
|
btAssert(linkDoF < body->getLink(link).m_dofCount);
|
||||||
|
|
||||||
m_maxAppliedImpulse = maxMotorImpulse;
|
m_maxAppliedImpulse = maxMotorImpulse;
|
||||||
}
|
}
|
||||||
btMultiBodyJointMotor::~btMultiBodyJointMotor()
|
btMultiBodyJointMotor::~btMultiBodyJointMotor()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodyJointMotor::getIslandIdA() const
|
int btMultiBodyJointMotor::getIslandIdA() const
|
||||||
{
|
{
|
||||||
if (this->m_linkA < 0)
|
if (this->m_linkA < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||||
{
|
{
|
||||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodyJointMotor::getIslandIdB() const
|
int btMultiBodyJointMotor::getIslandIdB() const
|
||||||
{
|
{
|
||||||
if (m_linkB < 0)
|
if (m_linkB < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||||
{
|
{
|
||||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||||
btMultiBodyJacobianData& data,
|
btMultiBodyJacobianData& data,
|
||||||
const btContactSolverInfo& infoGlobal)
|
const btContactSolverInfo& infoGlobal)
|
||||||
{
|
{
|
||||||
// only positions need to be updated -- data.m_jacobians and force
|
// only positions need to be updated -- data.m_jacobians and force
|
||||||
// directions were set in the ctor and never change.
|
// directions were set in the ctor and never change.
|
||||||
|
|
||||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||||
{
|
{
|
||||||
finalizeMultiDof();
|
finalizeMultiDof();
|
||||||
}
|
}
|
||||||
|
|
||||||
//don't crash
|
//don't crash
|
||||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
if (m_maxAppliedImpulse == 0.f)
|
if (m_maxAppliedImpulse == 0.f)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
const btScalar posError = 0;
|
const btScalar posError = 0;
|
||||||
const btVector3 dummy(0, 0, 0);
|
const btVector3 dummy(0, 0, 0);
|
||||||
|
|
||||||
for (int row = 0; row < getNumRows(); row++)
|
for (int row = 0; row < getNumRows(); row++)
|
||||||
{
|
{
|
||||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||||
|
|
||||||
int dof = 0;
|
int dof = 0;
|
||||||
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
|
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
|
||||||
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
|
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
|
||||||
btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
|
btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
|
||||||
|
|
||||||
btScalar velocityError = (m_desiredVelocity - currentVelocity);
|
btScalar velocityError = (m_desiredVelocity - currentVelocity);
|
||||||
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
|
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
|
||||||
if (rhs > m_rhsClamp)
|
if (rhs > m_rhsClamp)
|
||||||
{
|
{
|
||||||
rhs = m_rhsClamp;
|
rhs = m_rhsClamp;
|
||||||
}
|
}
|
||||||
if (rhs < -m_rhsClamp)
|
if (rhs < -m_rhsClamp)
|
||||||
{
|
{
|
||||||
rhs = -m_rhsClamp;
|
rhs = -m_rhsClamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
|
fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
|
||||||
constraintRow.m_orgConstraint = this;
|
constraintRow.m_orgConstraint = this;
|
||||||
constraintRow.m_orgDofIndex = row;
|
constraintRow.m_orgDofIndex = row;
|
||||||
{
|
{
|
||||||
//expect either prismatic or revolute joint type for now
|
//expect either prismatic or revolute joint type for now
|
||||||
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
|
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
|
||||||
switch (m_bodyA->getLink(m_linkA).m_jointType)
|
switch (m_bodyA->getLink(m_linkA).m_jointType)
|
||||||
{
|
{
|
||||||
case btMultibodyLink::eRevolute:
|
case btMultibodyLink::eRevolute:
|
||||||
{
|
{
|
||||||
constraintRow.m_contactNormal1.setZero();
|
constraintRow.m_contactNormal1.setZero();
|
||||||
constraintRow.m_contactNormal2.setZero();
|
constraintRow.m_contactNormal2.setZero();
|
||||||
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
|
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
|
||||||
constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
|
constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
|
||||||
constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
|
constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case btMultibodyLink::ePrismatic:
|
case btMultibodyLink::ePrismatic:
|
||||||
{
|
{
|
||||||
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
|
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
|
||||||
constraintRow.m_contactNormal1 = prismaticAxisInWorld;
|
constraintRow.m_contactNormal1 = prismaticAxisInWorld;
|
||||||
constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
|
constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
|
||||||
constraintRow.m_relpos1CrossNormal.setZero();
|
constraintRow.m_relpos1CrossNormal.setZero();
|
||||||
constraintRow.m_relpos2CrossNormal.setZero();
|
constraintRow.m_relpos2CrossNormal.setZero();
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
btAssert(0);
|
btAssert(0);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,77 +1,77 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
///This file was written by Erwin Coumans
|
///This file was written by Erwin Coumans
|
||||||
|
|
||||||
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
|
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
|
||||||
#define BT_MULTIBODY_JOINT_MOTOR_H
|
#define BT_MULTIBODY_JOINT_MOTOR_H
|
||||||
|
|
||||||
#include "btMultiBodyConstraint.h"
|
#include "btMultiBodyConstraint.h"
|
||||||
struct btSolverInfo;
|
struct btSolverInfo;
|
||||||
|
|
||||||
class btMultiBodyJointMotor : public btMultiBodyConstraint
|
class btMultiBodyJointMotor : public btMultiBodyConstraint
|
||||||
{
|
{
|
||||||
protected:
|
protected:
|
||||||
btScalar m_desiredVelocity;
|
btScalar m_desiredVelocity;
|
||||||
btScalar m_desiredPosition;
|
btScalar m_desiredPosition;
|
||||||
btScalar m_kd;
|
btScalar m_kd;
|
||||||
btScalar m_kp;
|
btScalar m_kp;
|
||||||
btScalar m_erp;
|
btScalar m_erp;
|
||||||
btScalar m_rhsClamp; //maximum error
|
btScalar m_rhsClamp; //maximum error
|
||||||
|
|
||||||
public:
|
public:
|
||||||
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||||
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||||
virtual ~btMultiBodyJointMotor();
|
virtual ~btMultiBodyJointMotor();
|
||||||
virtual void finalizeMultiDof();
|
virtual void finalizeMultiDof();
|
||||||
|
|
||||||
virtual int getIslandIdA() const;
|
virtual int getIslandIdA() const;
|
||||||
virtual int getIslandIdB() const;
|
virtual int getIslandIdB() const;
|
||||||
|
|
||||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||||
btMultiBodyJacobianData& data,
|
btMultiBodyJacobianData& data,
|
||||||
const btContactSolverInfo& infoGlobal);
|
const btContactSolverInfo& infoGlobal);
|
||||||
|
|
||||||
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
|
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
|
||||||
{
|
{
|
||||||
m_desiredVelocity = velTarget;
|
m_desiredVelocity = velTarget;
|
||||||
m_kd = kd;
|
m_kd = kd;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
|
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
|
||||||
{
|
{
|
||||||
m_desiredPosition = posTarget;
|
m_desiredPosition = posTarget;
|
||||||
m_kp = kp;
|
m_kp = kp;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void setErp(btScalar erp)
|
virtual void setErp(btScalar erp)
|
||||||
{
|
{
|
||||||
m_erp = erp;
|
m_erp = erp;
|
||||||
}
|
}
|
||||||
virtual btScalar getErp() const
|
virtual btScalar getErp() const
|
||||||
{
|
{
|
||||||
return m_erp;
|
return m_erp;
|
||||||
}
|
}
|
||||||
virtual void setRhsClamp(btScalar rhsClamp)
|
virtual void setRhsClamp(btScalar rhsClamp)
|
||||||
{
|
{
|
||||||
m_rhsClamp = rhsClamp;
|
m_rhsClamp = rhsClamp;
|
||||||
}
|
}
|
||||||
virtual void debugDraw(class btIDebugDraw* drawer)
|
virtual void debugDraw(class btIDebugDraw* drawer)
|
||||||
{
|
{
|
||||||
//todo(erwincoumans)
|
//todo(erwincoumans)
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //BT_MULTIBODY_JOINT_MOTOR_H
|
#endif //BT_MULTIBODY_JOINT_MOTOR_H
|
||||||
|
|||||||
@ -1,239 +1,239 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef BT_MULTIBODY_LINK_H
|
#ifndef BT_MULTIBODY_LINK_H
|
||||||
#define BT_MULTIBODY_LINK_H
|
#define BT_MULTIBODY_LINK_H
|
||||||
|
|
||||||
#include "LinearMath/btQuaternion.h"
|
#include "LinearMath/btQuaternion.h"
|
||||||
#include "LinearMath/btVector3.h"
|
#include "LinearMath/btVector3.h"
|
||||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||||
|
|
||||||
enum btMultiBodyLinkFlags
|
enum btMultiBodyLinkFlags
|
||||||
{
|
{
|
||||||
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
|
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
|
||||||
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
|
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
|
||||||
};
|
};
|
||||||
|
|
||||||
//both defines are now permanently enabled
|
//both defines are now permanently enabled
|
||||||
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||||
#define TEST_SPATIAL_ALGEBRA_LAYER
|
#define TEST_SPATIAL_ALGEBRA_LAYER
|
||||||
|
|
||||||
//
|
//
|
||||||
// Various spatial helper functions
|
// Various spatial helper functions
|
||||||
//
|
//
|
||||||
|
|
||||||
//namespace {
|
//namespace {
|
||||||
|
|
||||||
#include "LinearMath/btSpatialAlgebra.h"
|
#include "LinearMath/btSpatialAlgebra.h"
|
||||||
|
|
||||||
//}
|
//}
|
||||||
|
|
||||||
//
|
//
|
||||||
// Link struct
|
// Link struct
|
||||||
//
|
//
|
||||||
|
|
||||||
struct btMultibodyLink
|
struct btMultibodyLink
|
||||||
{
|
{
|
||||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
|
|
||||||
btScalar m_mass; // mass of link
|
btScalar m_mass; // mass of link
|
||||||
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
|
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
|
||||||
|
|
||||||
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
|
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
|
||||||
|
|
||||||
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
|
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
|
||||||
|
|
||||||
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
|
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
|
||||||
//this is set to zero for planar joint (see also m_eVector comment)
|
//this is set to zero for planar joint (see also m_eVector comment)
|
||||||
|
|
||||||
// m_eVector is constant, but depends on the joint type:
|
// m_eVector is constant, but depends on the joint type:
|
||||||
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
|
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
|
||||||
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
|
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
|
||||||
// todo: fix the planar so it is consistent with the other joints
|
// todo: fix the planar so it is consistent with the other joints
|
||||||
|
|
||||||
btVector3 m_eVector;
|
btVector3 m_eVector;
|
||||||
|
|
||||||
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
|
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
|
||||||
|
|
||||||
enum eFeatherstoneJointType
|
enum eFeatherstoneJointType
|
||||||
{
|
{
|
||||||
eRevolute = 0,
|
eRevolute = 0,
|
||||||
ePrismatic = 1,
|
ePrismatic = 1,
|
||||||
eSpherical = 2,
|
eSpherical = 2,
|
||||||
ePlanar = 3,
|
ePlanar = 3,
|
||||||
eFixed = 4,
|
eFixed = 4,
|
||||||
eInvalid
|
eInvalid
|
||||||
};
|
};
|
||||||
|
|
||||||
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
|
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
|
||||||
// for prismatic: m_axesTop[0] = zero;
|
// for prismatic: m_axesTop[0] = zero;
|
||||||
// m_axesBottom[0] = unit vector along the joint axis.
|
// m_axesBottom[0] = unit vector along the joint axis.
|
||||||
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
|
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
|
||||||
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
|
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
|
||||||
//
|
//
|
||||||
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
|
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
|
||||||
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
|
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
|
||||||
//
|
//
|
||||||
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
|
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
|
||||||
// m_axesTop[1][2] = zero
|
// m_axesTop[1][2] = zero
|
||||||
// m_axesBottom[0] = zero
|
// m_axesBottom[0] = zero
|
||||||
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
|
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
|
||||||
btSpatialMotionVector m_axes[6];
|
btSpatialMotionVector m_axes[6];
|
||||||
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
|
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
|
||||||
void setAxisBottom(int dof, const btVector3 &axis)
|
void setAxisBottom(int dof, const btVector3 &axis)
|
||||||
{
|
{
|
||||||
m_axes[dof].m_bottomVec = axis;
|
m_axes[dof].m_bottomVec = axis;
|
||||||
}
|
}
|
||||||
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
||||||
{
|
{
|
||||||
m_axes[dof].m_topVec.setValue(x, y, z);
|
m_axes[dof].m_topVec.setValue(x, y, z);
|
||||||
}
|
}
|
||||||
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
||||||
{
|
{
|
||||||
m_axes[dof].m_bottomVec.setValue(x, y, z);
|
m_axes[dof].m_bottomVec.setValue(x, y, z);
|
||||||
}
|
}
|
||||||
const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
|
const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
|
||||||
const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
|
const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
|
||||||
|
|
||||||
int m_dofOffset, m_cfgOffset;
|
int m_dofOffset, m_cfgOffset;
|
||||||
|
|
||||||
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
|
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
|
||||||
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
|
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
|
||||||
|
|
||||||
btVector3 m_appliedForce; // In WORLD frame
|
btVector3 m_appliedForce; // In WORLD frame
|
||||||
btVector3 m_appliedTorque; // In WORLD frame
|
btVector3 m_appliedTorque; // In WORLD frame
|
||||||
|
|
||||||
btVector3 m_appliedConstraintForce; // In WORLD frame
|
btVector3 m_appliedConstraintForce; // In WORLD frame
|
||||||
btVector3 m_appliedConstraintTorque; // In WORLD frame
|
btVector3 m_appliedConstraintTorque; // In WORLD frame
|
||||||
|
|
||||||
btScalar m_jointPos[7];
|
btScalar m_jointPos[7];
|
||||||
|
|
||||||
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
|
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
|
||||||
//It gets set to zero after each internal stepSimulation call
|
//It gets set to zero after each internal stepSimulation call
|
||||||
btScalar m_jointTorque[6];
|
btScalar m_jointTorque[6];
|
||||||
|
|
||||||
class btMultiBodyLinkCollider *m_collider;
|
class btMultiBodyLinkCollider *m_collider;
|
||||||
int m_flags;
|
int m_flags;
|
||||||
|
|
||||||
int m_dofCount, m_posVarCount; //redundant but handy
|
int m_dofCount, m_posVarCount; //redundant but handy
|
||||||
|
|
||||||
eFeatherstoneJointType m_jointType;
|
eFeatherstoneJointType m_jointType;
|
||||||
|
|
||||||
struct btMultiBodyJointFeedback *m_jointFeedback;
|
struct btMultiBodyJointFeedback *m_jointFeedback;
|
||||||
|
|
||||||
btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics
|
btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics
|
||||||
|
|
||||||
const char *m_linkName; //m_linkName memory needs to be managed by the developer/user!
|
const char *m_linkName; //m_linkName memory needs to be managed by the developer/user!
|
||||||
const char *m_jointName; //m_jointName memory needs to be managed by the developer/user!
|
const char *m_jointName; //m_jointName memory needs to be managed by the developer/user!
|
||||||
const void *m_userPtr; //m_userPtr ptr needs to be managed by the developer/user!
|
const void *m_userPtr; //m_userPtr ptr needs to be managed by the developer/user!
|
||||||
|
|
||||||
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
|
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
|
||||||
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
|
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
|
||||||
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||||
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||||
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||||
btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||||
|
|
||||||
// ctor: set some sensible defaults
|
// ctor: set some sensible defaults
|
||||||
btMultibodyLink()
|
btMultibodyLink()
|
||||||
: m_mass(1),
|
: m_mass(1),
|
||||||
m_parent(-1),
|
m_parent(-1),
|
||||||
m_zeroRotParentToThis(0, 0, 0, 1),
|
m_zeroRotParentToThis(0, 0, 0, 1),
|
||||||
m_cachedRotParentToThis(0, 0, 0, 1),
|
m_cachedRotParentToThis(0, 0, 0, 1),
|
||||||
m_collider(0),
|
m_collider(0),
|
||||||
m_flags(0),
|
m_flags(0),
|
||||||
m_dofCount(0),
|
m_dofCount(0),
|
||||||
m_posVarCount(0),
|
m_posVarCount(0),
|
||||||
m_jointType(btMultibodyLink::eInvalid),
|
m_jointType(btMultibodyLink::eInvalid),
|
||||||
m_jointFeedback(0),
|
m_jointFeedback(0),
|
||||||
m_linkName(0),
|
m_linkName(0),
|
||||||
m_jointName(0),
|
m_jointName(0),
|
||||||
m_userPtr(0),
|
m_userPtr(0),
|
||||||
m_jointDamping(0),
|
m_jointDamping(0),
|
||||||
m_jointFriction(0),
|
m_jointFriction(0),
|
||||||
m_jointLowerLimit(0),
|
m_jointLowerLimit(0),
|
||||||
m_jointUpperLimit(0),
|
m_jointUpperLimit(0),
|
||||||
m_jointMaxForce(0),
|
m_jointMaxForce(0),
|
||||||
m_jointMaxVelocity(0)
|
m_jointMaxVelocity(0)
|
||||||
{
|
{
|
||||||
m_inertiaLocal.setValue(1, 1, 1);
|
m_inertiaLocal.setValue(1, 1, 1);
|
||||||
setAxisTop(0, 0., 0., 0.);
|
setAxisTop(0, 0., 0., 0.);
|
||||||
setAxisBottom(0, 1., 0., 0.);
|
setAxisBottom(0, 1., 0., 0.);
|
||||||
m_dVector.setValue(0, 0, 0);
|
m_dVector.setValue(0, 0, 0);
|
||||||
m_eVector.setValue(0, 0, 0);
|
m_eVector.setValue(0, 0, 0);
|
||||||
m_cachedRVector.setValue(0, 0, 0);
|
m_cachedRVector.setValue(0, 0, 0);
|
||||||
m_appliedForce.setValue(0, 0, 0);
|
m_appliedForce.setValue(0, 0, 0);
|
||||||
m_appliedTorque.setValue(0, 0, 0);
|
m_appliedTorque.setValue(0, 0, 0);
|
||||||
m_appliedConstraintForce.setValue(0, 0, 0);
|
m_appliedConstraintForce.setValue(0, 0, 0);
|
||||||
m_appliedConstraintTorque.setValue(0, 0, 0);
|
m_appliedConstraintTorque.setValue(0, 0, 0);
|
||||||
//
|
//
|
||||||
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
|
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
|
||||||
m_jointPos[3] = 1.f; //"quat.w"
|
m_jointPos[3] = 1.f; //"quat.w"
|
||||||
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
|
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
|
||||||
m_cachedWorldTransform.setIdentity();
|
m_cachedWorldTransform.setIdentity();
|
||||||
}
|
}
|
||||||
|
|
||||||
// routine to update m_cachedRotParentToThis and m_cachedRVector
|
// routine to update m_cachedRotParentToThis and m_cachedRVector
|
||||||
void updateCacheMultiDof(btScalar *pq = 0)
|
void updateCacheMultiDof(btScalar *pq = 0)
|
||||||
{
|
{
|
||||||
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
|
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
|
||||||
|
|
||||||
switch (m_jointType)
|
switch (m_jointType)
|
||||||
{
|
{
|
||||||
case eRevolute:
|
case eRevolute:
|
||||||
{
|
{
|
||||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case ePrismatic:
|
case ePrismatic:
|
||||||
{
|
{
|
||||||
// m_cachedRotParentToThis never changes, so no need to update
|
// m_cachedRotParentToThis never changes, so no need to update
|
||||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
|
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case eSpherical:
|
case eSpherical:
|
||||||
{
|
{
|
||||||
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
|
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
|
||||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case ePlanar:
|
case ePlanar:
|
||||||
{
|
{
|
||||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||||
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
|
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case eFixed:
|
case eFixed:
|
||||||
{
|
{
|
||||||
m_cachedRotParentToThis = m_zeroRotParentToThis;
|
m_cachedRotParentToThis = m_zeroRotParentToThis;
|
||||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
//invalid type
|
//invalid type
|
||||||
btAssert(0);
|
btAssert(0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //BT_MULTIBODY_LINK_H
|
#endif //BT_MULTIBODY_LINK_H
|
||||||
|
|||||||
@ -1,216 +1,216 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
///This file was written by Erwin Coumans
|
///This file was written by Erwin Coumans
|
||||||
|
|
||||||
#include "btMultiBodyPoint2Point.h"
|
#include "btMultiBodyPoint2Point.h"
|
||||||
#include "btMultiBodyLinkCollider.h"
|
#include "btMultiBodyLinkCollider.h"
|
||||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||||
#include "LinearMath/btIDebugDraw.h"
|
#include "LinearMath/btIDebugDraw.h"
|
||||||
|
|
||||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||||
#define BTMBP2PCONSTRAINT_DIM 3
|
#define BTMBP2PCONSTRAINT_DIM 3
|
||||||
#else
|
#else
|
||||||
#define BTMBP2PCONSTRAINT_DIM 6
|
#define BTMBP2PCONSTRAINT_DIM 6
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
|
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),
|
||||||
m_rigidBodyA(0),
|
m_rigidBodyA(0),
|
||||||
m_rigidBodyB(bodyB),
|
m_rigidBodyB(bodyB),
|
||||||
m_pivotInA(pivotInA),
|
m_pivotInA(pivotInA),
|
||||||
m_pivotInB(pivotInB)
|
m_pivotInB(pivotInB)
|
||||||
{
|
{
|
||||||
m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
|
m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
|
||||||
}
|
}
|
||||||
|
|
||||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
|
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),
|
||||||
m_rigidBodyA(0),
|
m_rigidBodyA(0),
|
||||||
m_rigidBodyB(0),
|
m_rigidBodyB(0),
|
||||||
m_pivotInA(pivotInA),
|
m_pivotInA(pivotInA),
|
||||||
m_pivotInB(pivotInB)
|
m_pivotInB(pivotInB)
|
||||||
{
|
{
|
||||||
m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
|
m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyPoint2Point::finalizeMultiDof()
|
void btMultiBodyPoint2Point::finalizeMultiDof()
|
||||||
{
|
{
|
||||||
//not implemented yet
|
//not implemented yet
|
||||||
btAssert(0);
|
btAssert(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
|
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodyPoint2Point::getIslandIdA() const
|
int btMultiBodyPoint2Point::getIslandIdA() const
|
||||||
{
|
{
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
return m_rigidBodyA->getIslandTag();
|
return m_rigidBodyA->getIslandTag();
|
||||||
|
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
{
|
{
|
||||||
if (m_linkA < 0)
|
if (m_linkA < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodyPoint2Point::getIslandIdB() const
|
int btMultiBodyPoint2Point::getIslandIdB() const
|
||||||
{
|
{
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
return m_rigidBodyB->getIslandTag();
|
return m_rigidBodyB->getIslandTag();
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
{
|
{
|
||||||
if (m_linkB < 0)
|
if (m_linkB < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||||
btMultiBodyJacobianData& data,
|
btMultiBodyJacobianData& data,
|
||||||
const btContactSolverInfo& infoGlobal)
|
const btContactSolverInfo& infoGlobal)
|
||||||
{
|
{
|
||||||
// int i=1;
|
// int i=1;
|
||||||
int numDim = BTMBP2PCONSTRAINT_DIM;
|
int numDim = BTMBP2PCONSTRAINT_DIM;
|
||||||
for (int i = 0; i < numDim; i++)
|
for (int i = 0; i < numDim; i++)
|
||||||
{
|
{
|
||||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||||
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
|
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
|
||||||
constraintRow.m_orgConstraint = this;
|
constraintRow.m_orgConstraint = this;
|
||||||
constraintRow.m_orgDofIndex = i;
|
constraintRow.m_orgDofIndex = i;
|
||||||
constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
|
constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
|
||||||
constraintRow.m_contactNormal1.setValue(0, 0, 0);
|
constraintRow.m_contactNormal1.setValue(0, 0, 0);
|
||||||
constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
|
constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
|
||||||
constraintRow.m_contactNormal2.setValue(0, 0, 0);
|
constraintRow.m_contactNormal2.setValue(0, 0, 0);
|
||||||
constraintRow.m_angularComponentA.setValue(0, 0, 0);
|
constraintRow.m_angularComponentA.setValue(0, 0, 0);
|
||||||
constraintRow.m_angularComponentB.setValue(0, 0, 0);
|
constraintRow.m_angularComponentB.setValue(0, 0, 0);
|
||||||
|
|
||||||
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
||||||
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
||||||
|
|
||||||
btVector3 contactNormalOnB(0, 0, 0);
|
btVector3 contactNormalOnB(0, 0, 0);
|
||||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||||
contactNormalOnB[i] = -1;
|
contactNormalOnB[i] = -1;
|
||||||
#else
|
#else
|
||||||
contactNormalOnB[i % 3] = -1;
|
contactNormalOnB[i % 3] = -1;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Convert local points back to world
|
// Convert local points back to world
|
||||||
btVector3 pivotAworld = m_pivotInA;
|
btVector3 pivotAworld = m_pivotInA;
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
{
|
{
|
||||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||||
}
|
}
|
||||||
btVector3 pivotBworld = m_pivotInB;
|
btVector3 pivotBworld = m_pivotInB;
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
{
|
{
|
||||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||||
}
|
}
|
||||||
|
|
||||||
btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
|
btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
|
||||||
|
|
||||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||||
|
|
||||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0),
|
fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0),
|
||||||
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
|
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
|
||||||
posError,
|
posError,
|
||||||
infoGlobal,
|
infoGlobal,
|
||||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||||
//@todo: support the case of btMultiBody versus btRigidBody,
|
//@todo: support the case of btMultiBody versus btRigidBody,
|
||||||
//see btPoint2PointConstraint::getInfo2NonVirtual
|
//see btPoint2PointConstraint::getInfo2NonVirtual
|
||||||
#else
|
#else
|
||||||
const btVector3 dummy(0, 0, 0);
|
const btVector3 dummy(0, 0, 0);
|
||||||
|
|
||||||
btAssert(m_bodyA->isMultiDof());
|
btAssert(m_bodyA->isMultiDof());
|
||||||
|
|
||||||
btScalar* jac1 = jacobianA(i);
|
btScalar* jac1 = jacobianA(i);
|
||||||
const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy;
|
const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy;
|
||||||
const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy;
|
const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy;
|
||||||
|
|
||||||
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||||
|
|
||||||
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
|
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
|
||||||
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
|
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
|
||||||
posError,
|
posError,
|
||||||
infoGlobal,
|
infoGlobal,
|
||||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
|
void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
|
||||||
{
|
{
|
||||||
btTransform tr;
|
btTransform tr;
|
||||||
tr.setIdentity();
|
tr.setIdentity();
|
||||||
|
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
{
|
{
|
||||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||||
tr.setOrigin(pivot);
|
tr.setOrigin(pivot);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
{
|
{
|
||||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||||
tr.setOrigin(pivotAworld);
|
tr.setOrigin(pivotAworld);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
{
|
{
|
||||||
// that ideally should draw the same frame
|
// that ideally should draw the same frame
|
||||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||||
tr.setOrigin(pivot);
|
tr.setOrigin(pivot);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
{
|
{
|
||||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||||
tr.setOrigin(pivotBworld);
|
tr.setOrigin(pivotBworld);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,234 +1,234 @@
|
|||||||
/*
|
/*
|
||||||
Bullet Continuous Collision Detection and Physics Library
|
Bullet Continuous Collision Detection and Physics Library
|
||||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||||
|
|
||||||
This software is provided 'as-is', without any express or implied warranty.
|
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.
|
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,
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
including commercial applications, and to alter it and redistribute it freely,
|
including commercial applications, and to alter it and redistribute it freely,
|
||||||
subject to the following restrictions:
|
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.
|
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.
|
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.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
///This file was written by Erwin Coumans
|
///This file was written by Erwin Coumans
|
||||||
|
|
||||||
#include "btMultiBodySliderConstraint.h"
|
#include "btMultiBodySliderConstraint.h"
|
||||||
#include "btMultiBodyLinkCollider.h"
|
#include "btMultiBodyLinkCollider.h"
|
||||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||||
#include "LinearMath/btIDebugDraw.h"
|
#include "LinearMath/btIDebugDraw.h"
|
||||||
|
|
||||||
#define BTMBSLIDERCONSTRAINT_DIM 5
|
#define BTMBSLIDERCONSTRAINT_DIM 5
|
||||||
#define EPSILON 0.000001
|
#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)
|
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),
|
||||||
m_rigidBodyA(0),
|
m_rigidBodyA(0),
|
||||||
m_rigidBodyB(bodyB),
|
m_rigidBodyB(bodyB),
|
||||||
m_pivotInA(pivotInA),
|
m_pivotInA(pivotInA),
|
||||||
m_pivotInB(pivotInB),
|
m_pivotInB(pivotInB),
|
||||||
m_frameInA(frameInA),
|
m_frameInA(frameInA),
|
||||||
m_frameInB(frameInB),
|
m_frameInB(frameInB),
|
||||||
m_jointAxis(jointAxis)
|
m_jointAxis(jointAxis)
|
||||||
{
|
{
|
||||||
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
||||||
}
|
}
|
||||||
|
|
||||||
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)
|
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),
|
||||||
m_rigidBodyA(0),
|
m_rigidBodyA(0),
|
||||||
m_rigidBodyB(0),
|
m_rigidBodyB(0),
|
||||||
m_pivotInA(pivotInA),
|
m_pivotInA(pivotInA),
|
||||||
m_pivotInB(pivotInB),
|
m_pivotInB(pivotInB),
|
||||||
m_frameInA(frameInA),
|
m_frameInA(frameInA),
|
||||||
m_frameInB(frameInB),
|
m_frameInB(frameInB),
|
||||||
m_jointAxis(jointAxis)
|
m_jointAxis(jointAxis)
|
||||||
{
|
{
|
||||||
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodySliderConstraint::finalizeMultiDof()
|
void btMultiBodySliderConstraint::finalizeMultiDof()
|
||||||
{
|
{
|
||||||
//not implemented yet
|
//not implemented yet
|
||||||
btAssert(0);
|
btAssert(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
|
btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodySliderConstraint::getIslandIdA() const
|
int btMultiBodySliderConstraint::getIslandIdA() const
|
||||||
{
|
{
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
return m_rigidBodyA->getIslandTag();
|
return m_rigidBodyA->getIslandTag();
|
||||||
|
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
{
|
{
|
||||||
if (m_linkA < 0)
|
if (m_linkA < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int btMultiBodySliderConstraint::getIslandIdB() const
|
int btMultiBodySliderConstraint::getIslandIdB() const
|
||||||
{
|
{
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
return m_rigidBodyB->getIslandTag();
|
return m_rigidBodyB->getIslandTag();
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
{
|
{
|
||||||
if (m_linkB < 0)
|
if (m_linkB < 0)
|
||||||
{
|
{
|
||||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||||
if (col)
|
if (col)
|
||||||
return col->getIslandTag();
|
return col->getIslandTag();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
||||||
{
|
{
|
||||||
// Convert local points back to world
|
// Convert local points back to world
|
||||||
btVector3 pivotAworld = m_pivotInA;
|
btVector3 pivotAworld = m_pivotInA;
|
||||||
btMatrix3x3 frameAworld = m_frameInA;
|
btMatrix3x3 frameAworld = m_frameInA;
|
||||||
btVector3 jointAxis = m_jointAxis;
|
btVector3 jointAxis = m_jointAxis;
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
{
|
{
|
||||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||||
frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
||||||
jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
|
jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
|
||||||
}
|
}
|
||||||
else if (m_bodyA)
|
else if (m_bodyA)
|
||||||
{
|
{
|
||||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||||
frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
|
frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
|
||||||
jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
|
jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
|
||||||
}
|
}
|
||||||
btVector3 pivotBworld = m_pivotInB;
|
btVector3 pivotBworld = m_pivotInB;
|
||||||
btMatrix3x3 frameBworld = m_frameInB;
|
btMatrix3x3 frameBworld = m_frameInB;
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
{
|
{
|
||||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||||
frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
||||||
}
|
}
|
||||||
else if (m_bodyB)
|
else if (m_bodyB)
|
||||||
{
|
{
|
||||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||||
frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
|
frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
|
||||||
}
|
}
|
||||||
|
|
||||||
btVector3 constraintAxis[2];
|
btVector3 constraintAxis[2];
|
||||||
for (int i = 0; i < 3; ++i)
|
for (int i = 0; i < 3; ++i)
|
||||||
{
|
{
|
||||||
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
|
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
|
||||||
if (constraintAxis[0].safeNorm() > EPSILON)
|
if (constraintAxis[0].safeNorm() > EPSILON)
|
||||||
{
|
{
|
||||||
constraintAxis[0] = constraintAxis[0].normalized();
|
constraintAxis[0] = constraintAxis[0].normalized();
|
||||||
constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
|
constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
|
||||||
constraintAxis[1] = constraintAxis[1].normalized();
|
constraintAxis[1] = constraintAxis[1].normalized();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
||||||
btVector3 angleDiff;
|
btVector3 angleDiff;
|
||||||
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
||||||
|
|
||||||
int numDim = BTMBSLIDERCONSTRAINT_DIM;
|
int numDim = BTMBSLIDERCONSTRAINT_DIM;
|
||||||
for (int i = 0; i < numDim; i++)
|
for (int i = 0; i < numDim; i++)
|
||||||
{
|
{
|
||||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||||
constraintRow.m_orgConstraint = this;
|
constraintRow.m_orgConstraint = this;
|
||||||
constraintRow.m_orgDofIndex = i;
|
constraintRow.m_orgDofIndex = i;
|
||||||
constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
|
constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
|
||||||
constraintRow.m_contactNormal1.setValue(0, 0, 0);
|
constraintRow.m_contactNormal1.setValue(0, 0, 0);
|
||||||
constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
|
constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
|
||||||
constraintRow.m_contactNormal2.setValue(0, 0, 0);
|
constraintRow.m_contactNormal2.setValue(0, 0, 0);
|
||||||
constraintRow.m_angularComponentA.setValue(0, 0, 0);
|
constraintRow.m_angularComponentA.setValue(0, 0, 0);
|
||||||
constraintRow.m_angularComponentB.setValue(0, 0, 0);
|
constraintRow.m_angularComponentB.setValue(0, 0, 0);
|
||||||
|
|
||||||
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
||||||
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
||||||
|
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
{
|
{
|
||||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||||
}
|
}
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
{
|
{
|
||||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||||
}
|
}
|
||||||
|
|
||||||
btVector3 constraintNormalLin(0, 0, 0);
|
btVector3 constraintNormalLin(0, 0, 0);
|
||||||
btVector3 constraintNormalAng(0, 0, 0);
|
btVector3 constraintNormalAng(0, 0, 0);
|
||||||
btScalar posError = 0.0;
|
btScalar posError = 0.0;
|
||||||
if (i < 2)
|
if (i < 2)
|
||||||
{
|
{
|
||||||
constraintNormalLin = constraintAxis[i];
|
constraintNormalLin = constraintAxis[i];
|
||||||
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
||||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||||
constraintNormalLin, pivotAworld, pivotBworld,
|
constraintNormalLin, pivotAworld, pivotBworld,
|
||||||
posError,
|
posError,
|
||||||
infoGlobal,
|
infoGlobal,
|
||||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{ //i>=2
|
{ //i>=2
|
||||||
constraintNormalAng = frameAworld.getColumn(i % 3);
|
constraintNormalAng = frameAworld.getColumn(i % 3);
|
||||||
posError = angleDiff[i % 3];
|
posError = angleDiff[i % 3];
|
||||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||||
constraintNormalLin, pivotAworld, pivotBworld,
|
constraintNormalLin, pivotAworld, pivotBworld,
|
||||||
posError,
|
posError,
|
||||||
infoGlobal,
|
infoGlobal,
|
||||||
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
|
void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
|
||||||
{
|
{
|
||||||
btTransform tr;
|
btTransform tr;
|
||||||
tr.setIdentity();
|
tr.setIdentity();
|
||||||
|
|
||||||
if (m_rigidBodyA)
|
if (m_rigidBodyA)
|
||||||
{
|
{
|
||||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||||
tr.setOrigin(pivot);
|
tr.setOrigin(pivot);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_bodyA)
|
if (m_bodyA)
|
||||||
{
|
{
|
||||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||||
tr.setOrigin(pivotAworld);
|
tr.setOrigin(pivotAworld);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_rigidBodyB)
|
if (m_rigidBodyB)
|
||||||
{
|
{
|
||||||
// that ideally should draw the same frame
|
// that ideally should draw the same frame
|
||||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||||
tr.setOrigin(pivot);
|
tr.setOrigin(pivot);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
if (m_bodyB)
|
if (m_bodyB)
|
||||||
{
|
{
|
||||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||||
tr.setOrigin(pivotBworld);
|
tr.setOrigin(pivotBworld);
|
||||||
drawer->drawTransform(tr, 0.1);
|
drawer->drawTransform(tr, 0.1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,14 +1,14 @@
|
|||||||
project "BulletSoftBody"
|
project "BulletSoftBody"
|
||||||
|
|
||||||
kind "StaticLib"
|
kind "StaticLib"
|
||||||
|
|
||||||
includedirs {
|
includedirs {
|
||||||
"..",
|
"..",
|
||||||
}
|
}
|
||||||
if os.is("Linux") then
|
if os.is("Linux") then
|
||||||
buildoptions{"-fPIC"}
|
buildoptions{"-fPIC"}
|
||||||
end
|
end
|
||||||
files {
|
files {
|
||||||
"**.cpp",
|
"**.cpp",
|
||||||
"**.h"
|
"**.h"
|
||||||
}
|
}
|
||||||
Loading…
Reference in New Issue
Block a user