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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2016-12-11 09:17:04 -08:00
parent 710ac09b56 27d38a1ba8
commit 032e0fb3ce
75 changed files with 23316 additions and 778 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
build_cmake_pybullet_win32.bat
View File

@@ -1,4 +1,4 @@
mkdir cm
cd cm
cmake -DBUILD_PYBULLET=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.2\include -DPYTHON_LIBRARY=c:\python-3.5.2\libs\python35_d.lib ..
cmake -DBUILD_PYBULLET=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.2\include -DPYTHON_LIBRARY=c:\python-3.5.2\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.2\libs\python35_d.lib ..
start .

8
data/gripper/wsg50_one_motor_gripper_free_base.sdf
View File

@@ -303,6 +303,10 @@
</joint>
<link name='finger_right'>
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<pose frame=''>0.042 0 0.145 0 0 1.5708</pose>
<inertial>
<mass>0.2</mass>
@@ -343,6 +347,10 @@
</joint>
<link name='finger_left'>
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<pose frame=''>-0.042 0 0.145 0 0 4.71239</pose>
<inertial>
<mass>0.2</mass>

8
data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
View File

@@ -300,6 +300,10 @@
</joint>
<link name='finger_right'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
<inertial>
<mass>0.2</mass>
@@ -340,6 +344,10 @@
</joint>
<link name='finger_left'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
<inertial>
<mass>0.2</mass>

BIN
data/husky/meshes/base_link.stl Normal file
View File

Binary file not shown.

10
data/torus/plane_only.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

7913
data/torus/plane_only.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/torus/torus.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

1446
data/torus/torus.obj Normal file
View File

File diff suppressed because it is too large Load Diff

33
data/torus/torus.urdf Normal file
View File

@@ -0,0 +1,33 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="1.0"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".1"/>
<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="torus.obj" scale=".3 .3 .3"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="torus.obj" scale=".3 .3 .3"/>
</geometry>
</collision>
</link>
</robot>

12
data/torus/torus_only.mtl Normal file
View File

@@ -0,0 +1,12 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0.000000
Ka 0.000000 0.000000 0.000000
Kd 0.800000 0.800000 0.800000
Ks 0.800000 0.800000 0.800000
Ke 0.000000 0.000000 0.000000
Ni 1.000000
d 1.000000
illum 2

1474
data/torus/torus_only.obj Normal file
View File

File diff suppressed because it is too large Load Diff

10
data/torus/torus_with_plane.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

9384
data/torus/torus_with_plane.obj Normal file
View File

File diff suppressed because it is too large Load Diff

33
data/torus/torus_with_plane.urdf Normal file
View File

@@ -0,0 +1,33 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="1.0"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".1"/>
<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="torus_with_plane.obj" scale=".3 .3 .3"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="torus_with_plane.obj" scale=".3 .3 .3"/>
</geometry>
</collision>
</link>
</robot>

48
data/torus/torus_with_separate_plane.urdf Normal file
View File

@@ -0,0 +1,48 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="1.0"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".1"/>
<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="torus_only.obj" scale=".3 .3 .3"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane_only.obj" scale=".3 .3 .3"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="torus_only.obj" scale=".3 .3 .3"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane_only.obj" scale=".3 .3 .3"/>
</geometry>
</collision>
</link>
</robot>

BIN
data/tray/tray.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 361 KiB

13
data/tray/tray_textured2.mtl Normal file
View File

@@ -0,0 +1,13 @@
# Blender MTL File: 'tray_textured2.blend'
# Material Count: 1
newmtl None
Ns 0.000000
Ka 0.000000 0.000000 0.000000
Kd 0.800000 0.800000 0.800000
Ks 0.800000 0.800000 0.800000
Ke 0.000000 0.000000 0.000000
Ni 1.000000
d 1.000000
illum 2
map_Kd tray.jpg

255
data/tray/tray_textured2.obj Normal file
View File

@@ -0,0 +1,255 @@
# Blender v2.78 (sub 0) OBJ File: 'tray_textured2.blend'
# www.blender.org
mtllib tray_textured2.mtl
o edge_1_Cube.003
v 0.580000 0.590083 0.250354
v -0.419960 0.426691 -0.001860
v -0.580000 0.590083 0.250354
v 0.580000 0.573309 0.261247
v 0.420014 0.426691 -0.001059
v -0.580000 0.573309 0.261247
v 0.420014 0.409917 0.009834
v -0.419960 0.409917 0.009033
vt 0.8346 0.9187
vt 0.2203 0.8574
vt 0.1480 0.9187
vt 0.8346 0.9129
vt 0.7623 0.8574
vt 0.1480 0.9129
vt 0.7623 0.8511
vt 0.2203 0.8511
vn 0.0004 0.8386 -0.5448
vn 0.0001 0.8391 -0.5439
vn -0.0000 0.8393 -0.5437
vn 0.8823 -0.2564 -0.3948
vn -0.0004 -0.8392 0.5439
vn -0.0001 -0.8386 0.5447
vn 0.0000 -0.8385 0.5449
vn -0.8826 -0.2560 -0.3942
vn 0.0008 -0.5446 -0.8387
vn -0.0000 0.5446 0.8387
vn 0.0005 0.8383 -0.5452
vn -0.0005 -0.8394 0.5435
usemtl None
s 1
f 1/1/1 2/2/2 3/3/3
f 4/4/4 5/5/4 1/1/4
f 6/6/5 7/7/6 4/4/7
f 3/3/8 8/8/8 6/6/8
f 5/5/9 8/8/9 2/2/9
f 4/4/10 3/3/10 6/6/10
f 1/1/1 5/5/11 2/2/2
f 4/4/4 7/7/4 5/5/4
f 6/6/5 8/8/12 7/7/6
f 3/3/8 2/2/8 8/8/8
f 5/5/9 7/7/9 8/8/9
f 4/4/10 1/1/10 3/3/10
o edge_2_Cube
v 0.590083 0.580000 0.250354
v 0.409917 0.420060 0.009390
v 0.573309 0.580000 0.261247
v 0.590083 -0.580000 0.250354
v 0.426691 0.420060 -0.001503
v 0.573309 -0.580000 0.261247
v 0.426691 -0.419158 -0.002053
v 0.409917 -0.419158 0.008840
vt 0.9410 0.8520
vt 0.7523 0.8566
vt 0.9234 0.8524
vt 0.8896 0.1426
vt 0.7698 0.8562
vt 0.8721 0.1430
vt 0.7185 0.1468
vt 0.7009 0.1472
vn -0.2561 0.8826 -0.3943
vn 0.8394 0.0003 -0.5435
vn 0.8390 0.0001 -0.5441
vn 0.8389 0.0000 -0.5442
vn -0.2569 -0.8818 -0.3956
vn -0.8390 -0.0003 0.5441
vn -0.8394 -0.0001 0.5436
vn -0.8395 -0.0000 0.5434
vn -0.5446 0.0005 -0.8387
vn 0.5446 -0.0000 0.8387
vn 0.8396 0.0004 -0.5433
vn -0.8388 -0.0004 0.5444
usemtl None
s 1
f 9/9/13 10/10/13 11/11/13
f 12/12/14 13/13/15 9/9/16
f 14/14/17 15/15/17 12/12/17
f 11/11/18 16/16/19 14/14/20
f 13/13/21 16/16/21 10/10/21
f 12/12/22 11/11/22 14/14/22
f 9/9/13 13/13/13 10/10/13
f 12/12/14 15/15/23 13/13/15
f 14/14/17 16/16/17 15/15/17
f 11/11/18 10/10/24 16/16/19
f 13/13/21 15/15/21 16/16/21
f 12/12/22 9/9/22 11/11/22
o edge_3_Cube.002
v 0.580000 -0.573309 0.261247
v -0.419400 -0.409917 0.008678
v -0.580000 -0.573309 0.261247
v 0.580000 -0.590083 0.250354
v 0.419883 -0.409917 0.009162
v -0.580000 -0.590083 0.250354
v 0.419883 -0.426691 -0.001731
v -0.419400 -0.426691 -0.002215
vt 0.8690 0.1040
vt 0.1365 0.1739
vt 0.0188 0.1040
vt 0.8690 0.0968
vt 0.7517 0.1739
vt 0.0188 0.0968
vt 0.7517 0.1668
vt 0.1365 0.1668
vn -0.0002 0.8392 0.5438
vn -0.0000 0.8395 0.5433
vn -0.0000 0.8396 0.5432
vn 0.8825 0.2562 -0.3945
vn 0.0002 -0.8396 -0.5433
vn 0.0000 -0.8392 -0.5438
vn 0.0000 -0.8391 -0.5439
vn -0.8821 0.2565 -0.3950
vn 0.0005 0.5446 -0.8387
vn 0.0000 -0.5446 0.8387
vn -0.0003 0.8391 0.5440
vn 0.0003 -0.8397 -0.5430
usemtl None
s 1
f 17/17/25 18/18/26 19/19/27
f 20/20/28 21/21/28 17/17/28
f 22/22/29 23/23/30 20/20/31
f 19/19/32 24/24/32 22/22/32
f 21/21/33 24/24/33 18/18/33
f 20/20/34 19/19/34 22/22/34
f 17/17/25 21/21/35 18/18/26
f 20/20/28 23/23/28 21/21/28
f 22/22/29 24/24/36 23/23/30
f 19/19/32 18/18/32 24/24/32
f 21/21/33 23/23/33 24/24/33
f 20/20/34 17/17/34 19/19/34
o edge_5_Cube.005
v -0.153309 0.580000 0.261247
v -0.006691 0.419400 -0.002214
v -0.170083 0.580000 0.250354
v -0.153309 -0.580000 0.261247
v 0.010083 0.419400 0.008679
v -0.170083 -0.580000 0.250354
v 0.010083 -0.419883 0.009732
v -0.006691 -0.419883 -0.001161
vt 0.0506 0.8517
vt 0.1935 0.8492
vt 0.0342 0.8520
vt 0.0164 0.1914
vt 0.2099 0.8489
vt 0.0001 0.1917
vt 0.1757 0.1886
vt 0.1594 0.1889
vn 0.2565 0.8821 -0.3950
vn 0.8387 0.0005 0.5446
vn 0.8394 0.0001 0.5434
vn 0.8396 0.0000 0.5432
vn 0.2565 -0.8822 -0.3950
vn -0.8395 -0.0005 -0.5434
vn -0.8388 -0.0001 -0.5445
vn -0.8386 -0.0000 -0.5448
vn 0.5446 -0.0011 -0.8387
vn -0.5446 -0.0000 0.8387
vn 0.8384 0.0007 0.5451
vn -0.8398 -0.0007 -0.5429
usemtl None
s 1
f 25/25/37 26/26/37 27/27/37
f 28/28/38 29/29/39 25/25/40
f 30/30/41 31/31/41 28/28/41
f 27/27/42 32/32/43 30/30/44
f 29/29/45 32/32/45 26/26/45
f 28/28/46 27/27/46 30/30/46
f 25/25/37 29/29/37 26/26/37
f 28/28/38 31/31/47 29/29/39
f 30/30/41 32/32/41 31/31/41
f 27/27/42 26/26/48 32/32/43
f 29/29/45 31/31/45 32/32/45
f 28/28/46 25/25/46 27/27/46
o edge_4_Cube.001
v -0.573309 0.580000 0.261247
v -0.426691 0.419400 -0.002214
v -0.590083 0.580000 0.250354
v -0.573309 -0.580000 0.261247
v -0.409917 0.419400 0.008679
v -0.590083 -0.580000 0.250354
v -0.409917 -0.419400 0.009162
v -0.426691 -0.419400 -0.001731
vt 0.9046 0.2397
vt 0.7929 0.2434
vt 0.9174 0.2393
vt 0.9537 0.7559
vt 0.7801 0.2438
vt 0.9664 0.7554
vt 0.8291 0.7599
vt 0.8419 0.7595
vn 0.2565 0.8821 -0.3950
vn 0.8392 0.0002 0.5438
vn 0.8395 0.0000 0.5433
vn 0.8396 0.0000 0.5432
vn 0.2568 -0.8819 -0.3954
vn -0.8396 -0.0002 -0.5433
vn -0.8392 -0.0000 -0.5438
vn -0.8391 -0.0000 -0.5439
vn 0.5446 -0.0005 -0.8387
vn -0.5446 -0.0000 0.8387
vn 0.8391 0.0003 0.5440
vn -0.8397 -0.0003 -0.5430
usemtl None
s 1
f 33/33/49 34/34/49 35/35/49
f 36/36/50 37/37/51 33/33/52
f 38/38/53 39/39/53 36/36/53
f 35/35/54 40/40/55 38/38/56
f 37/37/57 40/40/57 34/34/57
f 36/36/58 35/35/58 38/38/58
f 33/33/49 37/37/49 34/34/49
f 36/36/50 39/39/59 37/37/51
f 38/38/53 40/40/53 39/39/53
f 35/35/54 34/34/60 40/40/55
f 37/37/57 39/39/57 40/40/57
f 36/36/58 33/33/58 35/35/58
o base_Cube.004
v 0.420000 0.420000 0.010000
v -0.420000 0.420000 -0.010000
v -0.420000 0.420000 0.010000
v 0.420000 -0.420000 0.010000
v 0.420000 0.420000 -0.010000
v -0.420000 -0.420000 0.010000
v 0.420000 -0.420000 -0.010000
v -0.420000 -0.420000 -0.010000
vt 0.7524 0.8072
vt -0.3038 0.8371
vt -0.3038 0.8371
vt 0.7012 0.1905
vt 0.7524 0.8072
vt -0.3550 0.2204
vt 0.7012 0.1905
vt -0.3550 0.2204
vn -0.0000 1.0000 0.0000
vn 1.0000 0.0000 0.0000
vn 0.0000 -1.0000 0.0000
vn -1.0000 -0.0000 0.0000
vn -0.0000 0.0000 -1.0000
vn 0.0000 -0.0000 1.0000
usemtl None
s 1
f 41/41/61 42/42/61 43/43/61
f 44/44/62 45/45/62 41/41/62
f 46/46/63 47/47/63 44/44/63
f 43/43/64 48/48/64 46/46/64
f 45/45/65 48/48/65 42/42/65
f 44/44/66 43/43/66 46/46/66
f 41/41/61 45/45/61 42/42/61
f 44/44/62 47/47/62 45/45/62
f 46/46/63 48/48/63 47/47/63
f 43/43/64 42/42/64 48/48/64
f 45/45/65 47/47/65 48/48/65
f 44/44/66 41/41/66 43/43/66

24
data/tray/tray_textured2.urdf Normal file
View File

@@ -0,0 +1,24 @@
<robot name="tabletop">
<link name="base_link">
<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="tray_textured2.obj" scale="0.5 0.5 0.5"/>
</geometry>
<material name="tray_material">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="tray_textured2.obj" scale="0.5 0.5 0.5"/>
</geometry>
</collision>
</link>
</robot>

4
examples/CommonInterfaces/CommonParameterInterface.h
View File

@@ -4,7 +4,7 @@
#pragma once
typedef void (*SliderParamChangedCallback) (float newVal);
typedef void (*SliderParamChangedCallback) (float newVal, void* userPointer);
#include "LinearMath/btScalar.h"
struct SliderParams
@@ -16,6 +16,7 @@ struct SliderParams
btScalar* m_paramValuePointer;
void* m_userPointer;
bool m_clampToNotches;
bool m_clampToIntegers;
bool m_showValues;
SliderParams(const char* name, btScalar* targetValuePointer)
@@ -26,6 +27,7 @@ struct SliderParams
m_paramValuePointer(targetValuePointer),
m_userPointer(0),
m_clampToNotches(true),
m_clampToIntegers(false),
m_showValues(true)
{
}

9
examples/Evolution/NN3DWalkers.cpp
View File

@@ -552,10 +552,6 @@ struct WalkerFilterCallback : public btOverlapFilterCallback
}
};
void floorNNSliderValue(float notUsed) {
gParallelEvaluations = floor(gParallelEvaluations);
}
void NN3DWalkersExample::initPhysics()
{
@@ -657,8 +653,7 @@ void NN3DWalkersExample::initPhysics()
SliderParams slider("Parallel evaluations", &gParallelEvaluations);
slider.m_minVal = 1;
slider.m_maxVal = NUM_WALKERS;
slider.m_clampToNotches = false;
slider.m_callback = floorNNSliderValue; // hack to get integer values
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -1035,6 +1030,7 @@ void NN3DWalkersExample::drawMarkings() {
}
void NN3DWalkersExample::printWalkerConfigs(){
#if 0
char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(3+15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n
char* runner = configString;
sprintf(runner,"Population configuration:");
@@ -1058,4 +1054,5 @@ void NN3DWalkersExample::printWalkerConfigs(){
}
runner[0] = '\0';
b3Printf(configString);
#endif
}

19
examples/Evolution/NN3DWalkersTimeWarpBase.h
View File

@@ -138,19 +138,15 @@ static btScalar gCFMSingularityAvoidance = 0;
//GUI related parameter changing helpers
inline void floorSliderValues(float notUsed) { // floor values that should be ints
gSolverIterations = floor(gSolverIterations);
}
inline void twxChangePhysicsStepsPerSecond(float physicsStepsPerSecond) { // function to change simulation physics steps per second
inline void twxChangePhysicsStepsPerSecond(float physicsStepsPerSecond, void*) { // function to change simulation physics steps per second
gPhysicsStepsPerSecond = physicsStepsPerSecond;
}
inline void twxChangeFPS(float framesPerSecond) {
inline void twxChangeFPS(float framesPerSecond, void*) {
gFramesPerSecond = framesPerSecond;
}
inline void twxChangeERPCFM(float notUsed) { // function to change ERP/CFM appropriately
inline void twxChangeERPCFM(float notUsed, void*) { // function to change ERP/CFM appropriately
gChangeErpCfm = true;
}
@@ -166,13 +162,12 @@ inline void changeSolver(int comboboxId, const char* item, void* userPointer) {
}
inline void twxChangeSolverIterations(float notUsed){ // change the solver iterations
inline void twxChangeSolverIterations(float notUsed, void* userPtr) { // change the solver iterations
floorSliderValues(0); // floor the values set by slider
}
inline void clampToCustomSpeedNotches(float speed) { // function to clamp to custom speed notches
inline void clampToCustomSpeedNotches(float speed, void*) { // function to clamp to custom speed notches
double minSpeed = 0;
double minSpeedDist = SimulationSpeeds::MAX_SPEED;
for (int i = 0; i < SimulationSpeeds::NUM_SPEEDS; i++) {
@@ -200,7 +195,7 @@ inline void switchMaximumSpeed(int buttonId, bool buttonState, void* userPointer
// b3Printf("Run maximum speed %s", gMaximumSpeed?"on":"off");
}
inline void setApplicationTick(float frequency){ // set internal application tick
inline void setApplicationTick(float frequency, void*){ // set internal application tick
gApplicationTick = 1000.0f/frequency;
}
@@ -383,7 +378,7 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase {
slider.m_minVal = 0;
slider.m_maxVal = 1000;
slider.m_callback = twxChangePhysicsStepsPerSecond;
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}

22
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
View File

@@ -28,18 +28,20 @@ template<typename T>
struct MySliderEventHandler : public Gwen::Event::Handler
{
SliderParamChangedCallback m_callback;
void* m_userPointer;
Gwen::Controls::TextBox* m_label;
Gwen::Controls::Slider* m_pSlider;
char m_variableName[1024];
T* m_targetValue;
bool m_showValue;
MySliderEventHandler(const char* varName, Gwen::Controls::TextBox* label, Gwen::Controls::Slider* pSlider,T* target,SliderParamChangedCallback callback)
MySliderEventHandler(const char* varName, Gwen::Controls::TextBox* label, Gwen::Controls::Slider* pSlider,T* target, SliderParamChangedCallback callback, void* userPtr)
:m_label(label),
m_pSlider(pSlider),
m_targetValue(target),
m_showValue(true),
m_callback(callback)
m_callback(callback),
m_userPointer(userPtr)
{
memcpy(m_variableName,varName,strlen(varName)+1);
}
@@ -55,7 +57,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
if (m_callback)
{
(*m_callback)(v);
(*m_callback)(v, m_userPointer);
}
}
@@ -223,12 +225,20 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
pSlider->SetPos( 10, m_gwenInternalData->m_curYposition );
pSlider->SetSize( 200, 20 );
pSlider->SetRange( params.m_minVal, params.m_maxVal);
pSlider->SetNotchCount(16);//float(params.m_maxVal-params.m_minVal)/100.f);
pSlider->SetClampToNotches( params.m_clampToNotches );
if (params.m_clampToIntegers)
{
pSlider->SetNotchCount( int( params.m_maxVal - params.m_minVal ) );
pSlider->SetClampToNotches( params.m_clampToNotches );
}
else
{
pSlider->SetNotchCount( 16 );//float(params.m_maxVal-params.m_minVal)/100.f);
pSlider->SetClampToNotches( params.m_clampToNotches );
}
pSlider->SetValue( *params.m_paramValuePointer);//dimensions[i] );
char labelName[1024];
sprintf(labelName,"%s",params.m_name);//axisNames[0]);
MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName,label,pSlider,params.m_paramValuePointer,params.m_callback);
MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName,label,pSlider,params.m_paramValuePointer,params.m_callback, params.m_userPointer);
handler->m_showValue = params.m_showValues;
m_paramInternalData->m_sliderEventHandlers.push_back(handler);

2
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -1167,7 +1167,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
BT_PROFILE("Render Scene");
sCurrentDemo->renderScene();
}
} else
{
B3_PROFILE("physicsDebugDraw");
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );

28
examples/ExtendedTutorials/InclinedPlane.cpp
View File

@@ -69,19 +69,19 @@ struct InclinedPlaneExample : public CommonRigidBodyBase
};
void onBoxFrictionChanged(float friction);
void onBoxFrictionChanged(float friction, void* userPtr);
void onBoxRestitutionChanged(float restitution);
void onBoxRestitutionChanged(float restitution, void* userPtr);
void onSphereFrictionChanged(float friction);
void onSphereFrictionChanged(float friction, void* userPtr);
void onSphereRestitutionChanged(float restitution);
void onSphereRestitutionChanged(float restitution, void* userPtr);
void onRampInclinationChanged(float inclination);
void onRampInclinationChanged(float inclination, void* userPtr);
void onRampFrictionChanged(float friction);
void onRampFrictionChanged(float friction, void* userPtr);
void onRampRestitutionChanged(float restitution);
void onRampRestitutionChanged(float restitution, void* userPtr);
void InclinedPlaneExample::initPhysics()
{
@@ -306,35 +306,35 @@ bool InclinedPlaneExample::keyboardCallback(int key, int state) {
// GUI parameter modifiers
void onBoxFrictionChanged(float friction){
void onBoxFrictionChanged(float friction, void*){
if(gBox){
gBox->setFriction(friction);
// b3Printf("Friction of box changed to %f",friction );
}
}
void onBoxRestitutionChanged(float restitution){
void onBoxRestitutionChanged(float restitution, void*){
if(gBox){
gBox->setRestitution(restitution);
//b3Printf("Restitution of box changed to %f",restitution);
}
}
void onSphereFrictionChanged(float friction){
void onSphereFrictionChanged(float friction, void*){
if(gSphere){
gSphere->setFriction(friction);
//b3Printf("Friction of sphere changed to %f",friction );
}
}
void onSphereRestitutionChanged(float restitution){
void onSphereRestitutionChanged(float restitution, void*){
if(gSphere){
gSphere->setRestitution(restitution);
//b3Printf("Restitution of sphere changed to %f",restitution);
}
}
void onRampInclinationChanged(float inclination){
void onRampInclinationChanged(float inclination, void*){
if(ramp){
btTransform startTransform;
startTransform.setIdentity();
@@ -351,14 +351,14 @@ void onRampInclinationChanged(float inclination){
}
}
void onRampFrictionChanged(float friction){
void onRampFrictionChanged(float friction, void*){
if(ramp){
ramp->setFriction(friction);
//b3Printf("Friction of ramp changed to %f \n",friction );
}
}
void onRampRestitutionChanged(float restitution){
void onRampRestitutionChanged(float restitution, void*){
if(ramp){
ramp->setRestitution(restitution);
//b3Printf("Restitution of ramp changed to %f \n",restitution);

21
examples/ExtendedTutorials/MultiPendulum.cpp
View File

@@ -71,11 +71,9 @@ struct MultiPendulumExample: public CommonRigidBodyBase {
static MultiPendulumExample* mex = NULL; // Handle to the example to access it via functions. Do not use this in your simulation!
void onMultiPendulaLengthChanged(float pendulaLength); // Change the pendula length
void onMultiPendulaLengthChanged(float pendulaLength, void*); // Change the pendula length
void onMultiPendulaRestitutionChanged(float pendulaRestitution); // change the pendula restitution
void floorMSliderValue(float notUsed); // floor the slider values which should be integers
void onMultiPendulaRestitutionChanged(float pendulaRestitution, void*); // change the pendula restitution
void applyMForceWithForceScalar(float forceScalar);
@@ -85,8 +83,7 @@ void MultiPendulumExample::initPhysics() { // Setup your physics scene
SliderParams slider("Number of Pendula", &gPendulaQty);
slider.m_minVal = 1;
slider.m_maxVal = 50;
slider.m_callback = floorMSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -95,8 +92,7 @@ void MultiPendulumExample::initPhysics() { // Setup your physics scene
SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
slider.m_minVal = 0;
slider.m_maxVal = 49;
slider.m_callback = floorMSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -397,7 +393,7 @@ void MultiPendulumExample::applyPendulumForce(btScalar pendulumForce){
// GUI parameter modifiers
void onMultiPendulaLengthChanged(float pendulaLength) { // Change the pendula length
void onMultiPendulaLengthChanged(float pendulaLength, void*) { // Change the pendula length
if (mex){
mex->changePendulaLength(pendulaLength);
}
@@ -405,18 +401,13 @@ void onMultiPendulaLengthChanged(float pendulaLength) { // Change the pendula le
}
void onMultiPendulaRestitutionChanged(float pendulaRestitution) { // change the pendula restitution
void onMultiPendulaRestitutionChanged(float pendulaRestitution, void*) { // change the pendula restitution
if (mex){
mex->changePendulaRestitution(pendulaRestitution);
}
}
void floorMSliderValue(float notUsed) { // floor the slider values which should be integers
gPendulaQty = floor(gPendulaQty);
gDisplacedPendula = floor(gDisplacedPendula);
}
void applyMForceWithForceScalar(float forceScalar) {
if(mex){
btScalar appliedForce = forceScalar * gDisplacementForce;

22
examples/ExtendedTutorials/NewtonsCradle.cpp
View File

@@ -71,11 +71,9 @@ struct NewtonsCradleExample: public CommonRigidBodyBase {
static NewtonsCradleExample* nex = NULL;
void onPendulaLengthChanged(float pendulaLength); // Change the pendula length
void onPendulaLengthChanged(float pendulaLength, void* userPtr); // Change the pendula length
void onPendulaRestitutionChanged(float pendulaRestitution); // change the pendula restitution
void floorSliderValue(float notUsed); // floor the slider values which should be integers
void onPendulaRestitutionChanged(float pendulaRestitution, void* userPtr); // change the pendula restitution
void applyForceWithForceScalar(float forceScalar);
@@ -85,8 +83,7 @@ void NewtonsCradleExample::initPhysics() {
SliderParams slider("Number of Pendula", &gPendulaQty);
slider.m_minVal = 1;
slider.m_maxVal = 50;
slider.m_callback = floorSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -95,8 +92,7 @@ void NewtonsCradleExample::initPhysics() {
SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
slider.m_minVal = 0;
slider.m_maxVal = 49;
slider.m_callback = floorSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -343,25 +339,19 @@ void NewtonsCradleExample::applyPendulumForce(btScalar pendulumForce){
// GUI parameter modifiers
void onPendulaLengthChanged(float pendulaLength) {
void onPendulaLengthChanged(float pendulaLength, void*) {
if (nex){
nex->changePendulaLength(pendulaLength);
//b3Printf("Pendula length changed to %f \n",sliderValue );
}
}
void onPendulaRestitutionChanged(float pendulaRestitution) {
void onPendulaRestitutionChanged(float pendulaRestitution, void*) {
if (nex){
nex->changePendulaRestitution(pendulaRestitution);
}
}
void floorSliderValue(float notUsed) {
gPendulaQty = floor(gPendulaQty);
gDisplacedPendula = floor(gDisplacedPendula);
}
void applyForceWithForceScalar(float forceScalar) {
if(nex){
btScalar appliedForce = forceScalar * gDisplacementForce;

21
examples/ExtendedTutorials/NewtonsRopeCradle.cpp
View File

@@ -105,9 +105,7 @@ struct NewtonsRopeCradleExample : public CommonRigidBodyBase {
static NewtonsRopeCradleExample* nex = NULL;
void onRopePendulaRestitutionChanged(float pendulaRestitution);
void floorRSliderValue(float notUsed);
void onRopePendulaRestitutionChanged(float pendulaRestitution, void*);
void applyRForceWithForceScalar(float forceScalar);
@@ -118,8 +116,7 @@ void NewtonsRopeCradleExample::initPhysics()
SliderParams slider("Number of Pendula", &gPendulaQty);
slider.m_minVal = 1;
slider.m_maxVal = 50;
slider.m_callback = floorRSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -128,8 +125,7 @@ void NewtonsRopeCradleExample::initPhysics()
SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
slider.m_minVal = 0;
slider.m_maxVal = 49;
slider.m_callback = floorRSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -148,8 +144,7 @@ void NewtonsRopeCradleExample::initPhysics()
SliderParams slider("Rope Resolution", &gRopeResolution);
slider.m_minVal = 1;
slider.m_maxVal = 20;
slider.m_clampToNotches = false;
slider.m_callback = floorRSliderValue;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
@@ -357,18 +352,12 @@ void NewtonsRopeCradleExample::applyPendulumForce(btScalar pendulumForce){
// GUI parameter modifiers
void onRopePendulaRestitutionChanged(float pendulaRestitution) {
void onRopePendulaRestitutionChanged(float pendulaRestitution, void*) {
if (nex){
nex->changePendulaRestitution(pendulaRestitution);
}
}
void floorRSliderValue(float notUsed) {
gPendulaQty = floor(gPendulaQty);
gDisplacedPendula = floor(gDisplacedPendula);
gRopeResolution = floor(gRopeResolution);
}
void applyRForceWithForceScalar(float forceScalar) {
if(nex){
btScalar appliedForce = forceScalar * gDisplacementForce;

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

@@ -143,6 +143,34 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
}
void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisionObject* col)
{
if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION) != 0)
{
col->setFriction(contactInfo.m_lateralFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_RESTITUTION) != 0)
{
col->setRestitution(contactInfo.m_restitution);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION) != 0)
{
col->setRollingFriction(contactInfo.m_rollingFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION) != 0)
{
col->setSpinningFriction(contactInfo.m_spinningFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING) != 0)
{
col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping);
}
}
void ConvertURDF2BulletInternal(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
URDF2BulletCachedData& cache, int urdfLinkIndex,
@@ -258,11 +286,18 @@ void ConvertURDF2BulletInternal(
world1->addRigidBody(body);
compoundShape->setUserIndex(graphicsIndex);
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
processContactParameters(contactInfo, body);
creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
//untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
} else
{
@@ -413,22 +448,7 @@ void ConvertURDF2BulletInternal(
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
{
col->setFriction(contactInfo.m_lateralFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0)
{
col->setRollingFriction(contactInfo.m_rollingFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION)!=0)
{
col->setSpinningFriction(contactInfo.m_spinningFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0)
{
col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping);
}
processContactParameters(contactInfo, col);
if (mbLinkIndex>=0) //???? double-check +/- 1
{

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

@@ -22,6 +22,7 @@ enum URDF_LinkContactFlags
URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
URDF_CONTACT_HAS_ROLLING_FRICTION=32,
URDF_CONTACT_HAS_SPINNING_FRICTION=64,
URDF_CONTACT_HAS_RESTITUTION=128,
};
@@ -30,6 +31,7 @@ struct URDFLinkContactInfo
btScalar m_lateralFriction;
btScalar m_rollingFriction;
btScalar m_spinningFriction;
btScalar m_restitution;
btScalar m_inertiaScaling;
btScalar m_contactCfm;
btScalar m_contactErp;
@@ -42,6 +44,7 @@ struct URDFLinkContactInfo
:m_lateralFriction(0.5),
m_rollingFriction(0),
m_spinningFriction(0),
m_restitution(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0),

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

@@ -672,6 +672,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
}
{
TiXmlElement *restitution_xml = ci->FirstChildElement("restitution");
if (restitution_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_restitution = urdfLexicalCast<double>(restitution_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION;
}
else
{
if (!restitution_xml->Attribute("value"))
{
logger->reportError("Link/contact: restitution element must have value attribute");
return false;
}
link.m_contactInfo.m_restitution = urdfLexicalCast<double>(restitution_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION;
}
}
}
{
TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction");
if (spinning_xml)

24
examples/MultiThreadedDemo/CommonRigidBodyMTBase.cpp
View File

@@ -590,6 +590,8 @@ public:
static bool gMultithreadedWorld = false;
static bool gDisplayProfileInfo = false;
static btScalar gSliderNumThreads = 1.0f; // should be int
static btScalar gSliderSolverIterations = 10.0f; // should be int
////////////////////////////////////
CommonRigidBodyMTBase::CommonRigidBodyMTBase( struct GUIHelperInterface* helper )
@@ -633,7 +635,7 @@ void apiSelectButtonCallback(int buttonId, bool buttonState, void* userPointer)
}
}
void setThreadCountCallback(float val)
void setThreadCountCallback(float val, void* userPtr)
{
if (gTaskMgr.getApi()==TaskManager::apiNone)
{
@@ -642,7 +644,14 @@ void setThreadCountCallback(float val)
else
{
gTaskMgr.setNumThreads( int( gSliderNumThreads ) );
gSliderNumThreads = float(gTaskMgr.getNumThreads());
}
}
void setSolverIterationCountCallback(float val, void* userPtr)
{
if (btDiscreteDynamicsWorld* world = reinterpret_cast<btDiscreteDynamicsWorld*>(userPtr))
{
world->getSolverInfo().m_numIterations = btMax(1, int(gSliderSolverIterations));
}
}
@@ -728,6 +737,15 @@ void CommonRigidBodyMTBase::createDefaultParameters()
button.m_callback = boolPtrButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
{
SliderParams slider( "Solver iterations", &gSliderSolverIterations );
slider.m_minVal = 1.0f;
slider.m_maxVal = 30.0f;
slider.m_callback = setSolverIterationCountCallback;
slider.m_userPointer = m_dynamicsWorld;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
if (m_multithreadedWorld)
{
// create a button for each supported threading API
@@ -750,7 +768,7 @@ void CommonRigidBodyMTBase::createDefaultParameters()
slider.m_minVal = 1.0f;
slider.m_maxVal = float(gTaskMgr.getMaxNumThreads()*2);
slider.m_callback = setThreadCountCallback;
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
}

13
examples/MultiThreadedDemo/MultiThreadedDemo.cpp
View File

@@ -90,7 +90,7 @@ MultiThreadedDemo::MultiThreadedDemo(struct GUIHelperInterface* helper)
static btScalar gSliderStackRows = 8.0f;
static btScalar gSliderStackColumns = 6.0f;
static btScalar gSliderStackHeight = 15.0f;
void MultiThreadedDemo::initPhysics()
{
@@ -98,18 +98,25 @@ void MultiThreadedDemo::initPhysics()
m_dynamicsWorld->setGravity( btVector3( 0, -10, 0 ) );
{
SliderParams slider( "Stack height", &gSliderStackHeight );
slider.m_minVal = 1.0f;
slider.m_maxVal = 30.0f;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
SliderParams slider( "Stack rows", &gSliderStackRows );
slider.m_minVal = 1.0f;
slider.m_maxVal = 20.0f;
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
SliderParams slider( "Stack columns", &gSliderStackColumns );
slider.m_minVal = 1.0f;
slider.m_maxVal = 20.0f;
slider.m_clampToNotches = false;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}

50
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -23,6 +23,7 @@ struct TinyRendererSetupInternalData
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<float> m_depthBuffer;
b3AlignedObjectArray<float> m_shadowBuffer;
b3AlignedObjectArray<int> m_segmentationMaskBuffer;
@@ -53,6 +54,7 @@ struct TinyRendererSetupInternalData
m_animateRenderer(0)
{
m_depthBuffer.resize(m_width*m_height);
m_shadowBuffer.resize(m_width*m_height);
// m_segmentationMaskBuffer.resize(m_width*m_height);
}
@@ -152,7 +154,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
const char* fileName = "textured_sphere_smooth.obj";
fileName = "cube.obj";
fileName = "torus/torus_with_plane.obj";
{
@@ -188,6 +190,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
TinyRenderObjectData* ob = new TinyRenderObjectData(
m_internalData->m_rgbColorBuffer,
m_internalData->m_depthBuffer,
&m_internalData->m_shadowBuffer,
&m_internalData->m_segmentationMaskBuffer,
m_internalData->m_renderObjects.size());
@@ -328,6 +331,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime)
{
m_internalData->m_rgbColorBuffer.set(x,y,clearColor);
m_internalData->m_depthBuffer[x+y*m_internalData->m_width] = -1e30f;
m_internalData->m_shadowBuffer[x+y*m_internalData->m_width] = -1e30f;
}
}
@@ -339,7 +343,44 @@ void TinyRendererSetup::stepSimulation(float deltaTime)
render->getActiveCamera()->getCameraViewMatrix(viewMat);
render->getActiveCamera()->getCameraProjectionMatrix(projMat);
for (int o=0;o<this->m_internalData->m_renderObjects.size();o++)
{
const btTransform& tr = m_internalData->m_transforms[o];
tr.getOpenGLMatrix(modelMat2);
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
{
m_internalData->m_renderObjects[o]->m_modelMatrix[i][j] = float(modelMat2[i+4*j]);
m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j];
m_internalData->m_renderObjects[o]->m_projectionMatrix[i][j] = projMat[i+4*j];
btVector3 lightDirWorld;
switch (m_app->getUpAxis())
{
case 1:
lightDirWorld = btVector3(-50.f,100,30);
break;
case 2:
lightDirWorld = btVector3(-50.f,30,100);
break;
default:{}
};
m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized();
btVector3 lightColor(1.0,1.0,1.0);
m_internalData->m_renderObjects[o]->m_lightColor = lightColor;
m_internalData->m_renderObjects[o]->m_lightDistance = 10.0;
}
}
TinyRenderer::renderObjectDepth(*m_internalData->m_renderObjects[o]);
}
for (int o=0;o<this->m_internalData->m_renderObjects.size();o++)
{
@@ -369,6 +410,11 @@ void TinyRendererSetup::stepSimulation(float deltaTime)
};
m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized();
btVector3 lightColor(1.0,1.0,1.0);
m_internalData->m_renderObjects[o]->m_lightColor = lightColor;
m_internalData->m_renderObjects[o]->m_lightDistance = 10.0;
}
}

535
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -317,6 +317,26 @@ int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHan
return 0;
}
int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_useSplitImpulse = useSplitImpulse;
command->m_updateFlags |= SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE;
return 0;
}
int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_splitImpulsePenetrationThreshold = splitImpulsePenetrationThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD;
return 0;
}
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -672,6 +692,40 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
return 0;
}
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1;
command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0];
command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1];
command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2];
return 0;
}
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1;
command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0];
command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1];
command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2];
return 0;
}
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -686,6 +740,8 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
return 0;
}
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1195,6 +1251,46 @@ int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)
return status->m_userDebugDrawArgs.m_debugItemUniqueId;
}
b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_SET_CUSTOM_OBJECT_COLOR;
command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
command->m_userDebugDrawArgs.m_objectDebugColorRGB[0] = objectColorRGB[0];
command->m_userDebugDrawArgs.m_objectDebugColorRGB[1] = objectColorRGB[1];
command->m_userDebugDrawArgs.m_objectDebugColorRGB[2] = objectColorRGB[2];
}
void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR;
command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
}
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
@@ -1232,209 +1328,273 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i = 0; i<3; i++)
{
command->m_requestPixelDataArguments.m_lightDirection[i] = lightDirection[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION;
}
void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i = 0; i<3; i++)
{
command->m_requestPixelDataArguments.m_lightColor[i] = lightColor[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR;
}
void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightDistance = lightDistance;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE;
}
void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_hasShadow = hasShadow;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SHADOW;
}
void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16])
{
b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
b3Vector3 f = (center - eye).normalized();
b3Vector3 u = up.normalized();
b3Vector3 s = (f.cross(u)).normalized();
u = s.cross(f);
viewMatrix[0 * 4 + 0] = s.x;
viewMatrix[1 * 4 + 0] = s.y;
viewMatrix[2 * 4 + 0] = s.z;
viewMatrix[0 * 4 + 1] = u.x;
viewMatrix[1 * 4 + 1] = u.y;
viewMatrix[2 * 4 + 1] = u.z;
viewMatrix[0 * 4 + 2] = -f.x;
viewMatrix[1 * 4 + 2] = -f.y;
viewMatrix[2 * 4 + 2] = -f.z;
viewMatrix[0 * 4 + 3] = 0.f;
viewMatrix[1 * 4 + 3] = 0.f;
viewMatrix[2 * 4 + 3] = 0.f;
viewMatrix[3 * 4 + 0] = -s.dot(eye);
viewMatrix[3 * 4 + 1] = -u.dot(eye);
viewMatrix[3 * 4 + 2] = f.dot(eye);
viewMatrix[3 * 4 + 3] = 1.f;
}
void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16])
{
b3Vector3 camUpVector;
b3Vector3 camForward;
b3Vector3 camPos;
b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 eyePos = b3MakeVector3(0, 0, 0);
int forwardAxis(-1);
{
switch (upAxis)
{
case 1:
{
forwardAxis = 0;
eyePos[forwardAxis] = -distance;
camForward = b3MakeVector3(eyePos[0], eyePos[1], eyePos[2]);
if (camForward.length2() < B3_EPSILON)
{
camForward.setValue(1.f, 0.f, 0.f);
}
else
{
camForward.normalize();
}
b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward, rollRad);
camUpVector = b3QuatRotate(rollRot, b3MakeVector3(0, 1, 0));
//gLightPos = b3MakeVector3(-50.f,100,30);
break;
}
case 2:
{
forwardAxis = 1;
eyePos[forwardAxis] = -distance;
camForward = b3MakeVector3(eyePos[0], eyePos[1], eyePos[2]);
if (camForward.length2() < B3_EPSILON)
{
camForward.setValue(1.f, 0.f, 0.f);
}
else
{
camForward.normalize();
}
b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward, rollRad);
camUpVector = b3QuatRotate(rollRot, b3MakeVector3(0, 0, 1));
//gLightPos = b3MakeVector3(-50.f,30,100);
break;
}
default:
{
//b3Assert(0);
return;
}
};
}
b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg
b3Quaternion pitchRot(camUpVector, pitchRad);
b3Vector3 right = camUpVector.cross(camForward);
b3Quaternion yawRot(right, -yawRad);
eyePos = b3Matrix3x3(pitchRot) * b3Matrix3x3(yawRot) * eyePos;
camPos = eyePos;
camPos += camTargetPos;
float camPosf[4] = { camPos[0],camPos[1],camPos[2],0 };
float camPosTargetf[4] = { camTargetPos[0],camTargetPos[1],camTargetPos[2],0 };
float camUpf[4] = { camUpVector[0],camUpVector[1],camUpVector[2],0 };
b3ComputeViewMatrixFromPositions(camPosf, camPosTargetf, camUpf,viewMatrix);
}
void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16])
{
projectionMatrix[0 * 4 + 0] = (float(2) * nearVal) / (right - left);
projectionMatrix[0 * 4 + 1] = float(0);
projectionMatrix[0 * 4 + 2] = float(0);
projectionMatrix[0 * 4 + 3] = float(0);
projectionMatrix[1 * 4 + 0] = float(0);
projectionMatrix[1 * 4 + 1] = (float(2) * nearVal) / (top - bottom);
projectionMatrix[1 * 4 + 2] = float(0);
projectionMatrix[1 * 4 + 3] = float(0);
projectionMatrix[2 * 4 + 0] = (right + left) / (right - left);
projectionMatrix[2 * 4 + 1] = (top + bottom) / (top - bottom);
projectionMatrix[2 * 4 + 2] = -(farVal + nearVal) / (farVal - nearVal);
projectionMatrix[2 * 4 + 3] = float(-1);
projectionMatrix[3 * 4 + 0] = float(0);
projectionMatrix[3 * 4 + 1] = float(0);
projectionMatrix[3 * 4 + 2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
projectionMatrix[3 * 4 + 3] = float(0);
}
void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16])
{
float yScale = 1.0 / tan((3.141592538 / 180.0) * fov / 2);
float xScale = yScale / aspect;
projectionMatrix[0 * 4 + 0] = xScale;
projectionMatrix[0 * 4 + 1] = float(0);
projectionMatrix[0 * 4 + 2] = float(0);
projectionMatrix[0 * 4 + 3] = float(0);
projectionMatrix[1 * 4 + 0] = float(0);
projectionMatrix[1 * 4 + 1] = yScale;
projectionMatrix[1 * 4 + 2] = float(0);
projectionMatrix[1 * 4 + 3] = float(0);
projectionMatrix[2 * 4 + 0] = 0;
projectionMatrix[2 * 4 + 1] = 0;
projectionMatrix[2 * 4 + 2] = (nearVal + farVal) / (nearVal - farVal);
projectionMatrix[2 * 4 + 3] = float(-1);
projectionMatrix[3 * 4 + 0] = float(0);
projectionMatrix[3 * 4 + 1] = float(0);
projectionMatrix[3 * 4 + 2] = (float(2) * farVal * nearVal) / (nearVal - farVal);
projectionMatrix[3 * 4 + 3] = float(0);
}
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3Vector3 camUpVector;
b3Vector3 camForward;
b3Vector3 camPos;
b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]);
b3Vector3 eyePos = b3MakeVector3(0,0,0);
int forwardAxis(-1);
{
switch (upAxis)
{
case 1:
{
forwardAxis = 0;
eyePos[forwardAxis] = -distance;
camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
if (camForward.length2() < B3_EPSILON)
{
camForward.setValue(1.f,0.f,0.f);
} else
{
camForward.normalize();
}
b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward,rollRad);
camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,1,0));
//gLightPos = b3MakeVector3(-50.f,100,30);
break;
}
case 2:
{
forwardAxis = 1;
eyePos[forwardAxis] = -distance;
camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
if (camForward.length2() < B3_EPSILON)
{
camForward.setValue(1.f,0.f,0.f);
} else
{
camForward.normalize();
}
b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward,rollRad);
camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,0,1));
//gLightPos = b3MakeVector3(-50.f,30,100);
break;
}
default:
{
//b3Assert(0);
return;
}
};
}
b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg
b3Quaternion pitchRot(camUpVector,pitchRad);
b3Vector3 right = camUpVector.cross(camForward);
b3Quaternion yawRot(right,-yawRad);
eyePos = b3Matrix3x3(pitchRot) * b3Matrix3x3(yawRot) * eyePos;
camPos = eyePos;
camPos += camTargetPos;
float camPosf[4] = {camPos[0],camPos[1],camPos[2],0};
float camPosTargetf[4] = {camTargetPos[0],camTargetPos[1],camTargetPos[2],0};
float camUpf[4] = {camUpVector[0],camUpVector[1],camUpVector[2],0};
b3RequestCameraImageSetViewMatrix(commandHandle,camPosf,camPosTargetf,camUpf);
b3ComputeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxis, command->m_requestPixelDataArguments.m_viewMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
{
b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
b3Vector3 f = (center - eye).normalized();
b3Vector3 u = up.normalized();
b3Vector3 s = (f.cross(u)).normalized();
u = s.cross(f);
float viewMatrix[16];
viewMatrix[0*4+0] = s.x;
viewMatrix[1*4+0] = s.y;
viewMatrix[2*4+0] = s.z;
viewMatrix[0*4+1] = u.x;
viewMatrix[1*4+1] = u.y;
viewMatrix[2*4+1] = u.z;
viewMatrix[0*4+2] =-f.x;
viewMatrix[1*4+2] =-f.y;
viewMatrix[2*4+2] =-f.z;
viewMatrix[0*4+3] = 0.f;
viewMatrix[1*4+3] = 0.f;
viewMatrix[2*4+3] = 0.f;
viewMatrix[3*4+0] = -s.dot(eye);
viewMatrix[3*4+1] = -u.dot(eye);
viewMatrix[3*4+2] = f.dot(eye);
viewMatrix[3*4+3] = 1.f;
float viewMatrix[16];
b3ComputeViewMatrixFromPositions(cameraPosition, cameraTargetPosition, cameraUp, viewMatrix);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
}
b3ComputeViewMatrixFromPositions(cameraPosition, cameraTargetPosition, cameraUp, command->m_requestPixelDataArguments.m_viewMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal)
{
float frustum[16];
frustum[0*4+0] = (float(2) * nearVal) / (right - left);
frustum[0*4+1] = float(0);
frustum[0*4+2] = float(0);
frustum[0*4+3] = float(0);
frustum[1*4+0] = float(0);
frustum[1*4+1] = (float(2) * nearVal) / (top - bottom);
frustum[1*4+2] = float(0);
frustum[1*4+3] = float(0);
frustum[2*4+0] = (right + left) / (right - left);
frustum[2*4+1] = (top + bottom) / (top - bottom);
frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal);
frustum[2*4+3] = float(-1);
frustum[3*4+0] = float(0);
frustum[3*4+1] = float(0);
frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
frustum[3*4+3] = float(0);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
}
b3ComputeProjectionMatrix(left, right, bottom, top, nearVal, farVal, command->m_requestPixelDataArguments.m_projectionMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal)
{
float yScale = 1.0 / tan((3.141592538 / 180.0) * fov / 2);
float xScale = yScale / aspect;
float frustum[16];
frustum[0*4+0] = xScale;
frustum[0*4+1] = float(0);
frustum[0*4+2] = float(0);
frustum[0*4+3] = float(0);
frustum[1*4+0] = float(0);
frustum[1*4+1] = yScale;
frustum[1*4+2] = float(0);
frustum[1*4+3] = float(0);
frustum[2*4+0] = 0;
frustum[2*4+1] = 0;
frustum[2*4+2] = (nearVal + farVal) / (nearVal - farVal);
frustum[2*4+3] = float(-1);
frustum[3*4+0] = float(0);
frustum[3*4+1] = float(0);
frustum[3*4+2] = (float(2) * farVal * nearVal) / (nearVal - farVal);
frustum[3*4+3] = float(0);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
}
b3ComputeProjectionMatrixFOV(fov, aspect, nearVal, farVal, command->m_requestPixelDataArguments.m_projectionMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
@@ -1469,6 +1629,8 @@ b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClient
command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
command->m_requestContactPointArguments.m_linkIndexAIndexFilter = -2;
command->m_requestContactPointArguments.m_linkIndexBIndexFilter = -2;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
@@ -1481,6 +1643,37 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body
command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
}
void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER;
command->m_requestContactPointArguments.m_linkIndexAIndexFilter= linkIndexA;
}
void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER;
command->m_requestContactPointArguments.m_linkIndexBIndexFilter = linkIndexB;
}
void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
{
b3SetContactFilterLinkA(commandHandle, linkIndexA);
}
void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
{
b3SetContactFilterLinkB(commandHandle, linkIndexB);
}
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

43
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -89,30 +89,59 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient);
void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]);
void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
///All debug items unique Ids are positive: a negative unique Id means failure.
int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis);
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3]);
void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]);
void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance);
void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices
void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]);
void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16]);
///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices
void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16]);
void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16]);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
///request an contact point information
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///compute the closest points between two bodies
b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
@@ -134,6 +163,9 @@ int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
//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
@@ -229,6 +261,9 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]);
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);

25
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -89,10 +89,10 @@ protected:
virtual void resetCamera()
{
float dist = 4;
float pitch = 193;
float yaw = 25;
float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
float dist = 4;
float pitch = 193;
float yaw = 25;
float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
@@ -481,6 +481,22 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
break;
}
case CMD_SET_SHADOW:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
float viewMatrix[16];
float projectionMatrix[16];
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
bool hasShadow = true;
b3RequestCameraImageSetShadow(commandHandle, hasShadow);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
@@ -556,6 +572,7 @@ void PhysicsClientExample::createButtons()
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
createButton("Save World",CMD_SAVE_WORLD, isTrigger);
createButton("Set Shadow",CMD_SET_SHADOW, isTrigger);
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Realtime Sim",CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger);

348
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -43,6 +43,7 @@ bool gCloseToKuka=false;
bool gEnableRealTimeSimVR=false;
bool gCreateDefaultRobotAssets = false;
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
int gHuskyId = -1;
btTransform huskyTr = btTransform::getIdentity();
@@ -542,7 +543,7 @@ struct PhysicsServerCommandProcessorInternalData
m_kukaGripperMultiBody(0),
m_kukaGripperRevolute1(0),
m_kukaGripperRevolute2(0),
m_allowRealTimeSimulation(true),
m_allowRealTimeSimulation(false),
m_huskyId(-1),
m_KukaId(-1),
m_sphereId(-1),
@@ -666,8 +667,6 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
}
@@ -716,8 +715,24 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
{
for (int i = 0; i < m_data->m_inverseKinematicsHelpers.size(); i++)
{
IKTrajectoryHelper** ikHelperPtr = m_data->m_inverseKinematicsHelpers.getAtIndex(i);
if (ikHelperPtr)
{
IKTrajectoryHelper* ikHelper = *ikHelperPtr;
delete ikHelper;
}
}
m_data->m_inverseKinematicsHelpers.clear();
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
{
for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++)
@@ -736,7 +751,7 @@ void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
{
deleteCachedInverseDynamicsBodies();
deleteCachedInverseKinematicsBodies();
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{
@@ -1416,6 +1431,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
// printf("-------------------------------\nRendering\n");
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0)
{
m_data->m_visualConverter.setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0)
{
m_data->m_visualConverter.setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0)
{
m_data->m_visualConverter.setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0)
{
m_data->m_visualConverter.setShadow(clientCmd.m_requestPixelDataArguments.m_hasShadow);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0)
{
@@ -2305,6 +2339,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE)
{
m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse;
}
if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD)
{
m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
{
m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
@@ -2330,9 +2375,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
btVector3 baseLinVel(0, 0, 0);
btVector3 baseAngVel(0, 0, 0);
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
{
baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
clientCmd.m_initPoseArgs.m_initialStateQdot[1],
clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
{
baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
clientCmd.m_initPoseArgs.m_initialStateQdot[4],
clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
}
btVector3 basePos(0, 0, 0);
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{
basePos = btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0],
clientCmd.m_initPoseArgs.m_initialStateQ[1],
clientCmd.m_initPoseArgs.m_initialStateQ[2]);
}
btQuaternion baseOrn(0, 0, 0, 1);
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
{
baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
clientCmd.m_initPoseArgs.m_initialStateQ[6]);
}
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
{
mb->setBaseVel(baseLinVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
{
mb->setBaseOmega(baseAngVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{
btVector3 zero(0,0,0);
@@ -2340,11 +2430,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
mb->setBaseVel(zero);
mb->setBasePos(btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0],
clientCmd.m_initPoseArgs.m_initialStateQ[1],
clientCmd.m_initPoseArgs.m_initialStateQ[2]));
mb->setBaseVel(baseLinVel);
mb->setBasePos(basePos);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
{
@@ -2353,11 +2440,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
mb->setBaseOmega(btVector3(0,0,0));
btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
clientCmd.m_initPoseArgs.m_initialStateQ[6]);
mb->setBaseOmega(baseAngVel);
btQuaternion invOrn(baseOrn);
mb->setWorldToBaseRot(invOrn.inverse());
}
@@ -2386,6 +2470,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
}
if (body && body->m_rigidBody)
{
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
{
body->m_rigidBody->setLinearVelocity(baseLinVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
{
body->m_rigidBody->setAngularVelocity(baseAngVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{
body->m_rigidBody->getWorldTransform().setOrigin(basePos);
body->m_rigidBody->setLinearVelocity(baseLinVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
{
body->m_rigidBody->getWorldTransform().setRotation(baseOrn);
body->m_rigidBody->setAngularVelocity(baseAngVel);
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2398,29 +2507,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_RESET_SIMULATION:
{
//clean up all data
deleteCachedInverseDynamicsBodies();
if (m_data && m_data->m_guiHelper)
{
m_data->m_guiHelper->removeAllGraphicsInstances();
}
if (m_data)
{
m_data->m_visualConverter.resetAll();
}
deleteDynamicsWorld();
createEmptyDynamicsWorld();
m_data->exitHandles();
m_data->initHandles();
resetSimulation();
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true;
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
break;
}
case CMD_CREATE_RIGID_BODY:
@@ -2765,6 +2857,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter;
int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter;
bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER));
bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER));
int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter;
int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter;
btAlignedObjectArray<btCollisionObject*> setA;
btAlignedObjectArray<btCollisionObject*> setB;
btAlignedObjectArray<int> setALinkIndex;
@@ -2779,15 +2877,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (bodyA->m_multiBody->getBaseCollider())
{
setA.push_back(bodyA->m_multiBody->getBaseCollider());
setALinkIndex.push_back(-1);
if (!hasLinkIndexAFilter || (linkIndexA == -1))
{
setA.push_back(bodyA->m_multiBody->getBaseCollider());
setALinkIndex.push_back(-1);
}
}
for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++)
{
if (bodyA->m_multiBody->getLink(i).m_collider)
{
setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
setALinkIndex.push_back(i);
if (!hasLinkIndexAFilter || (linkIndexA == i))
{
setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
setALinkIndex.push_back(i);
}
}
}
}
@@ -2807,15 +2911,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (bodyB->m_multiBody->getBaseCollider())
{
setB.push_back(bodyB->m_multiBody->getBaseCollider());
setBLinkIndex.push_back(-1);
if (!hasLinkIndexBFilter || (linkIndexB == -1))
{
setB.push_back(bodyB->m_multiBody->getBaseCollider());
setBLinkIndex.push_back(-1);
}
}
for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++)
{
if (bodyB->m_multiBody->getLink(i).m_collider)
{
setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
setBLinkIndex.push_back(i);
if (!hasLinkIndexBFilter || (linkIndexB ==i))
{
setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
setBLinkIndex.push_back(i);
}
}
}
}
@@ -3058,11 +3168,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
{
InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
@@ -3110,6 +3221,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
if (body && body->m_rigidBody)
{
btRigidBody* rb = body->m_rigidBody;
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 positionLocal(
clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);
btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis()*forceLocal;
btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis()*positionLocal;
rb->applyForce(forceWorld, relPosWorld);
}
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0)
{
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis()*torqueLocal;
rb->applyTorque(torqueWorld);
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -3509,7 +3650,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for( int i=0;i<numRb;i++)
{
btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
if (colObj)
{
btRigidBody* rb = btRigidBody::upcast(colObj);
@@ -3572,7 +3712,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
hasStatus = true;
if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR))
{
int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body)
{
btCollisionObject* destColObj = 0;
if (body->m_multiBody)
{
if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1)
{
destColObj = body->m_multiBody->getBaseCollider();
}
else
{
if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks())
{
destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider;
}
}
}
if (body->m_rigidBody)
{
destColObj = body->m_rigidBody;
}
if (destColObj)
{
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)
{
destColObj->removeCustomDebugColor();
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR)
{
btVector3 objectColorRGB;
objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0],
clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1],
clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]);
destColObj->setCustomDebugColor(objectColorRGB);
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
}
}
}
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT)
{
@@ -3829,7 +4016,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
btVector3 gVRGripperPos(0,0,0.2);
btVector3 gVRGripperPos(0.6, 0.4, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
@@ -3845,6 +4032,12 @@ double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (gResetSimulation)
{
resetSimulation();
gResetSimulation = false;
}
if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
@@ -3902,7 +4095,30 @@ void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
}
}
void PhysicsServerCommandProcessor::resetSimulation()
{
//clean up all data
deleteCachedInverseDynamicsBodies();
if (m_data && m_data->m_guiHelper)
{
m_data->m_guiHelper->removeAllGraphicsInstances();
}
if (m_data)
{
m_data->m_visualConverter.resetAll();
}
deleteDynamicsWorld();
createEmptyDynamicsWorld();
m_data->exitHandles();
m_data->initHandles();
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
}
//todo: move this to Python/scripting
void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
@@ -3933,14 +4149,14 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
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("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, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
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->getHandle(bodyId);
if (parentBody->m_multiBody)
@@ -3970,6 +4186,30 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
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;
InteralBodyData* kukaBody = m_data->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);
}
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());
@@ -3978,7 +4218,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
@@ -4020,6 +4260,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
gripperBody->m_multiBody->setHasSelfCollision(0);
@@ -4046,7 +4287,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
btTransform objectLocalTr[] = {
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.15, 0.64)),
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)),
@@ -4073,7 +4314,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Table area
loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].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());
@@ -4114,9 +4355,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (motor)
{
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
motor->setPositionTarget(posTarget, .2);
motor->setPositionTarget(posTarget, .8);
motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(5.0);
motor->setMaxAppliedImpulse(1.0);
}
}

3
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -23,6 +23,8 @@ class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
//todo: move this to physics client side / Python
void createDefaultRobotAssets();
void resetSimulation();
protected:
@@ -37,6 +39,7 @@ protected:
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
void deleteCachedInverseDynamicsBodies();
void deleteCachedInverseKinematicsBodies();
public:
PhysicsServerCommandProcessor();

160
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -27,6 +27,7 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos1(0,0,0);
btScalar gVRTeleportRotZ = 0;
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
@@ -39,16 +40,54 @@ extern bool gEnableRealTimeSimVR;
extern bool gCreateDefaultRobotAssets;
extern int gInternalSimFlags;
extern int gCreateObjectSimVR;
extern bool gResetSimulation;
extern int gEnableKukaControl;
int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed;
#if B3_USE_MIDI
const char* startFileNameVR = "0_VRDemoSettings.txt";
#include <vector>
//remember the settings (you don't want to re-tune again and again...)
static void saveCurrentSettingsVR()
{
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, "--camRotZ= %f\n", gVRTeleportRotZ);
fclose(f);
}
};
static void loadCurrentSettingsVR(b3CommandLineArgs& args)
{
int currentEntry = 0;
FILE* f = fopen(startFileNameVR, "r");
if (f)
{
char oneline[1024];
char* argv[] = { 0,&oneline[0] };
while (fgets(oneline, 1024, f) != NULL)
{
char *pos;
if ((pos = strchr(oneline, '\n')) != NULL)
*pos = '\0';
args.addArgs(2, argv);
}
fclose(f);
}
};
#if B3_USE_MIDI
static float getParamf(float rangeMin, float rangeMax, int midiVal)
{
float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
@@ -69,9 +108,10 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
{
if (message->at(1) == 16)
{
float rotZ = getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), rotZ);
b3Printf("gVRTeleportOrn rotZ = %f\n", rotZ);
gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
}
if (message->at(1) == 32)
@@ -84,6 +124,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
if (message->at(1) == i)
{
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
saveCurrentSettingsVR();
b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]);
}
@@ -273,7 +314,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_physicsServerPtr->removePickingConstraint();
}
if (!gEnableKukaControl)
// if (!gEnableKukaControl)
{
if (args->m_isVrControllerPicking[c])
{
@@ -825,6 +866,7 @@ public:
virtual bool wantsTermination();
virtual bool isConnected();
virtual void renderScene();
void drawUserDebugLines();
virtual void exitPhysics();
virtual void physicsDebugDraw(int debugFlags);
@@ -925,7 +967,7 @@ public:
virtual void processCommandLineArgs(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
loadCurrentSettingsVR(args);
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
{
printf("camPosX=%f\n", gVRTeleportPos1[0]);
@@ -1353,7 +1395,51 @@ extern int gHuskyId;
extern btTransform huskyTr;
void PhysicsServerExample::drawUserDebugLines()
{
static char line0[1024];
static char line1[1024];
//draw all user-debug-lines
//add array of lines
//draw all user- 'text3d' messages
if (m_multiThreadedHelper)
{
for (int i = 0; i<m_multiThreadedHelper->m_userDebugLines.size(); i++)
{
btVector3 from;
from.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
btVector3 toX;
toX.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
btVector3 color;
color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
}
for (int i = 0; i<m_multiThreadedHelper->m_userDebugText.size(); i++)
{
m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
m_multiThreadedHelper->m_userDebugText[i].textSize);
}
}
}
void PhysicsServerExample::renderScene()
{
@@ -1369,48 +1455,8 @@ void PhysicsServerExample::renderScene()
B3_PROFILE("PhysicsServerExample::RenderScene");
static char line0[1024];
static char line1[1024];
//draw all user-debug-lines
//add array of lines
//draw all user- 'text3d' messages
if (m_multiThreadedHelper)
{
for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
{
btVector3 from;
from.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
btVector3 toX;
toX.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
btVector3 color;
color.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
}
for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
{
m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
m_multiThreadedHelper->m_userDebugText[i].textSize);
}
}
drawUserDebugLines();
if (gEnableRealTimeSimVR)
{
@@ -1424,6 +1470,7 @@ void PhysicsServerExample::renderScene()
static int count = 0;
count++;
#if 0
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
@@ -1444,6 +1491,7 @@ void PhysicsServerExample::renderScene()
worseFps = 1000000;
}
}
#endif
#ifdef BT_ENABLE_VR
if ((gInternalSimFlags&2 ) && m_tinyVrGui==0)
@@ -1468,7 +1516,9 @@ void PhysicsServerExample::renderScene()
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);
@@ -1544,6 +1594,8 @@ void PhysicsServerExample::renderScene()
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
drawUserDebugLines();
///debug rendering
m_physicsServer.physicsDebugDraw(debugDrawFlags);
@@ -1653,13 +1705,16 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
return;
if (gGraspingController < 0)
{
gGraspingController = controllerId;
gEnableKukaControl = true;
}
if (controllerId != gGraspingController)
{
if (button == 1 && state == 0)
{
// gVRTeleportPos = gLastPickPos;
gResetSimulation = true;
//gVRTeleportPos1 = gLastPickPos;
}
} else
{
@@ -1671,7 +1726,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
} else
{
gDebugRenderToggle = 0;
#if 0//it confuses people, make it into a debug option in a VR GUI?
if (simTimeScalingFactor==0)
{
simTimeScalingFactor = 1;
@@ -1686,6 +1741,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
simTimeScalingFactor = 0;
}
}
#endif
}
} else
{
@@ -1703,7 +1759,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
}
else
{
gEnableKukaControl = !gEnableKukaControl;
// gEnableKukaControl = !gEnableKukaControl;
}
}

35
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -109,7 +109,9 @@ enum EnumInitPoseFlags
{
INIT_POSE_HAS_INITIAL_POSITION=1,
INIT_POSE_HAS_INITIAL_ORIENTATION=2,
INIT_POSE_HAS_JOINT_STATE=4
INIT_POSE_HAS_JOINT_STATE=4,
INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8,
INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16,
};
@@ -122,6 +124,8 @@ struct InitPoseArgs
int m_bodyUniqueId;
int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM];
};
@@ -138,12 +142,20 @@ struct RequestPixelDataArgs
int m_startPixelIndex;
int m_pixelWidth;
int m_pixelHeight;
float m_lightDirection[3];
float m_lightColor[3];
float m_lightDistance;
int m_hasShadow;
};
enum EnumRequestPixelDataUpdateFlags
{
REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=2,
REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION=4,
REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR=8,
REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE=16,
REQUEST_PIXEL_ARGS_SET_SHADOW=32,
//don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h
};
@@ -152,6 +164,8 @@ enum EnumRequestContactDataUpdateFlags
{
CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE=1,
CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2,
CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER = 4,
CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8,
};
struct RequestContactDataArgs
@@ -159,6 +173,8 @@ struct RequestContactDataArgs
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
int m_linkIndexAIndexFilter;
int m_linkIndexBIndexFilter;
double m_closestDistanceThreshold;
int m_mode;
};
@@ -271,7 +287,9 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32,
SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64
SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64,
SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE=128,
SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256,
};
enum EnumLoadBunnyUpdateFlags
@@ -296,6 +314,8 @@ struct SendPhysicsSimulationParameters
int m_numSimulationSubSteps;
int m_numSolverIterations;
bool m_allowRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;
int m_internalSimFlags;
double m_defaultContactERP;
};
@@ -518,7 +538,10 @@ enum EnumUserDebugDrawFlags
USER_DEBUG_HAS_LINE=1,
USER_DEBUG_HAS_TEXT=2,
USER_DEBUG_REMOVE_ONE_ITEM=4,
USER_DEBUG_REMOVE_ALL=8
USER_DEBUG_REMOVE_ALL=8,
USER_DEBUG_SET_CUSTOM_OBJECT_COLOR = 16,
USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32,
};
struct UserDebugDrawArgs
@@ -535,6 +558,10 @@ struct UserDebugDrawArgs
double m_textPositionXYZ[3];
double m_textColorRGB[3];
double m_textSize;
double m_objectDebugColorRGB[3];
int m_objectUniqueId;
int m_linkIndex;
};

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -40,6 +40,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_VISUAL_SHAPE_INFO,
CMD_UPDATE_VISUAL_SHAPE,
CMD_LOAD_TEXTURE,
CMD_SET_SHADOW,
CMD_USER_DEBUG_DRAW,
//don't go beyond this command!

145
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -36,7 +36,6 @@ subject to the following restrictions:
#include "../TinyRenderer/model.h"
#include "../ThirdPartyLibs/stb_image/stb_image.h"
enum MyFileType
{
MY_FILE_STL=1,
@@ -72,17 +71,29 @@ struct TinyRendererVisualShapeConverterInternalData
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<MyTexture2> m_textures;
b3AlignedObjectArray<float> m_depthBuffer;
b3AlignedObjectArray<float> m_shadowBuffer;
b3AlignedObjectArray<int> m_segmentationMaskBuffer;
btVector3 m_lightDirection;
bool m_hasLightDirection;
btVector3 m_lightColor;
bool m_hasLightColor;
float m_lightDistance;
bool m_hasLightDistance;
bool m_hasShadow;
SimpleCamera m_camera;
TinyRendererVisualShapeConverterInternalData()
:m_upAxis(2),
m_swWidth(START_WIDTH),
m_swHeight(START_HEIGHT),
m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB)
m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB),
m_hasLightDirection(false),
m_hasLightColor(false),
m_hasShadow(false)
{
m_depthBuffer.resize(m_swWidth*m_swHeight);
m_shadowBuffer.resize(m_swWidth*m_swHeight);
m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1);
}
@@ -108,8 +119,28 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()
delete m_data;
}
void TinyRendererVisualShapeConverter::setLightDirection(float x, float y, float z)
{
m_data->m_lightDirection.setValue(x, y, z);
m_data->m_hasLightDirection = true;
}
void TinyRendererVisualShapeConverter::setLightColor(float x, float y, float z)
{
m_data->m_lightColor.setValue(x, y, z);
m_data->m_hasLightColor = true;
}
void TinyRendererVisualShapeConverter::setLightDistance(float dist)
{
m_data->m_lightDistance = dist;
m_data->m_hasLightDistance = true;
}
void TinyRendererVisualShapeConverter::setShadow(bool hasShadow)
{
m_data->m_hasShadow = hasShadow;
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
{
@@ -537,7 +568,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
unsigned char* textureImage=0;
int textureWidth=0;
int textureHeight=0;
@@ -643,6 +674,7 @@ void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor)
{
m_data->m_rgbColorBuffer.set(x,y,clearColor);
m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f;
m_data->m_shadowBuffer[x+y*m_data->m_swWidth] = -1e30f;
m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1;
}
}
@@ -677,28 +709,89 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
btVector3 lightDirWorld(-5,200,-40);
switch (m_data->m_upAxis)
{
case 1:
lightDirWorld = btVector3(-50.f,100,30);
break;
case 2:
lightDirWorld = btVector3(-50.f,30,100);
break;
default:{}
};
if (m_data->m_hasLightDirection)
{
lightDirWorld = m_data->m_lightDirection;
}
else
{
switch (m_data->m_upAxis)
{
case 1:
lightDirWorld = btVector3(-50.f, 100, 30);
break;
case 2:
lightDirWorld = btVector3(-50.f, 30, 100);
break;
default: {}
};
}
lightDirWorld.normalize();
// printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
for (int i=0;i<m_data->m_swRenderInstances.size();i++)
btVector3 lightColor(1.0,1.0,1.0);
if (m_data->m_hasLightColor)
{
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
lightColor = m_data->m_lightColor;
}
float lightDistance = 2.0;
if (m_data->m_hasLightDistance)
{
lightDistance = m_data->m_hasLightDistance;
}
if (m_data->m_hasShadow)
{
for (int n=0;n<m_data->m_swRenderInstances.size();n++)
{
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(n);
if (0==visualArrayPtr)
continue;//can this ever happen?
TinyRendererObjectArray* visualArray = *visualArrayPtr;
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(n);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
for (int v=0;v<visualArray->m_renderObjects.size();v++)
{
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
//sync the object transform
const btTransform& tr = colObj->getWorldTransform();
tr.getOpenGLMatrix(modelMat);
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
{
renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
}
}
renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
renderObj->m_lightDirWorld = lightDirWorld;
renderObj->m_lightColor = lightColor;
renderObj->m_lightDistance = lightDistance;
TinyRenderer::renderObjectDepth(*renderObj);
}
}
}
for (int n=0;n<m_data->m_swRenderInstances.size();n++)
{
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(n);
if (0==visualArrayPtr)
continue;//can this ever happen?
TinyRendererObjectArray* visualArray = *visualArrayPtr;
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(i);
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(n);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
@@ -708,11 +801,11 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
//sync the object transform
const btTransform& tr = colObj->getWorldTransform();
tr.getOpenGLMatrix(modelMat);
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
@@ -721,10 +814,12 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
renderObj->m_lightDirWorld = lightDirWorld;
}
}
renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
renderObj->m_lightDirWorld = lightDirWorld;
renderObj->m_lightColor = lightColor;
renderObj->m_lightDistance = lightDistance;
TinyRenderer::renderObject(*renderObj);
}
}
@@ -743,6 +838,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
for (int i=0;i<m_data->m_swWidth;i++)
{
btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]);
btSwap(m_data->m_shadowBuffer[l1+i],m_data->m_shadowBuffer[l2+i]);
btSwap(m_data->m_segmentationMaskBuffer[l1+i],m_data->m_segmentationMaskBuffer[l2+i]);
}
}
@@ -762,6 +858,7 @@ void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
m_data->m_swHeight = height;
m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_shadowBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_segmentationMaskBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
@@ -886,4 +983,4 @@ int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
return registerTexture(image, width, height);
}
return -1;
}
}

6
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -32,7 +32,11 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
void getWidthAndHeight(int& width, int& height);
void setWidthAndHeight(int width, int height);
void setLightDirection(float x, float y, float z);
void setLightColor(float x, float y, float z);
void setLightDistance(float dist);
void setShadow(bool hasShadow);
void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
void render();

2
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -855,7 +855,7 @@ void CMainApplication::RenderFrame()
// SwapWindow
{
B3_PROFILE("m_app->swapBuffer");
// m_app->swapBuffer();
m_app->swapBuffer();
//SDL_GL_SwapWindow( m_pWindow );
}

244
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -13,81 +13,191 @@
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
struct DepthShader : public IShader {
Model* m_model;
Matrix& m_modelMat;
Matrix m_invModelMat;
Matrix& m_projectionMat;
Vec3f m_localScaling;
Matrix& m_lightModelView;
float m_lightDistance;
mat<2,3,float> varying_uv; // triangle uv coordinates, written by the vertex shader, read by the fragment shader
mat<4,3,float> varying_tri; // triangle coordinates (clip coordinates), written by VS, read by FS
mat<3,3,float> varying_nrm; // normal per vertex to be interpolated by FS
DepthShader(Model* model, Matrix& lightModelView, Matrix& projectionMat, Matrix& modelMat, Vec3f localScaling, float lightDistance)
:m_model(model),
m_lightModelView(lightModelView),
m_projectionMat(projectionMat),
m_modelMat(modelMat),
m_localScaling(localScaling),
m_lightDistance(lightDistance)
{
m_invModelMat = m_modelMat.invert_transpose();
}
virtual Vec4f vertex(int iface, int nthvert) {
Vec2f uv = m_model->uv(iface, nthvert);
varying_uv.set_col(nthvert, uv);
varying_nrm.set_col(nthvert, proj<3>(m_invModelMat*embed<4>(m_model->normal(iface, nthvert), 0.f)));
Vec3f unScaledVert = m_model->vert(iface, nthvert);
Vec3f scaledVert=Vec3f(unScaledVert[0]*m_localScaling[0],
unScaledVert[1]*m_localScaling[1],
unScaledVert[2]*m_localScaling[2]);
Vec4f gl_Vertex = m_projectionMat*m_lightModelView*embed<4>(scaledVert);
varying_tri.set_col(nthvert, gl_Vertex);
return gl_Vertex;
}
virtual bool fragment(Vec3f bar, TGAColor &color) {
Vec4f p = varying_tri*bar;
color = TGAColor(255, 255, 255)*(p[2]/m_lightDistance);
return false;
}
};
struct Shader : public IShader {
Model* m_model;
Vec3f m_light_dir_local;
Vec3f m_light_color;
Matrix& m_modelMat;
Matrix m_invModelMat;
Matrix m_invModelMat;
Matrix& m_modelView1;
Matrix& m_projectionMatrix;
Matrix& m_projectionMat;
Vec3f m_localScaling;
Matrix& m_lightModelView;
Vec4f m_colorRGBA;
Matrix& m_viewportMat;
b3AlignedObjectArray<float>* m_shadowBuffer;
int m_width;
int m_height;
int m_index;
mat<2,3,float> varying_uv; // triangle uv coordinates, written by the vertex shader, read by the fragment shader
mat<4,3,float> varying_tri; // triangle coordinates (clip coordinates), written by VS, read by FS
mat<4,3,float> varying_tri_light_view;
mat<3,3,float> varying_nrm; // normal per vertex to be interpolated by FS
//mat<3,3,float> ndc_tri; // triangle in normalized device coordinates
Shader(Model* model, Vec3f light_dir_local, Matrix& modelView, Matrix& projectionMatrix, Matrix& modelMat, Vec3f localScaling, const Vec4f& colorRGBA)
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)
:m_model(model),
m_light_dir_local(light_dir_local),
m_light_color(light_color),
m_modelView1(modelView),
m_projectionMatrix(projectionMatrix),
m_lightModelView(lightModelView),
m_projectionMat(projectionMat),
m_modelMat(modelMat),
m_localScaling(localScaling),
m_colorRGBA(colorRGBA)
m_viewportMat(viewportMat),
m_localScaling(localScaling),
m_colorRGBA(colorRGBA),
m_width(width),
m_height(height),
m_shadowBuffer(shadowBuffer)
{
m_invModelMat = m_modelMat.invert_transpose();
}
virtual Vec4f vertex(int iface, int nthvert) {
Vec2f uv = m_model->uv(iface, nthvert);
varying_uv.set_col(nthvert, uv);
//varying_nrm.set_col(nthvert, proj<3>((m_projectionMatrix*m_modelView).invert_transpose()*embed<4>(m_model->normal(iface, nthvert), 0.f)));
varying_nrm.set_col(nthvert, proj<3>(m_invModelMat*embed<4>(m_model->normal(iface, nthvert), 0.f)));
//m_localNormal = m_model->normal(iface, nthvert);
//varying_nrm.set_col(nthvert, m_model->normal(iface, nthvert));
Vec3f unScaledVert = m_model->vert(iface, nthvert);
Vec3f scaledVert=Vec3f(unScaledVert[0]*m_localScaling[0],
unScaledVert[1]*m_localScaling[1],
unScaledVert[2]*m_localScaling[2]);
Vec4f gl_Vertex = m_projectionMatrix*m_modelView1*embed<4>(scaledVert);
Vec4f gl_Vertex = m_projectionMat*m_modelView1*embed<4>(scaledVert);
varying_tri.set_col(nthvert, gl_Vertex);
Vec4f gl_VertexLightView = m_projectionMat*m_lightModelView*embed<4>(scaledVert);
varying_tri_light_view.set_col(nthvert, gl_VertexLightView);
return gl_Vertex;
}
virtual bool fragment(Vec3f bar, TGAColor &color) {
Vec4f p = m_viewportMat*(varying_tri_light_view*bar);
float depth = p[2];
p = p/p[3];
int index_x = b3Max(0, b3Min(m_width-1, int(p[0])));
int index_y = b3Max(0, b3Min(m_height-1, int(p[1])));
int idx = index_x + index_y*m_width; // index in the shadowbuffer array
float shadow = 0.8+0.2*(m_shadowBuffer->at(idx)<-depth+0.2); // magic coeff to avoid z-fighting
Vec3f bn = (varying_nrm*bar).normalize();
Vec2f uv = varying_uv*bar;
Vec3f reflection_direction = (bn * (bn * m_light_dir_local * 2.f) - m_light_dir_local).normalize();
float specular = pow(b3Max(reflection_direction.z, 0.f), m_model->specular(uv));
float diffuse = b3Max(0.f, bn * m_light_dir_local);
float ambient_coefficient = 0.6;
float ambient_coefficient = 0.6;
float diffuse_coefficient = 0.35;
float specular_coefficient = 0.05;
float intensity = ambient_coefficient + b3Min(diffuse * diffuse_coefficient + specular * specular_coefficient, 1.0f - ambient_coefficient);
color = m_model->diffuse(uv) * intensity;
color = m_model->diffuse(uv) * intensity * shadow;
//warning: bgra color is swapped to rgba to upload texture
color.bgra[0] *= m_colorRGBA[0];
color.bgra[1] *= m_colorRGBA[1];
color.bgra[2] *= m_colorRGBA[2];
color.bgra[3] *= m_colorRGBA[3];
color.bgra[0] *= m_light_color[0];
color.bgra[1] *= m_light_color[1];
color.bgra[2] *= m_light_color[2];
return false;
}
};
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_shadowBuffer(shadowBuffer),
m_segmentationMaskBufferPtr(0),
m_model(0),
m_userData(0),
m_userIndex(-1),
m_objectIndex(-1)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_shadowBuffer(shadowBuffer),
m_segmentationMaskBufferPtr(segmentationMaskBuffer),
m_model(0),
m_userData(0),
m_userIndex(-1),
m_objectIndex(objectIndex)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
@@ -101,13 +211,12 @@ m_objectIndex(-1)
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
@@ -121,6 +230,7 @@ m_objectIndex(objectIndex)
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
@@ -254,42 +364,80 @@ TinyRenderObjectData::~TinyRenderObjectData()
delete m_model;
}
void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
void TinyRenderer::renderObjectDepth(TinyRenderObjectData& renderData)
{
int width = renderData.m_rgbColorBuffer.get_width();
int height = renderData.m_rgbColorBuffer.get_height();
Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
int width = renderData.m_rgbColorBuffer.get_width();
int height = renderData.m_rgbColorBuffer.get_height();
Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
float light_distance = renderData.m_lightDistance;
Model* model = renderData.m_model;
if (0==model)
return;
renderData.m_viewportMatrix = viewport(0,0,width, height);
float* shadowBufferPtr = (renderData.m_shadowBuffer && renderData.m_shadowBuffer->size())?&renderData.m_shadowBuffer->at(0):0;
int* segmentationMaskBufferPtr = 0;
TGAImage depthFrame(width, height, TGAImage::RGB);
{
// light target is set to be the origin, and the up direction is set to be vertical up.
Matrix lightViewMatrix = lookat(light_dir_local*light_distance, Vec3f(0.0,0.0,0.0), Vec3f(0.0,0.0,1.0));
Matrix lightModelViewMatrix = lightViewMatrix*renderData.m_modelMatrix;
Matrix lightViewProjectionMatrix = renderData.m_projectionMatrix;
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++) {
shader.vertex(i, j);
}
triangle(shader.varying_tri, shader, depthFrame, shadowBufferPtr, segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
}
}
}
renderData.m_viewportMatrix = viewport(0,0,width, height);
void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
{
int width = renderData.m_rgbColorBuffer.get_width();
int height = renderData.m_rgbColorBuffer.get_height();
Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
Vec3f light_color = Vec3f(renderData.m_lightColor[0],renderData.m_lightColor[1],renderData.m_lightColor[2]);
float light_distance = renderData.m_lightDistance;
Model* model = renderData.m_model;
if (0==model)
return;
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
b3AlignedObjectArray<float>* shadowBufferPtr = renderData.m_shadowBuffer;
int* segmentationMaskBufferPtr = (renderData.m_segmentationMaskBufferPtr && renderData.m_segmentationMaskBufferPtr->size())?&renderData.m_segmentationMaskBufferPtr->at(0):0;
TGAImage& frame = renderData.m_rgbColorBuffer;
{
// light target is set to be the origin, and the up direction is set to be vertical up.
Matrix lightViewMatrix = lookat(light_dir_local*light_distance, Vec3f(0.0,0.0,0.0), Vec3f(0.0,0.0,1.0));
Matrix lightModelViewMatrix = lightViewMatrix*renderData.m_modelMatrix;
Matrix modelViewMatrix = renderData.m_viewMatrix*renderData.m_modelMatrix;
Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA());
//printf("Render %d triangles.\n",model->nfaces());
for (int i=0; i<model->nfaces(); i++)
{
Shader shader(model, light_dir_local, light_color, modelViewMatrix, lightModelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, renderData.m_viewportMatrix, localScaling, model->getColorRGBA(), width, height, shadowBufferPtr);
for (int i=0; i<model->nfaces(); i++)
{
for (int j=0; j<3; j++) {
shader.vertex(i, j);
}
triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
}
}
}

10
examples/TinyRenderer/TinyRenderer.h
View File

@@ -18,6 +18,8 @@ struct TinyRenderObjectData
Matrix m_viewportMatrix;
btVector3 m_localScaling;
btVector3 m_lightDirWorld;
btVector3 m_lightColor;
float m_lightDistance;
//Model (vertices, indices, textures, shader)
Matrix m_modelMatrix;
@@ -28,10 +30,13 @@ struct TinyRenderObjectData
TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;//required, hence a reference
b3AlignedObjectArray<float>* m_shadowBuffer;//optional, hence a pointer
b3AlignedObjectArray<int>* m_segmentationMaskBufferPtr;//optional, hence a pointer
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);
@@ -50,6 +55,7 @@ struct TinyRenderObjectData
class TinyRenderer
{
public:
static void renderObjectDepth(TinyRenderObjectData& renderData);
static void renderObject(TinyRenderObjectData& renderData);
};

55
examples/TinyRenderer/our_gl.cpp
View File

@@ -4,9 +4,6 @@
#include "our_gl.h"
#include "Bullet3Common/b3MinMax.h"
IShader::~IShader() {}
Matrix viewport(int x, int y, int w, int h)
@@ -15,10 +12,10 @@ Matrix viewport(int x, int y, int w, int h)
Viewport = Matrix::identity();
Viewport[0][3] = x+w/2.f;
Viewport[1][3] = y+h/2.f;
Viewport[2][3] = 1.f;
Viewport[2][3] = .5f;
Viewport[0][0] = w/2.f;
Viewport[1][1] = h/2.f;
Viewport[2][2] = 0;
Viewport[2][2] = .5f;
return Viewport;
}
@@ -30,19 +27,33 @@ Matrix projection(float coeff) {
}
Matrix lookat(Vec3f eye, Vec3f center, Vec3f up) {
Vec3f z = (eye-center).normalize();
Vec3f x = cross(up,z).normalize();
Vec3f y = cross(z,x).normalize();
Matrix Minv = Matrix::identity();
Matrix Tr = Matrix::identity();
for (int i=0; i<3; i++) {
Minv[0][i] = x[i];
Minv[1][i] = y[i];
Minv[2][i] = z[i];
Tr[i][3] = -center[i];
}
Vec3f f = (center - eye).normalize();
Vec3f u = up.normalize();
Vec3f s = cross(f,u).normalize();
u = cross(s,f);
Matrix ModelView;
ModelView = Minv*Tr;
ModelView[0][0] = s.x;
ModelView[0][1] = s.y;
ModelView[0][2] = s.z;
ModelView[1][0] = u.x;
ModelView[1][1] = u.y;
ModelView[1][2] = u.z;
ModelView[2][0] =-f.x;
ModelView[2][1] =-f.y;
ModelView[2][2] =-f.z;
ModelView[3][0] = 0.f;
ModelView[3][1] = 0.f;
ModelView[3][2] = 0.f;
ModelView[0][3] = -(s[0]*eye[0]+s[1]*eye[1]+s[2]*eye[2]);
ModelView[1][3] = -(u[0]*eye[0]+u[1]*eye[1]+u[2]*eye[2]);
ModelView[2][3] = f[0]*eye[0]+f[1]*eye[1]+f[2]*eye[2];
ModelView[3][3] = 1.f;
return ModelView;
}
@@ -66,13 +77,11 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) {
mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
//we don't clip triangles that cross the near plane, just discard them instead of showing artifacts
if (pts[0][3]<0 || pts[1][3] <0 || pts[2][3] <0)
return;
mat<3,2,float> pts2;
for (int i=0; i<3; i++) pts2[i] = proj<2>(pts[i]/pts[i][3]);
@@ -87,19 +96,15 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
}
}
Vec2i P;
TGAColor color;
for (P.x=bboxmin.x; P.x<=bboxmax.x; P.x++) {
for (P.y=bboxmin.y; P.y<=bboxmax.y; P.y++) {
Vec3f bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P);
Vec3f bc_clip = Vec3f(bc_screen.x/pts[0][3], bc_screen.y/pts[1][3], bc_screen.z/pts[2][3]);
bc_clip = bc_clip/(bc_clip.x+bc_clip.y+bc_clip.z);
float frag_depth = -1*(clipc[2]*bc_clip);
if (bc_screen.x<0 || bc_screen.y<0 || bc_screen.z<0 ||
if (bc_screen.x<0 || bc_screen.y<0 || bc_screen.z<0 ||
zbuffer[P.x+P.y*image.get_width()]>frag_depth)
continue;
bool discard = shader.fragment(bc_clip, color);

3
examples/pybullet/CMakeLists.txt
View File

@@ -110,6 +110,9 @@ ELSE(BUILD_PYBULLET_ENET)
ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS})
ENDIF(BUILD_PYBULLET_ENET)
SET_TARGET_PROPERTIES(pybullet PROPERTIES PREFIX "")
SET_TARGET_PROPERTIES(pybullet PROPERTIES POSTFIX "")
SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(pybullet PROPERTIES DEBUG_POSTFIX "_d")

1074
examples/pybullet/pybullet.c
View File

File diff suppressed because it is too large Load Diff

2
examples/pybullet/test.py
View File

@@ -3,7 +3,7 @@ import time
#choose connection method: GUI, DIRECT, SHARED_MEMORY
pybullet.connect(pybullet.GUI)
pybullet.loadURDF("plane.urdf",0,0,-1)
#load URDF, given a relative or absolute file+path
obj = pybullet.loadURDF("r2d2.urdf")

38
examples/pybullet/testrender.py
View File

@@ -5,7 +5,7 @@ import pybullet
pybullet.connect(pybullet.GUI)
pybullet.loadURDF("r2d2.urdf")
camTargetPos = [0,0,0]
camTargetPos = [0.,0.,0.]
cameraUp = [0,0,1]
cameraPos = [1,1,1]
yaw = 40
@@ -18,29 +18,29 @@ pixelWidth = 320
pixelHeight = 240
nearPlane = 0.01
farPlane = 1000
lightDirection = [0,1,0]
lightColor = [1,1,1]#optional argument
fov = 60
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
#renderImage(w, h, view[16], projection[16])
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
for pitch in range (0,360,10) :
img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov)
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
#print 'width = %d height = %d' % (w,h)
# reshape creates np array
np_img_arr = np.reshape(rgb, (h, w, 4))
np_img_arr = np_img_arr*(1./255.)
#show
plt.imshow(np_img_arr,interpolation='none')
#plt.show()
plt.pause(0.01)
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor)
w=img_arr[0]
h=img_arr[1]
rgb=img_arr[2]
dep=img_arr[3]
#print 'width = %d height = %d' % (w,h)
# reshape creates np array
np_img_arr = np.reshape(rgb, (h, w, 4))
np_img_arr = np_img_arr*(1./255.)
#show
plt.imshow(np_img_arr,interpolation='none')
plt.pause(0.01)
pybullet.resetSimulation()

39
examples/pybullet/testrender_np.py
View File

@@ -1,3 +1,6 @@
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled, otherwise use testrender.py (slower but compatible without numpy)
import numpy as np
import matplotlib.pyplot as plt
import pybullet
@@ -23,28 +26,30 @@ farPlane = 1000
fov = 60
main_start = time.time()
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
#renderImage(w, h, view[16], projection[16])
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
for pitch in range (0,360,10) :
start = time.time()
img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov)
stop = time.time()
print "renderImage %f" % (stop - start)
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, [0,1,0])
stop = time.time()
print ("renderImage %f" % (stop - start))
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
#print 'width = %d height = %d' % (w,h)
print 'width = %d height = %d' % (w,h)
#show
plt.imshow(rgb,interpolation='none')
#plt.show()
plt.pause(0.01)
#note that sending the data to matplotlib is really slow
#show
plt.imshow(rgb,interpolation='none')
#plt.show()
plt.pause(0.01)
main_stop = time.time()
print "Total time %f" % (main_stop - main_start)
print ("Total time %f" % (main_stop - main_start))
pybullet.resetSimulation()

8
src/BulletCollision/BroadphaseCollision/btDispatcher.h
View File

@@ -64,6 +64,12 @@ struct btDispatcherInfo
btScalar m_convexConservativeDistanceThreshold;
};
enum ebtDispatcherQueryType
{
BT_CONTACT_POINT_ALGORITHMS = 1,
BT_CLOSEST_POINT_ALGORITHMS = 2
};
///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
class btDispatcher
@@ -73,7 +79,7 @@ class btDispatcher
public:
virtual ~btDispatcher() ;
virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0;
virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;
virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0;

2
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
View File

@@ -100,7 +100,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
btScalar radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
normal.normalize();
normal.safeNormalize();
btVector3 p1ToCentre = sphereCenter - vertices[0];
btScalar distanceFromPlane = p1ToCentre.dot(normal);

3
src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
View File

@@ -40,6 +40,9 @@ public:
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
};
#endif //BT_COLLISION_CONFIGURATION

28
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
View File

@@ -50,8 +50,10 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
{
for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
{
m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
btAssert(m_doubleDispatch[i][j]);
m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
btAssert(m_doubleDispatchContactPoints[i][j]);
m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
}
}
@@ -61,7 +63,12 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
{
m_doubleDispatch[proxyType0][proxyType1] = createFunc;
m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
}
void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
{
m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
}
btCollisionDispatcher::~btCollisionDispatcher()
@@ -138,14 +145,23 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold)
btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
{
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = this;
ci.m_manifold = sharedManifold;
btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap);
btCollisionAlgorithm* algo = 0;
if (algoType == BT_CONTACT_POINT_ALGORITHMS)
{
algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
}
else
{
algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
}
return algo;
}
@@ -262,7 +278,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair,
//dispatcher will keep algorithms persistent in the collision pair
if (!collisionPair.m_algorithm)
{
collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap);
collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS);
}
if (collisionPair.m_algorithm)

8
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
View File

@@ -57,7 +57,9 @@ protected:
btPoolAllocator* m_persistentManifoldPoolAllocator;
btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionConfiguration* m_collisionConfiguration;
@@ -84,6 +86,8 @@ public:
///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc);
int getNumManifolds() const
{
return int( m_manifoldsPtr.size());
@@ -115,7 +119,7 @@ public:
virtual void clearManifold(btPersistentManifold* manifold);
btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0);
btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType);
virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);

1
src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
View File

@@ -28,6 +28,7 @@ btCollisionObject::btCollisionObject()
m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
m_islandTag1(-1),
m_companionId(-1),
m_worldArrayIndex(-1),
m_activationState1(1),
m_deactivationTime(btScalar(0.)),
m_friction(btScalar(0.5)),

35
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -79,7 +79,7 @@ protected:
int m_islandTag1;
int m_companionId;
int m_uniqueId;
int m_worldArrayIndex; // index of object in world's collisionObjects array
mutable int m_activationState1;
mutable btScalar m_deactivationTime;
@@ -122,6 +122,7 @@ protected:
///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
int m_updateRevision;
btVector3 m_customDebugColorRGB;
public:
@@ -136,7 +137,8 @@ public:
CF_CHARACTER_OBJECT = 16,
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
};
enum CollisionObjectTypes
@@ -456,14 +458,15 @@ public:
m_companionId = id;
}
SIMD_FORCE_INLINE int getUniqueId() const
SIMD_FORCE_INLINE int getWorldArrayIndex() const
{
return m_uniqueId;
return m_worldArrayIndex;
}
void setUniqueId( int id )
// only should be called by CollisionWorld
void setWorldArrayIndex(int ix)
{
m_uniqueId = id;
m_worldArrayIndex = ix;
}
SIMD_FORCE_INLINE btScalar getHitFraction() const
@@ -555,6 +558,26 @@ public:
return m_updateRevision;
}
void setCustomDebugColor(const btVector3& colorRGB)
{
m_customDebugColorRGB = colorRGB;
m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
}
void removeCustomDebugColor()
{
m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
}
bool getCustomDebugColor(btVector3& colorRGB) const
{
bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR));
if (hasCustomColor)
{
colorRGB = m_customDebugColorRGB;
}
return hasCustomColor;
}
inline bool checkCollideWith(const btCollisionObject* co) const
{

31
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -115,7 +115,9 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
//check that the object isn't already added
btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
btAssert(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world
collisionObject->setWorldArrayIndex(m_collisionObjects.size());
m_collisionObjects.push_back(collisionObject);
//calculate new AABB
@@ -195,6 +197,7 @@ void btCollisionWorld::updateAabbs()
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btAssert(colObj->getWorldArrayIndex() == i);
//only update aabb of active objects
if (m_forceUpdateAllAabbs || colObj->isActive())
@@ -253,9 +256,25 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
}
//swapremove
m_collisionObjects.remove(collisionObject);
int iObj = collisionObject->getWorldArrayIndex();
btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
if (iObj >= 0 && iObj < m_collisionObjects.size())
{
btAssert(collisionObject == m_collisionObjects[iObj]);
m_collisionObjects.swap(iObj, m_collisionObjects.size()-1);
m_collisionObjects.pop_back();
if (iObj < m_collisionObjects.size())
{
m_collisionObjects[iObj]->setWorldArrayIndex(iObj);
}
}
else
{
// slow linear search
//swapremove
m_collisionObjects.remove(collisionObject);
}
collisionObject->setWorldArrayIndex(-1);
}
@@ -1212,7 +1231,7 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback
btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1,0, BT_CLOSEST_POINT_ALGORITHMS);
if (algorithm)
{
btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
@@ -1248,7 +1267,7 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB, 0, BT_CLOSEST_POINT_ALGORITHMS);
if (algorithm)
{
btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
@@ -1553,6 +1572,8 @@ void btCollisionWorld::debugDrawWorld()
}
};
colObj->getCustomDebugColor(color);
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))

42
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
View File

@@ -65,7 +65,13 @@ void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionO
const btCollisionShape* childShape = compoundShape->getChildShape(i);
btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold);
m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
}
}
}
@@ -128,8 +134,14 @@ public:
btTransform newChildWorldTrans = orgTrans*childTrans ;
//perform an AABB check first
btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
btVector3 aabbMin0,aabbMax0;
childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= extendAabb;
aabbMax0 += extendAabb;
btVector3 aabbMin1, aabbMax1;
m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
if (gCompoundChildShapePairCallback)
@@ -142,12 +154,22 @@ public:
{
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
btCollisionAlgorithm* algo = 0;
//the contactpoint is still projected back using the original inverted worldtrans
if (!m_childCollisionAlgorithms[index])
m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold);
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
}
else
{
//the contactpoint is still projected back using the original inverted worldtrans
if (!m_childCollisionAlgorithms[index])
{
m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
}
algo = m_childCollisionAlgorithms[index];
}
const btCollisionObjectWrapper* tmpWrap = 0;
@@ -164,8 +186,7 @@ public:
m_resultOut->setShapeIdentifiersB(-1,index);
}
m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
#if 0
if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
@@ -271,6 +292,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
btTransform otherInCompoundSpace;
otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
localAabbMin -= extraExtends;
localAabbMax += extraExtends;
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
//process all children, that overlap with the given AABB bounds

29
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
View File

@@ -164,9 +164,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
aabbMin1 -= thresholdVec;
aabbMax0 += thresholdVec;
aabbMax1 += thresholdVec;
if (gCompoundCompoundChildShapePairCallback)
{
@@ -183,17 +181,24 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
btCollisionAlgorithm* colAlgo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
}
else
{
if (pair)
{
colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
if (pair)
{
colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
} else
{
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
btAssert(pair);
pair->m_userPointer = colAlgo;
}
else
{
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
btAssert(pair);
pair->m_userPointer = colAlgo;
}
}
btAssert(colAlgo);

15
src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
View File

@@ -118,8 +118,16 @@ partId, int triangleIndex)
btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
btCollisionAlgorithm* colAlgo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
}
else
{
colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
}
const btCollisionObjectWrapper* tmpWrap = 0;
if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
@@ -170,7 +178,8 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
btScalar extraMargin = collisionMarginTriangle;
btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
btVector3 extra(extraMargin,extraMargin,extraMargin);
m_aabbMax += extra;

80
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
View File

@@ -198,6 +198,86 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
}
btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1)
{
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_sphereSphereCF;
}
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
{
return m_sphereBoxCF;
}
if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_boxSphereCF;
}
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
{
return m_sphereTriangleCF;
}
if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_triangleSphereCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
{
return m_convexPlaneCF;
}
if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
{
return m_planeConvexCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
{
return m_convexConvexCreateFunc;
}
if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
{
return m_convexConcaveCreateFunc;
}
if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
{
return m_swappedConvexConcaveCreateFunc;
}
if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
{
return m_compoundCompoundCreateFunc;
}
if (btBroadphaseProxy::isCompound(proxyType0))
{
return m_compoundCreateFunc;
}
else
{
if (btBroadphaseProxy::isCompound(proxyType1))
{
return m_swappedCompoundCreateFunc;
}
}
//failed to find an algorithm
return m_emptyCreateFunc;
}
btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
{

2
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
View File

@@ -103,6 +103,8 @@ public:
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
///By default, this feature is disabled for best performance.
///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.

2
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
View File

@@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObje
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold);
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds

2
src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
View File

@@ -122,7 +122,7 @@ protected:
checkManifold(body0Wrap,body1Wrap);
btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
body0Wrap,body1Wrap,getLastManifold());
body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
return convex_algorithm ;
}

2
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -751,7 +751,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
// Kinematic bodies can be in multiple islands at once, so it is a
// race condition to write to them, so we use an alternate method
// to record the solverBodyId
int uniqueId = body.getUniqueId();
int uniqueId = body.getWorldArrayIndex();
const int INVALID_SOLVER_BODY_ID = -1;
if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
{

6
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
View File

@@ -149,7 +149,6 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
{
BT_PROFILE("solveConstraints");
m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
@@ -161,8 +160,3 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
}
void btDiscreteDynamicsWorldMt::addCollisionObject(btCollisionObject* collisionObject, short int collisionFilterGroup, short int collisionFilterMask)
{
collisionObject->setUniqueId(m_collisionObjects.size());
btDiscreteDynamicsWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask);
}

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
View File

@@ -35,8 +35,6 @@ protected:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btDiscreteDynamicsWorldMt();
};

14
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -378,7 +378,9 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
//split impulse is not implemented yet for btMultiBody*
//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
@@ -388,19 +390,23 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
//split impulse is not implemented yet for btMultiBody*
// if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
}
/*else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
*/
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;

7
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -528,19 +528,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
if(!isFriction)
{
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
// if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
}
/*else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
*/
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -384,7 +384,7 @@ btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBr
m_multiBodyConstraintSolver(constraintSolver)
{
//split impulse is not yet supported for Featherstone hierarchies
getSolverInfo().m_splitImpulse = false;
// getSolverInfo().m_splitImpulse = false;
getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
}

7
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
View File

@@ -120,8 +120,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
//btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr);
colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm();
@@ -164,7 +164,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//??
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr);
colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm();