Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-06-02 18:26:04 -07:00
parent 0a29c8d9af 518c7ec00e
commit bfcbb339cf
126 changed files with 111577 additions and 2758 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
CMakeLists.txt
View File

@@ -28,7 +28,15 @@ OPTION(USE_DOUBLE_PRECISION "Use double precision" OFF)
OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)
OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations" OFF)
OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
IF (BULLET2_USE_THREAD_LOCKS)
OPTION(BULLET2_USE_OPEN_MP_MULTITHREADING "Build Bullet 2 with support for multi-threading with OpenMP (requires a compiler with OpenMP support)" OFF)
OPTION(BULLET2_USE_TBB_MULTITHREADING "Build Bullet 2 with support for multi-threading with Intel Threading Building Blocks (requires the TBB library to be already installed)" OFF)
IF (MSVC)
OPTION(BULLET2_USE_PPL_MULTITHREADING "Build Bullet 2 with support for multi-threading with Microsoft Parallel Patterns Library (requires MSVC compiler)" OFF)
ENDIF (MSVC)
ENDIF (BULLET2_USE_THREAD_LOCKS)
OPTION(USE_MSVC_INCREMENTAL_LINKING "Use MSVC Incremental Linking" OFF)
OPTION(USE_CUSTOM_VECTOR_MATH "Use custom vectormath library" OFF)
@@ -208,6 +216,30 @@ IF(BULLET2_USE_THREAD_LOCKS)
ENDIF (NOT MSVC)
ENDIF (BULLET2_USE_THREAD_LOCKS)
IF (BULLET2_USE_OPEN_MP_MULTITHREADING)
ADD_DEFINITIONS("-DBT_USE_OPENMP=1")
IF (MSVC)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp")
ELSE (MSVC)
# GCC, Clang
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
ENDIF (MSVC)
ENDIF (BULLET2_USE_OPEN_MP_MULTITHREADING)
IF (BULLET2_USE_TBB_MULTITHREADING)
SET (BULLET2_TBB_INCLUDE_DIR "not found" CACHE PATH "Directory for Intel TBB includes.")
SET (BULLET2_TBB_LIB_DIR "not found" CACHE PATH "Directory for Intel TBB libraries.")
find_library(TBB_LIBRARY tbb PATHS ${BULLET2_TBB_LIB_DIR})
find_library(TBBMALLOC_LIBRARY tbbmalloc PATHS ${BULLET2_TBB_LIB_DIR})
ADD_DEFINITIONS("-DBT_USE_TBB=1")
INCLUDE_DIRECTORIES( ${BULLET2_TBB_INCLUDE_DIR} )
LINK_LIBRARIES( ${TBB_LIBRARY} ${TBBMALLOC_LIBRARY} )
ENDIF (BULLET2_USE_TBB_MULTITHREADING)
IF (BULLET2_USE_PPL_MULTITHREADING)
ADD_DEFINITIONS("-DBT_USE_PPL=1")
ENDIF (BULLET2_USE_PPL_MULTITHREADING)
IF (WIN32)
OPTION(USE_GLUT "Use Glut" ON)
ADD_DEFINITIONS( -D_CRT_SECURE_NO_WARNINGS )
@@ -350,14 +382,6 @@ IF(BUILD_BULLET2_DEMOS)
SUBDIRS(examples)
ENDIF()
IF (BULLET2_USE_THREAD_LOCKS)
OPTION(BULLET2_MULTITHREADED_OPEN_MP_DEMO "Build Bullet 2 MultithreadedDemo using OpenMP (requires a compiler with OpenMP support)" OFF)
OPTION(BULLET2_MULTITHREADED_TBB_DEMO "Build Bullet 2 MultithreadedDemo using Intel Threading Building Blocks (requires the TBB library to be already installed)" OFF)
IF (MSVC)
OPTION(BULLET2_MULTITHREADED_PPL_DEMO "Build Bullet 2 MultithreadedDemo using Microsoft Parallel Patterns Library (requires MSVC compiler)" OFF)
ENDIF (MSVC)
ENDIF (BULLET2_USE_THREAD_LOCKS)
ENDIF(BUILD_BULLET2_DEMOS)

38
Extras/obj2sdf/obj2sdf.cpp
View File

@@ -16,6 +16,24 @@
#define MAX_PATH_LEN 1024
std::string StripExtension( const std::string & sPath )
{
for( std::string::const_reverse_iterator i = sPath.rbegin(); i != sPath.rend(); i++ )
{
if( *i == '.' )
{
return std::string( sPath.begin(), i.base() - 1 );
}
// if we find a slash there is no extension
if( *i == '\\' || *i == '/' )
break;
}
// we didn't find an extension
return sPath;
}
int main(int argc, char* argv[])
{
@@ -23,6 +41,8 @@ int main(int argc, char* argv[])
char* fileName;
args.GetCmdLineArgument("fileName",fileName);
std::string matLibName = StripExtension(fileName);
printf("fileName = %s\n", fileName);
if (fileName==0)
{
@@ -58,7 +78,13 @@ int main(int argc, char* argv[])
}
char objFileName[MAX_PATH_LEN];
sprintf(objFileName,"%s/part%d.obj",materialPrefixPath,s);
if (strlen(materialPrefixPath)>0)
{
sprintf(objFileName,"%s/part%d.obj",materialPrefixPath,s);
} else
{
sprintf(objFileName,"part%d.obj",s);
}
FILE* f = fopen(objFileName,"w");
if (f==0)
{
@@ -66,7 +92,15 @@ int main(int argc, char* argv[])
exit(0);
}
fprintf(f,"# Exported using automatic converter by Erwin Coumans\n");
fprintf(f,"mtllib bedroom.mtl\n");
if (matLibName.length())
{
fprintf(f,"mtllib %s.mtl\n", matLibName.c_str());
} else
{
fprintf(f,"mtllib bedroom.mtl\n");
}
int faceCount = shape.mesh.indices.size();
int vertexCount = shape.mesh.positions.size();

40
data/kitchens/1.sdf
View File

@@ -4291,45 +4291,7 @@
</visual>
</link>
</model>
<model name='part110.obj'>
<static>1</static>
<pose frame=''>-12.0 -13.9 0 0 0 0</pose>
<link name='link_d110'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision concave='yes' name='collision_110'>
<geometry>
<mesh>
<scale>.1 .1 .1</scale>
<uri>fatihrmutfak/part110.obj</uri>
</mesh>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>.1 .1 .1</scale>
<uri>fatihrmutfak/part110.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1.000000 1.000000 1.000000 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
<model name='part111.obj'>
<static>1</static>
<pose frame=''>-12.0 -13.9 0 0 0 0</pose>

10
data/kuka_iiwa/meshes/link_0.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

6055
data/kuka_iiwa/meshes/link_0.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/kuka_iiwa/meshes/link_1.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

5576
data/kuka_iiwa/meshes/link_1.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/kuka_iiwa/meshes/link_2.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

2948
data/kuka_iiwa/meshes/link_2.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/kuka_iiwa/meshes/link_3.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

3924
data/kuka_iiwa/meshes/link_3.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/kuka_iiwa/meshes/link_4.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

3168
data/kuka_iiwa/meshes/link_4.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/kuka_iiwa/meshes/link_5.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

2732
data/kuka_iiwa/meshes/link_5.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/kuka_iiwa/meshes/link_6.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

2328
data/kuka_iiwa/meshes/link_6.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/kuka_iiwa/meshes/link_7.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

3155
data/kuka_iiwa/meshes/link_7.obj Normal file
View File

File diff suppressed because it is too large Load Diff

16
data/kuka_iiwa/model.urdf
View File

@@ -70,7 +70,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_0.stl"/>
<mesh filename="meshes/link_0.obj"/>
</geometry>
<material name="Grey"/>
</visual>
@@ -99,7 +99,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_1.stl"/>
<mesh filename="meshes/link_1.obj"/>
</geometry>
<material name="Blue"/>
</visual>
@@ -128,7 +128,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_2.stl"/>
<mesh filename="meshes/link_2.obj"/>
</geometry>
<material name="Blue"/>
</visual>
@@ -157,7 +157,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_3.stl"/>
<mesh filename="meshes/link_3.obj"/>
</geometry>
<material name="Orange"/>
</visual>
@@ -186,7 +186,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_4.stl"/>
<mesh filename="meshes/link_4.obj"/>
</geometry>
<material name="Blue"/>
</visual>
@@ -215,7 +215,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_5.stl"/>
<mesh filename="meshes/link_5.obj"/>
</geometry>
<material name="Blue"/>
</visual>
@@ -244,7 +244,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_6.stl"/>
<mesh filename="meshes/link_6.obj"/>
</geometry>
<material name="Orange"/>
</visual>
@@ -273,7 +273,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_7.stl"/>
<mesh filename="meshes/link_7.obj"/>
</geometry>
<material name="Grey"/>
</visual>

29
data/plane_with_restitution.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="0.0" ?>
<robot name="plane">
<link name="planeLink">
<contact>
<restitution value="0.5"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -5"/>
<geometry>
<box size="30 30 10"/>
</geometry>
</collision>
</link>
</robot>

23154
data/roboschool/models_outdoor/stadium/part0.obj Normal file
View File

File diff suppressed because it is too large Load Diff

16210
data/roboschool/models_outdoor/stadium/part1.obj Normal file
View File

File diff suppressed because it is too large Load Diff

8350
data/roboschool/models_outdoor/stadium/part2.obj Normal file
View File

File diff suppressed because it is too large Load Diff

16
data/roboschool/models_outdoor/stadium/stadium.mtl Normal file
View File

@@ -0,0 +1,16 @@
newmtl stadium_white
Ns 96.078431
Ka 0.000000 0.000000 0.000000
Kd 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
newmtl stadium_grass
Ka 0.000000 0.000000 0.000000
Kd 0.000000 0.500000 0.000000
Ks 0.000000 0.000000 0.000000
map_Kd stadium_grass.jpg
newmtl stadium_dirt
Ka 0.000000 0.000000 0.000000
Kd 0.600000 0.400000 0.000000
Ks 0.000000 0.000000 0.000000

30561
data/roboschool/models_outdoor/stadium/stadium.obj Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
data/roboschool/models_outdoor/stadium/stadium_grass.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.0 MiB

1
data/sphere2.urdf
View File

@@ -18,6 +18,7 @@
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
<specular rgb="11 1 1"/>
</material>
</visual>
<collision>

33
data/sphere_with_restitution.urdf Normal file
View File

@@ -0,0 +1,33 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="baseLink">
<contact>
<restitution value="0.5" />
<rolling_friction value="0.001"/>
<spinning_friction value="0.001"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</collision>
</link>
</robot>

107
data/stadium.sdf Normal file
View File

@@ -0,0 +1,107 @@
<sdf version='1.6'>
<world name='default'>
<gravity>0 0 -9.8</gravity>
<model name='roboschool/models_outdoor/stadium/part0.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d0'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>roboschool/models_outdoor/stadium/part0.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1.00000 1.00000 1.000000 1</diffuse>
<specular>0.1 .1 .1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
<model name='roboschool/models_outdoor/stadium/part1.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d1'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision_1'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>roboschool/models_outdoor/stadium/part1.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>0.600000 0.400000 0.000000 1</diffuse>
<specular>.5 .5 .5 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
<model name='part2.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d2'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>roboschool/models_outdoor/stadium/part2.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.000000 0.500000 0.000000 1</diffuse>
<specular>0.4 0.4 0.4 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
</world>
</sdf>

26
data/two_cubes.sdf
View File

@@ -20,7 +20,7 @@
<collision name='collision'>
<geometry>
<plane>
<normal>1 2 3</normal>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
@@ -46,7 +46,7 @@
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>4 5 6</normal>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
@@ -89,6 +89,7 @@
<static>1</static>
<pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
<link name='unit_box_0::link'>
<inertial>
<mass>1</mass>
<inertia>
@@ -154,26 +155,7 @@
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>

5
examples/Benchmarks/BenchmarkDemo.cpp
View File

@@ -32,7 +32,6 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include "../MultiThreadedDemo/ParallelFor.h"
class btDynamicsWorld;
@@ -230,7 +229,7 @@ public:
}
}
struct CastRaysLoopBody
struct CastRaysLoopBody : public btIParallelForBody
{
btCollisionWorld* mWorld;
btRaycastBar2* mRaycasts;
@@ -274,7 +273,7 @@ public:
{
CastRaysLoopBody rayLooper(cw, this);
int grainSize = 20; // number of raycasts per task
parallelFor( 0, NUMRAYS, grainSize, rayLooper );
btParallelFor( 0, NUMRAYS, grainSize, rayLooper );
}
else
#endif // USE_PARALLEL_RAYCASTS

12
examples/CommonInterfaces/CommonExampleInterface.h
View File

@@ -3,6 +3,13 @@
#ifndef COMMON_EXAMPLE_INTERFACE_H
#define COMMON_EXAMPLE_INTERFACE_H
struct CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()=0;
virtual void deleteCommandProcessor(CommandProcessorInterface*)=0;
};
struct CommonExampleOptions
{
struct GUIHelperInterface* m_guiHelper;
@@ -11,13 +18,14 @@ struct CommonExampleOptions
int m_option;
const char* m_fileName;
class SharedMemoryInterface* m_sharedMem;
CommandProcessorCreationInterface* m_commandProcessorCreation;
CommonExampleOptions(struct GUIHelperInterface* helper, int option=0)
:m_guiHelper(helper),
m_option(option),
m_fileName(0),
m_sharedMem(0)
m_sharedMem(0),
m_commandProcessorCreation(0)
{
}

2
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -39,6 +39,8 @@ struct GUIHelperInterface
virtual void removeAllGraphicsInstances()=0;
virtual void removeGraphicsInstance(int graphicsUid) {}
virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {}
virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {}
virtual Common2dCanvasInterface* get2dCanvasInterface()=0;

3
examples/CommonInterfaces/CommonRenderInterface.h
View File

@@ -79,6 +79,9 @@ struct CommonRenderInterface
virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex)=0;
virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex)=0;
virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex)=0;
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex)=0;
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex)=0;
virtual int getTotalNumInstances() const = 0;

30
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -110,29 +110,6 @@ ELSE(WIN32)
ENDIF(APPLE)
ENDIF(WIN32)
IF (BULLET2_MULTITHREADED_OPEN_MP_DEMO)
ADD_DEFINITIONS("-DBT_USE_OPENMP=1")
IF (MSVC)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp")
ELSE (MSVC)
# GCC, Clang
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
ENDIF (MSVC)
ENDIF (BULLET2_MULTITHREADED_OPEN_MP_DEMO)
IF (BULLET2_MULTITHREADED_PPL_DEMO)
ADD_DEFINITIONS("-DBT_USE_PPL=1")
ENDIF (BULLET2_MULTITHREADED_PPL_DEMO)
IF (BULLET2_MULTITHREADED_TBB_DEMO)
SET (BULLET2_TBB_INCLUDE_DIR "not found" CACHE PATH "Directory for Intel TBB includes.")
SET (BULLET2_TBB_LIB_DIR "not found" CACHE PATH "Directory for Intel TBB libraries.")
find_library(TBB_LIBRARY tbb PATHS ${BULLET2_TBB_LIB_DIR})
find_library(TBBMALLOC_LIBRARY tbbmalloc PATHS ${BULLET2_TBB_LIB_DIR})
ADD_DEFINITIONS("-DBT_USE_TBB=1")
INCLUDE_DIRECTORIES( ${BULLET2_TBB_INCLUDE_DIR} )
LINK_LIBRARIES( ${TBB_LIBRARY} ${TBBMALLOC_LIBRARY} )
ENDIF (BULLET2_MULTITHREADED_TBB_DEMO)
SET(ExtendedTutorialsSources
../ExtendedTutorials/Chain.cpp
@@ -175,6 +152,7 @@ SET(BulletExampleBrowser_SRCS
../SharedMemory/PhysicsClient.cpp
../SharedMemory/PhysicsClientC_API.cpp
../SharedMemory/PhysicsServerExample.cpp
../SharedMemory/PhysicsServerExampleBullet2.cpp
../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/PosixSharedMemory.cpp
../SharedMemory/Win32SharedMemory.cpp
@@ -207,7 +185,6 @@ SET(BulletExampleBrowser_SRCS
../MultiThreadedDemo/MultiThreadedDemo.h
../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
../MultiThreadedDemo/CommonRigidBodyMTBase.h
../MultiThreadedDemo/ParallelFor.h
../Tutorial/Tutorial.cpp
../Tutorial/Tutorial.h
../Tutorial/Dof6ConstraintTutorial.cpp
@@ -244,6 +221,7 @@ SET(BulletExampleBrowser_SRCS
../MultiThreading/b3PosixThreadSupport.cpp
../MultiThreading/b3Win32ThreadSupport.cpp
../MultiThreading/b3ThreadSupportInterface.cpp
../MultiThreading/btTaskScheduler.cpp
../RenderingExamples/TinyRendererSetup.cpp
../RenderingExamples/TimeSeriesCanvas.cpp
../RenderingExamples/TimeSeriesCanvas.h
@@ -386,7 +364,7 @@ ADD_CUSTOM_COMMAND(
COMMAND ${CMAKE_COMMAND} ARGS -E copy_directory ${BULLET_PHYSICS_SOURCE_DIR}/data ${PROJECT_BINARY_DIR}/data
)
IF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
IF (BULLET2_USE_TBB_MULTITHREADING AND WIN32)
# add a post build command to copy some dlls to the executable directory
set(TBB_VC_VER "vc12")
set(TBB_VC_ARCH "ia32")
@@ -400,7 +378,7 @@ IF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
COMMAND ${CMAKE_COMMAND} -E copy_if_different
"${BULLET2_TBB_INCLUDE_DIR}/../bin/${TBB_VC_ARCH}/${TBB_VC_VER}/tbbmalloc.dll"
$<TARGET_FILE_DIR:App_ExampleBrowser>)
ENDIF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
ENDIF (BULLET2_USE_TBB_MULTITHREADING AND WIN32)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)

6
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -39,7 +39,7 @@
#include "../FractureDemo/FractureDemo.h"
#include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/PhysicsClientExample.h"
#include "../Constraints/TestHingeTorque.h"
#include "../RenderingExamples/TimeSeriesExample.h"
@@ -138,7 +138,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(0,"Physics Client-Server"),
ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.",
PhysicsServerCreateFunc),
PhysicsServerCreateFuncBullet2),
ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
// ExampleEntry(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",
@@ -295,7 +295,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Fracture demo", "Create a basic custom implementation to model fracturing objects, based on a btCompoundShape. It explicitly propagates the collision impulses and breaks the rigid body into multiple rigid bodies. Press F to toggle fracture and glue mode.", FractureDemoCreateFunc),
ExampleEntry(1,"Planar 2D","Show the use of 2D collision shapes and rigid body simulation. The collision shape is wrapped into a btConvex2dShape. The rigid bodies are restricted in a plane using the 'setAngularFactor' and 'setLinearFactor' API call.",Planar2DCreateFunc),
#if BT_USE_OPENMP || BT_USE_TBB || BT_USE_PPL
#if BT_THREADSAFE
// only enable MultiThreaded demo if a task scheduler is available
ExampleEntry( 1, "Multithreaded Demo",
"Stacks of boxes that do not sleep. Good for testing performance with large numbers of bodies and contacts. Sliders can be used to change the number of stacks (restart needed after each change)."

10
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -28,6 +28,8 @@ void* ExampleBrowserMemoryFunc();
#include "EmptyExample.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/PhysicsClientExample.h"
#ifndef _WIN32
@@ -124,14 +126,14 @@ static ExampleEntryPhysicsServer gDefaultExamplesPhysicsServer[]=
ExampleEntryPhysicsServer(0,"Robotics Control"),
ExampleEntryPhysicsServer(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
PhysicsServerCreateFunc),
PhysicsServerCreateFuncBullet2),
ExampleEntryPhysicsServer(1,"Physics Server (RTC)", "Create a physics server that communicates with a physics client over shared memory. At each update, the Physics Server will continue calling 'stepSimulation' based on the real-time clock (RTC).",
PhysicsServerCreateFunc,PHYSICS_SERVER_USE_RTC_CLOCK),
PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_USE_RTC_CLOCK),
ExampleEntryPhysicsServer(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",
PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
ExampleEntryPhysicsServer(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
};

8
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -325,7 +325,13 @@ void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]
m_data->m_glApp->m_renderer->writeSingleInstanceColorToCPU(rgbaColor,instanceUid);
};
}
void OpenGLGuiHelper::changeSpecularColor(int instanceUid, const double specularColor[3])
{
if (instanceUid>=0)
{
m_data->m_glApp->m_renderer->writeSingleInstanceSpecularColorToCPU(specularColor,instanceUid);
};
}
int OpenGLGuiHelper::createCheckeredTexture(int red,int green, int blue)
{
int texWidth=1024;

3
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -27,7 +27,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void removeAllGraphicsInstances();
virtual void removeGraphicsInstance(int graphicsUid);
virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]);
virtual void changeSpecularColor(int instanceUid, const double specularColor[3]);
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld);

1
examples/ExampleBrowser/premake4.lua
View File

@@ -81,6 +81,7 @@ project "App_BulletExampleBrowser"
"../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsServerExampleBullet2.cpp",
"../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/PhysicsServer.cpp",
"../SharedMemory/PhysicsServerSharedMemory.cpp",

2
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -706,7 +706,7 @@ struct BulletMJCFImporterInternalData
if (!rgba.empty())
{
// "0 0.7 0.7 1"
parseVector4(geom.m_localMaterial.m_rgbaColor, rgba);
parseVector4(geom.m_localMaterial.m_matColor.m_rgbaColor, rgba);
geom.m_hasLocalMaterial = true;
geom.m_localMaterial.m_name = rgba;
}

36
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -42,6 +42,7 @@ struct MyTexture
unsigned char* textureData;
};
ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
{
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -51,7 +52,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
std::string m_sourceFile;
char m_pathPrefix[1024];
int m_bodyId;
btHashMap<btHashInt,btVector4> m_linkColors;
btHashMap<btHashInt,UrdfMaterialColor> m_linkColors;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
LinkVisualShapesConverter* m_customVisualShapesConverter;
@@ -567,6 +568,15 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
switch (collision->m_geometry.m_type)
{
case URDF_GEOM_PLANE:
{
btVector3 planeNormal = collision->m_geometry.m_planeNormal;
btScalar planeConstant = 0;//not available?
btStaticPlaneShape* plane = new btStaticPlaneShape(planeNormal,planeConstant);
shape = plane;
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_CAPSULE:
{
btScalar radius = collision->m_geometry.m_capsuleRadius;
@@ -788,7 +798,7 @@ upAxisMat.setIdentity();
default:
b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
// for example, URDF_GEOM_PLANE
}
return shape;
}
@@ -1094,7 +1104,10 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
{
UrdfMaterial *const mat = *matPtr;
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
UrdfMaterialColor matCol;
matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor;
matCol.m_specularColor = mat->m_matColor.m_specularColor;
m_data->m_linkColors.insert(linkIndex,matCol);
}
convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
@@ -1132,10 +1145,21 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
{
const btVector4* rgbaPtr = m_data->m_linkColors[linkIndex];
if (rgbaPtr)
const UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex];
if (matColPtr)
{
colorRGBA = *rgbaPtr;
colorRGBA = matColPtr->m_rgbaColor;
return true;
}
return false;
}
bool BulletURDFImporter::getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const
{
UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex];
if (matColPtr)
{
matCol = *matColPtr;
return true;
}
return false;

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -41,6 +41,8 @@ public:
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const;
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const;
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;

10
examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
View File

@@ -11,9 +11,17 @@ public:
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
{
createRigidBodyGraphicsInstance(linkIndex,body,colorRgba,graphicsIndex);
}
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
{
createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
}
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) =0;

24
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -194,15 +194,37 @@ void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
{
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
}
void MyMultiBodyCreator::createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
{
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
int graphicsInstanceId = body->getUserIndex();
btVector3DoubleData speculard;
specularColor.serializeDouble(speculard);
m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
}
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
{
m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
}
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
{
createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
int graphicsInstanceId = col->getUserIndex();
btVector3DoubleData speculard;
specularColor.serializeDouble(speculard);
m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
}
btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
{
return m_bulletMultiBody;

4
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -41,10 +41,12 @@ public:
virtual ~MyMultiBodyCreator() {}
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) ;
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex) ;
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor);
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);

32
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -272,8 +272,15 @@ void ConvertURDF2BulletInternal(
{
btVector4 color = selectColor2();
u2b.getLinkColor(urdfLinkIndex,color);
UrdfMaterialColor matColor;
btVector4 color2 = selectColor2();
btVector3 specular(0.5,0.5,0.5);
if (u2b.getLinkColor2(urdfLinkIndex,matColor))
{
color2 = matColor.m_rgbaColor;
specular = matColor.m_specularColor;
}
/*
if (visual->material.get())
{
@@ -315,7 +322,7 @@ void ConvertURDF2BulletInternal(
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
processContactParameters(contactInfo, body);
creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2,specular, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
@@ -362,8 +369,14 @@ void ConvertURDF2BulletInternal(
switch (jointType)
{
case URDFFloatingJoint:
case URDFPlanarJoint:
case URDFFixedJoint:
{
if ((jointType==URDFFloatingJoint)||(jointType==URDFPlanarJoint))
{
printf("Warning: joint unsupported, creating a fixed joint instead.");
}
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
@@ -484,9 +497,16 @@ void ConvertURDF2BulletInternal(
}
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
btVector4 color = selectColor2();//(0.0,0.0,0.5);
u2b.getLinkColor(urdfLinkIndex,color);
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
btVector4 color2 = selectColor2();//(0.0,0.0,0.5);
btVector3 specularColor(1,1,1);
UrdfMaterialColor matCol;
if (u2b.getLinkColor2(urdfLinkIndex,matCol))
{
color2 = matCol.m_rgbaColor;
specularColor = matCol.m_specularColor;
}
creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex,col,color2,specularColor);
u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId());

3
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -36,6 +36,9 @@ public:
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false;}
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0;}
///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}

11
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -63,4 +63,15 @@ enum UrdfCollisionFlags
URDF_HAS_COLLISION_MASK=4,
};
struct UrdfMaterialColor
{
btVector4 m_rgbaColor;
btVector3 m_specularColor;
UrdfMaterialColor()
:m_rgbaColor(0.8, 0.8, 0.8, 1),
m_specularColor(0.4,0.4,0.4)
{
}
};
#endif //URDF_JOINT_TYPES_H

66
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -100,17 +100,33 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
}
// color
TiXmlElement *c = config->FirstChildElement("color");
if (c)
{
if (c->Attribute("rgba"))
{
if (!parseVector4(material.m_rgbaColor,c->Attribute("rgba")))
TiXmlElement *c = config->FirstChildElement("color");
if (c)
{
if (c->Attribute("rgba"))
{
std::string msg = material.m_name+" has no rgba";
logger->reportWarning(msg.c_str());
if (!parseVector4(material.m_matColor.m_rgbaColor,c->Attribute("rgba")))
{
std::string msg = material.m_name+" has no rgba";
logger->reportWarning(msg.c_str());
}
}
}
}
}
{
// specular (non-standard)
TiXmlElement *s = config->FirstChildElement("specular");
if (s)
{
if (s->Attribute("rgb"))
{
if (!parseVector3(material.m_matColor.m_specularColor,s->Attribute("rgb"),logger))
{
}
}
}
}
return true;
@@ -552,16 +568,29 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
if (name_char)
matPtr->m_name = name_char;
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
if (diffuse) {
std::string diffuseText = diffuse->GetText();
btVector4 rgba(1,0,0,1);
parseVector4(rgba,diffuseText);
matPtr->m_rgbaColor = rgba;
{
TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
if (diffuse) {
std::string diffuseText = diffuse->GetText();
btVector4 rgba(1,0,0,1);
parseVector4(rgba,diffuseText);
matPtr->m_matColor.m_rgbaColor = rgba;
visual.m_materialName = matPtr->m_name;
visual.m_geometry.m_hasLocalMaterial = true;
}
visual.m_materialName = matPtr->m_name;
visual.m_geometry.m_hasLocalMaterial = true;
}
}
{
TiXmlElement *specular = mat->FirstChildElement("specular");
if (specular) {
std::string specularText = specular->GetText();
btVector3 rgba(1,1,1);
parseVector3(rgba,specularText,logger);
matPtr->m_matColor.m_specularColor = rgba;
visual.m_materialName = matPtr->m_name;
visual.m_geometry.m_hasLocalMaterial = true;
}
}
}
else
{
@@ -577,7 +606,8 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
TiXmlElement *s = mat->FirstChildElement("specular");
if (t||c||s)
{
if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
{

8
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -17,13 +17,15 @@ struct ErrorLogger
virtual void printMessage(const char* msg)=0;
};
struct UrdfMaterial
{
std::string m_name;
std::string m_textureFilename;
btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
UrdfMaterial():
m_rgbaColor(0.8, 0.8, 0.8, 1)
UrdfMaterialColor m_matColor;
UrdfMaterial()
{
}
};

609
examples/MultiThreadedDemo/CommonRigidBodyMTBase.cpp
View File

@@ -23,10 +23,10 @@ class btCollisionShape;
#include "CommonRigidBodyMTBase.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "ParallelFor.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btPoolAllocator.h"
#include "btBulletCollisionCommon.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h"
#include "BulletDynamics/Dynamics/btSimulationIslandManagerMt.h" // for setSplitIslands()
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
@@ -35,21 +35,8 @@ class btCollisionShape;
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
#include "../MultiThreading/btTaskScheduler.h"
TaskManager gTaskMgr;
#define USE_PARALLEL_NARROWPHASE 1 // detect collisions in parallel
#define USE_PARALLEL_ISLAND_SOLVER 1 // solve simulation islands in parallel
#define USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS 1
#define USE_PARALLEL_INTEGRATE_TRANSFORMS 1
#define USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION 1
#if defined (_MSC_VER) && _MSC_VER >= 1600
// give us a compile error if any signatures of overriden methods is changed
#define BT_OVERRIDE override
#else
#define BT_OVERRIDE
#endif
static int gNumIslands = 0;
@@ -124,7 +111,7 @@ public:
};
Profiler gProfiler;
static Profiler gProfiler;
class ProfileHelper
{
@@ -141,457 +128,84 @@ public:
}
};
int gThreadsRunningCounter = 0;
btSpinMutex gThreadsRunningCounterMutex;
void btPushThreadsAreRunning()
static void profileBeginCallback( btDynamicsWorld *world, btScalar timeStep )
{
gThreadsRunningCounterMutex.lock();
gThreadsRunningCounter++;
gThreadsRunningCounterMutex.unlock();
gProfiler.begin( Profiler::kRecordInternalTimeStep );
}
void btPopThreadsAreRunning()
static void profileEndCallback( btDynamicsWorld *world, btScalar timeStep )
{
gThreadsRunningCounterMutex.lock();
gThreadsRunningCounter--;
gThreadsRunningCounterMutex.unlock();
}
bool btThreadsAreRunning()
{
return gThreadsRunningCounter != 0;
gProfiler.end( Profiler::kRecordInternalTimeStep );
}
#if USE_PARALLEL_NARROWPHASE
class MyCollisionDispatcher : public btCollisionDispatcher
///
/// MyCollisionDispatcher -- subclassed for profiling purposes
///
class MyCollisionDispatcher : public btCollisionDispatcherMt
{
btSpinMutex m_manifoldPtrsMutex;
typedef btCollisionDispatcherMt ParentClass;
public:
MyCollisionDispatcher( btCollisionConfiguration* config ) : btCollisionDispatcher( config )
MyCollisionDispatcher( btCollisionConfiguration* config, int grainSize ) : btCollisionDispatcherMt( config, grainSize )
{
}
virtual ~MyCollisionDispatcher()
{
}
btPersistentManifold* getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) BT_OVERRIDE
{
// added spin-locks
//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
btScalar contactBreakingThreshold = ( m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) ?
btMin( body0->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ), body1->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ) )
: gContactBreakingThreshold;
btScalar contactProcessingThreshold = btMin( body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold() );
void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
if (NULL == mem)
{
//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
if ( ( m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION ) == 0 )
{
mem = btAlignedAlloc( sizeof( btPersistentManifold ), 16 );
}
else
{
btAssert( 0 );
//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
return 0;
}
}
btPersistentManifold* manifold = new(mem) btPersistentManifold( body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold );
m_manifoldPtrsMutex.lock();
manifold->m_index1a = m_manifoldsPtr.size();
m_manifoldsPtr.push_back( manifold );
m_manifoldPtrsMutex.unlock();
return manifold;
}
void releaseManifold( btPersistentManifold* manifold ) BT_OVERRIDE
{
clearManifold( manifold );
m_manifoldPtrsMutex.lock();
int findIndex = manifold->m_index1a;
btAssert( findIndex < m_manifoldsPtr.size() );
m_manifoldsPtr.swap( findIndex, m_manifoldsPtr.size() - 1 );
m_manifoldsPtr[ findIndex ]->m_index1a = findIndex;
m_manifoldsPtr.pop_back();
m_manifoldPtrsMutex.unlock();
manifold->~btPersistentManifold();
if ( m_persistentManifoldPoolAllocator->validPtr( manifold ) )
{
m_persistentManifoldPoolAllocator->freeMemory( manifold );
}
else
{
btAlignedFree( manifold );
}
}
struct Updater
{
btBroadphasePair* mPairArray;
btNearCallback mCallback;
btCollisionDispatcher* mDispatcher;
const btDispatcherInfo* mInfo;
Updater()
{
mPairArray = NULL;
mCallback = NULL;
mDispatcher = NULL;
mInfo = NULL;
}
void forLoop( int iBegin, int iEnd ) const
{
for ( int i = iBegin; i < iEnd; ++i )
{
btBroadphasePair* pair = &mPairArray[ i ];
mCallback( *pair, *mDispatcher, *mInfo );
}
}
};
virtual void dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) BT_OVERRIDE
{
ProfileHelper prof(Profiler::kRecordDispatchAllCollisionPairs);
int grainSize = 40; // iterations per task
int pairCount = pairCache->getNumOverlappingPairs();
Updater updater;
updater.mCallback = getNearCallback();
updater.mPairArray = pairCount > 0 ? pairCache->getOverlappingPairArrayPtr() : NULL;
updater.mDispatcher = this;
updater.mInfo = &info;
btPushThreadsAreRunning();
parallelFor( 0, pairCount, grainSize, updater );
btPopThreadsAreRunning();
if (m_manifoldsPtr.size() < 1)
return;
// reconstruct the manifolds array to ensure determinism
m_manifoldsPtr.resizeNoInitialize(0);
btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
for (int i = 0; i < pairCount; ++i)
{
btCollisionAlgorithm* algo = pairs[i].m_algorithm;
if (algo) algo->getAllContactManifolds(m_manifoldsPtr);
}
// update the indices (used when releasing manifolds)
for (int i = 0; i < m_manifoldsPtr.size(); ++i)
m_manifoldsPtr[i]->m_index1a = i;
ProfileHelper prof( Profiler::kRecordDispatchAllCollisionPairs );
ParentClass::dispatchAllCollisionPairs( pairCache, info, dispatcher );
}
};
#endif
#if USE_PARALLEL_ISLAND_SOLVER
///
/// MyConstraintSolverPool - masquerades as a constraint solver, but really it is a threadsafe pool of them.
///
/// Each solver in the pool is protected by a mutex. When solveGroup is called from a thread,
/// the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the
/// call to the solver.
/// So long as there are at least as many solvers as there are hardware threads, it should never need to
/// spin wait.
///
class MyConstraintSolverPool : public btConstraintSolver
/// myParallelIslandDispatch -- wrap default parallel dispatch for profiling and to get the number of simulation islands
//
void myParallelIslandDispatch( btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, btSimulationIslandManagerMt::IslandCallback* callback )
{
const static size_t kCacheLineSize = 128;
struct ThreadSolver
{
btConstraintSolver* solver;
btSpinMutex mutex;
char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ]; // keep mutexes from sharing a cache line
};
btAlignedObjectArray<ThreadSolver> m_solvers;
btConstraintSolverType m_solverType;
ThreadSolver* getAndLockThreadSolver()
{
while ( true )
{
for ( int i = 0; i < m_solvers.size(); ++i )
{
ThreadSolver& solver = m_solvers[ i ];
if ( solver.mutex.tryLock() )
{
return &solver;
}
}
}
return NULL;
}
void init( btConstraintSolver** solvers, int numSolvers )
{
m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
m_solvers.resize( numSolvers );
for ( int i = 0; i < numSolvers; ++i )
{
m_solvers[ i ].solver = solvers[ i ];
}
if ( numSolvers > 0 )
{
m_solverType = solvers[ 0 ]->getSolverType();
}
}
public:
// create the solvers for me
explicit MyConstraintSolverPool( int numSolvers )
{
btAlignedObjectArray<btConstraintSolver*> solvers;
solvers.reserve( numSolvers );
for ( int i = 0; i < numSolvers; ++i )
{
btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
solvers.push_back( solver );
}
init( &solvers[ 0 ], numSolvers );
}
// pass in fully constructed solvers (destructor will delete them)
MyConstraintSolverPool( btConstraintSolver** solvers, int numSolvers )
{
init( solvers, numSolvers );
}
virtual ~MyConstraintSolverPool()
{
// delete all solvers
for ( int i = 0; i < m_solvers.size(); ++i )
{
ThreadSolver& solver = m_solvers[ i ];
delete solver.solver;
solver.solver = NULL;
}
}
//virtual void prepareSolve( int /* numBodies */, int /* numManifolds */ ) { ; } // does nothing
///solve a group of constraints
virtual btScalar solveGroup( btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher
)
{
ThreadSolver* solver = getAndLockThreadSolver();
solver->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher );
solver->mutex.unlock();
return 0.0f;
}
//virtual void allSolved( const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */ ) { ; } // does nothing
///clear internal cached data and reset random seed
virtual void reset()
{
for ( int i = 0; i < m_solvers.size(); ++i )
{
ThreadSolver& solver = m_solvers[ i ];
solver.mutex.lock();
solver.solver->reset();
solver.mutex.unlock();
}
}
virtual btConstraintSolverType getSolverType() const
{
return m_solverType;
}
};
struct UpdateIslandDispatcher
{
btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
btSimulationIslandManagerMt::IslandCallback* callback;
void forLoop( int iBegin, int iEnd ) const
{
for ( int i = iBegin; i < iEnd; ++i )
{
btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
callback->processIsland( &island->bodyArray[ 0 ],
island->bodyArray.size(),
manifolds,
island->manifoldArray.size(),
constraintsPtr,
island->constraintArray.size(),
island->id
);
}
}
};
void parallelIslandDispatch( btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, btSimulationIslandManagerMt::IslandCallback* callback )
{
ProfileHelper prof(Profiler::kRecordDispatchIslands);
ProfileHelper prof( Profiler::kRecordDispatchIslands );
gNumIslands = islandsPtr->size();
int grainSize = 1; // iterations per task
UpdateIslandDispatcher dispatcher;
dispatcher.islandsPtr = islandsPtr;
dispatcher.callback = callback;
btPushThreadsAreRunning();
parallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
btPopThreadsAreRunning();
}
#endif //#if USE_PARALLEL_ISLAND_SOLVER
void profileBeginCallback(btDynamicsWorld *world, btScalar timeStep)
{
gProfiler.begin(Profiler::kRecordInternalTimeStep);
btSimulationIslandManagerMt::parallelIslandDispatch( islandsPtr, callback );
}
void profileEndCallback(btDynamicsWorld *world, btScalar timeStep)
{
gProfiler.end(Profiler::kRecordInternalTimeStep);
}
///
/// MyDiscreteDynamicsWorld
///
/// Should function exactly like btDiscreteDynamicsWorld.
/// 3 methods that iterate over all of the rigidbodies can run in parallel:
/// - predictUnconstraintMotion
/// - integrateTransforms
/// - createPredictiveContacts
/// MyDiscreteDynamicsWorld -- subclassed for profiling purposes
///
ATTRIBUTE_ALIGNED16( class ) MyDiscreteDynamicsWorld : public btDiscreteDynamicsWorldMt
{
typedef btDiscreteDynamicsWorld ParentClass;
typedef btDiscreteDynamicsWorldMt ParentClass;
protected:
#if USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION
struct UpdaterUnconstrainedMotion
{
btScalar timeStep;
btRigidBody** rigidBodies;
void forLoop( int iBegin, int iEnd ) const
{
for ( int i = iBegin; i < iEnd; ++i )
{
btRigidBody* body = rigidBodies[ i ];
if ( !body->isStaticOrKinematicObject() )
{
//don't integrate/update velocities here, it happens in the constraint solver
body->applyDamping( timeStep );
body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() );
}
}
}
};
virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE
{
ProfileHelper prof( Profiler::kRecordPredictUnconstrainedMotion );
BT_PROFILE( "predictUnconstraintMotion" );
int grainSize = 50; // num of iterations per task for TBB
int bodyCount = m_nonStaticRigidBodies.size();
UpdaterUnconstrainedMotion update;
update.timeStep = timeStep;
update.rigidBodies = bodyCount ? &m_nonStaticRigidBodies[ 0 ] : NULL;
btPushThreadsAreRunning();
parallelFor( 0, bodyCount, grainSize, update );
btPopThreadsAreRunning();
ParentClass::predictUnconstraintMotion( timeStep );
}
#endif // #if USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION
#if USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS
struct UpdaterCreatePredictiveContacts
{
btScalar timeStep;
btRigidBody** rigidBodies;
MyDiscreteDynamicsWorld* world;
void forLoop( int iBegin, int iEnd ) const
{
world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
}
};
virtual void createPredictiveContacts( btScalar timeStep )
virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE
{
ProfileHelper prof( Profiler::kRecordCreatePredictiveContacts );
releasePredictiveContacts();
int grainSize = 50; // num of iterations per task for TBB or OPENMP
if ( int bodyCount = m_nonStaticRigidBodies.size() )
{
UpdaterCreatePredictiveContacts update;
update.world = this;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
btPushThreadsAreRunning();
parallelFor( 0, bodyCount, grainSize, update );
btPopThreadsAreRunning();
}
ParentClass::createPredictiveContacts( timeStep );
}
#endif // #if USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS
#if USE_PARALLEL_INTEGRATE_TRANSFORMS
struct UpdaterIntegrateTransforms
{
btScalar timeStep;
btRigidBody** rigidBodies;
MyDiscreteDynamicsWorld* world;
void forLoop( int iBegin, int iEnd ) const
{
world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
}
};
virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE
{
ProfileHelper prof( Profiler::kRecordIntegrateTransforms );
BT_PROFILE( "integrateTransforms" );
int grainSize = 50; // num of iterations per task for TBB or OPENMP
if ( int bodyCount = m_nonStaticRigidBodies.size() )
{
UpdaterIntegrateTransforms update;
update.world = this;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
btPushThreadsAreRunning();
parallelFor( 0, bodyCount, grainSize, update );
btPopThreadsAreRunning();
}
ParentClass::integrateTransforms( timeStep );
}
#endif // #if USE_PARALLEL_INTEGRATE_TRANSFORMS
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
MyDiscreteDynamicsWorld( btDispatcher* dispatcher,
btBroadphaseInterface* pairCache,
btConstraintSolver* constraintSolver,
btConstraintSolverPoolMt* constraintSolver,
btCollisionConfiguration* collisionConfiguration
) :
btDiscreteDynamicsWorldMt( dispatcher, pairCache, constraintSolver, collisionConfiguration )
{
#if USE_PARALLEL_ISLAND_SOLVER
btSimulationIslandManagerMt* islandMgr = static_cast<btSimulationIslandManagerMt*>( m_islandManager );
islandMgr->setIslandDispatchFunction( parallelIslandDispatch );
#endif //#if USE_PARALLEL_ISLAND_SOLVER
islandMgr->setIslandDispatchFunction( myParallelIslandDispatch );
}
};
@@ -625,8 +239,69 @@ btConstraintSolver* createSolverByType( SolverType t )
}
///
/// btTaskSchedulerManager -- manage a number of task schedulers so we can switch between them
///
class btTaskSchedulerManager
{
btAlignedObjectArray<btITaskScheduler*> m_taskSchedulers;
btAlignedObjectArray<btITaskScheduler*> m_allocatedTaskSchedulers;
public:
btTaskSchedulerManager() {}
void init()
{
addTaskScheduler( btGetSequentialTaskScheduler() );
#if BT_THREADSAFE
if ( btITaskScheduler* ts = createDefaultTaskScheduler() )
{
m_allocatedTaskSchedulers.push_back( ts );
addTaskScheduler( ts );
}
addTaskScheduler( btGetOpenMPTaskScheduler() );
addTaskScheduler( btGetTBBTaskScheduler() );
addTaskScheduler( btGetPPLTaskScheduler() );
if ( getNumTaskSchedulers() > 1 )
{
// prefer a non-sequential scheduler if available
btSetTaskScheduler( m_taskSchedulers[ 1 ] );
}
else
{
btSetTaskScheduler( m_taskSchedulers[ 0 ] );
}
#endif // #if BT_THREADSAFE
}
void shutdown()
{
for ( int i = 0; i < m_allocatedTaskSchedulers.size(); ++i )
{
delete m_allocatedTaskSchedulers[ i ];
}
m_allocatedTaskSchedulers.clear();
}
void addTaskScheduler( btITaskScheduler* ts )
{
if ( ts )
{
m_taskSchedulers.push_back( ts );
}
}
int getNumTaskSchedulers() const { return m_taskSchedulers.size(); }
btITaskScheduler* getTaskScheduler( int i ) { return m_taskSchedulers[ i ]; }
};
static btTaskSchedulerManager gTaskSchedulerMgr;
#if BT_THREADSAFE
static bool gMultithreadedWorld = true;
static bool gDisplayProfileInfo = true;
#else
static bool gMultithreadedWorld = false;
static bool gDisplayProfileInfo = false;
#endif
static SolverType gSolverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE;
static int gSolverMode = SOLVER_SIMD |
SOLVER_USE_WARMSTARTING |
@@ -652,15 +327,17 @@ CommonRigidBodyMTBase::CommonRigidBodyMTBase( struct GUIHelperInterface* helper
{
m_multithreadedWorld = false;
m_multithreadCapable = false;
gTaskMgr.init();
if ( gTaskSchedulerMgr.getNumTaskSchedulers() == 0 )
{
gTaskSchedulerMgr.init();
}
}
CommonRigidBodyMTBase::~CommonRigidBodyMTBase()
{
gTaskMgr.shutdown();
}
void boolPtrButtonCallback(int buttonId, bool buttonState, void* userPointer)
static void boolPtrButtonCallback(int buttonId, bool buttonState, void* userPointer)
{
if (bool* val = static_cast<bool*>(userPointer))
{
@@ -668,7 +345,7 @@ void boolPtrButtonCallback(int buttonId, bool buttonState, void* userPointer)
}
}
void toggleSolverModeCallback(int buttonId, bool buttonState, void* userPointer)
static void toggleSolverModeCallback(int buttonId, bool buttonState, void* userPointer)
{
if (buttonState)
{
@@ -687,7 +364,7 @@ void toggleSolverModeCallback(int buttonId, bool buttonState, void* userPointer)
}
}
void setSolverTypeCallback(int buttonId, bool buttonState, void* userPointer)
static void setSolverTypeCallback(int buttonId, bool buttonState, void* userPointer)
{
if (buttonId >= 0 && buttonId < SOLVER_TYPE_COUNT)
{
@@ -695,32 +372,34 @@ void setSolverTypeCallback(int buttonId, bool buttonState, void* userPointer)
}
}
void apiSelectButtonCallback(int buttonId, bool buttonState, void* userPointer)
static void setNumThreads( int numThreads )
{
gTaskMgr.setApi(static_cast<TaskManager::Api>(buttonId));
if (gTaskMgr.getApi()==TaskManager::apiNone)
#if BT_THREADSAFE
int newNumThreads = ( std::min )( numThreads, int( BT_MAX_THREAD_COUNT ) );
int oldNumThreads = btGetTaskScheduler()->getNumThreads();
// only call when the thread count is different
if ( newNumThreads != oldNumThreads )
{
gSliderNumThreads = 1.0f;
}
else
{
gSliderNumThreads = float(gTaskMgr.getNumThreads());
btGetTaskScheduler()->setNumThreads( newNumThreads );
}
#endif // #if BT_THREADSAFE
}
void setThreadCountCallback(float val, void* userPtr)
static void apiSelectButtonCallback(int buttonId, bool buttonState, void* userPointer)
{
if (gTaskMgr.getApi()==TaskManager::apiNone)
{
gSliderNumThreads = 1.0f;
}
else
{
gTaskMgr.setNumThreads( int( gSliderNumThreads ) );
}
#if BT_THREADSAFE
// change the task scheduler
btSetTaskScheduler( gTaskSchedulerMgr.getTaskScheduler( buttonId ) );
setNumThreads( int( gSliderNumThreads ) );
#endif // #if BT_THREADSAFE
}
void setSolverIterationCountCallback(float val, void* userPtr)
static void setThreadCountCallback(float val, void* userPtr)
{
setNumThreads( int( gSliderNumThreads ) );
}
static void setSolverIterationCountCallback(float val, void* userPtr)
{
if (btDiscreteDynamicsWorld* world = reinterpret_cast<btDiscreteDynamicsWorld*>(userPtr))
{
@@ -733,40 +412,37 @@ void CommonRigidBodyMTBase::createEmptyDynamicsWorld()
gNumIslands = 0;
m_solverType = gSolverType;
#if BT_THREADSAFE && (BT_USE_OPENMP || BT_USE_PPL || BT_USE_TBB)
btAssert( btGetTaskScheduler() != NULL );
m_multithreadCapable = true;
#endif
if ( gMultithreadedWorld )
{
#if BT_THREADSAFE
m_dispatcher = NULL;
btDefaultCollisionConstructionInfo cci;
cci.m_defaultMaxPersistentManifoldPoolSize = 80000;
cci.m_defaultMaxCollisionAlgorithmPoolSize = 80000;
m_collisionConfiguration = new btDefaultCollisionConfiguration( cci );
#if USE_PARALLEL_NARROWPHASE
m_dispatcher = new MyCollisionDispatcher( m_collisionConfiguration );
#else
m_dispatcher = new btCollisionDispatcher( m_collisionConfiguration );
#endif //USE_PARALLEL_NARROWPHASE
m_dispatcher = new MyCollisionDispatcher( m_collisionConfiguration, 40 );
m_broadphase = new btDbvtBroadphase();
#if BT_THREADSAFE && USE_PARALLEL_ISLAND_SOLVER
btConstraintSolverPoolMt* solverPool;
{
btConstraintSolver* solvers[ BT_MAX_THREAD_COUNT ];
int maxThreadCount = btMin( int(BT_MAX_THREAD_COUNT), TaskManager::getMaxNumThreads() );
int maxThreadCount = BT_MAX_THREAD_COUNT;
for ( int i = 0; i < maxThreadCount; ++i )
{
solvers[ i ] = createSolverByType( m_solverType );
}
m_solver = new MyConstraintSolverPool( solvers, maxThreadCount );
solverPool = new btConstraintSolverPoolMt( solvers, maxThreadCount );
m_solver = solverPool;
}
#else
m_solver = createSolverByType( m_solverType );
#endif //#if USE_PARALLEL_ISLAND_SOLVER
btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration );
btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, solverPool, m_collisionConfiguration );
m_dynamicsWorld = world;
m_multithreadedWorld = true;
btAssert( btGetTaskScheduler() != NULL );
#endif // #if BT_THREADSAFE
}
else
{
@@ -885,29 +561,32 @@ void CommonRigidBodyMTBase::createDefaultParameters()
}
if (m_multithreadedWorld)
{
#if BT_THREADSAFE
// create a button for each supported threading API
for (int iApi = 0; iApi < TaskManager::apiCount; ++iApi)
for ( int iApi = 0; iApi < gTaskSchedulerMgr.getNumTaskSchedulers(); ++iApi )
{
TaskManager::Api api = static_cast<TaskManager::Api>(iApi);
if (gTaskMgr.isSupported(api))
{
char str[1024];
sprintf(str, "API %s", gTaskMgr.getApiName(api));
ButtonParams button( str, iApi, false );
button.m_callback = apiSelectButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
char str[ 1024 ];
sprintf( str, "API %s", gTaskSchedulerMgr.getTaskScheduler(iApi)->getName() );
ButtonParams button( str, iApi, false );
button.m_callback = apiSelectButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
{
// create a slider to set the number of threads to use
gSliderNumThreads = float(gTaskMgr.getNumThreads());
int numThreads = btGetTaskScheduler()->getNumThreads();
// if slider has not been set yet (by another demo),
if ( gSliderNumThreads <= 1.0f )
{
gSliderNumThreads = float( numThreads );
}
SliderParams slider("Thread count", &gSliderNumThreads);
slider.m_minVal = 1.0f;
slider.m_maxVal = float(gTaskMgr.getMaxNumThreads()*2);
slider.m_maxVal = float( BT_MAX_THREAD_COUNT );
slider.m_callback = setThreadCountCallback;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
#endif // #if BT_THREADSAFE
}
}
@@ -939,6 +618,7 @@ void CommonRigidBodyMTBase::drawScreenText()
{
if ( m_multithreadedWorld )
{
#if BT_THREADSAFE
int numManifolds = m_dispatcher->getNumManifolds();
int numContacts = 0;
for ( int i = 0; i < numManifolds; ++i )
@@ -946,17 +626,18 @@ void CommonRigidBodyMTBase::drawScreenText()
const btPersistentManifold* man = m_dispatcher->getManifoldByIndexInternal( i );
numContacts += man->getNumContacts();
}
const char* mtApi = TaskManager::getApiName( gTaskMgr.getApi() );
const char* mtApi = btGetTaskScheduler()->getName();
sprintf( msg, "islands=%d bodies=%d manifolds=%d contacts=%d [%s] threads=%d",
gNumIslands,
m_dynamicsWorld->getNumCollisionObjects(),
numManifolds,
numContacts,
mtApi,
gTaskMgr.getApi() == TaskManager::apiNone ? 1 : gTaskMgr.getNumThreads()
btGetTaskScheduler()->getNumThreads()
);
m_guiHelper->getAppInterface()->drawText( msg, 100, yCoord, 0.4f );
yCoord += yStep;
#endif // #if BT_THREADSAFE
}
{
int sm = gSolverMode;

5
examples/MultiThreadedDemo/MultiThreadedDemo.cpp
View File

@@ -28,11 +28,10 @@ class btCollisionShape;
#include "btBulletCollisionCommon.h"
#define BT_OVERRIDE
static btScalar gSliderStackRows = 8.0f;
static btScalar gSliderStackColumns = 6.0f;
static btScalar gSliderStackHeight = 15.0f;
static btScalar gSliderStackHeight = 10.0f;
static btScalar gSliderGroundHorizontalAmplitude = 0.0f;
static btScalar gSliderGroundVerticalAmplitude = 0.0f;
@@ -240,7 +239,7 @@ void MultiThreadedDemo::createSceneObjects()
const btVector3 halfExtents = btVector3( 0.5f, 0.25f, 0.5f );
int numStackRows = btMax(1, int(gSliderStackRows));
int numStackCols = btMax(1, int(gSliderStackColumns));
int stackHeight = 15;
int stackHeight = int(gSliderStackHeight);
float stackZSpacing = 3.0f;
float stackXSpacing = 20.0f;

336
examples/MultiThreadedDemo/ParallelFor.h
View File

@@ -1,336 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include <stdio.h> //printf debugging
#include <algorithm>
// choose threading providers:
#if BT_USE_TBB
#define USE_TBB 1 // use Intel Threading Building Blocks for thread management
#endif
#if BT_USE_PPL
#define USE_PPL 1 // use Microsoft Parallel Patterns Library (installed with Visual Studio 2010 and later)
#endif // BT_USE_PPL
#if BT_USE_OPENMP
#define USE_OPENMP 1 // use OpenMP (also need to change compiler options for OpenMP support)
#endif
#if USE_OPENMP
#include <omp.h>
#endif // #if USE_OPENMP
#if USE_PPL
#include <ppl.h> // if you get a compile error here, check whether your version of Visual Studio includes PPL
// Visual Studio 2010 and later should come with it
#include <concrtrm.h> // for GetProcessorCount()
#endif // #if USE_PPL
#if USE_TBB
#define __TBB_NO_IMPLICIT_LINKAGE 1
#include <tbb/tbb.h>
#include <tbb/task_scheduler_init.h>
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#endif // #if USE_TBB
class TaskManager
{
public:
enum Api
{
apiNone,
apiOpenMP,
apiTbb,
apiPpl,
apiCount
};
static const char* getApiName( Api api )
{
switch ( api )
{
case apiNone: return "None";
case apiOpenMP: return "OpenMP";
case apiTbb: return "Intel TBB";
case apiPpl: return "MS PPL";
default: return "unknown";
}
}
TaskManager()
{
m_api = apiNone;
m_numThreads = 0;
#if USE_TBB
m_tbbSchedulerInit = NULL;
#endif // #if USE_TBB
}
Api getApi() const
{
return m_api;
}
bool isSupported( Api api ) const
{
#if USE_OPENMP
if ( api == apiOpenMP )
{
return true;
}
#endif
#if USE_TBB
if ( api == apiTbb )
{
return true;
}
#endif
#if USE_PPL
if ( api == apiPpl )
{
return true;
}
#endif
// apiNone is always "supported"
return api == apiNone;
}
void setApi( Api api )
{
if (isSupported(api))
{
m_api = api;
}
else
{
// no compile time support for selected API, fallback to "none"
m_api = apiNone;
}
}
static int getMaxNumThreads()
{
#if USE_OPENMP
return omp_get_max_threads();
#elif USE_PPL
return concurrency::GetProcessorCount();
#elif USE_TBB
return tbb::task_scheduler_init::default_num_threads();
#endif
return 1;
}
int getNumThreads() const
{
return m_numThreads;
}
int setNumThreads( int numThreads )
{
m_numThreads = ( std::max )( 1, numThreads );
#if USE_OPENMP
omp_set_num_threads( m_numThreads );
#endif
#if USE_PPL
{
using namespace concurrency;
if ( CurrentScheduler::Id() != -1 )
{
CurrentScheduler::Detach();
}
SchedulerPolicy policy;
policy.SetConcurrencyLimits( m_numThreads, m_numThreads );
CurrentScheduler::Create( policy );
}
#endif
#if USE_TBB
if ( m_tbbSchedulerInit )
{
delete m_tbbSchedulerInit;
m_tbbSchedulerInit = NULL;
}
m_tbbSchedulerInit = new tbb::task_scheduler_init( m_numThreads );
#endif
return m_numThreads;
}
void init()
{
if (m_numThreads == 0)
{
#if USE_PPL
setApi( apiPpl );
#endif
#if USE_TBB
setApi( apiTbb );
#endif
#if USE_OPENMP
setApi( apiOpenMP );
#endif
setNumThreads(getMaxNumThreads());
}
else
{
setNumThreads(m_numThreads);
}
}
void shutdown()
{
#if USE_TBB
if ( m_tbbSchedulerInit )
{
delete m_tbbSchedulerInit;
m_tbbSchedulerInit = NULL;
}
#endif
}
private:
Api m_api;
int m_numThreads;
#if USE_TBB
tbb::task_scheduler_init* m_tbbSchedulerInit;
#endif // #if USE_TBB
};
extern TaskManager gTaskMgr;
inline static void initTaskScheduler()
{
gTaskMgr.init();
}
inline static void cleanupTaskScheduler()
{
gTaskMgr.shutdown();
}
#if USE_TBB
///
/// TbbBodyAdapter -- Converts a body object that implements the
/// "forLoop(int iBegin, int iEnd) const" function
/// into a TBB compatible object that takes a tbb::blocked_range<int> type.
///
template <class TBody>
struct TbbBodyAdapter
{
const TBody* mBody;
void operator()( const tbb::blocked_range<int>& range ) const
{
mBody->forLoop( range.begin(), range.end() );
}
};
#endif // #if USE_TBB
#if USE_PPL
///
/// PplBodyAdapter -- Converts a body object that implements the
/// "forLoop(int iBegin, int iEnd) const" function
/// into a PPL compatible object that implements "void operator()( int ) const"
///
template <class TBody>
struct PplBodyAdapter
{
const TBody* mBody;
int mGrainSize;
int mIndexEnd;
void operator()( int i ) const
{
mBody->forLoop( i, (std::min)(i + mGrainSize, mIndexEnd) );
}
};
#endif // #if USE_PPL
///
/// parallelFor -- interface for submitting work expressed as a for loop to the worker threads
///
template <class TBody>
void parallelFor( int iBegin, int iEnd, int grainSize, const TBody& body )
{
#if USE_OPENMP
if ( gTaskMgr.getApi() == TaskManager::apiOpenMP )
{
#pragma omp parallel for schedule(static, 1)
for ( int i = iBegin; i < iEnd; i += grainSize )
{
body.forLoop( i, (std::min)( i + grainSize, iEnd ) );
}
return;
}
#endif // #if USE_OPENMP
#if USE_PPL
if ( gTaskMgr.getApi() == TaskManager::apiPpl )
{
// PPL dispatch
PplBodyAdapter<TBody> pplBody;
pplBody.mBody = &body;
pplBody.mGrainSize = grainSize;
pplBody.mIndexEnd = iEnd;
// note: MSVC 2010 doesn't support partitioner args, so avoid them
concurrency::parallel_for( iBegin,
iEnd,
grainSize,
pplBody
);
return;
}
#endif //#if USE_PPL
#if USE_TBB
if ( gTaskMgr.getApi() == TaskManager::apiTbb )
{
// TBB dispatch
TbbBodyAdapter<TBody> tbbBody;
tbbBody.mBody = &body;
tbb::parallel_for( tbb::blocked_range<int>( iBegin, iEnd, grainSize ),
tbbBody,
tbb::simple_partitioner()
);
return;
}
#endif // #if USE_TBB
{
// run on main thread
body.forLoop( iBegin, iEnd );
}
}

6
examples/MultiThreading/b3Win32ThreadSupport.cpp
View File

@@ -279,9 +279,9 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
}
DWORD mask = 1;
mask = 1<<mask;
SetThreadAffinityMask(handle, mask);
//SetThreadAffinityMask(handle, 1 << 1); // this is what it was doing originally, a complete disaster for threading performance!
//SetThreadAffinityMask(handle, 1 << i); // I'm guessing this was the intention, but is still bad for performance due to one of the threads
// sometimes unable to execute because it wants to be on the same processor as the main thread (my guess)
threadStatus.m_taskId = i;
threadStatus.m_commandId = 0;

448
examples/MultiThreading/btTaskScheduler.cpp Normal file
View File

@@ -0,0 +1,448 @@
#include "LinearMath/btTransform.h"
#include "../Utils/b3Clock.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btThreads.h"
#include "LinearMath/btQuickprof.h"
#include <stdio.h>
#include <algorithm>
typedef void( *btThreadFunc )( void* userPtr, void* lsMemory );
typedef void* ( *btThreadLocalStorageFunc )();
#if BT_THREADSAFE
#if defined( _WIN32 )
#include "b3Win32ThreadSupport.h"
b3ThreadSupportInterface* createThreadSupport( int numThreads, btThreadFunc threadFunc, btThreadLocalStorageFunc localStoreFunc, const char* uniqueName )
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo constructionInfo( uniqueName, threadFunc, localStoreFunc, numThreads );
//constructionInfo.m_priority = 0; // highest priority (the default) -- can cause erratic performance when numThreads > numCores
// we don't want worker threads to be higher priority than the main thread or the main thread could get
// totally shut out and unable to tell the workers to stop
constructionInfo.m_priority = -1; // normal priority
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport( constructionInfo );
return threadSupport;
}
#else // #if defined( _WIN32 )
#include "b3PosixThreadSupport.h"
b3ThreadSupportInterface* createThreadSupport( int numThreads, btThreadFunc threadFunc, btThreadLocalStorageFunc localStoreFunc, const char* uniqueName)
{
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo( uniqueName, threadFunc, localStoreFunc, numThreads );
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport( constructionInfo );
return threadSupport;
}
#endif // #else // #if defined( _WIN32 )
///
/// getNumHardwareThreads()
///
///
/// https://stackoverflow.com/questions/150355/programmatically-find-the-number-of-cores-on-a-machine
///
#if __cplusplus >= 201103L
#include <thread>
int getNumHardwareThreads()
{
return std::thread::hardware_concurrency();
}
#elif defined( _WIN32 )
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
int getNumHardwareThreads()
{
// caps out at 32
SYSTEM_INFO info;
GetSystemInfo( &info );
return info.dwNumberOfProcessors;
}
#else
int getNumHardwareThreads()
{
return 0; // don't know
}
#endif
struct WorkerThreadStatus
{
enum Type
{
kInvalid,
kWaitingForWork,
kWorking,
kSleeping,
};
};
struct IJob
{
virtual void executeJob() = 0;
};
class ParallelForJob : public IJob
{
const btIParallelForBody* mBody;
int mBegin;
int mEnd;
public:
ParallelForJob()
{
mBody = NULL;
mBegin = 0;
mEnd = 0;
}
void init( int iBegin, int iEnd, const btIParallelForBody& body )
{
mBody = &body;
mBegin = iBegin;
mEnd = iEnd;
}
virtual void executeJob() override
{
BT_PROFILE( "executeJob" );
// call the functor body to do the work
mBody->forLoop( mBegin, mEnd );
}
};
struct JobContext
{
JobContext()
{
m_queueLock = NULL;
m_headIndex = 0;
m_tailIndex = 0;
m_workersShouldCheckQueue = false;
m_useSpinMutex = false;
}
b3CriticalSection* m_queueLock;
btSpinMutex m_mutex;
volatile bool m_workersShouldCheckQueue;
btAlignedObjectArray<IJob*> m_jobQueue;
bool m_queueIsEmpty;
int m_tailIndex;
int m_headIndex;
bool m_useSpinMutex;
void lockQueue()
{
if ( m_useSpinMutex )
{
m_mutex.lock();
}
else
{
m_queueLock->lock();
}
}
void unlockQueue()
{
if ( m_useSpinMutex )
{
m_mutex.unlock();
}
else
{
m_queueLock->unlock();
}
}
void clearQueue()
{
lockQueue();
m_headIndex = 0;
m_tailIndex = 0;
m_queueIsEmpty = true;
unlockQueue();
m_jobQueue.resizeNoInitialize( 0 );
}
void submitJob( IJob* job )
{
m_jobQueue.push_back( job );
lockQueue();
m_tailIndex++;
m_queueIsEmpty = false;
unlockQueue();
}
IJob* consumeJob()
{
if ( m_queueIsEmpty )
{
// lock free path. even if this is taken erroneously it isn't harmful
return NULL;
}
IJob* job = NULL;
lockQueue();
if ( !m_queueIsEmpty )
{
job = m_jobQueue[ m_headIndex++ ];
if ( m_headIndex == m_tailIndex )
{
m_queueIsEmpty = true;
}
}
unlockQueue();
return job;
}
};
struct WorkerThreadLocalStorage
{
int threadId;
WorkerThreadStatus::Type status;
};
static void WorkerThreadFunc( void* userPtr, void* lsMemory )
{
BT_PROFILE( "WorkerThreadFunc" );
WorkerThreadLocalStorage* localStorage = (WorkerThreadLocalStorage*) lsMemory;
localStorage->status = WorkerThreadStatus::kWaitingForWork;
//printf( "WorkerThreadFunc: worker %d start working\n", localStorage->threadId );
JobContext* jobContext = (JobContext*) userPtr;
while ( jobContext->m_workersShouldCheckQueue )
{
if ( IJob* job = jobContext->consumeJob() )
{
localStorage->status = WorkerThreadStatus::kWorking;
job->executeJob();
localStorage->status = WorkerThreadStatus::kWaitingForWork;
}
else
{
// todo: spin wait a bit to avoid hammering the empty queue
}
}
//printf( "WorkerThreadFunc stop working\n" );
localStorage->status = WorkerThreadStatus::kSleeping;
// go idle
}
static void* WorkerThreadAllocFunc()
{
return new WorkerThreadLocalStorage;
}
class btTaskSchedulerDefault : public btITaskScheduler
{
JobContext m_jobContext;
b3ThreadSupportInterface* m_threadSupport;
btAlignedObjectArray<ParallelForJob> m_jobs;
btSpinMutex m_antiNestingLock; // prevent nested parallel-for
int m_numThreads;
int m_numWorkerThreads;
int m_numWorkersRunning;
public:
btTaskSchedulerDefault() : btITaskScheduler("ThreadSupport")
{
m_threadSupport = NULL;
m_numThreads = getNumHardwareThreads();
// if can't detect number of cores,
if ( m_numThreads == 0 )
{
// take a guess
m_numThreads = 4;
}
m_numWorkerThreads = m_numThreads - 1;
m_numWorkersRunning = 0;
}
virtual ~btTaskSchedulerDefault()
{
shutdown();
}
void init()
{
int maxNumWorkerThreads = BT_MAX_THREAD_COUNT - 1;
m_threadSupport = createThreadSupport( maxNumWorkerThreads, WorkerThreadFunc, WorkerThreadAllocFunc, "TaskScheduler" );
m_jobContext.m_queueLock = m_threadSupport->createCriticalSection();
for ( int i = 0; i < maxNumWorkerThreads; i++ )
{
WorkerThreadLocalStorage* storage = (WorkerThreadLocalStorage*) m_threadSupport->getThreadLocalMemory( i );
btAssert( storage );
storage->threadId = i;
storage->status = WorkerThreadStatus::kSleeping;
}
setWorkersActive( false ); // no work for them yet
}
virtual void shutdown()
{
setWorkersActive( false );
waitForWorkersToSleep();
m_threadSupport->deleteCriticalSection( m_jobContext.m_queueLock );
m_jobContext.m_queueLock = NULL;
delete m_threadSupport;
m_threadSupport = NULL;
}
void setWorkersActive( bool active )
{
m_jobContext.m_workersShouldCheckQueue = active;
}
virtual int getMaxNumThreads() const BT_OVERRIDE
{
return BT_MAX_THREAD_COUNT;
}
virtual int getNumThreads() const BT_OVERRIDE
{
return m_numThreads;
}
virtual void setNumThreads( int numThreads ) BT_OVERRIDE
{
m_numThreads = btMax( btMin(numThreads, int(BT_MAX_THREAD_COUNT)), 1 );
m_numWorkerThreads = m_numThreads - 1;
}
void waitJobs()
{
BT_PROFILE( "waitJobs" );
// have the main thread work until the job queue is empty
for ( ;; )
{
if ( IJob* job = m_jobContext.consumeJob() )
{
job->executeJob();
}
else
{
break;
}
}
// done with jobs for now, tell workers to rest
setWorkersActive( false );
waitForWorkersToSleep();
}
void wakeWorkers()
{
BT_PROFILE( "wakeWorkers" );
btAssert( m_jobContext.m_workersShouldCheckQueue );
// tell each worker thread to start working
for ( int i = 0; i < m_numWorkerThreads; i++ )
{
m_threadSupport->runTask( B3_THREAD_SCHEDULE_TASK, &m_jobContext, i );
m_numWorkersRunning++;
}
}
void waitForWorkersToSleep()
{
BT_PROFILE( "waitForWorkersToSleep" );
while ( m_numWorkersRunning > 0 )
{
int iThread;
int threadStatus;
m_threadSupport->waitForResponse( &iThread, &threadStatus ); // wait for worker threads to finish working
m_numWorkersRunning--;
}
//m_threadSupport->waitForAllTasksToComplete();
for ( int i = 0; i < m_numWorkerThreads; i++ )
{
//m_threadSupport->waitForTaskCompleted( i );
WorkerThreadLocalStorage* storage = (WorkerThreadLocalStorage*) m_threadSupport->getThreadLocalMemory( i );
btAssert( storage );
btAssert( storage->status == WorkerThreadStatus::kSleeping );
}
}
virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE
{
BT_PROFILE( "parallelFor_ThreadSupport" );
btAssert( iEnd >= iBegin );
btAssert( grainSize >= 1 );
int iterationCount = iEnd - iBegin;
if ( iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock() )
{
int jobCount = ( iterationCount + grainSize - 1 ) / grainSize;
btAssert( jobCount >= 2 ); // need more than one job for multithreading
if ( jobCount > m_jobs.size() )
{
m_jobs.resize( jobCount );
}
if ( jobCount > m_jobContext.m_jobQueue.capacity() )
{
m_jobContext.m_jobQueue.reserve( jobCount );
}
m_jobContext.clearQueue();
// prepare worker threads for incoming work
setWorkersActive( true );
wakeWorkers();
// submit all of the jobs
int iJob = 0;
for ( int i = iBegin; i < iEnd; i += grainSize )
{
btAssert( iJob < jobCount );
int iE = btMin( i + grainSize, iEnd );
ParallelForJob& job = m_jobs[ iJob ];
job.init( i, iE, body );
m_jobContext.submitJob( &job );
iJob++;
}
// put the main thread to work on emptying the job queue and then wait for all workers to finish
waitJobs();
m_antiNestingLock.unlock();
}
else
{
BT_PROFILE( "parallelFor_mainThread" );
// just run on main thread
body.forLoop( iBegin, iEnd );
}
}
};
btITaskScheduler* createDefaultTaskScheduler()
{
btTaskSchedulerDefault* ts = new btTaskSchedulerDefault();
ts->init();
return ts;
}
#else // #if BT_THREADSAFE
btITaskScheduler* createDefaultTaskScheduler()
{
return NULL;
}
#endif // #else // #if BT_THREADSAFE

26
examples/MultiThreading/btTaskScheduler.h Normal file
View File

@@ -0,0 +1,26 @@
/*
Copyright (c) 2003-2014 Erwin Coumans http://bullet.googlecode.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TASK_SCHEDULER_H
#define BT_TASK_SCHEDULER_H
class btITaskScheduler;
btITaskScheduler* createDefaultTaskScheduler();
#endif // BT_TASK_SCHEDULER_H

162
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -129,6 +129,8 @@ struct b3GraphicsInstance
int m_instanceOffset;
int m_vertexArrayOffset;
int m_primitiveType;
float m_materialShinyNess;
b3Vector3 m_materialSpecularColor;
b3GraphicsInstance()
:m_cube_vao(-1),
@@ -139,7 +141,9 @@ struct b3GraphicsInstance
m_numGraphicsInstances(0),
m_instanceOffset(0),
m_vertexArrayOffset(0),
m_primitiveType(B3_GL_TRIANGLES)
m_primitiveType(B3_GL_TRIANGLES),
m_materialShinyNess(41),
m_materialSpecularColor(b3MakeVector3(.5,.5,.5))
{
}
@@ -204,8 +208,10 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
GLfloat m_projectionMatrix[16];
GLfloat m_viewMatrix[16];
GLfloat m_viewMatrixInverse[16];
b3Vector3 m_lightPos;
b3Vector3 m_lightSpecularIntensity;
GLuint m_defaultTexturehandle;
b3AlignedObjectArray<InternalTextureHandle> m_textureHandles;
@@ -225,13 +231,15 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
m_shadowTexture(0),
m_renderFrameBuffer(0)
{
m_lightPos=b3MakeVector3(-50,50,50);
m_lightPos=b3MakeVector3(-50,30,40);
m_lightSpecularIntensity.setValue(1,1,1);
//clear to zero to make it obvious if the matrix is used uninitialized
for (int i=0;i<16;i++)
{
m_projectionMatrix[i]=0;
m_viewMatrix[i]=0;
m_viewMatrixInverse[i]=0;
}
}
@@ -280,9 +288,14 @@ GLuint linesVertexArrayObject=0;
GLuint linesIndexVbo = 0;
static GLint useShadow_ViewMatrixInverse=0;
static GLint useShadow_ModelViewMatrix=0;
static GLint useShadow_lightSpecularIntensity = 0;
static GLint useShadow_materialSpecularColor = 0;
static GLint useShadow_MVP=0;
static GLint useShadow_lightDirIn=0;
static GLint useShadow_lightPosIn=0;
static GLint useShadow_cameraPositionIn = 0;
static GLint useShadow_materialShininessIn = 0;
static GLint useShadow_ProjectionMatrix=0;
static GLint useShadow_DepthBiasModelViewMatrix=0;
@@ -296,7 +309,7 @@ static GLint ProjectionMatrix=0;
static GLint regularLightDirIn=0;
static GLint uniform_texture_diffuse = 0;
static GLint uniform_texture_diffuse = 0;
static GLint screenWidthPointSprite=0;
static GLint ModelViewMatrixPointSprite=0;
@@ -400,11 +413,15 @@ bool GLInstancingRenderer::readSingleInstanceTransformToCPU(float* position, flo
return false;
}
void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
if (pg==0)
return;
int srcIndex = pg->m_internalInstanceIndex;
b3Assert(srcIndex<m_data->m_totalNumInstances);
@@ -422,9 +439,9 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
}
void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId, float* position, float* orientation)
void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex2, float* position, float* orientation)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@@ -440,9 +457,9 @@ void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId,
orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@@ -452,9 +469,9 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, in
m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]);
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@@ -464,9 +481,9 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int
m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@@ -475,9 +492,63 @@ void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int
m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const double* scale, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int graphicsIndex = pg->m_internalInstanceIndex;
int totalNumInstances = 0;
int gfxObjIndex = -1;
for (int i=0;i<m_graphicsInstances.size();i++)
{
totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances;
if (srcIndex2<totalNumInstances)
{
gfxObjIndex = i;
break;
}
}
if (gfxObjIndex>0)
{
m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[0] = specular[0];
m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[1] = specular[1];
m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[2] = specular[2];
}
}
void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
int totalNumInstances = 0;
int gfxObjIndex = -1;
for (int i=0;i<m_graphicsInstances.size();i++)
{
totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances;
if (srcIndex2<totalNumInstances)
{
gfxObjIndex = i;
break;
}
}
if (gfxObjIndex>0)
{
m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[0] = specular[0];
m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[1] = specular[1];
m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[2] = specular[2];
}
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const double* scale, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@@ -694,8 +765,8 @@ void GLInstancingRenderer::rebuildGraphicsInstances()
for (int i=0;i<usedObjects.size();i++)
{
int bodyUniqueId = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
int srcIndex2 = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@@ -722,9 +793,9 @@ void GLInstancingRenderer::rebuildGraphicsInstances()
}
for (int i=0;i<usedObjects.size();i++)
{
int bodyUniqueId = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(bodyUniqueId);
int srcIndex2 = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(srcIndex2);
}
int curOffset = 0;
@@ -1099,13 +1170,18 @@ void GLInstancingRenderer::InitShaders()
glLinkProgram(useShadowMapInstancingShader);
glUseProgram(useShadowMapInstancingShader);
useShadow_ViewMatrixInverse = glGetUniformLocation(useShadowMapInstancingShader, "ViewMatrixInverse");
useShadow_ModelViewMatrix = glGetUniformLocation(useShadowMapInstancingShader, "ModelViewMatrix");
useShadow_lightSpecularIntensity = glGetUniformLocation(useShadowMapInstancingShader, "lightSpecularIntensityIn");
useShadow_materialSpecularColor = glGetUniformLocation(useShadowMapInstancingShader, "materialSpecularColorIn");
useShadow_MVP = glGetUniformLocation(useShadowMapInstancingShader, "MVP");
useShadow_ProjectionMatrix = glGetUniformLocation(useShadowMapInstancingShader, "ProjectionMatrix");
useShadow_DepthBiasModelViewMatrix = glGetUniformLocation(useShadowMapInstancingShader, "DepthBiasModelViewProjectionMatrix");
useShadow_uniform_texture_diffuse = glGetUniformLocation(useShadowMapInstancingShader, "Diffuse");
useShadow_shadowMap = glGetUniformLocation(useShadowMapInstancingShader,"shadowMap");
useShadow_lightDirIn = glGetUniformLocation(useShadowMapInstancingShader,"lightDirIn");
useShadow_lightPosIn = glGetUniformLocation(useShadowMapInstancingShader,"lightPosIn");
useShadow_cameraPositionIn = glGetUniformLocation(useShadowMapInstancingShader,"cameraPositionIn");
useShadow_materialShininessIn = glGetUniformLocation(useShadowMapInstancingShader,"materialShininessIn");
createShadowMapInstancingShader = gltLoadShaderPair(createShadowMapInstancingVertexShader,createShadowMapInstancingFragmentShader);
glLinkProgram(createShadowMapInstancingShader);
@@ -1272,6 +1348,15 @@ void GLInstancingRenderer::setActiveCamera(CommonCameraInterface* cam)
m_data->m_activeCamera = cam;
}
void GLInstancingRenderer::setLightSpecularIntensity(const float lightSpecularIntensity[3])
{
m_data->m_lightSpecularIntensity[0] = lightSpecularIntensity[0];
m_data->m_lightSpecularIntensity[1] = lightSpecularIntensity[1];
m_data->m_lightSpecularIntensity[2] = lightSpecularIntensity[2];
}
void GLInstancingRenderer::setLightPosition(const float lightPos[3])
{
m_data->m_lightPos[0] = lightPos[0];
@@ -1298,6 +1383,21 @@ void GLInstancingRenderer::updateCamera(int upAxis)
m_data->m_defaultCamera1.update();
m_data->m_activeCamera->getCameraProjectionMatrix(m_data->m_projectionMatrix);
m_data->m_activeCamera->getCameraViewMatrix(m_data->m_viewMatrix);
b3Scalar viewMat[16];
b3Scalar viewMatInverse[16];
for (int i=0;i<16;i++)
{
viewMat[i] = m_data->m_viewMatrix[i];
}
b3Transform tr;
tr.setFromOpenGLMatrix(viewMat);
tr = tr.inverse();
tr.getOpenGLMatrix(viewMatInverse);
for (int i=0;i<16;i++)
{
m_data->m_viewMatrixInverse[i]=viewMatInverse[i];
}
}
@@ -2127,12 +2227,24 @@ b3Assert(glGetError() ==GL_NO_ERROR);
glUseProgram(useShadowMapInstancingShader);
glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]);
glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]);
glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]);
float MVP[16];
b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]);
b3Vector3 gLightDir = m_data->m_lightPos;
gLightDir.normalize();
glUniform3f(useShadow_lightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]);
//gLightDir.normalize();
glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]);
float camPos[3];
m_data->m_activeCamera->getCameraPosition(camPos);
glUniform3f(useShadow_cameraPositionIn,camPos[0],camPos[1],camPos[2]);
glUniform1f(useShadow_materialShininessIn,gfxObj->m_materialShinyNess);
glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]);
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture);

5
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@@ -95,6 +95,7 @@ public:
}
virtual void readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation);
virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
@@ -102,6 +103,9 @@ public:
virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex);
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex2);
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex2);
virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex);
@@ -124,6 +128,7 @@ public:
virtual void setLightPosition(const float lightPos[3]);
virtual void setLightPosition(const double lightPos[3]);
void setLightSpecularIntensity(const float lightSpecularIntensity[3]);
virtual void resize(int width, int height);
virtual int getScreenWidth()

90
examples/OpenGLWindow/GLPrimitiveRenderer.cpp
View File

@@ -1,10 +1,10 @@
#ifndef NO_OPENGL3
#include "GLPrimitiveRenderer.h"
#include "GLPrimInternalData.h"
//#include "Bullet3Common/b3Logging.h"
#include "Bullet3Common/b3Scalar.h"
#include "LoadShader.h"
#include <assert.h>
static const char* vertexShader3D= \
"#version 150 \n"
@@ -75,27 +75,27 @@ m_screenHeight(screenHeight)
m_data->m_viewmatUniform = glGetUniformLocation(m_data->m_shaderProg,"viewMatrix");
if (m_data->m_viewmatUniform < 0) {
assert(0);
b3Assert(0);
}
m_data->m_projMatUniform = glGetUniformLocation(m_data->m_shaderProg,"projMatrix");
if (m_data->m_projMatUniform < 0) {
assert(0);
b3Assert(0);
}
m_data->m_positionUniform = glGetUniformLocation(m_data->m_shaderProg, "p");
if (m_data->m_positionUniform < 0) {
assert(0);
b3Assert(0);
}
m_data->m_colourAttribute = glGetAttribLocation(m_data->m_shaderProg, "colour");
if (m_data->m_colourAttribute < 0) {
assert(0);
b3Assert(0);
}
m_data->m_positionAttribute = glGetAttribLocation(m_data->m_shaderProg, "position");
if (m_data->m_positionAttribute < 0) {
assert(0);
b3Assert(0);
}
m_data->m_textureAttribute = glGetAttribLocation(m_data->m_shaderProg,"texuv");
if (m_data->m_textureAttribute < 0) {
assert(0);
b3Assert(0);
}
loadBufferData();
@@ -127,7 +127,7 @@ void GLPrimitiveRenderer::loadBufferData()
glBufferData(GL_ARRAY_BUFFER, MAX_VERTICES2 * sizeof(PrimVertex), 0, GL_DYNAMIC_DRAW);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
@@ -153,14 +153,14 @@ void GLPrimitiveRenderer::loadBufferData()
glEnableVertexAttribArray(m_data->m_positionAttribute);
glEnableVertexAttribArray(m_data->m_colourAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_textureAttribute);
glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
@@ -200,7 +200,7 @@ void GLPrimitiveRenderer::loadBufferData()
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 256,256,0,GL_RGB,GL_UNSIGNED_BYTE,image);
glGenerateMipmap(GL_TEXTURE_2D);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
delete[] image;
@@ -226,14 +226,14 @@ void GLPrimitiveRenderer::drawLine()
void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float color[4])
{
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glActiveTexture(GL_TEXTURE0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
drawTexturedRect(x0,y0,x1,y1,color,0,0,1,1);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
}
@@ -242,7 +242,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
{
//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glUseProgram(m_data->m_shaderProg);
@@ -250,7 +250,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, viewMat);
glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, projMat);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
glBindVertexArray(m_data->m_vertexArrayObject);
@@ -277,7 +277,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
if (useRGBA)
@@ -290,47 +290,47 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_positionAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_colourAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_textureAttribute);
glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
//glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
int indexCount = 6;
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindVertexArray(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
//glDisableVertexAttribArray(m_data->m_textureAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glUseProgram(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
}
@@ -349,7 +349,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
}
//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
float identity[16]={1,0,0,0,
0,1,0,0,
0,0,1,0,
@@ -360,7 +360,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, identity);
glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, identity);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2);
glBindVertexArray(m_data->m_vertexArrayObject2);
@@ -388,7 +388,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
if (useRGBA)
@@ -401,47 +401,47 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_positionAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_colourAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glEnableVertexAttribArray(m_data->m_textureAttribute);
glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2);
//glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
int indexCount = (numVertices/4)*6;
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindVertexArray(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
//glDisableVertexAttribArray(m_data->m_textureAttribute);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glUseProgram(0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
}
@@ -477,7 +477,7 @@ void GLPrimitiveRenderer::flushBatchedRects()
return;
glActiveTexture(GL_TEXTURE0);
assert(glGetError()==GL_NO_ERROR);
b3Assert(glGetError()==GL_NO_ERROR);
glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
drawTexturedRect3D2(m_data2->m_verticesRect, m_data2->m_numVerticesRect,0);
m_data2->m_numVerticesRect=0;

33
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl
View File

@@ -13,9 +13,14 @@ in Vert
uniform sampler2D Diffuse;
uniform sampler2DShadow shadowMap;
uniform mat4 ViewMatrixInverse;
in vec3 lightDir,normal,ambient;
in vec3 lightPos,cameraPosition, normal,ambient;
in vec4 ShadowCoord;
in vec4 vertexPos;
in float materialShininess;
in vec3 lightSpecularIntensity;
in vec3 materialSpecularColor;
out vec4 color;
@@ -28,8 +33,11 @@ void main(void)
float intensity,at,af;
if (fragment.color.w==0)
discard;
intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
vec3 lightDir = normalize(lightPos);
vec3 normalDir = normalize(normal);
intensity = 0.5+0.5*clamp( dot( normalDir,lightDir ), -1,1 );
af = 1.0;
@@ -38,7 +46,24 @@ void main(void)
//float bias = 0.005f;
vec3 specularReflection;
if (dot(normalDir, lightDir) < 0.0)
{
specularReflection = vec3(0.0, 0.0, 0.0);
}
else // light source on the right side
{
vec3 surfaceToLight = normalize(lightPos - vertexPos.xyz);
vec3 surfaceToCamera = normalize(cameraPosition - vertexPos.xyz);
float specularCoefficient = 0.0;
specularCoefficient = pow(max(0.0, dot(surfaceToCamera, reflect(-surfaceToLight, normalDir))), materialShininess);
specularReflection = specularCoefficient * materialSpecularColor * lightSpecularIntensity;
}
float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));
if (intensity<0.5)
@@ -46,6 +71,6 @@ void main(void)
intensity = 0.7*intensity + 0.3*intensity*visibility;
cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;
cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient+specularReflection*visibility;
color = vec4(ct * cf, fragment.color.w);
}

32
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h
View File

@@ -12,8 +12,13 @@ static const char* useShadowMapInstancingFragmentShader= \
"} vert;\n"
"uniform sampler2D Diffuse;\n"
"uniform sampler2DShadow shadowMap;\n"
"in vec3 lightDir,normal,ambient;\n"
"uniform mat4 ViewMatrixInverse;\n"
"in vec3 lightPos,cameraPosition, normal,ambient;\n"
"in vec4 ShadowCoord;\n"
"in vec4 vertexPos;\n"
"in float materialShininess;\n"
"in vec3 lightSpecularIntensity;\n"
"in vec3 materialSpecularColor;\n"
"out vec4 color;\n"
"void main(void)\n"
"{\n"
@@ -22,7 +27,11 @@ static const char* useShadowMapInstancingFragmentShader= \
" float intensity,at,af;\n"
" if (fragment.color.w==0)\n"
" discard;\n"
" intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
" vec3 lightDir = normalize(lightPos);\n"
" \n"
" vec3 normalDir = normalize(normal);\n"
" \n"
" intensity = 0.5+0.5*clamp( dot( normalDir,lightDir ), -1,1 );\n"
" \n"
" af = 1.0;\n"
" \n"
@@ -31,13 +40,30 @@ static const char* useShadowMapInstancingFragmentShader= \
" \n"
" //float bias = 0.005f;\n"
" \n"
" vec3 specularReflection;\n"
" \n"
" if (dot(normalDir, lightDir) < 0.0) \n"
" {\n"
" specularReflection = vec3(0.0, 0.0, 0.0);\n"
" }\n"
" else // light source on the right side\n"
" {\n"
" vec3 surfaceToLight = normalize(lightPos - vertexPos.xyz);\n"
" vec3 surfaceToCamera = normalize(cameraPosition - vertexPos.xyz);\n"
" \n"
" \n"
" float specularCoefficient = 0.0;\n"
" specularCoefficient = pow(max(0.0, dot(surfaceToCamera, reflect(-surfaceToLight, normalDir))), materialShininess);\n"
" specularReflection = specularCoefficient * materialSpecularColor * lightSpecularIntensity;\n"
" \n"
" }\n"
" \n"
" float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));\n"
" if (intensity<0.5)\n"
" visibility = 0;\n"
" intensity = 0.7*intensity + 0.3*intensity*visibility;\n"
" \n"
" cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"
" cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient+specularReflection*visibility;\n"
" color = vec4(ct * cf, fragment.color.w);\n"
"}\n"
;

31
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl
View File

@@ -15,7 +15,12 @@ uniform mat4 ModelViewMatrix;
uniform mat4 ProjectionMatrix;
uniform mat4 DepthBiasModelViewProjectionMatrix;
uniform mat4 MVP;
uniform vec3 lightDirIn;
uniform vec3 lightPosIn;
uniform vec3 cameraPositionIn;
uniform mat4 ViewMatrixInverse;
uniform float materialShininessIn;
uniform vec3 lightSpecularIntensityIn;
uniform vec3 materialSpecularColorIn;
out vec4 ShadowCoord;
@@ -60,7 +65,13 @@ vec4 quatRotate ( in vec4 p, in vec4 q )
return quatMul ( temp, vec4 ( -q.x, -q.y, -q.z, q.w ) );
}
out vec3 lightDir,normal,ambient;
out vec3 lightPos,normal,ambient;
out vec4 vertexPos;
out vec3 cameraPosition;
out float materialShininess;
out vec3 lightSpecularIntensity;
out vec3 materialSpecularColor;
void main(void)
{
@@ -69,15 +80,19 @@ void main(void)
vec4 worldNormal = (quatRotate3( vertexnormal,q));
normal = normalize(worldNormal).xyz;
normal = worldNormal.xyz;
lightDir = lightDirIn;
lightPos = lightPosIn;
cameraPosition = cameraPositionIn;
materialShininess = materialShininessIn;
lightSpecularIntensity = lightSpecularIntensityIn;
materialSpecularColor = materialSpecularColorIn;
vec4 localcoord = quatRotate3( position.xyz*instance_scale,q);
vec4 vertexPos = MVP* vec4((instance_position+localcoord).xyz,1);
gl_Position = vertexPos;
vertexPos = vec4((instance_position+localcoord).xyz,1);
vec4 vertexLoc = MVP* vec4((instance_position+localcoord).xyz,1);
gl_Position = vertexLoc;
ShadowCoord = DepthBiasModelViewProjectionMatrix * vec4((instance_position+localcoord).xyz,1);
fragment.color = instance_color;

29
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h
View File

@@ -13,7 +13,12 @@ static const char* useShadowMapInstancingVertexShader= \
"uniform mat4 ProjectionMatrix;\n"
"uniform mat4 DepthBiasModelViewProjectionMatrix;\n"
"uniform mat4 MVP;\n"
"uniform vec3 lightDirIn;\n"
"uniform vec3 lightPosIn;\n"
"uniform vec3 cameraPositionIn;\n"
"uniform mat4 ViewMatrixInverse;\n"
"uniform float materialShininessIn;\n"
"uniform vec3 lightSpecularIntensityIn;\n"
"uniform vec3 materialSpecularColorIn;\n"
"out vec4 ShadowCoord;\n"
"out Fragment\n"
"{\n"
@@ -51,7 +56,12 @@ static const char* useShadowMapInstancingVertexShader= \
" vec4 temp = quatMul ( q, p );\n"
" return quatMul ( temp, vec4 ( -q.x, -q.y, -q.z, q.w ) );\n"
"}\n"
"out vec3 lightDir,normal,ambient;\n"
"out vec3 lightPos,normal,ambient;\n"
"out vec4 vertexPos;\n"
"out vec3 cameraPosition;\n"
"out float materialShininess;\n"
"out vec3 lightSpecularIntensity;\n"
"out vec3 materialSpecularColor;\n"
"void main(void)\n"
"{\n"
" vec4 q = instance_quaternion;\n"
@@ -59,13 +69,18 @@ static const char* useShadowMapInstancingVertexShader= \
" \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" \n"
" normal = normalize(worldNormal).xyz;\n"
" lightDir = lightDirIn;\n"
" \n"
" normal = worldNormal.xyz;\n"
" lightPos = lightPosIn;\n"
" cameraPosition = cameraPositionIn;\n"
" materialShininess = materialShininessIn;\n"
" lightSpecularIntensity = lightSpecularIntensityIn;\n"
" materialSpecularColor = materialSpecularColorIn;\n"
" \n"
" vec4 localcoord = quatRotate3( position.xyz*instance_scale,q);\n"
" vec4 vertexPos = MVP* vec4((instance_position+localcoord).xyz,1);\n"
" gl_Position = vertexPos;\n"
" vertexPos = vec4((instance_position+localcoord).xyz,1);\n"
" \n"
" vec4 vertexLoc = MVP* vec4((instance_position+localcoord).xyz,1);\n"
" gl_Position = vertexLoc;\n"
" ShadowCoord = DepthBiasModelViewProjectionMatrix * vec4((instance_position+localcoord).xyz,1);\n"
" fragment.color = instance_color;\n"
" vert.texcoord = uvcoords;\n"

10
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -415,3 +415,13 @@ float SimpleCamera::getCameraFrustumNear() const
{
return m_data->m_frustumZNear;
}
void SimpleCamera::setCameraFrustumFar(float far)
{
m_data->m_frustumZFar = far;
}
void SimpleCamera::setCameraFrustumNear(float near)
{
m_data->m_frustumZNear = near;
}

3
examples/OpenGLWindow/SimpleCamera.h
View File

@@ -54,6 +54,9 @@ struct SimpleCamera : public CommonCameraInterface
virtual float getCameraFrustumFar() const;
virtual float getCameraFrustumNear() const;
virtual void setCameraFrustumFar(float far);
virtual void setCameraFrustumNear(float near);
};
#endif //SIMPLE_CAMERA_H

3
examples/OpenGLWindow/SimpleOpenGL2Renderer.h
View File

@@ -39,6 +39,9 @@ public:
virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex);
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex){}
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex){}
virtual void getCameraViewMatrix(float viewMat[16]) const;
virtual void getCameraProjectionMatrix(float projMat[16]) const;
virtual void drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float color[4], int textureIndex=-1, int vertexLayout=0)

1
examples/RobotSimulator/CMakeLists.txt
View File

@@ -32,6 +32,7 @@ SET(RobotSimulator_SRCS
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h

1
examples/RobotSimulator/premake4.lua
View File

@@ -10,6 +10,7 @@ myfiles =
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",

1
examples/SharedMemory/CMakeLists.txt
View File

@@ -7,6 +7,7 @@ SET(SharedMemory_SRCS
PhysicsClientSharedMemory.cpp
PhysicsClientExample.cpp
PhysicsServerExample.cpp
PhysicsServerExampleBullet2.cpp
PhysicsServerSharedMemory.cpp
PhysicsServerSharedMemory.h
PhysicsServer.cpp

98
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -392,6 +392,18 @@ int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle
}
int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_restitutionVelocityThreshold = restitutionVelocityThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD ;
return 0;
}
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1297,6 +1309,42 @@ int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHa
return 0;
}
int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_spinningFriction = friction;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION;
return 0;
}
int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_rollingFriction = friction;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION;
return 0;
}
int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_restitution = restitution;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION;
return 0;
}
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -2394,6 +2442,21 @@ void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, dou
}
}
void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
{
command->m_updateVisualShapeDataArguments.m_specularColor[0] = specularColor[0];
command->m_updateVisualShapeDataArguments.m_specularColor[1] = specularColor[1];
command->m_updateVisualShapeDataArguments.m_specularColor[2] = specularColor[2];
command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR;
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -3101,3 +3164,38 @@ double b3GetTimeOut(b3PhysicsClientHandle physClient)
}
return -1;
}
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4])
{
b3Transform trA;
b3Transform trB;
trA.setOrigin(b3MakeVector3(posA[0],posA[1],posA[2]));
trA.setRotation(b3Quaternion(ornA[0],ornA[1],ornA[2],ornA[3]));
trB.setOrigin(b3MakeVector3(posB[0],posB[1],posB[2]));
trB.setRotation(b3Quaternion(ornB[0],ornB[1],ornB[2],ornB[3]));
b3Transform res = trA*trB;
outPos[0] = res.getOrigin()[0];
outPos[1] = res.getOrigin()[1];
outPos[2] = res.getOrigin()[2];
b3Quaternion orn = res.getRotation();
outOrn[0] = orn[0];
outOrn[1] = orn[1];
outOrn[2] = orn[2];
outOrn[3] = orn[3];
}
void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4])
{
b3Transform tr;
tr.setOrigin(b3MakeVector3(pos[0],pos[1],pos[2]));
tr.setRotation(b3Quaternion(orn[0],orn[1],orn[2],orn[3]));
b3Transform trInv = tr.inverse();
outPos[0] = trInv.getOrigin()[0];
outPos[1] = trInv.getOrigin()[1];
outPos[2] = trInv.getOrigin()[2];
b3Quaternion invOrn = trInv.getRotation();
outOrn[0] = invOrn[0];
outOrn[1] = invOrn[1];
outOrn[2] = invOrn[2];
outOrn[3] = invOrn[3];
}

14
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -84,7 +84,12 @@ int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3Dynamics
b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
@@ -206,6 +211,8 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu
b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]);
void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
@@ -223,6 +230,9 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl
int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms);
int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching);
int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
//b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
//Use at own risk: magic things may or my not happen when calling this API
@@ -409,6 +419,8 @@ void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle comma
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
double b3GetTimeOut(b3PhysicsClientHandle physClient);
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]);
void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]);
#ifdef __cplusplus
}

20
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -5,7 +5,7 @@
#include "../CommonInterfaces/Common2dCanvasInterface.h"
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsServerCommandProcessor.h"
#include "PhysicsClientC_API.h"
#include "PhysicsClient.h"
//#include "SharedMemoryCommands.h"
@@ -37,6 +37,7 @@ class PhysicsClientExample : public SharedMemoryCommon
{
protected:
b3PhysicsClientHandle m_physicsClientHandle;
//this m_physicsServer is only used when option eCLIENTEXAMPLE_SERVER is enabled
PhysicsServerSharedMemory m_physicsServer;
@@ -532,10 +533,26 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
struct Bullet2CommandProcessorCreation3 : public CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()
{
PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
return proc;
}
virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
{
delete proc;
}
};
static Bullet2CommandProcessorCreation3 sB2PC2;
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper, int options)
:SharedMemoryCommon(helper),
m_physicsClientHandle(0),
m_physicsServer(&sB2PC2,0,0),
m_wantsTermination(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_selectedBody(-1),
@@ -577,6 +594,7 @@ PhysicsClientExample::~PhysicsClientExample()
m_canvas->destroyCanvas(m_canvasSegMaskIndex);
}
b3Printf("~PhysicsClientExample\n");
}

32
examples/SharedMemory/PhysicsCommandProcessorInterface.h
View File

@@ -1,7 +1,7 @@
#ifndef PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
#define PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
enum PhysicsCOmmandRenderFlags
enum PhysicsCommandRenderFlags
{
COV_DISABLE_SYNC_RENDERING=1
};
@@ -29,4 +29,34 @@ public:
};
class btVector3;
class btQuaternion;
class CommandProcessorInterface : public PhysicsCommandProcessorInterface
{
public:
virtual ~CommandProcessorInterface(){}
virtual void syncPhysicsToGraphics()=0;
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)=0;
virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0;
virtual bool isRealTimeSimulationEnabled() const=0;
virtual void enableCommandLogging(bool enable, const char* fileName)=0;
virtual void replayFromLogFile(const char* fileName)=0;
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes )=0;
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual void removePickingConstraint()=0;
virtual const btVector3& getVRTeleportPosition() const=0;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos)=0;
virtual const btQuaternion& getVRTeleportOrientation() const=0;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn)=0;
};
#endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H

26
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -2,25 +2,44 @@
#include "PhysicsServerSharedMemory.h"
#include "PhysicsClientSharedMemory.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "PhysicsServerCommandProcessor.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
struct PhysicsLoopBackInternalData
{
CommandProcessorInterface* m_commandProcessor;
PhysicsClientSharedMemory* m_physicsClient;
PhysicsServerSharedMemory* m_physicsServer;
DummyGUIHelper m_noGfx;
PhysicsLoopBackInternalData()
:m_physicsClient(0),
:m_commandProcessor(0),
m_physicsClient(0),
m_physicsServer(0)
{
}
};
struct Bullet2CommandProcessorCreation2 : public CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()
{
PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
return proc;
}
virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
{
delete proc;
}
};
static Bullet2CommandProcessorCreation2 sB2Proc;
PhysicsLoopBack::PhysicsLoopBack()
{
m_data = new PhysicsLoopBackInternalData;
m_data->m_physicsServer = new PhysicsServerSharedMemory();
m_data->m_physicsServer = new PhysicsServerSharedMemory(&sB2Proc, 0,0);
m_data->m_physicsClient = new PhysicsClientSharedMemory();
}
@@ -28,6 +47,7 @@ PhysicsLoopBack::~PhysicsLoopBack()
{
delete m_data->m_physicsClient;
delete m_data->m_physicsServer;
delete m_data->m_commandProcessor;
delete m_data;
}

14
examples/SharedMemory/PhysicsServer.h
View File

@@ -25,18 +25,18 @@ public:
//@todo(erwincoumans) Should we have shared memory commands for picking objects?
///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual void removePickingConstraint()=0;
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
virtual void removePickingConstraint(){}
//for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory
virtual void physicsDebugDraw(int debugDrawFlags)=0;
virtual void renderScene(int renderFlags)=0;
virtual void physicsDebugDraw(int debugDrawFlags){}
virtual void renderScene(int renderFlags){}
virtual void enableCommandLogging(bool enable, const char* fileName)=0;
virtual void replayFromLogFile(const char* fileName)=0;
virtual void enableCommandLogging(bool enable, const char* fileName){}
virtual void replayFromLogFile(const char* fileName){}
};

796
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -49,9 +49,8 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
btVector3 gLastPickPos(0, 0, 0);
bool gCloseToKuka=false;
bool gEnableRealTimeSimVR=false;
bool gCreateDefaultRobotAssets = false;
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
int gVRTrackingObjectUniqueId = -1;
@@ -59,9 +58,9 @@ int gVRTrackingObjectFlag = VR_CAMERA_TRACK_OBJECT_ORIENTATION;
btTransform gVRTrackingObjectTr = btTransform::getIdentity();
int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads
int gCreateObjectSimVR = -1;
int gEnableKukaControl = 0;
btVector3 gVRTeleportPos1(0,0,0);
btQuaternion gVRTeleportOrn(0, 0, 0,1);
@@ -1137,25 +1136,13 @@ struct PhysicsServerCommandProcessorInternalData
bool m_allowRealTimeSimulation;
bool m_hasGround;
b3VRControllerEvents m_vrControllerEvents;
btAlignedObjectArray<b3KeyboardEvent> m_keyboardEvents;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
btMultiBodyFixedConstraint* m_kukaGripperFixed;
btMultiBody* m_kukaGripperMultiBody;
btMultiBodyPoint2Point* m_kukaGripperRevolute1;
btMultiBodyPoint2Point* m_kukaGripperRevolute2;
int m_huskyId;
int m_KukaId;
int m_sphereId;
int m_gripperId;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -1231,17 +1218,6 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData()
:
m_allowRealTimeSimulation(false),
m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_kukaGripperFixed(0),
m_kukaGripperMultiBody(0),
m_kukaGripperRevolute1(0),
m_kukaGripperRevolute2(0),
m_huskyId(-1),
m_KukaId(-1),
m_sphereId(-1),
m_gripperId(-1),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
@@ -3915,52 +3891,124 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO");
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
btAssert(bodyUniqueId >= 0);
btAssert(linkIndex >= -1);
int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction;
double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
btAssert(bodyUniqueId >= 0);
btAssert(linkIndex >= -1);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
if (linkIndex == -1)
{
btMultiBody* mb = body->m_multiBody;
if (linkIndex == -1)
if (mb->getBaseCollider())
{
mb->setBaseMass(mass);
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{
mb->getBaseCollider()->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
mb->getBaseCollider()->setFriction(lateralFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
{
mb->getBaseCollider()->setSpinningFriction(spinningFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{
mb->getBaseCollider()->setRollingFriction(rollingFriction);
}
}
else
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
mb->getLink(linkIndex).m_mass = mass;
mb->setBaseMass(mass);
if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape())
{
btVector3 localInertia;
mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass,localInertia);
mb->setBaseInertia(localInertia);
}
}
}
else
{
if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
{
if (mb->getLinkCollider(linkIndex))
{
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{
mb->getLinkCollider(linkIndex)->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
{
mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{
mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
mb->getLink(linkIndex).m_mass = mass;
if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape())
{
btVector3 localInertia;
mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia);
mb->getLink(linkIndex).m_inertiaLocal = localInertia;
}
}
}
}
} else
{
if (body && body->m_rigidBody)
{
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{
body->m_rigidBody->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
body->m_rigidBody->setFriction(lateralFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
{
body->m_rigidBody->setSpinningFriction(spinningFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{
body->m_rigidBody->setRollingFriction(rollingFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
btVector3 localInertia;
if (body->m_rigidBody->getCollisionShape())
{
body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass,localInertia);
}
body->m_rigidBody->setMassProps(mass,localInertia);
}
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
btAssert(bodyUniqueId >= 0);
btAssert(linkIndex >= -1);
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
if (linkIndex == -1)
{
mb->getBaseCollider()->setFriction(lateralFriction);
}
else
{
mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
@@ -4017,7 +4065,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS)
{
//these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
gCreateDefaultRobotAssets = (clientCmd.m_physSimParamArgs.m_internalSimFlags & SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS);
gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags;
}
@@ -4041,11 +4088,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
}
if (clientCmd.m_updateFlags&SIM_PARAM_MAX_CMD_PER_1MS)
{
gMaxNumCmdPer1ms = clientCmd.m_physSimParamArgs.m_maxNumCmdPer1ms;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{
m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
@@ -4070,6 +4113,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
{
m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
}
if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING)
{
@@ -5621,7 +5670,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
{
int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId;
int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex;
@@ -5637,7 +5685,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
{
m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
}
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR)
{
m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor);
}
}
} else
{
@@ -5647,7 +5703,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex();
m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
{
m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
}
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR)
{
m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor);
}
}
}
}
@@ -6210,7 +6274,7 @@ btQuaternion gVRController2Orn(0,0,0,1);
btScalar gVRGripper2Analog = 0;
btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false;
int gDroppedSimulationSteps = 0;
@@ -6223,6 +6287,11 @@ void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTime
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
}
bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
{
return m_data->m_allowRealTimeSimulation;
}
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents,const struct b3KeyboardEvent* keyEvents, int numKeyEvents)
{
m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents);
@@ -6276,23 +6345,22 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const
if (gVRTrackingObjectUniqueId>=0)
{
gVRTrackingObjectTr.setOrigin(bodyHandle->m_multiBody->getBaseWorldTransform().getOrigin());
gVRTeleportPos1 = gVRTrackingObjectTr.getOrigin();
}
if (gVRTrackingObjectFlag&VR_CAMERA_TRACK_OBJECT_ORIENTATION)
{
gVRTrackingObjectTr.setBasis(bodyHandle->m_multiBody->getBaseWorldTransform().getBasis());
gVRTeleportOrn = gVRTrackingObjectTr.getRotation();
}
}
}
if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
if (gCreateDefaultRobotAssets)
{
createDefaultRobotAssets();
}
int maxSteps = m_data->m_numSimulationSubSteps+3;
if (m_data->m_numSimulationSubSteps)
{
@@ -6341,553 +6409,29 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_bodyHandles.exitHandles();
m_data->m_bodyHandles.initHandles();
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
}
//todo: move this to Python/scripting (it is almost ready to be removed!)
void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int bodyId = 0;
if (gCreateObjectSimVR >= 0)
{
gCreateObjectSimVR = -1;
btMatrix3x3 mat(gVRGripperOrn);
btScalar spawnDistance = 0.1;
btVector3 spawnDir = mat.getColumn(0);
btVector3 shiftPos = spawnDir*spawnDistance;
btVector3 spawnPos = gVRGripperPos + shiftPos;
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size(),0);
//loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_sphereId = bodyId;
InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setBaseVel(spawnDir * 5);
}
}
if (!m_data->m_hasGround)
{
m_data->m_hasGround = true;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// loadUrdf("quadruped/quadruped.urdf", btVector3(2, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
if (m_data->m_gripperRigidbodyFixed == 0)
{
int bodyId = 0;
if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(0);
btVector3 pivotInParent(0.2, 0, 0);
btMatrix3x3 frameInParent;
//frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
frameInParent.setIdentity();
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
m_data->m_gripperMultiBody->setJointPos(0, 0);
m_data->m_gripperMultiBody->setJointPos(2, 0);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(500);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
}
loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
if (m_data->m_KukaId>=0)
{
InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
{
btScalar q[7];
q[0] = 0;// -SIMD_HALF_PI;
q[1] = 0;
q[2] = 0;
q[3] = SIMD_HALF_PI;
q[4] = 0;
q[5] = -SIMD_HALF_PI*0.66;
q[6] = 0;
for (int i = 0; i < 7; i++)
{
kukaBody->m_multiBody->setJointPos(i, q[i]);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
int nLinks = kukaBody->m_multiBody->getNumLinks();
scratch_q.resize(nLinks + 1);
scratch_m.resize(nLinks + 1);
kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
}
}
#if 1
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
// loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#if 1
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true,CUF_USE_SDF);
m_data->m_gripperId = bodyId + 1;
{
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++)
{
if (supportsJointMotor(gripperBody->m_multiBody, i))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)gripperBody->m_multiBody->getLink(i).m_userPtr;
if (motor)
{
motor->setMaxAppliedImpulse(0);
}
}
}
}
#endif
#if 1
for (int i = 0; i < 6; i++)
{
loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
#endif
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Add slider joint for fingers
btVector3 pivotInParent1(-0.055, 0, 0.02);
btVector3 pivotInChild1(0, 0, 0);
btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0));
btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0));
btVector3 jointAxis1(1.0, 0, 0);
btVector3 pivotInParent2(0.055, 0, 0.02);
btVector3 pivotInChild2(0, 0, 0);
btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0));
btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
btVector3 jointAxis2(1.0, 0, 0);
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
}
if (m_data->m_KukaId>=0)
{
InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
gripperBody->m_multiBody->setHasSelfCollision(0);
btVector3 pivotInParent(0, 0, 0.05);
btMatrix3x3 frameInParent;
frameInParent.setIdentity();
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(kukaBody->m_multiBody, 6, gripperBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody;
m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
}
}
}
#if 0
for (int i = 0; i < 10; i++)
{
loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
btTransform objectLocalTr[] = {
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8))
};
btAlignedObjectArray<btTransform> objectWorldTr;
int numOb = sizeof(objectLocalTr) / sizeof(btTransform);
objectWorldTr.resize(numOb);
btTransform tr;
tr.setIdentity();
tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI));
tr.setOrigin(btVector3(1.0, -0.2, 0));
for (int i = 0; i < numOb; i++)
{
objectWorldTr[i] = tr*objectLocalTr[i];
}
// Table area
loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Shelf area
loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true, CUF_USE_SDF);
loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Chess area
loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
}
if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody)
{
InteralBodyData* childBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
// Add gripper controller
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr;
if (motor)
{
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
motor->setPositionTarget(posTarget, .8);
motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(1.0);
}
}
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
btScalar avg = 0.f;
for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr;
if (motor)
{
motor->setErp(0.2);
btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
btScalar maxPosTarget = 0.55;
btScalar correction = 0.f;
if (avg)
{
correction = m_data->m_gripperMultiBody->getJointPos(i) - avg;
}
if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
{
m_data->m_gripperMultiBody->setJointPos(i,0);
}
if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget)
{
m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
}
if (avg)
{
motor->setPositionTarget(avg, 1);
}
else
{
motor->setPositionTarget(posTarget, 1);
}
motor->setVelocityTarget(0, 0.5);
btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
avg = m_data->m_gripperMultiBody->getJointPos(i);
//motor->setRhsClamp(gRhsClamp);
}
}
}
}
// Inverse kinematics for KUKA
if (m_data->m_KukaId>=0)
{
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)
{
btMultiBody* mb = bodyHandle->m_multiBody;
btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2();
btScalar distanceThreshold = 1.3;
gCloseToKuka=(sqLen<(distanceThreshold*distanceThreshold));
int numDofs = bodyHandle->m_multiBody->getNumDofs();
btAlignedObjectArray<double> q_new;
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
}
q_new.resize(numDofs);
//sensible rest-pose
q_new[0] = 0;// -SIMD_HALF_PI;
q_new[1] = 0;
q_new[2] = 0;
q_new[3] = SIMD_HALF_PI;
q_new[4] = 0;
q_new[5] = -SIMD_HALF_PI*0.66;
q_new[6] = 0;
if (gCloseToKuka && gEnableKukaControl)
{
double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0};
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;
if (ikHelperPtrPtr)
{
ikHelperPtr = *ikHelperPtrPtr;
}
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
}
int endEffectorLinkIndex = 6;
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
jacSize = jacobian_linear.size();
// Set jacobian value
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
qdot[i + baseDofs] = 0;
nu[i+baseDofs] = 0;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3,numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[i*numDofs+j] = jac_t(i,j);
jacobian_angular[i*numDofs+j] = jac_r(i,j);
}
}
}
}
int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;//IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3DoubleData targetWorldPosition;
btVector3DoubleData targetWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
// Prescribed position and orientation
static btScalar time=0.f;
time+=0.01;
btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time));
targetPos +=mb->getBasePos();
btVector4 downOrn(0,1,0,0);
// Controller orientation
btVector4 controllerOrn(gVRController2Orn.x(), gVRController2Orn.y(), gVRController2Orn.z(), gVRController2Orn.w());
// Set position and orientation
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
downOrn.serializeDouble(targetWorldOrientation);
//targetPos.serializeDouble(targetWorldPosition);
gVRController2Pos.serializeDouble(targetWorldPosition);
//controllerOrn.serializeDouble(targetWorldOrientation);
if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
rest_pose.resize(numDofs);
lower_limit[0] = -.967;
lower_limit[1] = -2.0;
lower_limit[2] = -2.96;
lower_limit[3] = 0.19;
lower_limit[4] = -2.96;
lower_limit[5] = -2.09;
lower_limit[6] = -3.05;
upper_limit[0] = .96;
upper_limit[1] = 2.0;
upper_limit[2] = 2.96;
upper_limit[3] = 2.29;
upper_limit[4] = 2.96;
upper_limit[5] = 2.09;
upper_limit[6] = 3.05;
joint_range[0] = 5.8;
joint_range[1] = 4;
joint_range[2] = 5.8;
joint_range[3] = 4;
joint_range[4] = 5.8;
joint_range[5] = 4;
joint_range[6] = 6;
rest_pose[0] = 0;
rest_pose[1] = 0;
rest_pose[2] = 0;
rest_pose[3] = SIMD_HALF_PI;
rest_pose[4] = 0;
rest_pose[5] = -SIMD_HALF_PI*0.66;
rest_pose[6] = 0;
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk);
}
}
//directly set the position of the links, only for debugging IK, don't use this method!
#if 0
if (0)
{
for (int i=0;i<mb->getNumLinks();i++)
{
btScalar desiredPosition = q_new[i];
mb->setJointPosMultiDof(i,&desiredPosition);
}
} else
#endif
{
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for (int link=0;link<mb->getNumLinks();link++)
{
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
btScalar desiredVelocity = 0.f;
btScalar desiredPosition = q_new[link];
motor->setRhsClamp(gRhsClamp);
//printf("link %d: %f", link, q_new[link]);
motor->setVelocityTarget(desiredVelocity,1.0);
motor->setPositionTarget(desiredPosition,0.6);
btScalar maxImp = 1.0;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
}
velIndex += mb->getLink(link).m_dofCount;
posIndex += mb->getLink(link).m_posVarCount;
}
}
}
}
}
}
void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/)
{
}
const btVector3& PhysicsServerCommandProcessor::getVRTeleportPosition() const
{
return gVRTeleportPos1;
}
void PhysicsServerCommandProcessor::setVRTeleportPosition(const btVector3& vrTeleportPos)
{
gVRTeleportPos1 = vrTeleportPos;
}
const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() const
{
return gVRTeleportOrn;
}
void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
{
gVRTeleportOrn = vrTeleportOrn;
}

25
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -15,16 +15,11 @@ struct SharedMemLines
///todo: naming. Perhaps PhysicsSdkCommandprocessor?
class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
class PhysicsServerCommandProcessor : public CommandProcessorInterface
{
struct PhysicsServerCommandProcessorInternalData* m_data;
//todo: move this to physics client side / Python
void createDefaultRobotAssets();
void resetSimulation();
protected:
@@ -91,19 +86,27 @@ public:
virtual void removePickingConstraint();
//logging /playback the shared memory commands
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
virtual void enableCommandLogging(bool enable, const char* fileName);
virtual void replayFromLogFile(const char* fileName);
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
//logging of object states (position etc)
void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep);
void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
void enableRealTimeSimulation(bool enableRealTimeSim);
virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
void applyJointDamping(int bodyUniqueId);
virtual void setTimeOut(double timeOutInSeconds);
virtual const btVector3& getVRTeleportPosition() const;
virtual void setVRTeleportPosition(const btVector3& vrTeleportPos);
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
};
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H

389
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -4,9 +4,7 @@
#include "PhysicsServerExample.h"
#ifdef B3_USE_MIDI
#include "RtMidi.h"
#endif//B3_USE_MIDI
#include "PhysicsServerSharedMemory.h"
#include "Bullet3Common/b3CommandLineArgs.h"
@@ -24,37 +22,26 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos;
bool gEnablePicking=true;
bool gEnableTeleporting=true;
bool gEnableRendering= true;
bool gEnableSyncPhysicsRendering= true;
bool gEnableUpdateDebugDrawLines = true;
//extern btVector3 gLastPickPos;
btVector3 gVRTeleportPosLocal(0,0,0);
btQuaternion gVRTeleportOrnLocal(0,0,0,1);
extern btVector3 gVRTeleportPos1;
extern btQuaternion gVRTeleportOrn;
btScalar gVRTeleportRotZ = 0;
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
extern btVector3 gVRController2Pos;
extern btQuaternion gVRController2Orn;
extern btScalar gVRGripperAnalog;
extern btScalar gVRGripper2Analog;
extern bool gCloseToKuka;
extern bool gEnableRealTimeSimVR;
extern bool gCreateDefaultRobotAssets;
extern int gInternalSimFlags;
extern int gCreateObjectSimVR;
extern bool gResetSimulation;
extern int gEnableKukaControl;
int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
bool gBatchUserDebugLines = true;
extern bool gVRGripperClosed;
const char* startFileNameVR = "0_VRDemoSettings.txt";
@@ -82,70 +69,20 @@ static void loadCurrentSettingsVR(b3CommandLineArgs& args)
};
//remember the settings (you don't want to re-tune again and again...)
static void saveCurrentSettingsVR()
static void saveCurrentSettingsVR(const btVector3& VRTeleportPos1)
{
FILE* f = fopen(startFileNameVR, "w");
if (f)
{
fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
fprintf(f, "--camPosX= %f\n", VRTeleportPos1[0]);
fprintf(f, "--camPosY= %f\n", VRTeleportPos1[1]);
fprintf(f, "--camPosZ= %f\n", VRTeleportPos1[2]);
fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
fclose(f);
}
};
#if B3_USE_MIDI
static float getParamf(float rangeMin, float rangeMax, int midiVal)
{
float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
return v;
}
void midiCallback(double deltatime, std::vector< unsigned char > *message, void *userData)
{
unsigned int nBytes = message->size();
for (unsigned int i = 0; i<nBytes; i++)
std::cout << "Byte " << i << " = " << (int)message->at(i) << ", ";
if (nBytes > 0)
std::cout << "stamp = " << deltatime << std::endl;
if (nBytes > 2)
{
if (message->at(0) == 176)
{
if (message->at(1) == 16)
{
gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
// b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ);
}
if (message->at(1) == 32)
{
gCreateDefaultRobotAssets = 1;
}
for (int i = 0; i < 3; i++)
{
if (message->at(1) == i)
{
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
saveCurrentSettingsVR();
// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPosLocal[i]);
}
}
}
}
}
#endif //B3_USE_MIDI
bool gDebugRenderToggle = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
@@ -186,6 +123,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIDumpFramesToVideo,
eGUIHelperRemoveGraphicsInstance,
eGUIHelperChangeGraphicsInstanceRGBAColor,
eGUIHelperChangeGraphicsInstanceSpecularColor,
eGUIHelperSetVisualizerFlag,
};
@@ -291,7 +229,6 @@ struct MotionThreadLocalStorage
float clampedDeltaTime = 0.2;
extern int gMaxNumCmdPer1ms;
void MotionThreadFunc(void* userPtr,void* lsMemory)
@@ -325,16 +262,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
{
if (gMaxNumCmdPer1ms>0)
{
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
{
BT_PROFILE("usleep(10)");
b3Clock::usleep(10);
numCmdSinceSleep1ms = 0;
sleepClock.reset();
}
}
if (sleepClock.getTimeMilliseconds()>1)
{
sleepClock.reset();
@@ -600,7 +528,7 @@ struct ColorWidth
ATTRIBUTE_ALIGNED16( class )MultithreadedDebugDrawer : public btIDebugDraw
{
class GUIHelperInterface* m_guiHelper;
struct GUIHelperInterface* m_guiHelper;
int m_debugMode;
btAlignedObjectArray< btAlignedObjectArray<unsigned int> > m_sortedIndices;
@@ -991,6 +919,19 @@ public:
workerThreadWait();
}
double m_specularColor[3];
int m_graphicsInstanceChangeSpecular;
virtual void changeSpecularColor(int instanceUid, const double specularColor[3])
{
m_graphicsInstanceChangeSpecular = instanceUid;
m_specularColor[0] = specularColor[0];
m_specularColor[1] = specularColor[1];
m_specularColor[2] = specularColor[2];
m_cs->lock();
m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceSpecularColor);
workerThreadWait();
}
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;
@@ -1229,9 +1170,7 @@ class PhysicsServerExample : public SharedMemoryCommon
MotionArgs m_args[MAX_MOTION_NUM_THREADS];
MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
bool m_wantsShutdown;
#ifdef B3_USE_MIDI
RtMidiIn* m_midi;
#endif
bool m_isConnected;
btClock m_clock;
bool m_replay;
@@ -1243,7 +1182,7 @@ class PhysicsServerExample : public SharedMemoryCommon
public:
PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem=0, int options=0);
virtual ~PhysicsServerExample();
@@ -1434,43 +1373,54 @@ public:
if (window->isModifierKeyPressed(B3G_SHIFT))
shift=0.01;
btVector3 VRTeleportPos =this->m_physicsServer.getVRTeleportPosition();
if (key=='w' && state)
{
gVRTeleportPos1[0]+=shift;
saveCurrentSettingsVR();
VRTeleportPos[0]+=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='s' && state)
{
gVRTeleportPos1[0]-=shift;
saveCurrentSettingsVR();
VRTeleportPos[0]-=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='a' && state)
{
gVRTeleportPos1[1]-=shift;
saveCurrentSettingsVR();
VRTeleportPos[1]-=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='d' && state)
{
gVRTeleportPos1[1]+=shift;
saveCurrentSettingsVR();
VRTeleportPos[1]+=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='q' && state)
{
gVRTeleportPos1[2]+=shift;
saveCurrentSettingsVR();
VRTeleportPos[2]+=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='e' && state)
{
gVRTeleportPos1[2]-=shift;
saveCurrentSettingsVR();
VRTeleportPos[2]-=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='z' && state)
{
gVRTeleportRotZ+=shift;
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
m_physicsServer.setVRTeleportOrientation(VRTeleportOrn);
saveCurrentSettingsVR(VRTeleportPos);
}
return false;
}
@@ -1490,33 +1440,33 @@ public:
setSharedMemoryKey(shmemKey);
}
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
btVector3 vrTeleportPos = m_physicsServer.getVRTeleportPosition();
if (args.GetCmdLineArgument("camPosX", vrTeleportPos[0]))
{
printf("camPosX=%f\n", gVRTeleportPos1[0]);
printf("camPosX=%f\n", vrTeleportPos[0]);
}
if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1]))
if (args.GetCmdLineArgument("camPosY", vrTeleportPos[1]))
{
printf("camPosY=%f\n", gVRTeleportPos1[1]);
printf("camPosY=%f\n", vrTeleportPos[1]);
}
if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2]))
if (args.GetCmdLineArgument("camPosZ", vrTeleportPos[2]))
{
printf("camPosZ=%f\n", gVRTeleportPos1[2]);
printf("camPosZ=%f\n", vrTeleportPos[2]);
}
m_physicsServer.setVRTeleportPosition(vrTeleportPos);
float camRotZ = 0.f;
if (args.GetCmdLineArgument("camRotZ", camRotZ))
{
printf("camRotZ = %f\n", camRotZ);
btQuaternion ornZ(btVector3(0, 0, 1), camRotZ);
gVRTeleportOrn = ornZ;
m_physicsServer.setVRTeleportOrientation(ornZ);
}
if (args.CheckCmdLineFlag("robotassets"))
{
gCreateDefaultRobotAssets = true;
}
if (args.CheckCmdLineFlag("realtimesimulation"))
{
@@ -1524,53 +1474,17 @@ public:
m_physicsServer.enableRealTimeSimulation(true);
}
if (args.CheckCmdLineFlag("norobotassets"))
{
gCreateDefaultRobotAssets = false;
}
}
};
#ifdef B3_USE_MIDI
static bool chooseMidiPort(RtMidiIn *rtmidi)
{
/*
std::cout << "\nWould you like to open a virtual input port? [y/N] ";
std::string keyHit;
std::getline( std::cin, keyHit );
if ( keyHit == "y" ) {
rtmidi->openVirtualPort();
return true;
}
*/
std::string portName;
unsigned int i = 0, nPorts = rtmidi->getPortCount();
if (nPorts == 0) {
std::cout << "No midi input ports available!" << std::endl;
return false;
}
if (nPorts > 0) {
std::cout << "\nOpening midi input port " << rtmidi->getPortName() << std::endl;
}
// std::getline( std::cin, keyHit ); // used to clear out stdin
rtmidi->openPort(i);
return true;
}
#endif //B3_USE_MIDI
PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options)
PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper,CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int options)
:SharedMemoryCommon(helper),
m_physicsServer(sharedMem),
m_physicsServer(commandProcessorCreator,sharedMem,0),
m_wantsShutdown(false),
m_isConnected(false),
m_replay(false)
@@ -1579,14 +1493,7 @@ m_replay(false)
,m_tinyVrGui(0)
#endif
{
#ifdef B3_USE_MIDI
m_midi = new RtMidiIn();
chooseMidiPort(m_midi);
m_midi->setCallback(&midiCallback);
// Don't ignore sysex, timing, or active sensing messages.
m_midi->ignoreTypes(false, false, false);
#endif
m_multiThreadedHelper = helper;
// b3Printf("Started PhysicsServer\n");
}
@@ -1595,10 +1502,7 @@ m_replay(false)
PhysicsServerExample::~PhysicsServerExample()
{
#ifdef B3_USE_MIDI
delete m_midi;
m_midi = 0;
#endif
#ifdef BT_ENABLE_VR
delete m_tinyVrGui;
#endif
@@ -1776,22 +1680,22 @@ void PhysicsServerExample::updateGraphics()
if (flag==COV_ENABLE_VR_TELEPORTING)
{
gEnableTeleporting = enable;
gEnableTeleporting = (enable!=0);
}
if (flag == COV_ENABLE_VR_PICKING)
{
gEnablePicking = enable;
gEnablePicking = (enable!=0);
}
if (flag ==COV_ENABLE_SYNC_RENDERING_INTERNAL)
{
gEnableSyncPhysicsRendering = enable;
gEnableSyncPhysicsRendering = (enable!=0);
}
if (flag == COV_ENABLE_RENDERING)
{
gEnableRendering = enable;
gEnableRendering = (enable!=0);
}
m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag,m_multiThreadedHelper->m_visualizerEnable);
@@ -1813,6 +1717,13 @@ void PhysicsServerExample::updateGraphics()
}
case eGUIHelperRemoveAllGraphicsInstances:
{
#ifdef BT_ENABLE_VR
if (m_tinyVrGui)
{
delete m_tinyVrGui;
m_tinyVrGui=0;
}
#endif //BT_ENABLE_VR
m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
{
@@ -1837,6 +1748,13 @@ void PhysicsServerExample::updateGraphics()
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIHelperChangeGraphicsInstanceSpecularColor:
{
m_multiThreadedHelper->m_childGuiHelper->changeSpecularColor(m_multiThreadedHelper->m_graphicsInstanceChangeSpecular,m_multiThreadedHelper->m_specularColor);
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIHelperCopyCameraImageData:
{
@@ -2000,7 +1918,6 @@ extern int gDroppedSimulationSteps;
extern int gNumSteps;
extern double gDtInSec;
extern double gSubStep;
extern int gVRTrackingObjectUniqueId;
extern btTransform gVRTrackingObjectTr;
@@ -2199,33 +2116,23 @@ void PhysicsServerExample::drawUserDebugLines()
void PhysicsServerExample::renderScene()
{
btTransform vrTrans;
//gVRTeleportPos1 = gVRTeleportPosLocal;
//gVRTeleportOrn = gVRTeleportOrnLocal;
///little VR test to follow/drive Husky vehicle
if (gVRTrackingObjectUniqueId >= 0)
{
btTransform vrTrans;
vrTrans.setOrigin(gVRTeleportPosLocal);
vrTrans.setRotation(gVRTeleportOrnLocal);
vrTrans = vrTrans * gVRTrackingObjectTr;
gVRTeleportPos1 = vrTrans.getOrigin();
gVRTeleportOrn = vrTrans.getRotation();
}
B3_PROFILE("PhysicsServerExample::RenderScene");
drawUserDebugLines();
if (gEnableRealTimeSimVR)
if (m_physicsServer.isRealTimeSimulationEnabled())
{
static int frameCount=0;
//static btScalar prevTime = m_clock.getTimeSeconds();
static btScalar prevTime = m_clock.getTimeSeconds();
frameCount++;
static char line0[1024]={0};
static char line1[1024]={0};
#if 0
@@ -2233,7 +2140,8 @@ void PhysicsServerExample::renderScene()
int numFrames = 200;
static int count = 0;
count++;
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
@@ -2274,14 +2182,13 @@ void PhysicsServerExample::renderScene()
{
b3Transform tr;tr.setIdentity();
tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
btVector3 VRController2Pos = m_physicsServer.getVRTeleportPosition();
btQuaternion VRController2Orn = m_physicsServer.getVRTeleportOrientation();
tr.setOrigin(b3MakeVector3(VRController2Pos[0],VRController2Pos[1],VRController2Pos[2]));
tr.setRotation(b3Quaternion(VRController2Orn[0],VRController2Orn[1],VRController2Orn[2],VRController2Orn[3]));
tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
b3Scalar dt = 0.01;
m_tinyVrGui->clearTextArea();
static char line0[1024];
static char line1[1024];
m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
@@ -2296,8 +2203,8 @@ void PhysicsServerExample::renderScene()
btTransform tr2a, tr2;
tr2a.setIdentity();
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a;
btTransform trInv = trTotal.inverse();
@@ -2360,9 +2267,8 @@ void PhysicsServerExample::renderScene()
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
if (!gEnableRealTimeSimVR)
if (!m_physicsServer.isRealTimeSimulationEnabled())
{
gEnableRealTimeSimVR = true;
m_physicsServer.enableRealTimeSimulation(1);
}
}
@@ -2462,35 +2368,6 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y)
}
extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
{
MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper,
options.m_sharedMem,
options.m_option);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING)
{
example->enableCommandLogging();
}
if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG)
{
example->replayFromLogFile();
}
return example;
}
void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
@@ -2503,7 +2380,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
if (gGraspingController < 0)
{
gGraspingController = controllerId;
gEnableKukaControl = true;
}
btTransform trLocal;
@@ -2522,8 +2398,8 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
@@ -2533,8 +2409,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
{
if (button == 1 && state == 0)
{
//gResetSimulation = true;
gVRTeleportPos1 = gLastPickPos;
}
} else
{
@@ -2575,7 +2450,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
if (controllerId == gGraspingController)
{
gCreateObjectSimVR = 1;
}
else
{
@@ -2591,7 +2465,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
if (controllerId == gGraspingController && (button == 33))
{
gVRGripperClosed =(state!=0);
}
else
{
@@ -2661,25 +2535,18 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
if (controllerId == gGraspingController)
{
gVRGripperAnalog = analogAxis;
gVRGripperPos = trTotal.getOrigin();
gVRGripperOrn = trTotal.getRotation();
}
else
{
gVRGripper2Analog = analogAxis;
gVRController2Pos = trTotal.getOrigin();
gVRController2Orn = trTotal.getRotation();
m_args[0].m_vrControllerPos[controllerId] = trTotal.getOrigin();
m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
}
@@ -2722,8 +2589,8 @@ void PhysicsServerExample::vrHMDMoveCallback(int controllerId, float pos[4], flo
tr2a.setIdentity();
btTransform tr2;
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
@@ -2764,8 +2631,8 @@ void PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float
tr2a.setIdentity();
btTransform tr2;
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
m_args[0].m_csGUI->lock();
@@ -2782,4 +2649,36 @@ void PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float
m_args[0].m_csGUI->unlock();
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)
extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options)
{
MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper,
options.m_commandProcessorCreation,
options.m_sharedMem,
options.m_option);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING)
{
example->enableCommandLogging();
}
if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG)
{
example->replayFromLogFile();
}
return example;
}

4
examples/SharedMemory/PhysicsServerExample.h
View File

@@ -8,7 +8,9 @@ enum PhysicsServerOptions
PHYSICS_SERVER_USE_RTC_CLOCK = 4,
};
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options);
///Don't use PhysicsServerCreateFuncInternal directly
///Use PhysicsServerCreateFuncBullet2 instead, or initialize options.m_commandProcessor
class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_H

35
examples/SharedMemory/PhysicsServerExampleBullet2.cpp Normal file
View File

@@ -0,0 +1,35 @@
#include "PhysicsServerExampleBullet2.h"
#include "PhysicsServerExample.h"
#include "PhysicsServerCommandProcessor.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
struct Bullet2CommandProcessorCreation : public CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()
{
PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
return proc;
}
virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
{
delete proc;
}
};
static Bullet2CommandProcessorCreation sBullet2CommandCreator;
CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options)
{
options.m_commandProcessorCreation = &sBullet2CommandCreator;
CommonExampleInterface* example = PhysicsServerCreateFuncInternal(options);
return example;
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFuncBullet2)

9
examples/SharedMemory/PhysicsServerExampleBullet2.h Normal file
View File

@@ -0,0 +1,9 @@
#ifndef PHYSICS_SERVER_EXAMPLE_BULLET_2_H
#define PHYSICS_SERVER_EXAMPLE_BULLET_2_H
class CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_BULLET_2_H

49
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -3,7 +3,7 @@
#include "Win32SharedMemory.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h"
@@ -30,8 +30,9 @@ struct PhysicsServerSharedMemoryInternalData
int m_sharedMemoryKey;
bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS];
bool m_verboseOutput;
PhysicsServerCommandProcessor* m_commandProcessor;
CommandProcessorInterface* m_commandProcessor;
CommandProcessorCreationInterface* m_commandProcessorCreator;
PhysicsServerSharedMemoryInternalData()
:m_sharedMemory(0),
m_ownsSharedMemory(false),
@@ -64,9 +65,10 @@ struct PhysicsServerSharedMemoryInternalData
};
PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* sharedMem)
PhysicsServerSharedMemory::PhysicsServerSharedMemory(CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int bla)
{
m_data = new PhysicsServerSharedMemoryInternalData();
m_data->m_commandProcessorCreator = commandProcessorCreator;
if (sharedMem)
{
m_data->m_sharedMemory = sharedMem;
@@ -81,17 +83,13 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar
m_data->m_ownsSharedMemory = true;
}
m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
m_data->m_commandProcessor = commandProcessorCreator->createCommandProcessor();
}
PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
{
m_data->m_commandProcessor->deleteDynamicsWorld();
delete m_data->m_commandProcessor;
if (m_data->m_sharedMemory)
{
if (m_data->m_verboseOutput)
@@ -105,15 +103,16 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
m_data->m_sharedMemory = 0;
}
m_data->m_commandProcessorCreator->deleteCommandProcessor(m_data->m_commandProcessor);
delete m_data;
}
void PhysicsServerSharedMemory::resetDynamicsWorld()
/*void PhysicsServerSharedMemory::resetDynamicsWorld()
{
m_data->m_commandProcessor->deleteDynamicsWorld();
m_data->m_commandProcessor ->createEmptyDynamicsWorld();
}
*/
void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
@@ -188,7 +187,7 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
{
m_data->m_commandProcessor->deleteDynamicsWorld();
//m_data->m_commandProcessor->deleteDynamicsWorld();
m_data->m_commandProcessor->setGuiHelper(0);
@@ -237,6 +236,11 @@ void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
}
bool PhysicsServerSharedMemory::isRealTimeSimulationEnabled() const
{
return m_data->m_commandProcessor->isRealTimeSimulationEnabled();
}
void PhysicsServerSharedMemory::processClientCommands()
@@ -314,3 +318,24 @@ void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName)
{
m_data->m_commandProcessor->replayFromLogFile(fileName);
}
const btVector3& PhysicsServerSharedMemory::getVRTeleportPosition() const
{
return m_data->m_commandProcessor->getVRTeleportPosition();
}
void PhysicsServerSharedMemory::setVRTeleportPosition(const btVector3& vrTeleportPos)
{
m_data->m_commandProcessor->setVRTeleportPosition(vrTeleportPos);
}
const btQuaternion& PhysicsServerSharedMemory::getVRTeleportOrientation() const
{
return m_data->m_commandProcessor->getVRTeleportOrientation();
}
void PhysicsServerSharedMemory::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
{
m_data->m_commandProcessor->setVRTeleportOrientation(vrTeleportOrn);
}

15
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@@ -2,7 +2,8 @@
#define PHYSICS_SERVER_SHARED_MEMORY_H
#include "PhysicsServer.h"
#include "LinearMath/btQuaternion.h"
class PhysicsServerSharedMemory : public PhysicsServer
{
struct PhysicsServerSharedMemoryInternalData* m_data;
@@ -14,7 +15,7 @@ protected:
public:
PhysicsServerSharedMemory(class SharedMemoryInterface* sharedMem=0);
PhysicsServerSharedMemory(struct CommandProcessorCreationInterface* commandProcessorCreator, class SharedMemoryInterface* sharedMem, int bla);
virtual ~PhysicsServerSharedMemory();
virtual void setSharedMemoryKey(int key);
@@ -29,6 +30,7 @@ public:
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);
@@ -38,6 +40,14 @@ public:
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual void removePickingConstraint();
virtual const btVector3& getVRTeleportPosition() const;
virtual void setVRTeleportPosition(const btVector3& vrTeleportPos);
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
//for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory
@@ -48,7 +58,6 @@ public:
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
void resetDynamicsWorld();
};

10
examples/SharedMemory/SharedMemoryCommandProcessor.cpp
View File

@@ -73,7 +73,15 @@ bool SharedMemoryCommandProcessor::connect()
if (m_data->m_testBlock1) {
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
b3Error("Error: please start server before client\n");
if ((m_data->m_testBlock1->m_magicId < 211705023) &&
(m_data->m_testBlock1->m_magicId >=201705023))
{
b3Error("Error: physics server version mismatch (expected %d got %d)\n",SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
} else
{
b3Error("Error connecting to shared memory: please start server before client\n");
}
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;

11
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -114,6 +114,9 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_MASS=1,
CHANGE_DYNAMICS_INFO_SET_COM=2,
CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION=4,
CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION=8,
CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION=16,
CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32,
};
struct ChangeDynamicsInfoArgs
@@ -123,6 +126,9 @@ struct ChangeDynamicsInfoArgs
double m_mass;
double m_COM[3];
double m_lateralFriction;
double m_spinningFriction;
double m_rollingFriction;
double m_restitution;
};
struct GetDynamicsInfoArgs
@@ -249,6 +255,7 @@ enum EnumUpdateVisualShapeData
{
CMD_UPDATE_VISUAL_SHAPE_TEXTURE=1,
CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR=2,
CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR=4,
};
struct UpdateVisualShapeDataArgs
@@ -258,6 +265,7 @@ struct UpdateVisualShapeDataArgs
int m_shapeIndex;
int m_textureUniqueId;
double m_rgbaColor[4];
double m_specularColor[3];
};
struct LoadTextureArgs
@@ -354,6 +362,8 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024,
SIM_PARAM_MAX_CMD_PER_1MS = 2048,
SIM_PARAM_ENABLE_FILE_CACHING = 4096,
SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192,
};
@@ -387,6 +397,7 @@ struct SendPhysicsSimulationParameters
double m_defaultContactERP;
int m_collisionFilterMode;
int m_enableFileCaching;
double m_restitutionVelocityThreshold;
};
struct LoadBunnyArgs

6
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -4,7 +4,9 @@
#include "PhysicsClientSharedMemory.h"
#include"../ExampleBrowser/InProcessExampleBrowser.h"
#include "PhysicsServerExample.h"
#include "PhysicsServerExampleBullet2.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "InProcessMemory.h"
@@ -142,7 +144,7 @@ public:
CommonExampleOptions options(guiHelper);
options.m_sharedMem = m_sharedMem;
m_physicsServerExample = PhysicsServerCreateFunc(options);
m_physicsServerExample = PhysicsServerCreateFuncBullet2(options);
m_physicsServerExample ->initPhysics();
m_physicsServerExample ->resetCamera();
setSharedMemoryInterface(m_sharedMem);

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -4,7 +4,7 @@
#define SHARED_MEMORY_KEY 12347
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201705025
#define SHARED_MEMORY_MAGIC_NUMBER 201706001
//#define SHARED_MEMORY_MAGIC_NUMBER 201703024
enum EnumSharedMemoryClientCommand

15
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -179,10 +179,10 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
visualShapeOut.m_dimensions[2] = 0;
visualShapeOut.m_meshAssetFileName[0] = 0;
if (visual->m_geometry.m_hasLocalMaterial) {
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0];
visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1];
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2];
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3];
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0];
visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[1];
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2];
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3];
}
GLInstanceGraphicsShape* glmesh = 0;
@@ -555,7 +555,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
{
for (int i=0; i<4; i++)
{
rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
rgbaColor[i] = (*matPtr)->m_matColor.m_rgbaColor[i];
}
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
@@ -754,7 +754,12 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
clearColor.bgra[3] = 255;
clearBuffers(clearColor);
float near = projMat[14]/(projMat[10]-1);
float far = projMat[14]/(projMat[10]+1);
m_data->m_camera.setCameraFrustumNear( near);
m_data->m_camera.setCameraFrustumFar(far);
ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);

5
examples/SharedMemory/main.cpp
View File

@@ -14,7 +14,8 @@ subject to the following restrictions:
*/
#include "PhysicsServerExample.h"
#include "PhysicsServerExampleBullet2.h"
#include "Bullet3Common/b3CommandLineArgs.h"
@@ -77,7 +78,7 @@ int main(int argc, char* argv[])
// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
// options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
example = (SharedMemoryCommon*)PhysicsServerCreateFuncBullet2(options);
example->initPhysics();

1
examples/SharedMemory/premake4.lua
View File

@@ -23,6 +23,7 @@ myfiles =
"PhysicsClientSharedMemory.cpp",
"PhysicsClientExample.cpp",
"PhysicsServerExample.cpp",
"PhysicsServerExampleBullet2.cpp",
"PhysicsServerSharedMemory.cpp",
"PhysicsServerSharedMemory.h",
"PhysicsServer.cpp",

10
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -38,6 +38,9 @@ struct DepthShader : public IShader {
m_lightModelView(lightModelView),
m_lightDistance(lightDistance)
{
m_nearPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]-1);
m_farPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]+1);
m_invModelMat = m_modelMat.invert_transpose();
}
virtual Vec4f vertex(int iface, int nthvert) {
@@ -91,7 +94,8 @@ struct Shader : public IShader {
mat<4,3,float> varying_tri_light_view;
mat<3,3,float> varying_nrm; // normal per vertex to be interpolated by FS
mat<4,3,float> world_tri; // model triangle coordinates in the world space used for backface culling, written by VS
Shader(Model* model, Vec3f light_dir_local, Vec3f light_color, Matrix& modelView, Matrix& lightModelView, Matrix& projectionMat, Matrix& modelMat, Matrix& viewportMat, Vec3f localScaling, const Vec4f& colorRGBA, int width, int height, b3AlignedObjectArray<float>* shadowBuffer, float ambient_coefficient=0.6, float diffuse_coefficient=0.35, float specular_coefficient=0.05)
:m_model(model),
m_light_dir_local(light_dir_local),
@@ -112,6 +116,9 @@ struct Shader : public IShader {
m_height(height)
{
m_nearPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]-1);
m_farPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]+1);
//printf("near=%f, far=%f\n", m_nearPlane, m_farPlane);
m_invModelMat = m_modelMat.invert_transpose();
m_projectionModelViewMat = m_projectionMat*m_modelView1;
m_projectionLightViewMat = m_projectionMat*m_lightModelView;
@@ -576,7 +583,6 @@ void TinyRenderer::renderObjectDepth(TinyRenderObjectData& renderData)
Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
DepthShader shader(model, lightModelViewMatrix, lightViewProjectionMatrix,renderData.m_modelMatrix, localScaling, light_distance);
for (int i=0; i<model->nfaces(); i++)
{
for (int j=0; j<3; j++) {

5
examples/TinyRenderer/our_gl.cpp
View File

@@ -180,6 +180,11 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
zbuffer[P.x+P.y*image.get_width()]>frag_depth)
continue;
bool discard = shader.fragment(bc_clip, color);
if (frag_depth<-shader.m_farPlane)
discard=true;
if (frag_depth>-shader.m_nearPlane)
discard=true;
if (!discard) {
zbuffer[P.x+P.y*image.get_width()] = frag_depth;
if (segmentationMaskBuffer)

2
examples/TinyRenderer/our_gl.h
View File

@@ -11,6 +11,8 @@ Matrix projection(float coeff=0.f); // coeff = -1/c
Matrix lookat(Vec3f eye, Vec3f center, Vec3f up);
struct IShader {
float m_nearPlane;
float m_farPlane;
virtual ~IShader();
virtual Vec4f vertex(int iface, int nthvert) = 0;
virtual bool fragment(Vec3f bar, TGAColor &color) = 0;

6
examples/Utils/ChromeTraceUtil.cpp
View File

@@ -184,7 +184,7 @@ void MyEnterProfileZoneFunc(const char* msg)
return;
#ifndef BT_NO_PROFILE
int threadId = btQuickprofGetCurrentThreadIndex2();
if (threadId<0)
if (threadId<0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
if (gStackDepths[threadId] >= MAX_NESTING)
@@ -208,8 +208,8 @@ void MyLeaveProfileZoneFunc()
return;
#ifndef BT_NO_PROFILE
int threadId = btQuickprofGetCurrentThreadIndex2();
if (threadId<0)
return;
if (threadId<0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
if (gStackDepths[threadId] <= 0)
{

19
examples/Utils/b3Clock.cpp
View File

@@ -46,7 +46,6 @@ struct b3ClockData
#ifdef B3_USE_WINDOWS_TIMERS
LARGE_INTEGER mClockFrequency;
DWORD mStartTick;
LARGE_INTEGER mStartTime;
#else
#ifdef __CELLOS_LV2__
@@ -88,11 +87,24 @@ b3Clock& b3Clock::operator=(const b3Clock& other)
/// Resets the initial reference time.
void b3Clock::reset()
void b3Clock::reset(bool zeroReference)
{
if (zeroReference)
{
#ifdef B3_USE_WINDOWS_TIMERS
m_data->mStartTime.QuadPart = 0;
#else
#ifdef __CELLOS_LV2__
m_data->mStartTime = 0;
#else
m_data->mStartTime = (struct timeval){0};
#endif
#endif
} else
{
#ifdef B3_USE_WINDOWS_TIMERS
QueryPerformanceCounter(&m_data->mStartTime);
m_data->mStartTick = GetTickCount();
#else
#ifdef __CELLOS_LV2__
@@ -105,6 +117,7 @@ void b3Clock::reset()
gettimeofday(&m_data->mStartTime, 0);
#endif
#endif
}
}
/// Returns the time in ms since the last call to reset or since

4
examples/Utils/b3Clock.h
View File

@@ -13,8 +13,8 @@ public:
~b3Clock();
/// Resets the initial reference time.
void reset();
/// Resets the initial reference time. If zeroReference is true, will set reference to absolute 0.
void reset(bool zeroReference=false);
/// Returns the time in ms since the last call to reset or since
/// the b3Clock was created.

48
examples/Utils/b3ResourcePath.cpp
View File

@@ -53,11 +53,31 @@ int b3ResourcePath::getExePath(char* path, int maxPathLenInBytes)
return numBytes;
}
int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePath, int resourcePathMaxNumBytes)
struct TempResourcePath
{
char* m_path;
TempResourcePath(int len)
{
m_path = (char*)malloc(len);
memset(m_path,0,len);
}
virtual ~TempResourcePath()
{
free(m_path);
}
};
int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes)
{
//first find in a resource/<exeName> location, then in various folders within 'data' using b3FileUtils
char exePath[B3_MAX_EXE_PATH_LEN];
bool res = b3FileUtils::findFile(resourceName, resourcePathOut, resourcePathMaxNumBytes);
if (res)
{
return strlen(resourcePathOut);
}
int l = b3ResourcePath::getExePath(exePath, B3_MAX_EXE_PATH_LEN);
if (l)
{
@@ -66,33 +86,31 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
int exeNamePos = b3FileUtils::extractPath(exePath,pathToExe,B3_MAX_EXE_PATH_LEN);
if (exeNamePos)
{
sprintf(resourcePath,"%s../data/%s",pathToExe,resourceName);
TempResourcePath tmpPath(resourcePathMaxNumBytes+1024);
char* resourcePathIn = tmpPath.m_path;
sprintf(resourcePathIn,"%s../data/%s",pathToExe,resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePath);
return strlen(resourcePathOut);
}
sprintf(resourcePath,"%s../resources/%s/%s",pathToExe,&exePath[exeNamePos],resourceName);
sprintf(resourcePathIn,"%s../resources/%s/%s",pathToExe,&exePath[exeNamePos],resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePath);
return strlen(resourcePathOut);
}
sprintf(resourcePath,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName);
sprintf(resourcePathIn,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePath);
return strlen(resourcePathOut);
}
}
}
bool res = b3FileUtils::findFile(resourceName, resourcePath, resourcePathMaxNumBytes);
if (res)
{
return strlen(resourcePath);
}
return 0;
}

1
examples/pybullet/CMakeLists.txt
View File

@@ -33,6 +33,7 @@ SET(pybullet_SRCS
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h

Some files were not shown because too many files have changed in this diff Show More