AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
erwin coumans 2016-12-11 18:43:13 -08:00
commit a96fca486b
79 changed files with 22915 additions and 644 deletions
Show Stats 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>

100
data/quadruped/quadruped.urdf
View File

@ -5,8 +5,8 @@
<geometry>
<box size=".226 0.16 .07"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
@ -26,9 +26,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -55,9 +53,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -84,9 +80,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -113,9 +107,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -142,9 +134,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -171,9 +161,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -200,9 +188,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -229,9 +215,7 @@
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<material name="black"/>
</visual>
<collision>
<geometry>
@ -258,8 +242,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -286,8 +270,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -315,8 +299,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -345,8 +329,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -374,8 +358,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -402,8 +386,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -431,8 +415,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -461,8 +445,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -493,8 +477,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -521,8 +505,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -550,8 +534,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -580,8 +564,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -609,8 +593,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -637,8 +621,8 @@
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -666,8 +650,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
@ -696,8 +680,8 @@
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>

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);
}

20
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);
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 );
}

57
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,8 +343,6 @@ 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++)
{
@ -370,6 +372,55 @@ 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;
m_internalData->m_renderObjects[o]->m_lightAmbientCoeff = 0.6;
m_internalData->m_renderObjects[o]->m_lightDiffuseCoeff = 0.35;
m_internalData->m_renderObjects[o]->m_lightSpecularCoeff = 0.05;
}
}
TinyRenderer::renderObjectDepth(*m_internalData->m_renderObjects[o]);
}
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;
m_internalData->m_renderObjects[o]->m_lightAmbientCoeff = 0.6;
m_internalData->m_renderObjects[o]->m_lightDiffuseCoeff = 0.35;
m_internalData->m_renderObjects[o]->m_lightSpecularCoeff = 0.05;
}
}
TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]);

185
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)
@ -1244,6 +1340,62 @@ void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHa
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 b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightAmbientCoeff = lightAmbientCoeff;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF;
}
void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightDiffuseCoeff = lightDiffuseCoeff;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF;
}
void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightSpecularCoeff = lightSpecularCoeff;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF;
}
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])
{
@ -1504,6 +1656,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;
}
@ -1516,6 +1670,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;

22
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -89,14 +89,26 @@ 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 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 b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff);
void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff);
void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff);
void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
@ -123,12 +135,16 @@ void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle comm
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);
@ -150,6 +166,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
@ -245,6 +264,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);

17
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -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);

345
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++)
{
@ -1421,6 +1436,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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_SET_AMBIENT_COEFF) != 0)
{
m_data->m_visualConverter.setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0)
{
m_data->m_visualConverter.setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0)
{
m_data->m_visualConverter.setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0)
{
m_data->m_visualConverter.render(
@ -2309,6 +2354,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;
@ -2334,9 +2390,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);
@ -2344,11 +2445,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)
{
@ -2357,11 +2455,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());
}
@ -2390,6 +2485,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;
@ -2402,29 +2522,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:
@ -2769,6 +2872,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;
@ -2782,19 +2891,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (bodyA->m_multiBody)
{
if (bodyA->m_multiBody->getBaseCollider())
{
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)
{
if (!hasLinkIndexAFilter || (linkIndexA == i))
{
setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
setALinkIndex.push_back(i);
}
}
}
}
if (bodyA->m_rigidBody)
{
setA.push_back(bodyA->m_rigidBody);
@ -2810,19 +2925,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (bodyB->m_multiBody)
{
if (bodyB->m_multiBody->getBaseCollider())
{
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)
{
if (!hasLinkIndexBFilter || (linkIndexB ==i))
{
setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
setBLinkIndex.push_back(i);
}
}
}
}
if (bodyB->m_rigidBody)
{
setB.push_back(bodyB->m_rigidBody);
@ -3062,10 +3183,11 @@ 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)
{
@ -3114,6 +3236,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;
@ -3513,7 +3665,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);
@ -3577,6 +3728,53 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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)
{
@ -3833,7 +4031,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);
@ -3847,9 +4045,21 @@ int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim)
{
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
}
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
if (gResetSimulation)
{
resetSimulation();
gResetSimulation = false;
}
if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
@ -3906,7 +4116,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()
{
@ -3937,14 +4170,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)
@ -3974,6 +4207,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());
@ -3982,7 +4239,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
@ -4024,6 +4281,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);
@ -4050,7 +4308,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)),
@ -4077,7 +4335,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());
@ -4118,9 +4376,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);
}
}

4
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();
@ -82,6 +85,7 @@ public:
void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
void stepSimulationRealTime(double dtInSec);
void enableRealTimeSimulation(bool enableRealTimeSim);
void applyJointDamping(int bodyUniqueId);
};

164
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,6 +1516,8 @@ 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);
@ -1533,8 +1583,12 @@ void PhysicsServerExample::renderScene()
}
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
if (!gEnableRealTimeSimVR)
{
gEnableRealTimeSimVR = true;
m_physicsServer.enableRealTimeSimulation(1);
}
}
@ -1544,6 +1598,8 @@ void PhysicsServerExample::renderScene()
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
drawUserDebugLines();
///debug rendering
m_physicsServer.physicsDebugDraw(debugDrawFlags);
@ -1653,13 +1709,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 +1730,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 +1745,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
simTimeScalingFactor = 0;
}
}
#endif
}
} else
{
@ -1703,7 +1763,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
}
else
{
gEnableKukaControl = !gEnableKukaControl;
// gEnableKukaControl = !gEnableKukaControl;
}
}
@ -1763,8 +1823,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
{
gEnableRealTimeSimVR = true;
if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);

6
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@ -241,6 +241,12 @@ void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
}
void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
{
m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
}
void PhysicsServerSharedMemory::processClientCommands()
{

2
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@ -28,6 +28,8 @@ public:
virtual void stepSimulationRealTime(double dtInSec);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);

41
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];
};
@ -139,13 +143,25 @@ struct RequestPixelDataArgs
int m_pixelWidth;
int m_pixelHeight;
float m_lightDirection[3];
float m_lightColor[3];
float m_lightDistance;
float m_lightAmbientCoeff;
float m_lightDiffuseCoeff;
float m_lightSpecularCoeff;
int m_hasShadow;
};
enum EnumRequestPixelDataUpdateFlags
{
REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION=8,
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,
REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF=64,
REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF=128,
REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF=256,
//don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h
};
@ -154,6 +170,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
@ -161,6 +179,8 @@ struct RequestContactDataArgs
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
int m_linkIndexAIndexFilter;
int m_linkIndexBIndexFilter;
double m_closestDistanceThreshold;
int m_mode;
};
@ -273,7 +293,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
@ -298,6 +320,8 @@ struct SendPhysicsSimulationParameters
int m_numSimulationSubSteps;
int m_numSolverIterations;
bool m_allowRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;
int m_internalSimFlags;
double m_defaultContactERP;
};
@ -520,7 +544,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
@ -537,6 +564,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,9 +71,21 @@ 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;
float m_lightAmbientCoeff;
bool m_hasLightAmbientCoeff;
float m_lightDiffuseCoeff;
bool m_hasLightDiffuseCoeff;
float m_lightSpecularCoeff;
bool m_hasLightSpecularCoeff;
bool m_hasShadow;
SimpleCamera m_camera;
@ -83,9 +94,12 @@ struct TinyRendererVisualShapeConverterInternalData
m_swWidth(START_WIDTH),
m_swHeight(START_HEIGHT),
m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB),
m_hasLightDirection(false)
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);
}
@ -117,6 +131,40 @@ void TinyRendererVisualShapeConverter::setLightDirection(float x, float y, float
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 TinyRendererVisualShapeConverter::setLightAmbientCoeff(float ambientCoeff)
{
m_data->m_lightAmbientCoeff = ambientCoeff;
m_data->m_hasLightAmbientCoeff = true;
}
void TinyRendererVisualShapeConverter::setLightDiffuseCoeff(float diffuseCoeff)
{
m_data->m_lightDiffuseCoeff = diffuseCoeff;
m_data->m_hasLightDiffuseCoeff = true;
}
void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff)
{
m_data->m_lightSpecularCoeff = specularCoeff;
m_data->m_hasLightSpecularCoeff = true;
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
{
@ -544,7 +592,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;
@ -650,6 +698,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;
}
}
@ -704,15 +753,46 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
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_lightDistance;
}
float lightAmbientCoeff = 0.6;
if (m_data->m_hasLightAmbientCoeff)
{
lightAmbientCoeff = m_data->m_lightAmbientCoeff;
}
float lightDiffuseCoeff = 0.35;
if (m_data->m_hasLightDiffuseCoeff)
{
lightDiffuseCoeff = m_data->m_lightDiffuseCoeff;
}
float lightSpecularCoeff = 0.05;
if (m_data->m_hasLightSpecularCoeff)
{
lightSpecularCoeff = m_data->m_lightSpecularCoeff;
}
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(i);
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(n);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
@ -735,10 +815,59 @@ 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_lightColor = lightColor;
renderObj->m_lightDistance = lightDistance;
renderObj->m_lightAmbientCoeff = lightAmbientCoeff;
renderObj->m_lightDiffuseCoeff = lightDiffuseCoeff;
renderObj->m_lightSpecularCoeff = lightSpecularCoeff;
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(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;
renderObj->m_lightAmbientCoeff = lightAmbientCoeff;
renderObj->m_lightDiffuseCoeff = lightDiffuseCoeff;
renderObj->m_lightSpecularCoeff = lightSpecularCoeff;
TinyRenderer::renderObject(*renderObj);
}
}
@ -757,6 +886,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]);
}
}
@ -776,6 +906,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);

6
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@ -33,6 +33,12 @@ 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 setLightAmbientCoeff(float ambientCoeff);
void setLightDiffuseCoeff(float diffuseCoeff);
void setLightSpecularCoeff(float specularCoeff);
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);

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 );
}

7
examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
View File

@ -209,10 +209,13 @@ public:
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_lightAmbientCoeff = 0.6;
renderObj->m_lightDiffuseCoeff = 0.35;
renderObj->m_lightSpecularCoeff = 0.05;
TinyRenderer::renderObject(*renderObj);
}
}

7
examples/StandaloneMain/main_tinyrenderer_single_example.cpp
View File

@ -230,10 +230,13 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
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_lightAmbientCoeff = 0.6;
renderObj->m_lightDiffuseCoeff = 0.35;
renderObj->m_lightSpecularCoeff = 0.05;
TinyRenderer::renderObject(*renderObj);
}
}

244
examples/TinyRenderer/TinyRenderer.cpp
View File

@ -13,56 +13,126 @@
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
struct Shader : public IShader {
struct DepthShader : public IShader {
Model* m_model;
Vec3f m_light_dir_local;
Matrix& m_modelMat;
Matrix m_invModelMat;
Matrix& m_modelView1;
Matrix& m_projectionMatrix;
Matrix& m_projectionMat;
Vec3f m_localScaling;
Vec4f m_colorRGBA;
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
//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)
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_light_dir_local(light_dir_local),
m_modelView1(modelView),
m_projectionMatrix(projectionMatrix),
m_lightModelView(lightModelView),
m_projectionMat(projectionMat),
m_modelMat(modelMat),
m_localScaling(localScaling),
m_colorRGBA(colorRGBA)
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_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_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_modelView1;
Matrix& m_projectionMat;
Vec3f m_localScaling;
Matrix& m_lightModelView;
Vec4f m_colorRGBA;
Matrix& m_viewportMat;
float m_ambient_coefficient;
float m_diffuse_coefficient;
float m_specular_coefficient;
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
Shader(Model* model, Vec3f light_dir_local, Vec3f light_color, Matrix& modelView, Matrix& lightModelView, Matrix& projectionMat, Matrix& modelMat, Matrix& viewportMat, Vec3f localScaling, const Vec4f& colorRGBA, int width, int height, b3AlignedObjectArray<float>* shadowBuffer, float ambient_coefficient=0.6, float diffuse_coefficient=0.35, float specular_coefficient=0.05)
:m_model(model),
m_light_dir_local(light_dir_local),
m_light_color(light_color),
m_ambient_coefficient(ambient_coefficient),
m_diffuse_coefficient(diffuse_coefficient),
m_specular_coefficient(specular_coefficient),
m_modelView1(modelView),
m_lightModelView(lightModelView),
m_projectionMat(projectionMat),
m_modelMat(modelMat),
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_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_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.05); // magic coeff to avoid z-fighting
Vec3f bn = (varying_nrm*bar).normalize();
Vec2f uv = varying_uv*bar;
@ -70,24 +140,68 @@ struct Shader : public IShader {
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 diffuse_coefficient = 0.35;
float specular_coefficient = 0.05;
color = m_model->diffuse(uv);
color[0] *= m_colorRGBA[0];
color[1] *= m_colorRGBA[1];
color[2] *= m_colorRGBA[2];
color[3] *= m_colorRGBA[3];
float intensity = ambient_coefficient + b3Min(diffuse * diffuse_coefficient + specular * specular_coefficient, 1.0f - ambient_coefficient);
color = m_model->diffuse(uv) * intensity;
//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];
for (int i = 0; i < 3; ++i)
{
color[i] = b3Min(int(m_ambient_coefficient*color[i] + shadow*(m_diffuse_coefficient*diffuse+m_specular_coefficient*specular)*color[i]*m_light_color[i]), 255);
}
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();
m_lightAmbientCoeff = 0.6;
m_lightDiffuseCoeff = 0.35;
m_lightSpecularCoeff = 0.05;
}
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();
m_lightAmbientCoeff = 0.6;
m_lightDiffuseCoeff = 0.35;
m_lightSpecularCoeff = 0.05;
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
@ -101,13 +215,15 @@ 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();
m_lightAmbientCoeff = 0.6;
m_lightDiffuseCoeff = 0.35;
m_lightSpecularCoeff = 0.05;
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
@ -121,8 +237,12 @@ 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();
m_lightAmbientCoeff = 0.6;
m_lightDiffuseCoeff = 0.35;
m_lightSpecularCoeff = 0.05;
}
@ -254,42 +374,80 @@ TinyRenderObjectData::~TinyRenderObjectData()
delete m_model;
}
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]);
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);
}
}
}
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());
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, renderData.m_lightAmbientCoeff, renderData.m_lightDiffuseCoeff, renderData.m_lightSpecularCoeff);
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);
}
}
}

11
examples/TinyRenderer/TinyRenderer.h
View File

@ -18,6 +18,11 @@ struct TinyRenderObjectData
Matrix m_viewportMatrix;
btVector3 m_localScaling;
btVector3 m_lightDirWorld;
btVector3 m_lightColor;
float m_lightDistance;
float m_lightAmbientCoeff;
float m_lightDiffuseCoeff;
float m_lightSpecularCoeff;
//Model (vertices, indices, textures, shader)
Matrix m_modelMatrix;
@ -28,10 +33,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 +58,7 @@ struct TinyRenderObjectData
class TinyRenderer
{
public:
static void renderObjectDepth(TinyRenderObjectData& renderData);
static void renderObject(TinyRenderObjectData& renderData);
};

51
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;
}
@ -67,12 +78,10 @@ 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,15 +96,11 @@ 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);

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")

728
examples/pybullet/pybullet.c
View File

@ -30,6 +30,123 @@ enum eCONNECT_METHOD {
static PyObject* SpamError;
static b3PhysicsClientHandle sm = 0;
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
double v = 0.0;
PyObject* item;
if (PyList_Check(seq)) {
item = PyList_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
else {
item = PyTuple_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
return v;
}
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
if (seq)
{
len = PySequence_Size(objMat);
if (len == 16) {
for (i = 0; i < len; i++) {
matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// internal function to set a float vector[3]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// vector - float[3] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
int i, len;
PyObject* seq = 0;
if (objVec == NULL)
return 0;
seq = PySequence_Fast(objVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(objVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
if (obVec == NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(obVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
if (obVec == NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
// Step through one timestep of the simulation
static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) {
if (0 == sm) {
@ -302,6 +419,67 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
return Py_None;
}
static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject *keywds)
{
double fixedTimeStep = -1;
int numSolverIterations = -1;
int useSplitImpulse = -1;
double splitImpulsePenetrationThreshold = -1;
int numSubSteps = -1;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps))
{
return NULL;
}
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
if (numSolverIterations >= 0)
{
b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
}
if (numSubSteps >= 0)
{
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
}
if (fixedTimeStep >= 0)
{
b3PhysicsParamSetTimeStep(command, fixedTimeStep);
}
if (useSplitImpulse >= 0)
{
b3PhysicsParamSetUseSplitImpulse(command,useSplitImpulse);
}
if (splitImpulsePenetrationThreshold >= 0)
{
b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
}
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
#if 0
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx, double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
#endif
Py_INCREF(Py_None);
return Py_None;
}
// Load a robot from a URDF file (universal robot description format)
@ -309,8 +487,14 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
// to position (0,0,1) with orientation(0,0,0,1)
// els(x,y,z) or
// loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w)
static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *keywds)
{
int size = PySequence_Size(args);
static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase", NULL };
static char *kwlistBackwardCompatible4[] = { "fileName", "startPosX", "startPosY", "startPosZ", NULL };
static char *kwlistBackwardCompatible8[] = { "fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY","startOrnZ","startOrnW", NULL };
int bodyIndex = -1;
const char* urdfFileName = "";
@ -322,25 +506,80 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
double startOrnY = 0.0;
double startOrnZ = 0.0;
double startOrnW = 1.0;
int useMaximalCoordinates = 0;
int useFixedBase = 0;
int backwardsCompatibilityArgs = 0;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size == 1) {
if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL;
if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX,
&startPosY, &startPosZ))
{
backwardsCompatibilityArgs = 1;
}
if (size == 4) {
if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY,
&startPosZ))
else
{
PyErr_Clear();
}
if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8,&urdfFileName, &startPosX,
&startPosY, &startPosZ, &startOrnX, &startOrnY,&startOrnZ, &startOrnW))
{
backwardsCompatibilityArgs = 1;
}
else
{
PyErr_Clear();
}
if (!backwardsCompatibilityArgs)
{
PyObject* basePosObj = 0;
PyObject* baseOrnObj = 0;
double basePos[3];
double baseOrn[4];
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase))
{
return NULL;
}
if (size == 8) {
if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX,
&startPosY, &startPosZ, &startOrnX, &startOrnY,
&startOrnZ, &startOrnW))
else
{
if (basePosObj)
{
if (!pybullet_internalSetVectord(basePosObj, basePos))
{
PyErr_SetString(SpamError, "Cannot convert basePosition.");
return NULL;
}
startPosX = basePos[0];
startPosY = basePos[1];
startPosZ = basePos[2];
}
if (baseOrnObj)
{
if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn))
{
PyErr_SetString(SpamError, "Cannot convert baseOrientation.");
return NULL;
}
startOrnX = baseOrn[0];
startOrnY = baseOrn[1];
startOrnZ = baseOrn[2];
startOrnW = baseOrn[3];
}
}
}
if (strlen(urdfFileName)) {
// printf("(%f, %f, %f) (%f, %f, %f, %f)\n",
@ -356,6 +595,15 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ);
b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,
startOrnZ, startOrnW);
if (useMaximalCoordinates)
{
b3LoadUrdfCommandSetUseMultiBody(command, 0);
}
if (useFixedBase)
{
b3LoadUrdfCommandSetUseFixedBase(command, 1);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_URDF_LOADING_COMPLETED) {
@ -371,19 +619,6 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
return PyLong_FromLong(bodyIndex);
}
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
double v = 0.0;
PyObject* item;
if (PyList_Check(seq)) {
item = PyList_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
} else {
item = PyTuple_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
return v;
}
@ -768,11 +1003,68 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args)
return Py_None;
}
static int pybullet_internalGetBaseVelocity(
int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) {
baseLinearVelocity[0] = 0.;
baseLinearVelocity[1] = 0.;
baseLinearVelocity[2] = 0.;
baseAngularVelocity[0] = 0.;
baseAngularVelocity[1] = 0.;
baseAngularVelocity[2] = 0.;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return 0;
}
{
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQdot;
// const double* jointReactionForces[];
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
PyErr_SetString(SpamError, "getBaseVelocity failed.");
return 0;
}
b3GetStatusActualState(
status_handle, 0 /* body_unique_id */,
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
0 /*root_local_inertial_frame*/, 0,
&actualStateQdot, 0 /* joint_reaction_forces */);
// printf("joint reaction forces=");
// for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
// printf("%f ", jointReactionForces[i]);
// }
// now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
// and orientation x,y,z,w =
// actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
baseLinearVelocity[0] = actualStateQdot[0];
baseLinearVelocity[1] = actualStateQdot[1];
baseLinearVelocity[2] = actualStateQdot[2];
baseAngularVelocity[0] = actualStateQdot[3];
baseAngularVelocity[1] = actualStateQdot[4];
baseAngularVelocity[2] = actualStateQdot[5];
}
}
return 1;
}
// Internal function used to get the base position and orientation
// Orientation is returned in quaternions
static int pybullet_internalGetBasePositionAndOrientation(
int bodyIndex, double basePosition[3], double baseOrientation[3]) {
int bodyIndex, double basePosition[3], double baseOrientation[4]) {
basePosition[0] = 0.;
basePosition[1] = 0.;
basePosition[2] = 0.;
@ -855,8 +1147,7 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
if (0 == pybullet_internalGetBasePositionAndOrientation(
bodyIndex, basePosition, baseOrientation)) {
PyErr_SetString(SpamError,
"GetBasePositionAndOrientation failed (#joints/links "
"exceeds maximum?).");
"GetBasePositionAndOrientation failed.");
return NULL;
}
@ -891,6 +1182,62 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
}
}
static PyObject* pybullet_getBaseVelocity(PyObject* self,
PyObject* args) {
int bodyIndex = -1;
double baseLinearVelocity[3];
double baseAngularVelocity[3];
PyObject* pylistLinVel=0;
PyObject* pylistAngVel=0;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
PyErr_SetString(SpamError, "Expected a body index (integer).");
return NULL;
}
if (0 == pybullet_internalGetBaseVelocity(
bodyIndex, baseLinearVelocity, baseAngularVelocity)) {
PyErr_SetString(SpamError,
"getBaseVelocity failed.");
return NULL;
}
{
PyObject* item;
int i;
int num = 3;
pylistLinVel = PyTuple_New(num);
for (i = 0; i < num; i++) {
item = PyFloat_FromDouble(baseLinearVelocity[i]);
PyTuple_SetItem(pylistLinVel, i, item);
}
}
{
PyObject* item;
int i;
int num = 3;
pylistAngVel = PyTuple_New(num);
for (i = 0; i < num; i++) {
item = PyFloat_FromDouble(baseAngularVelocity[i]);
PyTuple_SetItem(pylistAngVel, i, item);
}
}
{
PyObject* pylist;
pylist = PyTuple_New(2);
PyTuple_SetItem(pylist, 0, pylistLinVel);
PyTuple_SetItem(pylist, 1, pylistAngVel);
return pylist;
}
}
static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
{
if (0 == sm) {
@ -1035,6 +1382,66 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) {
return NULL;
}
static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds)
{
static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL };
if (0 == sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int bodyIndex=0;
PyObject* linVelObj=0;
PyObject* angVelObj=0;
double linVel[3] = { 0, 0, 0 };
double angVel[3] = { 0, 0, 0 };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj))
{
return NULL;
}
if (linVelObj || angVelObj)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
if (linVelObj)
{
pybullet_internalSetVectord(linVelObj, linVel);
b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVel);
}
if (angVelObj)
{
pybullet_internalSetVectord(angVelObj, angVel);
b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVel);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
else
{
PyErr_SetString(SpamError, "expected at least linearVelocity and/or angularVelocity.");
return NULL;
}
}
PyErr_SetString(SpamError, "error in resetJointState.");
return NULL;
}
// Reset the position and orientation of the base/root link, position [x,y,z]
// and orientation quaternion [x,y,z,w]
static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
@ -1366,105 +1773,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
return Py_None;
}
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
if (seq)
{
len = PySequence_Size(objMat);
if (len == 16) {
for (i = 0; i < len; i++) {
matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// internal function to set a float vector[3]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// vector - float[3] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
int i, len;
PyObject* seq=0;
if (objVec==NULL)
return 0;
seq = PySequence_Fast(objVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(objVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(obVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds)
{
@ -1501,7 +1809,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
res = pybullet_internalSetVectord(textPositionObj,posXYZ);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineFrom[3]");
PyErr_SetString(SpamError, "Error converting textPositionObj[3]");
return NULL;
}
@ -1510,7 +1818,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineTo[3]");
PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]");
return NULL;
}
}
@ -1648,6 +1956,43 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
return Py_None;
}
static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds)
{
PyObject* objectColorRGBObj = 0;
double objectColorRGB[3];
int objectUniqueId = -1;
int linkIndex = -2;
static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|O", kwlist,
&objectUniqueId, &linkIndex, &objectColorRGBObj))
return NULL;
if (objectColorRGBObj)
{
if (pybullet_internalSetVectord(objectColorRGBObj, objectColorRGB))
{
b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm);
b3SetDebugObjectColor(commandHandle, objectUniqueId, linkIndex, objectColorRGB);
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
}
}
else
{
b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm);
b3RemoveDebugObjectColor(commandHandle, objectUniqueId, linkIndex);
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
@ -1848,23 +2193,22 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5-6-7 double m_positionOnAInWS[3];//contact point location on object A,
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
8-9-10 double m_positionOnBInWS[3];//contact point location on object
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
11-12-13 double m_contactNormalOnBInWS[3];//the separating contact
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
14 double m_contactDistance;//negative number is penetration, positive
8 double m_contactDistance;//negative number is penetration, positive
is distance.
15 double m_normalForce;
9 double m_normalForce;
*/
int i;
PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints);
for (i = 0; i < contactPointPtr->m_numContactPoints; i++) {
PyObject* contactObList = PyTuple_New(16); // see above 16 fields
PyObject* contactObList = PyTuple_New(10); // see above 10 fields
PyObject* item;
item =
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
@ -1881,42 +2225,61 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
item =
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList, 4, item);
{
PyObject* posAObj = PyTuple_New(3);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList, 5, item);
PyTuple_SetItem(posAObj, 0, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList, 6, item);
PyTuple_SetItem(posAObj, 1, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList, 7, item);
PyTuple_SetItem(posAObj, 2, item);
PyTuple_SetItem(contactObList, 5, posAObj);
}
{
PyObject* posBObj = PyTuple_New(3);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(contactObList, 8, item);
PyTuple_SetItem(posBObj, 0, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(contactObList, 9, item);
PyTuple_SetItem(posBObj, 1, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(contactObList, 10, item);
PyTuple_SetItem(posBObj, 2, item);
PyTuple_SetItem(contactObList, 6, posBObj);
}
{
PyObject* normalOnB = PyTuple_New(3);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(contactObList, 11, item);
PyTuple_SetItem(normalOnB, 0, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(contactObList, 12, item);
PyTuple_SetItem(normalOnB, 1, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(contactObList, 13, item);
PyTuple_SetItem(normalOnB, 2, item);
PyTuple_SetItem(contactObList, 7, normalOnB);
}
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList, 14, item);
PyTuple_SetItem(contactObList, 8, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList, 15, item);
PyTuple_SetItem(contactObList, 9, item);
PyTuple_SetItem(pyResultList, i, contactObList);
}
@ -1982,6 +2345,9 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
int size = PySequence_Size(args);
int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
double distanceThreshold = 0.f;
b3SharedMemoryCommandHandle commandHandle;
@ -1991,15 +2357,15 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
PyObject* pyResultList = 0;
static char *kwlist[] = { "bodyA", "bodyB", "distance", NULL };
static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB",NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|ii", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB))
return NULL;
@ -2007,7 +2373,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
if (linkIndexA >= -1)
{
b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA);
}
if (linkIndexB >= -1)
{
b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@ -2209,17 +2582,23 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
/// Render an image from the current timestep of the simulation, width, height are required, other args are optional
// getCameraImage(w, h, view[16], projection[16], lightpos[3])
// getCameraImage(w, h, view[16], projection[16], lightDir[3], lightColor[3], lightDist, hasShadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff)
static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject *keywds)
{
/// request an image from a simulated camera, using a software renderer.
struct b3CameraImageData imageData;
PyObject* objViewMat = 0, *objProjMat = 0, *lightDirObj = 0;
PyObject* objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0;
int width, height;
int size = PySequence_Size(args);
float viewMatrix[16];
float projectionMatrix[16];
float lightDir[3];
float lightColor[3];
float lightDist = 10.0;
int hasShadow = 0;
float lightAmbientCoeff = 0.6;
float lightDiffuseCoeff = 0.35;
float lightSpecularCoeff = 0.05;
// inialize cmd
b3SharedMemoryCommandHandle command;
@ -2231,10 +2610,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
command = b3InitRequestCameraImage(sm);
// set camera resolution, optionally view, projection matrix, light direction
static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection",NULL };
// set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow
static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOO", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifff", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff))
{
return NULL;
}
@ -2245,12 +2624,24 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
{
b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
}
//set light pos only if function succeeds
//set light direction only if function succeeds
if (pybullet_internalSetVector(lightDirObj, lightDir))
{
b3RequestCameraImageSetLightDirection(command, lightDir);
}
//set light color only if function succeeds
if (pybullet_internalSetVector(lightColorObj, lightColor))
{
b3RequestCameraImageSetLightColor(command, lightColor);
}
b3RequestCameraImageSetLightDistance(command, lightDist);
b3RequestCameraImageSetShadow(command, hasShadow);
b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff);
b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff);
b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff);
if (b3CanSubmitCommand(sm))
{
@ -3231,9 +3622,6 @@ static PyMethodDef SpamMethods[] = {
"is seconds, typically range is 0.01 or 0.001)"},
{"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
"Set the amount of time to proceed at each call to stepSimulation."
" (unit is seconds, typically range is 0.01 or 0.001)"},
{"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS,
"Set the amount of contact penetration Error Recovery Paramater "
@ -3245,10 +3633,13 @@ static PyMethodDef SpamMethods[] = {
"Enable or disable real time simulation (using the real time clock,"
" RTC) in the physics server. Expects one integer argument, 0 or 1" },
{ "setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS,
"Set some internal physics engine parameter, such as cfm or erp etc." },
{ "setInternalSimFlags", pybullet_setInternalSimFlags, METH_VARARGS,
"This is for experimental purposes, use at own risk, magic may or not happen"},
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
{"loadURDF", (PyCFunction) pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS,
"Create a multibody by loading a URDF file."},
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
@ -3295,6 +3686,19 @@ static PyMethodDef SpamMethods[] = {
"instantaneously, not through physics simulation. (x,y,z) position vector "
"and (x,y,z,w) quaternion orientation."},
{ "getBaseVelocity", pybullet_getBaseVelocity,
METH_VARARGS,
"Get the linear and angular velocity of the base of the object "
" in world space coordinates. "
"(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." },
{ "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS,
"Reset the linear and/or angular velocity of the base of the object "
" in world space coordinates. "
"linearVelocity (x,y,z) and angularVelocity (x,y,z)." },
{"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
"Get the number of joints for an object."},
@ -3333,7 +3737,7 @@ static PyMethodDef SpamMethods[] = {
{ "getCameraImage",(PyCFunction)pybullet_getCameraImage, METH_VARARGS| METH_KEYWORDS,
"Render an image (given the pixel resolution width, height, camera viewMatrix "
", projectionMatrix and lightDirection), and return the "
", projectionMatrix, lightDirection, lightColor, lightDistance, shadow, lightAmbientCoeff, lightDiffuseCoeff, and lightSpecularCoeff), and return the "
"8-8-8bit RGB pixel data and floating point depth values"
#ifdef PYBULLET_USE_NUMPY
" as NumPy arrays"
@ -3393,6 +3797,10 @@ static PyMethodDef SpamMethods[] = {
"remove all user debug draw items"
},
{ "setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS,
"Override the wireframe debug drawing color for a particular object unique id / link index."
"If you ommit the color, the custom color will be removed."
},
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,

12
examples/pybullet/quadruped.py
View File

@ -4,9 +4,10 @@ import math
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
p.setGravity(0,0,-1)
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/quadruped.urdf",10,-2,2)
#p.getNumJoints(1)
#right front leg
p.resetJointState(quadruped,0,1.57)
p.resetJointState(quadruped,2,-2.2)
@ -64,16 +65,17 @@ p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
p.setGravity(0,0,-10)
p_gain = 2
speed = 10
amplitude = 1.3
#stand still
t_end = time.time() + 5
t_end = time.time() + 2
while time.time() < t_end:
p.stepSimulation()
p.setGravity(0,0,-10)
jump_amp = 0.5
@ -125,4 +127,4 @@ while time.time() < t_end:
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.stepSimulation()
p.setRealTimeSimulation(1)

3
examples/pybullet/testrender.py
View File

@ -19,6 +19,7 @@ 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)
@ -28,7 +29,7 @@ for pitch in range (0,360,10) :
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)
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor)
w=img_arr[0]
h=img_arr[1]
rgb=img_arr[2]

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
{

27
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)
}
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))

36
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)
@ -143,11 +155,21 @@ public:
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
btCollisionAlgorithm* algo = 0;
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);
{
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

17
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,18 +181,25 @@ 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;
} else
}
else
{
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
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);

13
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();
};

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

@ -570,7 +570,7 @@ void btMultiBody::clearForcesAndTorques()
void btMultiBody::clearVelocities()
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
for (int i = 0; i < 6 + getNumDofs(); ++i)
{
m_realBuf[i] = 0.f;
}

12
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,18 +390,22 @@ 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;

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();