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

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

Browse Source
This commit is contained in:
Erwin Coumans
2018-10-28 12:35:51 -07:00
parent f90fbed54e df9b4fb005
commit eedca6020b
114 changed files with 35709 additions and 505 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Extras/InverseDynamics/btMultiBodyFromURDF.hpp
View File

@@ -11,7 +11,7 @@
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Utils/b3BulletDefaultFileIO.h"
/// Create a btMultiBody model from URDF.
/// This is adapted from Bullet URDF loader example
class MyBtMultiBodyFromURDF
@@ -48,7 +48,8 @@ public:
{
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(m_gravity);
BulletURDFImporter urdf_importer(&m_nogfx, 0, 1, 0);
b3BulletDefaultFileIO fileIO;
BulletURDFImporter urdf_importer(&m_nogfx, 0, &fileIO, 1, 0);
URDFImporterInterface &u2b(urdf_importer);
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);

7
Extras/obj2sdf/obj2sdf.cpp
View File

@@ -14,6 +14,7 @@
#include "../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "Bullet3Common/b3HashMap.h"
#include "../Utils/b3BulletDefaultFileIO.h"
struct ShapeContainer
{
@@ -71,12 +72,14 @@ int main(int argc, char* argv[])
bool mergeMaterials = args.CheckCmdLineFlag("mergeMaterials");
char fileNameWithPath[MAX_PATH_LEN];
bool fileFound = (b3ResourcePath::findResourcePath(fileName, fileNameWithPath, MAX_PATH_LEN)) > 0;
bool fileFound = (b3ResourcePath::findResourcePath(fileName, fileNameWithPath, MAX_PATH_LEN,0)) > 0;
char materialPrefixPath[MAX_PATH_LEN];
b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN);
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fileNameWithPath, materialPrefixPath);
b3BulletDefaultFileIO fileIO;
std::string err = tinyobj::LoadObj(shapes, fileNameWithPath, materialPrefixPath,&fileIO);
char sdfFileName[MAX_PATH_LEN];
sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf");

1
MANIFEST.in
View File

@@ -8,5 +8,6 @@ recursive-include src *.hpp
recursive-include examples/pybullet/gym *.*
include examples/ThirdPartyLibs/enet/unix.c
include examples/OpenGLWindow/*.*
recursive-include examples/SharedMemory/plugins *.*
recursive-include examples/ThirdPartyLibs/glad *.*
include examples/ThirdPartyLibs/enet/win32.c

4
build3/premake4.lua
View File

@@ -399,8 +399,8 @@ end
else
xcodebuildsettings
{
'ARCHS = "$(ARCHS_STANDARD_32_BIT) $(ARCHS_STANDARD_64_BIT)"',
'VALID_ARCHS = "x86_64 i386"',
'ARCHS = "$(ARCHS_STANDARD_64_BIT)"',
'VALID_ARCHS = "x86_64"',
-- 'SDKROOT = "macosx10.9"',
}
end

BIN
data/multibody.bullet
View File

Binary file not shown.

122
data/threecubes/newsdf.sdf Normal file
View File

@@ -0,0 +1,122 @@
<sdf version='1.6'>
<world name='default'>
<gravity>0 0 -9.8</gravity>
<model name='part0.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d0'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision_0'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part0.obj</uri>
</mesh>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part0.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.000000 0.640000 0.000000 1.000000</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
<model name='part1.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d1'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision_1'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part1.obj</uri>
</mesh>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part1.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.000000 0.000000 0.640000 1.000000</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
<model name='part2.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d2'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision_2'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part2.obj</uri>
</mesh>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part2.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.640000 0.000000 0.000000 1.000000</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
</world>
</sdf>

60
data/threecubes/part0.obj Normal file
View File

@@ -0,0 +1,60 @@
# Exported using automatic converter by Erwin Coumans
mtllib three_cubes.mtl
#object Cube.001
v -4.474365 4.513344 -0.488734
v -4.474365 2.513344 -0.488735
v -4.474365 2.513344 1.511265
v -2.474365 4.513344 -0.488734
v -2.474365 2.513344 -0.488735
v -4.474365 2.513344 -0.488735
v -2.474365 4.513343 1.511266
v -2.474365 2.513344 1.511265
v -2.474365 2.513344 -0.488735
v -4.474365 4.513343 1.511266
v -4.474365 2.513344 1.511265
v -2.474365 2.513344 1.511265
v -4.474365 2.513344 -0.488735
v -2.474365 4.513343 1.511266
v -2.474365 4.513344 -0.488734
v -4.474365 4.513343 1.511266
v -4.474365 4.513344 -0.488734
v -2.474365 4.513344 -0.488734
v -2.474365 4.513343 1.511266
v -4.474365 2.513344 1.511265
usemtl Material.001
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
s off
f 1/1/1 2/2/2 3/3/3
f 4/4/4 5/5/5 6/6/6
f 7/7/7 8/8/8 9/9/9
f 10/10/10 11/11/11 12/12/12
f 13/13/13 5/5/5 12/12/12
f 10/10/10 14/14/14 15/15/15
f 16/16/16 1/1/1 3/3/3
f 17/17/17 4/4/4 6/6/6
f 18/18/18 7/7/7 9/9/9
f 19/19/19 10/10/10 12/12/12
f 20/20/20 13/13/13 12/12/12
f 17/17/17 10/10/10 15/15/15

60
data/threecubes/part1.obj Normal file
View File

@@ -0,0 +1,60 @@
# Exported using automatic converter by Erwin Coumans
mtllib three_cubes.mtl
#object Cube.002
v -4.474365 4.513344 -3.663786
v -4.474365 2.513345 -3.663786
v -4.474365 2.513344 -1.663787
v -2.474365 4.513344 -3.663786
v -2.474365 2.513345 -3.663786
v -4.474365 2.513345 -3.663786
v -2.474365 4.513344 -1.663786
v -2.474365 2.513344 -1.663787
v -2.474365 2.513345 -3.663786
v -4.474365 4.513344 -1.663786
v -4.474365 2.513344 -1.663787
v -2.474365 2.513344 -1.663787
v -4.474365 2.513345 -3.663786
v -2.474365 4.513344 -1.663786
v -2.474365 4.513344 -3.663786
v -4.474365 4.513344 -1.663786
v -4.474365 4.513344 -3.663786
v -2.474365 4.513344 -3.663786
v -2.474365 4.513344 -1.663786
v -4.474365 2.513344 -1.663787
usemtl Material.002
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
s off
f 1/1/1 2/2/2 3/3/3
f 4/4/4 5/5/5 6/6/6
f 7/7/7 8/8/8 9/9/9
f 10/10/10 11/11/11 12/12/12
f 13/13/13 5/5/5 12/12/12
f 10/10/10 14/14/14 15/15/15
f 16/16/16 1/1/1 3/3/3
f 17/17/17 4/4/4 6/6/6
f 18/18/18 7/7/7 9/9/9
f 19/19/19 10/10/10 12/12/12
f 20/20/20 13/13/13 12/12/12
f 17/17/17 10/10/10 15/15/15

60
data/threecubes/part2.obj Normal file
View File

@@ -0,0 +1,60 @@
# Exported using automatic converter by Erwin Coumans
mtllib three_cubes.mtl
#object Cube
v -4.474365 4.513343 2.535691
v -4.474365 2.513344 2.535691
v -4.474365 2.513343 4.535690
v -2.474365 4.513343 2.535691
v -2.474365 2.513344 2.535691
v -4.474365 2.513344 2.535691
v -2.474365 4.513343 4.535691
v -2.474365 2.513343 4.535690
v -2.474365 2.513344 2.535691
v -4.474365 4.513343 4.535691
v -4.474365 2.513343 4.535690
v -2.474365 2.513343 4.535690
v -4.474365 2.513344 2.535691
v -2.474365 4.513343 4.535691
v -2.474365 4.513343 2.535691
v -4.474365 4.513343 4.535691
v -4.474365 4.513343 2.535691
v -2.474365 4.513343 2.535691
v -2.474365 4.513343 4.535691
v -4.474365 2.513343 4.535690
usemtl Material
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
s off
f 1/1/1 2/2/2 3/3/3
f 4/4/4 5/5/5 6/6/6
f 7/7/7 8/8/8 9/9/9
f 10/10/10 11/11/11 12/12/12
f 13/13/13 5/5/5 12/12/12
f 10/10/10 14/14/14 15/15/15
f 16/16/16 1/1/1 3/3/3
f 17/17/17 4/4/4 6/6/6
f 18/18/18 7/7/7 9/9/9
f 19/19/19 10/10/10 12/12/12
f 20/20/20 13/13/13 12/12/12
f 17/17/17 10/10/10 15/15/15

31
data/threecubes/three_cubes.mtl Normal file
View File

@@ -0,0 +1,31 @@
# Blender MTL File: 'None'
# Material Count: 3
newmtl Material
Ns 92.156863
Ka 0.000000 0.000000 0.000000
Kd 0.640000 0.000000 0.000000
Ks 0.500000 0.500000 0.500000
Ni 1.000000
d 1.000000
illum 2
newmtl Material.001
Ns 92.156863
Ka 0.000000 0.000000 0.000000
Kd 0.000000 0.640000 0.000000
Ks 0.500000 0.500000 0.500000
Ni 1.000000
d 1.000000
illum 2
newmtl Material.002
Ns 92.156863
Ka 0.000000 0.000000 0.000000
Kd 0.000000 0.000000 0.640000
Ks 0.500000 0.500000 0.500000
Ni 1.000000
d 1.000000
illum 2

84
data/threecubes/three_cubes.obj Normal file
View File

@@ -0,0 +1,84 @@
# Blender v2.71 (sub 0) OBJ File: ''
# www.blender.org
mtllib three_cubes.mtl
o Cube.001
v -4.474365 4.513343 1.511266
v -4.474365 4.513344 -0.488734
v -4.474365 2.513344 -0.488735
v -4.474365 2.513344 1.511265
v -2.474365 4.513344 -0.488734
v -2.474365 2.513344 -0.488735
v -2.474365 4.513343 1.511266
v -2.474365 2.513344 1.511265
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
usemtl Material.001
s off
f 2/1 3/2 4/3
f 5/1 6/2 3/3
f 7/1 8/2 6/3
f 1/1 4/2 8/3
f 3/1 6/2 8/3
f 1/1 7/2 5/3
f 1/4 2/1 4/3
f 2/4 5/1 3/3
f 5/4 7/1 6/3
f 7/4 1/1 8/3
f 4/4 3/1 8/3
f 2/4 1/1 5/3
o Cube.002
v -4.474365 4.513344 -1.663786
v -4.474365 4.513344 -3.663786
v -4.474365 2.513345 -3.663786
v -4.474365 2.513344 -1.663787
v -2.474365 4.513344 -3.663786
v -2.474365 2.513345 -3.663786
v -2.474365 4.513344 -1.663786
v -2.474365 2.513344 -1.663787
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
usemtl Material.002
s off
f 10/5 11/6 12/7
f 13/5 14/6 11/7
f 15/5 16/6 14/7
f 9/5 12/6 16/7
f 11/5 14/6 16/7
f 9/5 15/6 13/7
f 9/8 10/5 12/7
f 10/8 13/5 11/7
f 13/8 15/5 14/7
f 15/8 9/5 16/7
f 12/8 11/5 16/7
f 10/8 9/5 13/7
o Cube
v -4.474365 4.513343 4.535691
v -4.474365 4.513343 2.535691
v -4.474365 2.513344 2.535691
v -4.474365 2.513343 4.535690
v -2.474365 4.513343 2.535691
v -2.474365 2.513344 2.535691
v -2.474365 4.513343 4.535691
v -2.474365 2.513343 4.535690
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
usemtl Material
s off
f 18/9 19/10 20/11
f 21/9 22/10 19/11
f 23/9 24/10 22/11
f 17/9 20/10 24/11
f 19/9 22/10 24/11
f 17/9 23/10 21/11
f 17/12 18/9 20/11
f 18/12 21/9 19/11
f 21/12 23/9 22/11
f 23/12 17/9 24/11
f 20/12 19/9 24/11
f 18/12 17/9 21/11

8
data/threecubes/threecubes.py Normal file
View File

@@ -0,0 +1,8 @@
import pybullet as p
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
p.loadSDF("newsdf.sdf")
while (1):
p.getCameraImage(320,240, flags=p.ER_NO_SEGMENTATION_MASK)
p.stepSimulation()

32
data/threecubes/threecubes.urdf Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="1.0" ?>
<robot name="cube">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.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.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="three_cubes.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="three_cubes.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

22
data/threecubes/threecubes_org.py Normal file
View File

@@ -0,0 +1,22 @@
import pybullet as p
useEGL = False
useEGLGUI = False
if useEGL:
if useEGLGUI:
p.connect(p.GUI, "window_backend=2")
else:
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
while (1):
viewmat= [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0]
projmat= [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
p.getCameraImage(64,64, viewMatrix=viewmat, projectionMatrix=projmat, flags=p.ER_NO_SEGMENTATION_MASK )
p.stepSimulation()

19
examples/CommonInterfaces/CommonFileIOInterface.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef COMMON_FILE_IO_INTERFACE_H
#define COMMON_FILE_IO_INTERFACE_H
struct CommonFileIOInterface
{
virtual ~CommonFileIOInterface()
{
}
virtual int fileOpen(const char* fileName, const char* mode)=0;
virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)=0;
virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)=0;
virtual void fileClose(int fileHandle)=0;
virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)=0;
virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)=0;
virtual int getFileSize(int fileHandle)=0;
};
#endif //COMMON_FILE_IO_INTERFACE_H

4
examples/CommonInterfaces/CommonGraphicsAppInterface.h
View File

@@ -13,10 +13,10 @@ struct DrawGridData
int upAxis;
float gridColor[4];
DrawGridData(int upAxis = 1)
DrawGridData(int upAx = 1)
: gridSize(10),
upOffset(0.001f),
upAxis(upAxis)
upAxis(upAx)
{
gridColor[0] = 0.6f;
gridColor[1] = 0.6f;

2
examples/CommonInterfaces/CommonRigidBodyBase.h
View File

@@ -10,6 +10,7 @@
#include "CommonGraphicsAppInterface.h"
#include "CommonWindowInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
struct CommonRigidBodyBase : public CommonExampleInterface
{
@@ -317,6 +318,7 @@ struct CommonRigidBodyBase : public CommonExampleInterface
btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest;
m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
if (rayCallback.hasHit())
{

2
examples/Evolution/NN3DWalkersTimeWarpBase.h
View File

@@ -52,7 +52,7 @@ static double /*7*/ CENTUPLE_SPEED = 100;
static double /*8*/ QUINCENTUPLE_SPEED = 500;
static double /*9*/ MILLITUPLE_SPEED = 1000;
static double /*0*/ MAX_SPEED = MILLITUPLE_SPEED;
static double /**/ NUM_SPEEDS = 11;
static double /**/ NUM_SPEEDS = 10;
}; // namespace SimulationSpeeds
// add speeds from the namespace here

6
examples/ExtendedTutorials/RigidBodyFromObj.cpp
View File

@@ -24,6 +24,7 @@ subject to the following restrictions:
#include "Bullet3Common/b3FileUtils.h"
#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "../Utils/b3BulletDefaultFileIO.h"
struct RigidBodyFromObjExample : public CommonRigidBodyBase
{
@@ -73,13 +74,14 @@ void RigidBodyFromObjExample::initPhysics()
//load our obj mesh
const char* fileName = "teddy.obj"; //sphere8.obj";//sponza_closed.obj";//sphere8.obj";
char relativeFileName[1024];
if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0))
{
char pathPrefix[1024];
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, "");
b3BulletDefaultFileIO fileIO;
GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, "",&fileIO);
printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName);
const GLInstanceVertex& v = glmesh->m_vertices->at(0);

9
examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp
View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
#include "LoadMeshFromCollada.h"
#include "Bullet3Common/b3FileUtils.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
class ImportColladaSetup : public CommonRigidBodyBase
@@ -79,7 +79,7 @@ void ImportColladaSetup::initPhysics()
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0))
return;
btVector3 shift(0, 0, 0);
@@ -94,7 +94,7 @@ void ImportColladaSetup::initPhysics()
btTransform upAxisTrans;
upAxisTrans.setIdentity();
btVector3 color(0, 0, 1);
btVector4 color(0, 0, 1,1);
#ifdef COMPARE_WITH_ASSIMP
static int useAssimp = 0;
@@ -119,7 +119,8 @@ void ImportColladaSetup::initPhysics()
{
fileIndex = 0;
}
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis);
b3BulletDefaultFileIO fileIO;
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis,&fileIO);
#endif // COMPARE_WITH_ASSIMP
//at the moment our graphics engine requires instances that share the same visual shape to be added right after registering the shape

27
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -22,10 +22,11 @@ subject to the following restrictions:
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
using namespace tinyxml2;
#include "Bullet3Common/b3FileUtils.h"
#include "LinearMath/btHashMap.h"
#include <assert.h>
#include "btMatrix4x4.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#define MAX_VISUAL_SHAPES 512
@@ -561,7 +562,7 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl
}
}
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis)
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis, struct CommonFileIOInterface* fileIO)
{
// GLInstanceGraphicsShape* instance = 0;
@@ -570,16 +571,32 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
float extraScaling = 1; //0.01;
btHashMap<btHashString, int> name2ShapeIndex;
b3FileUtils f;
char filename[1024];
if (!f.findFile(relativeFileName, filename, 1024))
if (!fileIO->findResourcePath(relativeFileName, filename, 1024))
{
b3Warning("File not found: %s\n", filename);
return;
}
XMLDocument doc;
if (doc.LoadFile(filename) != XML_SUCCESS)
//doc.Parse((const char*)filedata, 0, TIXML_ENCODING_UTF8);
b3AlignedObjectArray<char> xmlString;
int fileHandle = fileIO->fileOpen(filename,"r");
if (fileHandle>=0)
{
int size = fileIO->getFileSize(fileHandle);
xmlString.resize(size);
int actual = fileIO->fileRead(fileHandle, &xmlString[0],size);
if (actual==size)
{
}
}
if (xmlString.size()==0)
return;
if (doc.Parse(&xmlString[0], xmlString.size()) != XML_SUCCESS)
//if (doc.LoadFile(filename) != XML_SUCCESS)
return;
//We need units to be in meter, so apply a scaling using the asset/units meter

3
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h
View File

@@ -28,7 +28,8 @@ void LoadMeshFromCollada(const char* relativeFileName,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans,
float& unitMeterScaling,
int clientUpAxis);
int clientUpAxis,
struct CommonFileIOInterface* fileIO);
//#define COMPARE_WITH_ASSIMP
#ifdef COMPARE_WITH_ASSIMP

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

@@ -6,6 +6,8 @@
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "../../CommonInterfaces/CommonRenderInterface.h"
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "../ImportURDFDemo/UrdfFindMeshFile.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
#include <iostream>
@@ -32,6 +34,7 @@
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
using namespace tinyxml2;
#define mjcf_sphere_indiced textured_detailed_sphere_indices
@@ -203,13 +206,16 @@ struct BulletMJCFImporterInternalData
int m_flags;
int m_textureId;
CommonFileIOInterface* m_fileIO;
BulletMJCFImporterInternalData()
: m_inertiaFromGeom(true),
m_activeModel(-1),
m_activeBodyUniqueId(-1),
m_flags(0),
m_textureId(-1)
m_textureId(-1),
m_fileIO(0)
{
m_pathPrefix[0] = 0;
}
@@ -887,7 +893,7 @@ struct BulletMJCFImporterInternalData
{
geom.m_type = URDF_GEOM_MESH;
geom.m_meshFileName = assetPtr->m_fileName;
bool exists = findExistingMeshFile(
bool exists = UrdfFindMeshFile(m_fileIO,
m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
&geom.m_meshFileName,
&geom.m_meshFileType);
@@ -1408,13 +1414,14 @@ struct BulletMJCFImporterInternalData
}
};
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags)
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, CommonFileIOInterface* fileIO, int flags)
{
m_data = new BulletMJCFImporterInternalData();
m_data->m_guiHelper = helper;
m_data->m_customVisualShapesConverter = customConverter;
m_data->m_flags = flags;
m_data->m_textureId = -1;
m_data->m_fileIO = fileIO;
}
BulletMJCFImporter::~BulletMJCFImporter()
@@ -1433,7 +1440,7 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024) > 0);
bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0);
m_data->m_sourceFileName = relativeFileName;
std::string xml_string;
@@ -1962,7 +1969,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case UrdfGeometry::FILE_OBJ:
{
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO))
{
if (meshData.m_textureImage1)
{
@@ -1980,7 +1987,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case UrdfGeometry::FILE_STL:
{
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO);
break;
}
@@ -1998,7 +2005,8 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
upAxis,
m_data->m_fileIO);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
@@ -2243,7 +2251,7 @@ void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
if (m_data->m_customVisualShapesConverter)
{
const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex, m_data->m_fileIO);
}
}
@@ -2379,12 +2387,12 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
{
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0);
glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0,m_data->m_fileIO);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
@@ -2393,7 +2401,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
}
case UrdfGeometry::FILE_STL:
{
glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO);
break;
}
default:

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

@@ -27,7 +27,7 @@ class BulletMJCFImporter : public URDFImporterInterface
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MJCFURDFTexture>& texturesOut) const;
public:
BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags);
BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO, int flags);
virtual ~BulletMJCFImporter();
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);

5
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@@ -10,7 +10,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../ImportURDFDemo/MyMultiBodyCreator.h"
@@ -200,7 +200,8 @@ void ImportMJCFSetup::initPhysics()
}
int flags = 0;
BulletMJCFImporter importer(m_guiHelper, 0, flags);
b3BulletDefaultFileIO fileIO;
BulletMJCFImporter importer(m_guiHelper, 0, &fileIO, flags);
MyMJCFLogger logger;
bool result = importer.loadMJCF(m_fileName, &logger);
if (result)

47
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@@ -9,6 +9,7 @@
#include "stb_image/stb_image.h"
#include "../ImportObjDemo/LoadMeshFromObj.h"
#include "Bullet3Common/b3HashMap.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
struct CachedTextureResult
{
@@ -45,17 +46,18 @@ struct CachedTextureManager
};
static CachedTextureManager sTexCacheMgr;
bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO)
{
B3_PROFILE("loadAndRegisterMeshFromFileInternal");
meshData.m_gfxShape = 0;
meshData.m_textureImage1 = 0;
meshData.m_textureHeight = 0;
meshData.m_textureWidth = 0;
meshData.m_flags = 0;
meshData.m_isCached = false;
char relativeFileName[1024];
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
char pathPrefix[1024];
@@ -65,7 +67,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
std::vector<tinyobj::shape_t> shapes;
{
B3_PROFILE("tinyobj::LoadObj");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix);
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix,fileIO);
//std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
}
@@ -77,6 +79,18 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++)
{
const tinyobj::shape_t& shape = shapes[i];
meshData.m_rgbaColor[0] = shape.material.diffuse[0];
meshData.m_rgbaColor[1] = shape.material.diffuse[1];
meshData.m_rgbaColor[2] = shape.material.diffuse[2];
meshData.m_rgbaColor[3] = shape.material.transparency;
meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR;
meshData.m_specularColor[0] = shape.material.specular[0];
meshData.m_specularColor[1] = shape.material.specular[1];
meshData.m_specularColor[2] = shape.material.specular[2];
meshData.m_specularColor[3] = 1;
meshData.m_flags |= B3_IMPORT_MESH_HAS_SPECULAR_COLOR;
if (shape.material.diffuse_texname.length() > 0)
{
int width, height, n;
@@ -91,7 +105,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
char relativeFileName[1024];
sprintf(relativeFileName, "%s%s", prefix[i], filename);
char relativeFileName2[1024];
if (b3ResourcePath::findResourcePath(relativeFileName, relativeFileName2, 1024))
if (fileIO->findResourcePath(relativeFileName, relativeFileName2, 1024))
{
if (b3IsFileCachingEnabled())
{
@@ -110,7 +124,30 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
if (image == 0)
{
image = stbi_load(relativeFileName, &width, &height, &n, 3);
b3AlignedObjectArray<char> buffer;
buffer.reserve(1024);
int fileId = fileIO->fileOpen(relativeFileName,"rb");
if (fileId>=0)
{
int size = fileIO->getFileSize(fileId);
if (size>0)
{
buffer.resize(size);
int actual = fileIO->fileRead(fileId,&buffer[0],size);
if (actual != size)
{
b3Warning("STL filesize mismatch!\n");
buffer.resize(0);
}
}
}
if (buffer.size())
{
image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
}
//image = stbi_load(relativeFileName, &width, &height, &n, 3);
meshData.m_textureImage1 = image;

22
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
View File

@@ -3,6 +3,12 @@
#include <string>
enum b3ImportMeshDataFlags
{
B3_IMPORT_MESH_HAS_RGBA_COLOR=1,
B3_IMPORT_MESH_HAS_SPECULAR_COLOR=2,
};
struct b3ImportMeshData
{
struct GLInstanceGraphicsShape* m_gfxShape;
@@ -11,12 +17,26 @@ struct b3ImportMeshData
bool m_isCached;
int m_textureWidth;
int m_textureHeight;
double m_rgbaColor[4];
double m_specularColor[4];
int m_flags;
b3ImportMeshData()
:m_gfxShape(0),
m_textureImage1(0),
m_isCached(false),
m_textureWidth(0),
m_textureHeight(0),
m_flags(0)
{
}
};
class b3ImportMeshUtility
{
public:
static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);
static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO);
};
#endif //B3_IMPORT_MESH_UTILITY_H

6
examples/Importers/ImportObjDemo/ImportObjExample.cpp
View File

@@ -7,6 +7,7 @@
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "Wavefront2GLInstanceGraphicsShape.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "Bullet3Common/b3FileUtils.h"
#include "stb_image/stb_image.h"
@@ -56,7 +57,8 @@ int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterf
int shapeId = -1;
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
b3BulletDefaultFileIO fileIO;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO))
{
int textureIndex = -1;
@@ -94,7 +96,7 @@ void ImportObjSetup::initPhysics()
btQuaternion orn = trans.getRotation();
btVector3 scaling(1, 1, 1);
btVector3 color(1, 1, 1);
btVector4 color(1, 1, 1,1);
int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
if (shapeId >= 0)

10
examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
View File

@@ -33,7 +33,9 @@ void b3EnableFileCaching(int enable)
std::string LoadFromCachedOrFromObj(
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath)
const char* mtl_basepath,
struct CommonFileIOInterface* fileIO
)
{
CachedObjResult* resultPtr = gCachedObjResults[filename];
if (resultPtr)
@@ -43,7 +45,7 @@ std::string LoadFromCachedOrFromObj(
return result.m_msg;
}
std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath);
std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath,fileIO);
CachedObjResult result;
result.m_msg = err;
result.m_shapes = shapes;
@@ -54,13 +56,13 @@ std::string LoadFromCachedOrFromObj(
return err;
}
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath)
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO)
{
B3_PROFILE("LoadMeshFromObj");
std::vector<tinyobj::shape_t> shapes;
{
B3_PROFILE("tinyobj::LoadObj2");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath);
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO);
}
{

6
examples/Importers/ImportObjDemo/LoadMeshFromObj.h
View File

@@ -8,11 +8,13 @@ struct GLInstanceGraphicsShape;
int b3IsFileCachingEnabled();
void b3EnableFileCaching(int enable);
std::string LoadFromCachedOrFromObj(
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath);
const char* mtl_basepath,
struct CommonFileIOInterface* fileIO);
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath);
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath,struct CommonFileIOInterface* fileIO);
#endif //LOAD_MESH_FROM_OBJ_H

2
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -162,7 +162,7 @@ void ImportSDFSetup::initPhysics()
m_dynamicsWorld->setGravity(gravity);
BulletURDFImporter u2b(m_guiHelper, 0, 1, 0);
BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
bool loadOk = u2b.loadSDF(m_fileName);

8
examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
View File

@@ -7,6 +7,7 @@
#include "LoadMeshFromSTL.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
class ImportSTLSetup : public CommonRigidBodyBase
{
@@ -55,7 +56,7 @@ void ImportSTLSetup::initPhysics()
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024,0))
{
b3Warning("Cannot find file %s\n", m_fileName);
return;
@@ -65,7 +66,8 @@ void ImportSTLSetup::initPhysics()
// int index=10;
{
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
b3BulletDefaultFileIO fileIO;
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName,&fileIO);
btTransform trans;
trans.setIdentity();
@@ -74,7 +76,7 @@ void ImportSTLSetup::initPhysics()
btVector3 position = trans.getOrigin();
btQuaternion orn = trans.getRotation();
btVector3 color(0, 0, 1);
btVector4 color(0, 0, 1,1);
int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);

19
examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
View File

@@ -5,6 +5,7 @@
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
struct MySTLTriangle
{
@@ -14,25 +15,21 @@ struct MySTLTriangle
float vertex2[3];
};
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName, struct CommonFileIOInterface* fileIO)
{
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName, "rb");
if (file)
int fileHandle = fileIO->fileOpen(relativeFileName, "rb");
if (fileHandle>=0)
{
int size = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
size = fileIO->getFileSize(fileHandle);
{
b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
}
else
{
if (size)
if (size>=0)
{
//b3Warning("Open STL file of %d bytes\n",size);
char* memoryBuffer = new char[size + 1];
int actualBytesRead = fread(memoryBuffer, 1, size, file);
int actualBytesRead = fileIO->fileRead(fileHandle, memoryBuffer, size);
if (actualBytesRead != size)
{
b3Warning("Error reading from file %s", relativeFileName);
@@ -97,7 +94,7 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
delete[] memoryBuffer;
}
}
fclose(file);
fileIO->fileClose(fileHandle);
}
if (shape)
{

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

@@ -22,9 +22,14 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btShapeHull.h" //to create a tesselation of a generic btConvexShape
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "URDF2Bullet.h" //for flags
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
@@ -35,13 +40,16 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001;
#include <list>
#include "UrdfParser.h"
ATTRIBUTE_ALIGNED16(struct)
BulletURDFInternalData
{
BT_DECLARE_ALIGNED_ALLOCATOR();
b3BulletDefaultFileIO m_defaultFileIO;
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
struct CommonFileIOInterface* m_fileIO;
std::string m_sourceFile;
char m_pathPrefix[1024];
int m_bodyId;
@@ -63,7 +71,9 @@ BulletURDFInternalData
m_pathPrefix[sizeof(m_pathPrefix) - 1] = 0; // required, strncpy doesn't write zero on overflow
}
BulletURDFInternalData()
BulletURDFInternalData(CommonFileIOInterface* fileIO)
:m_urdfParser(fileIO? fileIO : &m_defaultFileIO),
m_fileIO(fileIO? fileIO : &m_defaultFileIO)
{
m_enableTinyRenderer = true;
m_pathPrefix[0] = 0;
@@ -74,6 +84,8 @@ BulletURDFInternalData
{
m_urdfParser.setGlobalScaling(scaling);
}
};
void BulletURDFImporter::printTree()
@@ -81,9 +93,12 @@ void BulletURDFImporter::printTree()
// btAssert(0);
}
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling, int flags)
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO,double globalScaling, int flags)
{
m_data = new BulletURDFInternalData;
m_data = new BulletURDFInternalData(fileIO);
m_data->setGlobalScaling(globalScaling);
m_data->m_flags = flags;
m_data->m_guiHelper = helper;
@@ -128,7 +143,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0;
bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0;
std::string xml_string;
@@ -143,6 +158,23 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
fu.extractPath(relativeFileName, path, sizeof(path));
m_data->setSourceFile(relativeFileName, path);
//read file
int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
char destBuffer[8192];
char* line = 0;
do
{
line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
if (line)
{
xml_string += (std::string(destBuffer) + "\n");
}
}
while (line);
m_data->m_fileIO->fileClose(fileId);
#if 0
std::fstream xml_file(relativeFileName, std::fstream::in);
while (xml_file.good())
{
@@ -151,11 +183,18 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
xml_string += (line + "\n");
}
xml_file.close();
#endif
}
BulletErrorLogger loggie;
m_data->m_urdfParser.setParseSDF(false);
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS));
bool result = false;
if (xml_string.length())
{
result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS));
}
return result;
}
@@ -178,7 +217,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0;
bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)) > 0;
std::string xml_string;
@@ -189,24 +228,36 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
}
else
{
char path[1024];
fu.extractPath(relativeFileName, path, sizeof(path));
m_data->setSourceFile(relativeFileName, path);
std::fstream xml_file(relativeFileName, std::fstream::in);
while (xml_file.good())
//read file
int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
char destBuffer[8192];
char* line = 0;
do
{
std::string line;
std::getline(xml_file, line);
xml_string += (line + "\n");
line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
if (line)
{
xml_string += (std::string(destBuffer) + "\n");
}
}
xml_file.close();
while (line);
m_data->m_fileIO->fileClose(fileId);
}
BulletErrorLogger loggie;
//todo: quick test to see if we can re-use the URDF parser for SDF or not
m_data->m_urdfParser.setParseSDF(true);
bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
bool result = false;
if (xml_string.length())
{
result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
}
return result;
}
@@ -507,107 +558,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
return compound;
}
bool findExistingMeshFile(
const std::string& urdf_path, std::string fn,
const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type)
{
if (fn.size() <= 4)
{
b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
std::string ext;
std::string ext_ = fn.substr(fn.size() - 4);
for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i)
{
ext += char(tolower(*i));
}
if (ext == ".dae")
{
*out_type = UrdfGeometry::FILE_COLLADA;
}
else if (ext == ".stl")
{
*out_type = UrdfGeometry::FILE_STL;
}
else if (ext == ".obj")
{
*out_type = UrdfGeometry::FILE_OBJ;
}
else if (ext == ".cdf")
{
*out_type = UrdfGeometry::FILE_CDF;
}
else
{
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
return false;
}
std::string drop_it_pack = "package://";
std::string drop_it_model = "model://";
if (fn.substr(0, drop_it_pack.length()) == drop_it_pack)
fn = fn.substr(drop_it_pack.length());
else if (fn.substr(0, drop_it_model.length()) == drop_it_model)
fn = fn.substr(drop_it_model.length());
std::list<std::string> shorter;
shorter.push_back("../..");
shorter.push_back("..");
shorter.push_back(".");
int cnt = urdf_path.size();
for (int i = 0; i < cnt; ++i)
{
if (urdf_path[i] == '/' || urdf_path[i] == '\\')
{
shorter.push_back(urdf_path.substr(0, i));
}
}
shorter.reverse();
std::string existing_file;
{
std::string attempt = fn;
FILE* f = fopen(attempt.c_str(), "rb");
if (f)
{
existing_file = attempt;
fclose(f);
}
}
if (existing_file.empty())
{
for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
{
std::string attempt = *x + "/" + fn;
FILE* f = fopen(attempt.c_str(), "rb");
if (!f)
{
//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
continue;
}
fclose(f);
existing_file = attempt;
//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
break;
}
}
if (existing_file.empty())
{
b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
else
{
*out_found_filename = existing_file;
return true;
}
}
int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
{
@@ -708,7 +659,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
@@ -758,16 +709,16 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix);
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix,m_data->m_fileIO);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str());
std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
@@ -777,7 +728,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
break;
case UrdfGeometry::FILE_STL:
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO);
break;
case UrdfGeometry::FILE_COLLADA:
@@ -787,7 +738,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
btTransform upAxisTrans;
upAxisTrans.setIdentity();
float unitMeterScaling = 1;
LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2);
LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2, m_data->m_fileIO);
glmesh = new GLInstanceGraphicsShape;
glmesh->m_indices = new b3AlignedObjectArray<int>();
@@ -922,7 +873,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
return shape;
}
void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<BulletURDFTexture>& texturesOut) const
void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const
{
BT_PROFILE("convertURDFToVisualShapeInternal");
@@ -981,8 +932,8 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
{
case UrdfGeometry::FILE_OBJ:
{
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO))
{
if (meshData.m_textureImage1)
{
@@ -1000,7 +951,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case UrdfGeometry::FILE_STL:
{
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(),m_data->m_fileIO);
break;
}
@@ -1018,7 +969,8 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
upAxis,
m_data->m_fileIO);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
@@ -1210,16 +1162,49 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str());
UrdfMaterial* const* matPtr = model.m_materials[matName];
if (matPtr)
b3ImportMeshData meshData;
convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures,meshData);
if (m_data->m_flags&CUF_USE_MATERIAL_COLORS_FROM_MTL)
{
UrdfMaterial* const mat = *matPtr;
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
UrdfMaterialColor matCol;
matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor;
matCol.m_specularColor = mat->m_matColor.m_specularColor;
m_data->m_linkColors.insert(linkIndex, matCol);
if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) &&
(meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR))
{
UrdfMaterialColor matCol;
if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
{
matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
meshData.m_rgbaColor[1],
meshData.m_rgbaColor[2],
meshData.m_rgbaColor[3]);
} else
{
matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
meshData.m_rgbaColor[1],
meshData.m_rgbaColor[2],
1);
}
matCol.m_specularColor.setValue(meshData.m_specularColor[0],
meshData.m_specularColor[1],
meshData.m_specularColor[2]);
m_data->m_linkColors.insert(linkIndex, matCol);
}
} else
{
if (matPtr)
{
UrdfMaterial* const mat = *matPtr;
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
UrdfMaterialColor matCol;
matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor;
matCol.m_specularColor = mat->m_matColor.m_specularColor;
m_data->m_linkColors.insert(linkIndex, matCol);
}
}
convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures);
}
}
if (vertices.size() && indices.size())
@@ -1326,7 +1311,8 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex);
if (linkPtr)
{
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId);
m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO);
}
}
}
@@ -1403,18 +1389,18 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn
{
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col, pathPrefix);
m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*)childShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i));
}
}
if (childShape)
{
m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*)childShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i));
}
}
btTransform childTrans = col.m_linkLocalFrame;
compoundShape->addChildShape(localInertiaFrame.inverse() * childTrans, childShape);

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

@@ -19,7 +19,7 @@ class BulletURDFImporter : public URDFImporterInterface
struct BulletURDFInternalData* m_data;
public:
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling = 1, int flags = 0);
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO=0,double globalScaling=1, int flags=0);
virtual ~BulletURDFImporter();
@@ -88,7 +88,7 @@ public:
virtual int getAllocatedTexture(int index) const;
virtual void setEnableTinyRenderer(bool enable);
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut) const;
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
};
#endif //BULLET_URDF_IMPORTER_H

3
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -171,7 +171,8 @@ void ImportUrdfSetup::initPhysics()
int flags = 0;
double globalScaling = 1;
BulletURDFImporter u2b(m_guiHelper, 0, globalScaling, flags);
BulletURDFImporter u2b(m_guiHelper, 0, 0, globalScaling, flags);
bool loadOk = u2b.loadURDF(m_fileName);

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

@@ -32,6 +32,8 @@ enum ConvertURDFFlags
CUF_INITIALIZE_SAT_FEATURES = 4096,
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
CUF_PARSE_SENSORS = 16384,
CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738,
};
struct UrdfVisualShapeCache

118
examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h Normal file
View File

@@ -0,0 +1,118 @@
#ifndef URDF_FIND_MESH_FILE_H
#define URDF_FIND_MESH_FILE_H
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3Logging.h"
#include <string>
#include <list>
#include "UrdfParser.h"
static bool UrdfFindMeshFile(
CommonFileIOInterface* fileIO,
const std::string& urdf_path, std::string fn,
const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type)
{
if (fn.size() <= 4)
{
b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
std::string ext;
std::string ext_ = fn.substr(fn.size() - 4);
for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i)
{
ext += char(tolower(*i));
}
if (ext == ".dae")
{
*out_type = UrdfGeometry::FILE_COLLADA;
}
else if (ext == ".stl")
{
*out_type = UrdfGeometry::FILE_STL;
}
else if (ext == ".obj")
{
*out_type = UrdfGeometry::FILE_OBJ;
}
else if (ext == ".cdf")
{
*out_type = UrdfGeometry::FILE_CDF;
}
else
{
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
return false;
}
std::string drop_it_pack = "package://";
std::string drop_it_model = "model://";
if (fn.substr(0, drop_it_pack.length()) == drop_it_pack)
fn = fn.substr(drop_it_pack.length());
else if (fn.substr(0, drop_it_model.length()) == drop_it_model)
fn = fn.substr(drop_it_model.length());
std::list<std::string> shorter;
shorter.push_back("../..");
shorter.push_back("..");
shorter.push_back(".");
int cnt = urdf_path.size();
for (int i = 0; i < cnt; ++i)
{
if (urdf_path[i] == '/' || urdf_path[i] == '\\')
{
shorter.push_back(urdf_path.substr(0, i));
}
}
shorter.reverse();
std::string existing_file;
{
for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
{
std::string attempt = *x + "/" + fn;
int f = fileIO->fileOpen(attempt.c_str(), "rb");
if (f<0)
{
//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
continue;
}
fileIO->fileClose(f);
existing_file = attempt;
//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
break;
}
}
if (existing_file.empty())
{
std::string attempt = fn;
int f = fileIO->fileOpen(attempt.c_str(), "rb");
if (f>=0)
{
existing_file = attempt;
fileIO->fileClose(f);
}
}
if (existing_file.empty())
{
b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
else
{
*out_found_filename = existing_file;
return true;
}
}
#endif //URDF_FIND_MESH_FILE_H

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

@@ -2,12 +2,15 @@
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
#include "urdfStringSplit.h"
#include "urdfLexicalCast.h"
#include "UrdfFindMeshFile.h"
using namespace tinyxml2;
UrdfParser::UrdfParser()
UrdfParser::UrdfParser(CommonFileIOInterface* fileIO)
: m_parseSDF(false),
m_activeSdfModel(-1),
m_urdfScaling(1)
m_urdfScaling(1),
m_fileIO(fileIO)
{
m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
}
@@ -343,14 +346,27 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* l
if (type_name == "sphere")
{
geom.m_type = URDF_GEOM_SPHERE;
if (!shape->Attribute("radius"))
if (m_parseSDF)
{
logger->reportError("Sphere shape must have a radius attribute");
return false;
XMLElement* size = shape->FirstChildElement("radius");
if (0 == size)
{
logger->reportError("sphere requires a radius child element");
return false;
}
geom.m_sphereRadius = urdfLexicalCast<double>(size->GetText());
}
else
{
geom.m_sphereRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
if (!shape->Attribute("radius"))
{
logger->reportError("Sphere shape must have a radius attribute");
return false;
}
else
{
geom.m_sphereRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
}
}
}
else if (type_name == "box")
@@ -495,7 +511,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* l
}
geom.m_meshFileName = fn;
bool success = findExistingMeshFile(
bool success = UrdfFindMeshFile(m_fileIO,
m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape),
&geom.m_meshFileName, &geom.m_meshFileType);
if (!success)

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

@@ -80,6 +80,7 @@ struct UrdfGeometry
FILE_COLLADA = 2,
FILE_OBJ = 3,
FILE_CDF = 4,
MEMORY_VERTICES = 5,
};
int m_meshFileType;
@@ -106,9 +107,6 @@ struct UrdfGeometry
}
};
bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
struct UrdfShape
{
@@ -256,6 +254,9 @@ protected:
int m_activeSdfModel;
btScalar m_urdfScaling;
struct CommonFileIOInterface* m_fileIO;
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
@@ -270,7 +271,7 @@ protected:
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
public:
UrdfParser();
UrdfParser(struct CommonFileIOInterface* fileIO);
virtual ~UrdfParser();
void setParseSDF(bool useSDF)

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

@@ -15,7 +15,7 @@ struct UrdfRenderingInterface
virtual ~UrdfRenderingInterface() {}
///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape.
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUid, int bodyUniqueId) = 0;
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
///remove a visual shapes, based on the shape unique id (shapeUid)
virtual void removeVisualShape(int collisionObjectUid) = 0;
@@ -87,10 +87,14 @@ struct UrdfRenderingInterface
virtual void render(const float viewMat[16], const float projMat[16]) = 0;
///load a texture from file, in png or other popular/supported format
virtual int loadTextureFile(const char* filename) = 0;
//virtual int loadTextureFile(const char* filename) = 0;
virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO)=0;
///register a texture using an in-memory pixel buffer of a given width and height
virtual int registerTexture(unsigned char* texels, int width, int height) = 0;
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) {}
virtual void setProjectiveTexture(bool useProjectiveTexture) {}
};
#endif //LINK_VISUAL_SHAPES_CONVERTER_H

2
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@@ -152,7 +152,7 @@ void InverseDynamicsExample::initPhysics()
{
case BT_ID_LOAD_URDF:
{
BulletURDFImporter u2b(m_guiHelper, 0, 1, 0);
BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf"); // lwr / kuka.urdf");
if (loadOk)
{

2
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -346,7 +346,7 @@ void TestJointTorqueSetup::initPhysics()
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
char resourcePath[1024];
if (b3ResourcePath::findResourcePath("multibody.bullet", resourcePath, 1024))
if (b3ResourcePath::findResourcePath("multibody.bullet", resourcePath, 1024,0))
{
FILE* f = fopen(resourcePath, "wb");
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);

4
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -1009,6 +1009,10 @@ void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId)
{
gfxObj->m_textureIndex = textureId;
gfxObj->m_flags |= eGfxHasTexture;
} else
{
gfxObj->m_textureIndex = -1;
gfxObj->m_flags &= ~eGfxHasTexture;
}
}
}

2
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -78,7 +78,7 @@ struct SimpleInternalData
m_ffmpegFile(0),
m_renderTexture(0),
m_userPointer(0),
m_upAxis(0),
m_upAxis(1),
m_customViewPortWidth(-1),
m_customViewPortHeight(-1)
{

5
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -16,7 +16,7 @@
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../Utils/b3BulletDefaultFileIO.h"
struct TinyRendererSetupInternalData
{
TGAImage m_rgbColorBuffer;
@@ -149,7 +149,8 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
int shapeId = -1;
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
b3BulletDefaultFileIO fileIO;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO))
{
int textureIndex = -1;

4
examples/RenderingExamples/TinyVRGui.cpp
View File

@@ -7,6 +7,7 @@
#include "../RenderingExamples/TimeSeriesFontData.h"
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
@@ -129,7 +130,8 @@ bool TinyVRGui::init()
int shapeId = -1;
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
b3BulletDefaultFileIO fileIO;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO))
{
shapeId = m_data->m_renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,

2
examples/RollingFrictionDemo/RollingFrictionDemo.cpp
View File

@@ -218,7 +218,7 @@ void RollingFrictionDemo::initPhysics()
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
char resourcePath[1024];
if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024))
if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024,0))
{
FILE* f = fopen(resourcePath, "wb");
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);

142
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1192,6 +1192,92 @@ B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle comm
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0)
{
int i=0;
if (numVertices>B3_MAX_NUM_VERTICES)
numVertices=B3_MAX_NUM_VERTICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0;
for (i=0;i<numVertices;i++)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2];
}
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0 && numIndices >=0)
{
int i=0;
if (numVertices>B3_MAX_NUM_VERTICES)
numVertices=B3_MAX_NUM_VERTICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
for (i=0;i<numVertices;i++)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2];
}
if (numIndices>B3_MAX_NUM_INDICES)
numIndices = B3_MAX_NUM_INDICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
for (i=0;i<numIndices;i++)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_indices[i]=indices[i];
}
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
@@ -2898,6 +2984,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys
command->m_requestRaycastIntersections.m_numCommandRays = 0;
command->m_requestRaycastIntersections.m_numStreamingRays = 0;
command->m_requestRaycastIntersections.m_numThreads = 1;
command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1;
command->m_requestRaycastIntersections.m_parentLinkIndex=-1;
return (b3SharedMemoryCommandHandle)command;
}
@@ -2947,6 +3035,16 @@ B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3Sha
}
}
B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
command->m_requestRaycastIntersections.m_parentObjectUniqueId = parentObjectUniqueId;
command->m_requestRaycastIntersections.m_parentLinkIndex = parentLinkIndex;
}
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -3997,6 +4095,27 @@ B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHa
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_UPDATE_VISUAL_SHAPE;
command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex;
command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex;
command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId;
command->m_updateFlags = 0;
if (textureUniqueId >= 0)
{
command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_TEXTURE;
}
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape2(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -4007,16 +4126,27 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClien
command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex;
command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex;
command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId;
command->m_updateVisualShapeDataArguments.m_textureUniqueId = -2;
command->m_updateFlags = 0;
if (textureUniqueId >= 0)
{
command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_TEXTURE;
}
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3UpdateVisualShapeTexture(b3SharedMemoryCommandHandle commandHandle, int textureUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
{
if (textureUniqueId >= -1)
{
command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_TEXTURE;
command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId;
}
}
}
B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, const double rgbaColor[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -309,6 +309,8 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape2(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex);
B3_SHARED_API void b3UpdateVisualShapeTexture(b3SharedMemoryCommandHandle commandHandle, int textureUniqueId);
B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, const double rgbaColor[/*4*/]);
B3_SHARED_API void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, const double specularColor[/*3*/]);
@@ -458,6 +460,8 @@ extern "C"
B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant);
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]);
B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices);
B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices);
B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags);
B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/]);
B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
@@ -564,6 +568,7 @@ extern "C"
B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
//max num rays for b3RaycastBatchAddRays is MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING
B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays);
B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex);
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);

4
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -495,9 +495,9 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
int objectUniqueId = 0;
int linkIndex = -1;
int shapeIndex = -1;
int textureIndex = -1;
int textureIndex = -2;
double rgbaColor[4] = {0.0, 1.0, 0.0, 1.0};
b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex, textureIndex);
b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape2(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex);
b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;

468
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4,10 +4,12 @@
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Importers/ImportURDFDemo/UrdfFindMeshFile.h"
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
@@ -54,6 +56,7 @@
#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
#endif
#ifdef ENABLE_STATIC_GRPC_PLUGIN
#include "plugins/grpcPlugin/grpcPlugin.h"
#endif //ENABLE_STATIC_GRPC_PLUGIN
@@ -74,6 +77,11 @@
#include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
#endif
#ifdef B3_ENABLE_FILEIO_PLUGIN
#include "plugins/fileIOPlugin/fileIOPlugin.h"
#endif//B3_DISABLE_FILEIO_PLUGIN
#ifdef B3_ENABLE_TINY_AUDIO
#include "../TinyAudio/b3SoundEngine.h"
#endif
@@ -104,6 +112,11 @@ btQuaternion gVRTeleportOrn(0, 0, 0, 1);
btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
#include "../CommonInterfaces/CommonFileIOInterface.h"
struct UrdfLinkNameMapUtil
{
btMultiBody* m_mb;
@@ -1639,6 +1652,7 @@ struct PhysicsServerCommandProcessorInternalData
std::string m_profileTimingFileName;
struct GUIHelperInterface* m_guiHelper;
int m_sharedMemoryKey;
bool m_enableTinyRenderer;
@@ -1704,43 +1718,66 @@ struct PhysicsServerCommandProcessorInternalData
{
//register static plugins:
#ifdef STATIC_LINK_VR_PLUGIN
m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin, preTickPluginCallback_vrSyncPlugin, 0, 0, 0, 0);
b3PluginFunctions funcs(initPlugin_vrSyncPlugin,exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin);
funcs.m_preTickFunc = preTickPluginCallback_vrSyncPlugin;
m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", funcs);
#endif //STATIC_LINK_VR_PLUGIN
}
#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
{
m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0, 0, 0);
//int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin)
b3PluginFunctions funcs(initPlugin_pdControlPlugin,exitPlugin_pdControlPlugin,executePluginCommand_pdControlPlugin);
funcs.m_preTickFunc = preTickPluginCallback_pdControlPlugin;
m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", funcs);
}
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifndef SKIP_COLLISION_FILTER_PLUGIN
{
m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin, 0, 0, 0, 0, getCollisionInterface_collisionFilterPlugin);
b3PluginFunctions funcs(initPlugin_collisionFilterPlugin,exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin);
funcs.m_getCollisionFunc = getCollisionInterface_collisionFilterPlugin;
m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", funcs );
m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin);
}
#endif
#ifdef ENABLE_STATIC_GRPC_PLUGIN
{
m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin, 0, 0, 0, processClientCommands_grpcPlugin, 0);
b3PluginFunctions funcs(initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin);
funcs.m_processClientCommandsFunc = processClientCommands_grpcPlugin;
m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", funcs);
}
#endif //ENABLE_STATIC_GRPC_PLUGIN
#ifdef STATIC_EGLRENDERER_PLUGIN
{
bool initPlugin = false;
int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin, 0, 0, getRenderInterface_eglRendererPlugin, 0, 0, initPlugin);
b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin);
funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin;
int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin);
m_pluginManager.selectPluginRenderer(renderPluginId);
}
#endif //STATIC_EGLRENDERER_PLUGIN
#ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
{
int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin, 0, 0, getRenderInterface_tinyRendererPlugin, 0, 0);
b3PluginFunctions funcs(initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin);
funcs.m_getRendererFunc=getRenderInterface_tinyRendererPlugin;
int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", funcs);
m_pluginManager.selectPluginRenderer(renderPluginId);
}
#endif
}
#ifdef B3_ENABLE_FILEIO_PLUGIN
{
b3PluginFunctions funcs(initPlugin_fileIOPlugin, exitPlugin_fileIOPlugin, executePluginCommand_fileIOPlugin);
funcs.m_fileIoFunc = getFileIOFunc_fileIOPlugin;
int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("fileIOPlugin", funcs);
m_pluginManager.selectFileIOPlugin(renderPluginId);
}
#endif
m_vrControllerEvents.init();
@@ -1979,11 +2016,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
const b3CreateMultiBodyArgs& m_createBodyArgs;
mutable b3AlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
PhysicsServerCommandProcessorInternalData* m_data;
int m_flags;
ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data)
ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data, int flags)
: m_bodyUniqueId(-1),
m_createBodyArgs(bodyArgs),
m_data(data)
m_data(data),
m_flags(flags)
{
}
@@ -2031,19 +2070,38 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
return false;
}
mutable btHashMap<btHashInt, UrdfMaterialColor> m_linkColors;
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
{
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
if (m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
const UrdfMaterialColor* matColPtr = m_linkColors[linkIndex];
if (matColPtr)
{
for (int i = 0; i < visHandle->m_visualShapes.size(); i++)
matCol = *matColPtr;
if ((m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)==0)
{
if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial)
matCol.m_rgbaColor[3] = 1;
}
return true;
}
} else
{
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
for (int i = 0; i < visHandle->m_visualShapes.size(); i++)
{
matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor;
return true;
if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial)
{
matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor;
return true;
}
}
}
}
@@ -2212,7 +2270,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
int graphicsIndex = -1;
double globalScaling = 1.f; //todo!
int flags = 0;
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(),fileIO, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btAlignedObjectArray<GLInstanceVertex> vertices;
@@ -2235,7 +2295,21 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
{
for (int v = 0; v < visHandle->m_visualShapes.size(); v++)
{
u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures);
b3ImportMeshData meshData;
u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures, meshData);
if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) &&
(meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR))
{
UrdfMaterialColor matCol;
matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
meshData.m_rgbaColor[1],
meshData.m_rgbaColor[2],
meshData.m_rgbaColor[3]);
matCol.m_specularColor.setValue(meshData.m_specularColor[0],
meshData.m_specularColor[1],
meshData.m_specularColor[2]);
m_linkColors.insert(linkIndex, matCol);
}
}
if (vertices.size() && indices.size())
@@ -2300,7 +2374,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
//UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
if (m_data->m_pluginManager.getRenderInterface())
{
m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId);
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO);
}
}
virtual void setBodyUniqueId(int bodyId)
@@ -2680,7 +2755,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
SaveWorldObjectData sd;
sd.m_fileName = fileName;
int currentOpenGLTextureIndex = 0;
for (int m = 0; m < u2b.getNumModels(); m++)
{
@@ -2896,38 +2971,38 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
}
{
int startShapeIndex = 0;
if (m_data->m_pluginManager.getRenderInterface())
{
int currentOpenGLTextureIndex=0;
int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(bodyUniqueId);
//int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
b3VisualShapeData tmpShape;
int remain = totalNumVisualShapes - startShapeIndex;
int shapeIndex = startShapeIndex;
int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(bodyUniqueId, shapeIndex, &tmpShape);
if (success)
for (int shapeIndex=0;shapeIndex<totalNumVisualShapes;shapeIndex++)
{
if (tmpShape.m_tinyRendererTextureId >= 0)
b3VisualShapeData tmpShape;
int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(bodyUniqueId, shapeIndex, &tmpShape);
if (success)
{
int openglTextureUniqueId = -1;
//find companion opengl texture unique id and create a 'textureUid'
if (currentOpenGLTextureIndex < u2b.getNumAllocatedTextures())
if (tmpShape.m_tinyRendererTextureId >= 0)
{
openglTextureUniqueId = u2b.getAllocatedTexture(currentOpenGLTextureIndex);
currentOpenGLTextureIndex++;
}
int openglTextureUniqueId = -1;
int texHandle = m_data->m_textureHandles.allocHandle();
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
if (texH)
{
texH->m_tinyRendererTextureId = tmpShape.m_tinyRendererTextureId;
texH->m_openglTextureId = openglTextureUniqueId;
//find companion opengl texture unique id and create a 'textureUid'
if (currentOpenGLTextureIndex<u2b.getNumAllocatedTextures())
{
openglTextureUniqueId = u2b.getAllocatedTexture(currentOpenGLTextureIndex++);
}
//if (openglTextureUniqueId>=0)
{
int texHandle = m_data->m_textureHandles.allocHandle();
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
if (texH)
{
texH->m_tinyRendererTextureId = tmpShape.m_tinyRendererTextureId;
texH->m_openglTextureId = openglTextureUniqueId;
}
}
}
}
}
@@ -3015,7 +3090,8 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
m_data->m_sdfRecentLoadedBodies.clear();
BulletMJCFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), flags);
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
BulletMJCFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, flags);
bool useFixedBase = false;
MyMJCFLogger2 logger;
@@ -3037,8 +3113,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
}
m_data->m_sdfRecentLoadedBodies.clear();
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
bool forceFixedBase = false;
@@ -3067,7 +3143,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
return false;
}
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
@@ -3645,10 +3722,42 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
if (m_data->m_pluginManager.getRenderInterface())
{
if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(true);
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0)
{
for (int i = 0; i < 16; i++)
{
projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i];
projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i];
}
}
else // If no specified matrices for projective texture, then use the camera matrices.
{
for (int i = 0; i < 16; i++)
{
projTextureViewMat[i] = viewMat[i];
projTextureProjMat[i] = projMat[i];
}
}
m_data->m_pluginManager.getRenderInterface()->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat);
}
else
{
m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
}
if ((flags & ER_NO_SEGMENTATION_MASK) != 0)
{
segmentationMaskBuffer = 0;
}
m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
depthBuffer, numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex, &width, &height, &numPixelsCopied);
m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
}
m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
@@ -4073,9 +4182,9 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
urdfColObj.m_geometry.m_meshFileName = fileName;
urdfColObj.m_geometry.m_meshScale = meshScale;
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
@@ -4084,14 +4193,90 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
std::string out_found_filename;
int out_type;
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices)
{
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(m_data->m_defaultCollisionMargin);
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices)
{
BT_PROFILE("convert trimesh2");
btTriangleMesh* meshInterface = new btTriangleMesh();
this->m_data->m_meshInterfaces.push_back(meshInterface);
{
BT_PROFILE("convert vertices2");
for (int j = 0; j < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices / 3; j++)
{
int i0 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+0];
int i1 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+1];
int i2 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+2];
btVector3 v0( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+2]);
btVector3 v1( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+2]);
btVector3 v2( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+2]);
meshInterface->addTriangle(v0, v1, v2);
}
}
{
BT_PROFILE("create btBvhTriangleMeshShape");
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
m_data->m_collisionShapes.push_back(trimesh);
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
{
btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(trimesh, triangleInfoMap);
}
shape = trimesh;
if (compound)
{
compound->addChildShape(childTransform, shape);
shape->setMargin(m_data->m_defaultCollisionMargin);
}
}
}
else
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
for (int v = 0; v < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; v++)
{
btVector3 pt( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+2]);
convexHull->addPoint(pt, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform, convexHull);
}
urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES;
break;
}
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (foundFile)
{
urdfColObj.m_geometry.m_meshFileType = out_type;
if (out_type == UrdfGeometry::FILE_STL)
{
glmesh = LoadMeshFromSTL(relativeFileName);
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
glmesh = LoadMeshFromSTL(relativeFileName,fileIO);
}
if (out_type == UrdfGeometry::FILE_OBJ)
{
@@ -4099,12 +4284,13 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
glmesh = LoadMeshFromObj(relativeFileName, pathPrefix,fileIO);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str());
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO);
//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
@@ -4281,7 +4467,8 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED;
double globalScaling = 1.f;
int flags = 0;
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btTransform localInertiaFrame;
localInertiaFrame.setIdentity();
@@ -4346,12 +4533,12 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName = fileName;
@@ -4388,6 +4575,7 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
}
else
{
visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue(1,1,1,1);
}
if (hasSpecular)
{
@@ -4977,6 +5165,53 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
memcpy(&rays[numCommandRays], bufferServerToClient, numStreamingRays * sizeof(b3RayData));
}
if (clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId>=0)
{
btTransform tr;
tr.setIdentity();
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId);
if (bodyHandle)
{
int linkIndex = -1;
if (bodyHandle->m_multiBody)
{
int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex;
if (linkIndex == -1)
{
tr = bodyHandle->m_multiBody->getBaseWorldTransform();
}
else
{
if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks())
{
tr = bodyHandle->m_multiBody->getLink(linkIndex).m_cachedWorldTransform;
}
}
}
if (bodyHandle->m_rigidBody)
{
tr = bodyHandle->m_rigidBody->getWorldTransform();
}
//convert all rays into world space
for (int i=0;i<totalRays;i++)
{
btVector3 localPosTo(rays[i].m_rayToPosition[0],rays[i].m_rayToPosition[1],rays[i].m_rayToPosition[2]);
btVector3 worldPosTo = tr*localPosTo;
btVector3 localPosFrom(rays[i].m_rayFromPosition[0],rays[i].m_rayFromPosition[1],rays[i].m_rayFromPosition[2]);
btVector3 worldPosFrom = tr*localPosFrom;
rays[i].m_rayFromPosition[0] = worldPosFrom[0];
rays[i].m_rayFromPosition[1] = worldPosFrom[1];
rays[i].m_rayFromPosition[2] = worldPosFrom[2];
rays[i].m_rayToPosition[0] = worldPosTo[0];
rays[i].m_rayToPosition[1] = worldPosTo[1];
rays[i].m_rayToPosition[2] = worldPosTo[2];
}
}
}
BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays);
batchRayCaster.castRays(numThreads);
@@ -6144,7 +6379,13 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
}
pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->m_physicsDeltaTime;
pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_data->m_physicsDeltaTime;
for (int j = 0; j < 3; j++)
{
pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j];
pt.m_linearFrictionDirection2[j] = srcPt.m_lateralFrictionDir2[j];
}
m_data->m_cachedContactPoints.push_back(pt);
}
}
@@ -6369,7 +6610,13 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
}
pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_deltaTime;
pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_deltaTime;
for (int j = 0; j < 3; j++)
{
pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j];
pt.m_linearFrictionDirection2[j] = srcPt.m_lateralFrictionDir2[j];
}
m_cachedContactPoints.push_back(pt);
}
return 1;
@@ -6508,7 +6755,14 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
{
m_data->m_sdfRecentLoadedBodies.clear();
ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data);
int flags = 0;
if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS)
{
flags = clientCmd.m_createMultiBodyArgs.m_flags;
}
ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data, flags);
bool useMultiBody = true;
if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES)
@@ -6516,12 +6770,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
useMultiBody = false;
}
int flags = 0;
if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS)
{
flags = clientCmd.m_createMultiBodyArgs.m_flags;
}
bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
@@ -6652,19 +6901,21 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
}
{
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str());
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO);
if (shapes.size() > 0)
{
const tinyobj::shape_t& shape = shapes[0];
@@ -8577,6 +8828,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
{
btMultiBodyWorldImporter* importer = m_data->m_worldImporters[foundIndex];
m_data->m_worldImporters.removeAtIndex(foundIndex);
importer->deleteAllData();
delete importer;
m_data->m_userCollisionShapeHandles.freeHandle(removeCollisionShapeId);
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
@@ -9601,9 +9853,12 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
{
texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId);
if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0)
{
texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId);
}
if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId >= 0)
if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId >= -1)
{
if (texHandle)
{
@@ -9614,6 +9869,12 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,
texHandle->m_tinyRendererTextureId);
}
} else
{
m_data->m_pluginManager.getRenderInterface()->changeShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId,
clientCmd.m_updateVisualShapeDataArguments.m_jointIndex,
clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,
-1);
}
}
}
@@ -9634,10 +9895,13 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
{
int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
if (texHandle)
{
int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
} else
{
m_data->m_guiHelper->replaceTexture(shapeIndex, -1);
}
}
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
@@ -9665,11 +9929,15 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex();
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
{
int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
if (texHandle)
{
int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
}
else
{
m_data->m_guiHelper->replaceTexture(shapeIndex, -1);
}
}
if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
{
@@ -9761,7 +10029,8 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
char relativeFileName[1024];
char pathPrefix[1024];
if (b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName, relativeFileName, 1024))
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
if (fileIO->findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName, relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
@@ -9775,7 +10044,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
int uid = -1;
if (m_data->m_pluginManager.getRenderInterface())
{
uid = m_data->m_pluginManager.getRenderInterface()->loadTextureFile(relativeFileName);
uid = m_data->m_pluginManager.getRenderInterface()->loadTextureFile(relativeFileName, fileIO);
}
if (uid >= 0)
{
@@ -9784,8 +10053,37 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
{
int width, height, n;
unsigned char* imageData = stbi_load(relativeFileName, &width, &height, &n, 3);
unsigned char* imageData = 0;
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
if (fileIO)
{
b3AlignedObjectArray<char> buffer;
buffer.reserve(1024);
int fileId = fileIO->fileOpen(relativeFileName,"rb");
if (fileId>=0)
{
int size = fileIO->getFileSize(fileId);
if (size>0)
{
buffer.resize(size);
int actual = fileIO->fileRead(fileId,&buffer[0],size);
if (actual != size)
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
}
}
}
if (buffer.size())
{
imageData = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
}
} else
{
imageData = stbi_load(relativeFileName, &width, &height, &n, 3);
}
if (imageData)
{
texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height);
@@ -10455,6 +10753,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
MyResultCallback rayCallback(rayFromWorld, rayToWorld);
rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest;
m_data->m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
if (rayCallback.hasHit())
{
@@ -10775,14 +11074,19 @@ void PhysicsServerCommandProcessor::addTransformChangedNotifications()
{
continue;
}
if (bodyData->m_multiBody && bodyData->m_multiBody->isAwake())
if (bodyData->m_multiBody)
{
btMultiBody* mb = bodyData->m_multiBody;
m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, -1, mb->getBaseCollider()));
if (mb->getBaseCollider()->isActive())
{
m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, -1, mb->getBaseCollider()));
}
for (int linkIndex = 0; linkIndex < mb->getNumLinks(); linkIndex++)
{
m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, linkIndex, mb->getLinkCollider(linkIndex)));
if (mb->getLinkCollider(linkIndex)->isActive())
{
m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, linkIndex, mb->getLinkCollider(linkIndex)));
}
}
}
else if (bodyData->m_rigidBody && bodyData->m_rigidBody->isActive())

118
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -1239,10 +1239,35 @@ public:
m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex;
m_tmpLine.m_replaceItemUid = replaceItemUid;
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddLine);
m_resultDebugLineUid = -1;
workerThreadWait();
//don't block when replacing an item
if (replaceItemUid>=0 && replaceItemUid<m_userDebugLines.size())
{
//find the right slot
int slot=-1;
for (int i=0;i<m_userDebugLines.size();i++)
{
if (replaceItemUid == m_userDebugLines[i].m_itemUniqueId)
{
slot = i;
}
}
if (slot>=0)
{
m_userDebugLines[slot] = m_tmpLine;
}
m_resultDebugLineUid = replaceItemUid;
}
else
{
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddLine);
m_resultDebugLineUid = -1;
workerThreadWait();
}
return m_resultDebugLineUid;
}
@@ -1520,48 +1545,51 @@ public:
if (gEnableDefaultKeyboardShortcuts)
{
if (key == 'w' && state)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
VRTeleportPos[0] += shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 's' && state)
{
VRTeleportPos[0] -= shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'a' && state)
{
VRTeleportPos[1] -= shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'd' && state)
{
VRTeleportPos[1] += shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'q' && state)
{
VRTeleportPos[2] += shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'e' && state)
{
VRTeleportPos[2] -= shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'z' && state)
{
gVRTeleportRotZ += shift;
btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
m_physicsServer.setVRTeleportOrientation(VRTeleportOrn);
saveCurrentSettingsVR(VRTeleportPos);
if (key == 'w' && state)
{
VRTeleportPos[0] += shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 's' && state)
{
VRTeleportPos[0] -= shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'a' && state)
{
VRTeleportPos[1] -= shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'd' && state)
{
VRTeleportPos[1] += shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'q' && state)
{
VRTeleportPos[2] += shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'e' && state)
{
VRTeleportPos[2] -= shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key == 'z' && state)
{
gVRTeleportRotZ += shift;
btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
m_physicsServer.setVRTeleportOrientation(VRTeleportOrn);
saveCurrentSettingsVR(VRTeleportPos);
}
}
}

13
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -30,7 +30,7 @@ typedef unsigned long long int smUint64_t;
#endif
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 128
#define MAX_DEGREE_OF_FREEDOM 128
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_SDF_FILENAME_LENGTH 1024
@@ -38,6 +38,7 @@ typedef unsigned long long int smUint64_t;
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH
struct TmpFloat3
{
float m_x;
@@ -293,6 +294,9 @@ struct RequestRaycastIntersections
b3RayData m_fromToRays[MAX_RAY_INTERSECTION_BATCH_SIZE];
int m_numStreamingRays;
//optional m_parentObjectUniqueId (-1 for unused)
int m_parentObjectUniqueId;
int m_parentLinkIndex;
//streaming ray data stored in shared memory streaming part. (size m_numStreamingRays )
};
@@ -922,7 +926,10 @@ struct b3CreateUserShapeData
double m_meshScale[3];
int m_collisionFlags;
int m_visualFlags;
int m_numVertices;
double m_vertices[B3_MAX_NUM_VERTICES*3];
int m_numIndices;
int m_indices[B3_MAX_NUM_INDICES];
double m_rgbaColor[4];
double m_specularColor[3];
};
@@ -940,7 +947,7 @@ struct b3CreateUserShapeResultArgs
int m_userShapeUniqueId;
};
#define MAX_CREATE_MULTI_BODY_LINKS 128
#define MAX_CREATE_MULTI_BODY_LINKS MAX_DEGREE_OF_FREEDOM
enum eCreateMultiBodyEnum
{
MULTI_BODY_HAS_BASE = 1,

34
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -7,7 +7,8 @@
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 2018090300
#define SHARED_MEMORY_MAGIC_NUMBER 201810250
//#define SHARED_MEMORY_MAGIC_NUMBER 201809030
//#define SHARED_MEMORY_MAGIC_NUMBER 201809010
//#define SHARED_MEMORY_MAGIC_NUMBER 201807040
//#define SHARED_MEMORY_MAGIC_NUMBER 201806150
@@ -572,13 +573,10 @@ struct b3ContactPointData
double m_normalForce;
//todo: expose the friction forces as well
// double m_linearFrictionDirection0[3];
// double m_linearFrictionForce0;
// double m_linearFrictionDirection1[3];
// double m_linearFrictionForce1;
// double m_angularFrictionDirection[3];
// double m_angularFrictionForce;
double m_linearFrictionForce1;
double m_linearFrictionForce2;
double m_linearFrictionDirection1[3];
double m_linearFrictionDirection2[3];
};
enum
@@ -822,6 +820,8 @@ enum eURDF_Flags
URDF_INITIALIZE_SAT_FEATURES = 4096,
URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
URDF_PARSE_SENSORS = 16384,
URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
};
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
@@ -912,4 +912,22 @@ enum eConstraintSolverTypes
eConstraintSolverLCP_BLOCK_PGS,
};
enum eFileIOActions
{
eAddFileIOAction = 1024,//avoid collision with eFileIOTypes
eRemoveFileIOAction,
};
enum eFileIOTypes
{
ePosixFileIO = 1,
eZipFileIO,
eCNSFileIO,
};
//limits for vertices/indices in PyBullet::createCollisionShape
#define B3_MAX_NUM_VERTICES 1024
#define B3_MAX_NUM_INDICES 1024
#endif //SHARED_MEMORY_PUBLIC_H

72
examples/SharedMemory/b3PluginManager.cpp
View File

@@ -5,7 +5,7 @@
#include "PhysicsClientC_API.h"
#include "PhysicsDirect.h"
#include "plugins/b3PluginContext.h"
#include "../Utils/b3BulletDefaultFileIO.h"
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#define VC_EXTRALEAN
@@ -48,6 +48,7 @@ struct b3Plugin
PFN_GET_RENDER_INTERFACE m_getRendererFunc;
PFN_GET_COLLISION_INTERFACE m_getCollisionFunc;
PFN_GET_FILEIO_INTERFACE m_getFileIOFunc;
void* m_userPointer;
@@ -65,6 +66,7 @@ struct b3Plugin
m_processClientCommandsFunc(0),
m_getRendererFunc(0),
m_getCollisionFunc(0),
m_getFileIOFunc(0),
m_userPointer(0)
{
}
@@ -84,6 +86,7 @@ struct b3Plugin
m_processClientCommandsFunc = 0;
m_getRendererFunc = 0;
m_getCollisionFunc = 0;
m_getFileIOFunc = 0;
m_userPointer = 0;
m_isInitialized = false;
}
@@ -105,9 +108,11 @@ struct b3PluginManagerInternalData
int m_activeRendererPluginUid;
int m_activeCollisionPluginUid;
int m_numNotificationPlugins;
int m_activeFileIOPluginUid;
b3BulletDefaultFileIO m_defaultFileIO;
b3PluginManagerInternalData()
: m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1), m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0)
: m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1), m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0), m_activeFileIOPluginUid(-1)
{
}
};
@@ -208,7 +213,8 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
std::string processClientCommandsStr = std::string("processClientCommands") + postFix;
std::string getRendererStr = std::string("getRenderInterface") + postFix;
std::string getCollisionStr = std::string("getCollisionInterface") + postFix;
std::string getFileIOStr = std::string("getFileIOInterface") + postFix;
plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str());
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.c_str());
plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, executePluginCommandStr.c_str());
@@ -224,6 +230,8 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
plugin->m_getRendererFunc = (PFN_GET_RENDER_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getRendererStr.c_str());
plugin->m_getCollisionFunc = (PFN_GET_COLLISION_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getCollisionStr.c_str());
plugin->m_getFileIOFunc = (PFN_GET_FILEIO_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getFileIOStr.c_str());
if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
{
@@ -293,6 +301,15 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
selectCollisionPlugin(pluginUniqueId);
}
}
//for now, automatically select the loaded plugin as active fileIO plugin.
if (pluginUniqueId >= 0)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin && plugin->m_getFileIOFunc)
{
selectFileIOPlugin(pluginUniqueId);
}
}
return pluginUniqueId;
}
@@ -438,7 +455,9 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu
return result;
}
int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin)
int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, b3PluginFunctions& functions, bool initPlugin)
{
b3Plugin orgPlugin;
@@ -447,14 +466,15 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT
pluginHandle->m_pluginHandle = 0;
pluginHandle->m_ownsPluginHandle = false;
pluginHandle->m_pluginUniqueId = pluginUniqueId;
pluginHandle->m_executeCommandFunc = executeCommandFunc;
pluginHandle->m_exitFunc = exitFunc;
pluginHandle->m_initFunc = initFunc;
pluginHandle->m_preTickFunc = preTickFunc;
pluginHandle->m_postTickFunc = postTickFunc;
pluginHandle->m_getRendererFunc = getRendererFunc;
pluginHandle->m_getCollisionFunc = getCollisionFunc;
pluginHandle->m_processClientCommandsFunc = processClientCommandsFunc;
pluginHandle->m_executeCommandFunc = functions.m_executeCommandFunc;
pluginHandle->m_exitFunc = functions.m_exitFunc;
pluginHandle->m_initFunc = functions.m_initFunc;
pluginHandle->m_preTickFunc = functions.m_preTickFunc;
pluginHandle->m_postTickFunc = functions.m_postTickFunc;
pluginHandle->m_getRendererFunc = functions.m_getRendererFunc;
pluginHandle->m_getCollisionFunc = functions.m_getCollisionFunc;
pluginHandle->m_processClientCommandsFunc = functions.m_processClientCommandsFunc;
pluginHandle->m_getFileIOFunc = functions.m_fileIoFunc;
pluginHandle->m_pluginHandle = 0;
pluginHandle->m_pluginPath = pluginPath;
pluginHandle->m_userPointer = 0;
@@ -502,6 +522,32 @@ UrdfRenderingInterface* b3PluginManager::getRenderInterface()
return renderer;
}
void b3PluginManager::selectFileIOPlugin(int pluginUniqueId)
{
m_data->m_activeFileIOPluginUid = pluginUniqueId;
}
struct CommonFileIOInterface* b3PluginManager::getFileIOInterface()
{
CommonFileIOInterface* fileIOInterface = 0;
if (m_data->m_activeFileIOPluginUid >= 0)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(m_data->m_activeFileIOPluginUid);
if (plugin && plugin->m_getFileIOFunc)
{
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
fileIOInterface = plugin->m_getFileIOFunc(&context);
}
}
if (fileIOInterface==0)
{
return &m_data->m_defaultFileIO;
}
return fileIOInterface;
}
void b3PluginManager::selectCollisionPlugin(int pluginUniqueId)
{
m_data->m_activeCollisionPluginUid = pluginUniqueId;

36
examples/SharedMemory/b3PluginManager.h
View File

@@ -10,6 +10,37 @@ enum b3PluginManagerTickMode
B3_PROCESS_CLIENT_COMMANDS_TICK,
};
struct b3PluginFunctions
{
//required
PFN_INIT m_initFunc;
PFN_EXIT m_exitFunc;
PFN_EXECUTE m_executeCommandFunc;
//optional
PFN_TICK m_preTickFunc;
PFN_TICK m_postTickFunc;
PFN_GET_RENDER_INTERFACE m_getRendererFunc;
PFN_TICK m_processClientCommandsFunc;
PFN_TICK m_processNotificationsFunc;
PFN_GET_COLLISION_INTERFACE m_getCollisionFunc;
PFN_GET_FILEIO_INTERFACE m_fileIoFunc;
b3PluginFunctions(PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc)
:m_initFunc(initFunc),
m_exitFunc(exitFunc),
m_executeCommandFunc(executeCommandFunc),
m_preTickFunc(0),
m_postTickFunc(0),
m_getRendererFunc(0),
m_processClientCommandsFunc(0),
m_processNotificationsFunc(0),
m_getCollisionFunc(0),
m_fileIoFunc(0)
{
}
};
class b3PluginManager
{
struct b3PluginManagerInternalData* m_data;
@@ -29,11 +60,14 @@ public:
void tickPlugins(double timeStep, b3PluginManagerTickMode tickMode);
int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin = true);
int registerStaticLinkedPlugin(const char* pluginPath, b3PluginFunctions& functions, bool initPlugin = true);
void selectPluginRenderer(int pluginUniqueId);
struct UrdfRenderingInterface* getRenderInterface();
void selectFileIOPlugin(int pluginUniqueId);
struct CommonFileIOInterface* getFileIOInterface();
void selectCollisionPlugin(int pluginUniqueId);
struct b3PluginCollisionInterface* getCollisionInterface();
};

7
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -190,7 +190,12 @@ bool b3RobotSimulatorClientAPI_NoDirect::changeVisualShape(const struct b3RobotS
b3SharedMemoryStatusHandle statusHandle;
int statusType;
commandHandle = b3InitUpdateVisualShape(m_data->m_physicsClientHandle, objectUniqueId, jointIndex, shapeIndex, textureUniqueId);
commandHandle = b3InitUpdateVisualShape2(m_data->m_physicsClientHandle, objectUniqueId, jointIndex, shapeIndex);
if (textureUniqueId>=-1)
{
b3UpdateVisualShapeTexture(commandHandle, textureUniqueId);
}
if (args.m_hasSpecularColor)
{

2
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -75,7 +75,7 @@ struct b3RobotSimulatorChangeVisualShapeArgs
: m_objectUniqueId(-1),
m_linkIndex(-1),
m_shapeIndex(-1),
m_textureUniqueId(-1),
m_textureUniqueId(-2),
m_rgbaColor(0, 0, 0, 1),
m_hasRgbaColor(false),
m_specularColor(1, 1, 1),

2
examples/SharedMemory/plugins/b3PluginAPI.h
View File

@@ -31,7 +31,7 @@ extern "C"
typedef B3_API_ENTRY struct UrdfRenderingInterface*(B3_API_CALL* PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct b3PluginCollisionInterface*(B3_API_CALL* PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct CommonFileIOInterface*(B3_API_CALL* PFN_GET_FILEIO_INTERFACE)(struct b3PluginContext* context);
#ifdef __cplusplus
}
#endif

1
examples/SharedMemory/plugins/b3PluginFileIOInterface.h Normal file
View File

@@ -0,0 +1 @@
#error

80
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
View File

@@ -87,8 +87,8 @@ struct EGLRendererObjectArray
}
};
#define START_WIDTH 640
#define START_HEIGHT 480
#define START_WIDTH 2560
#define START_HEIGHT 2048
struct EGLRendererVisualShapeConverterInternalData
{
@@ -270,13 +270,14 @@ void EGLRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff)
}
///todo: merge into single file with TinyRendererVisualShapeConverter
static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture3>& texturesOut, b3VisualShapeData& visualShapeOut)
static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture3>& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO, int flags)
{
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
visualShapeOut.m_dimensions[0] = 0;
visualShapeOut.m_dimensions[1] = 0;
visualShapeOut.m_dimensions[2] = 0;
memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName));
#if 0
if (visual->m_geometry.m_hasLocalMaterial)
{
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0];
@@ -284,6 +285,7 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2];
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3];
}
#endif
GLInstanceGraphicsShape* glmesh = 0;
@@ -410,8 +412,25 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP
{
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO))
{
if (flags&URDF_USE_MATERIAL_COLORS_FROM_MTL)
{
if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR)
{
visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0];
visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1];
visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2];
if (flags&URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
{
visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3];
} else
{
visualShapeOut.m_rgbaColor[3] = 1;
}
}
}
if (meshData.m_textureImage1)
{
MyTexture3 texData;
@@ -426,7 +445,7 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP
break;
}
case UrdfGeometry::FILE_STL:
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), fileIO);
break;
case UrdfGeometry::FILE_COLLADA:
{
@@ -442,7 +461,7 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
upAxis, fileIO);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
@@ -633,7 +652,7 @@ static btVector4 sColors[4] =
void EGLRendererVisualShapeConverter::convertVisualShapes(
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const UrdfLink* linkPtr, const UrdfModel* model,
int collisionObjectUniqueId, int bodyUniqueId)
int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO)
{
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
if (linkPtr)
@@ -752,7 +771,7 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
visualShape.m_rgbaColor[3] = rgbaColor[3];
{
B3_PROFILE("convertURDFToVisualShape2");
convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape);
convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags);
}
m_data->m_visualShapes.push_back(visualShape);
@@ -886,6 +905,7 @@ void EGLRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int link
m_data->m_visualShapes[i].m_rgbaColor[1] = rgbaColor[1];
m_data->m_visualShapes[i].m_rgbaColor[2] = rgbaColor[2];
m_data->m_visualShapes[i].m_rgbaColor[3] = rgbaColor[3];
m_data->m_instancingRenderer->writeSingleInstanceColorToCPU(rgbaColor,i);
}
}
@@ -983,6 +1003,16 @@ void EGLRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
}
void EGLRendererVisualShapeConverter::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
{
m_data->m_instancingRenderer->setProjectiveTextureMatrices(viewMatrix,projectionMatrix);
}
void EGLRendererVisualShapeConverter::setProjectiveTexture(bool useProjectiveTexture)
{
m_data->m_instancingRenderer->setProjectiveTexture(useProjectiveTexture);
}
//copied from OpenGLGuiHelper.cpp
void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
@@ -1044,7 +1074,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
glstat = glGetError();
b3Assert(glstat == GL_NO_ERROR);
}
if ((sourceWidth * sourceHeight * sizeof(float)) == depthBufferSizeInPixels)
if ((sourceWidth * sourceHeight) == depthBufferSizeInPixels)
{
glReadPixels(0, 0, sourceWidth, sourceHeight, GL_DEPTH_COMPONENT, GL_FLOAT, &(m_data->m_sourceDepthBuffer[0]));
int glstat;
@@ -1306,12 +1336,40 @@ int EGLRendererVisualShapeConverter::registerTexture(unsigned char* texels, int
return m_data->m_textures.size() - 1;
}
int EGLRendererVisualShapeConverter::loadTextureFile(const char* filename)
int EGLRendererVisualShapeConverter::loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO)
{
B3_PROFILE("loadTextureFile");
int width, height, n;
unsigned char* image = 0;
image = stbi_load(filename, &width, &height, &n, 3);
if (fileIO)
{
b3AlignedObjectArray<char> buffer;
buffer.reserve(1024);
int fileId = fileIO->fileOpen(filename,"rb");
if (fileId>=0)
{
int size = fileIO->getFileSize(fileId);
if (size>0)
{
buffer.resize(size);
int actual = fileIO->fileRead(fileId,&buffer[0],size);
if (actual != size)
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
}
}
}
if (buffer.size())
{
image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
}
} else
{
image = stbi_load(filename, &width, &height, &n, 3);
}
if (image && (width >= 0) && (height >= 0))
{
return registerTexture(image, width, height);

7
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h
View File

@@ -11,7 +11,7 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual ~EGLRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex);
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
virtual int getNumVisualShapes(int bodyUniqueId);
@@ -48,9 +48,12 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void render();
virtual void render(const float viewMat[16], const float projMat[16]);
virtual int loadTextureFile(const char* filename);
virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO);
virtual int registerTexture(unsigned char* texels, int width, int height);
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]);
virtual void setProjectiveTexture(bool useProjectiveTexture);
virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);
};

649
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp Normal file
View File

@@ -0,0 +1,649 @@
#include "fileIOPlugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
#include "../../../CommonInterfaces/CommonFileIOInterface.h"
#include "../../../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3HashMap.h"
#include <string.h> //memcpy/strlen
#ifndef B3_EXCLUDE_DEFAULT_FILEIO
#include "../../../Utils/b3BulletDefaultFileIO.h"
#endif //B3_EXCLUDE_DEFAULT_FILEIO
#ifdef B3_USE_ZIPFILE_FILEIO
#include "zipFileIO.h"
#endif //B3_USE_ZIPFILE_FILEIO
#ifdef B3_USE_CNS_FILEIO
#include "CNSFileIO.h"
#endif //B3_USE_CNS_FILEIO
#define B3_MAX_FILEIO_INTERFACES 1024
struct WrapperFileHandle
{
CommonFileIOInterface* childFileIO;
int m_childFileHandle;
};
struct InMemoryFile
{
char* m_buffer;
int m_fileSize;
};
struct InMemoryFileAccessor
{
InMemoryFile* m_file;
int m_curPos;
};
struct InMemoryFileIO : public CommonFileIOInterface
{
b3HashMap<b3HashString,InMemoryFile*> m_fileCache;
InMemoryFileAccessor m_fileHandles[B3_MAX_FILEIO_INTERFACES];
int m_numAllocs;
int m_numFrees;
InMemoryFileIO()
{
m_numAllocs=0;
m_numFrees=0;
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
{
m_fileHandles[i].m_curPos = 0;
m_fileHandles[i].m_file = 0;
}
}
virtual ~InMemoryFileIO()
{
clearCache();
if (m_numAllocs != m_numFrees)
{
printf("Error: InMemoryFile::~InMemoryFileIO (numAllocs %d numFrees %d\n", m_numAllocs, m_numFrees);
}
}
void clearCache()
{
for (int i=0;i<m_fileCache.size();i++)
{
InMemoryFile** memPtr = m_fileCache.getAtIndex(i);
if (memPtr && *memPtr)
{
InMemoryFile* mem = *memPtr;
freeBuffer(mem->m_buffer);
m_numFrees++;
delete (mem);
m_numFrees++;
}
}
}
char* allocateBuffer(int len)
{
char* buffer = 0;
if (len)
{
m_numAllocs++;
buffer = new char[len];
}
return buffer;
}
void freeBuffer(char* buffer)
{
delete[] buffer;
}
virtual int registerFile(const char* fileName, char* buffer, int len)
{
m_numAllocs++;
InMemoryFile* f = new InMemoryFile();
f->m_buffer = buffer;
f->m_fileSize = len;
b3HashString key(fileName);
m_fileCache.insert(key,f);
return 0;
}
void removeFileFromCache(const char* fileName)
{
InMemoryFile* f = getInMemoryFile(fileName);
if (f)
{
m_fileCache.remove(fileName);
freeBuffer(f->m_buffer);
delete (f);
}
}
InMemoryFile* getInMemoryFile(const char* fileName)
{
InMemoryFile** fPtr = m_fileCache[fileName];
if (fPtr && *fPtr)
{
return *fPtr;
}
return 0;
}
virtual int fileOpen(const char* fileName, const char* mode)
{
//search a free slot
int slot = -1;
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
{
if (m_fileHandles[i].m_file==0)
{
slot=i;
break;
}
}
if (slot>=0)
{
InMemoryFile* f = getInMemoryFile(fileName);
if (f)
{
m_fileHandles[slot].m_curPos = 0;
m_fileHandles[slot].m_file = f;
} else
{
slot=-1;
}
}
//printf("InMemoryFileIO fileOpen %s, %d\n", fileName, slot);
return slot;
}
virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
//if (numBytes>1)
// printf("curPos = %d\n", f.m_curPos);
if (f.m_curPos+numBytes <= f.m_file->m_fileSize)
{
memcpy(destBuffer,f.m_file->m_buffer+f.m_curPos,numBytes);
f.m_curPos+=numBytes;
//if (numBytes>1)
// printf("read %d bytes, now curPos = %d\n", numBytes, f.m_curPos);
return numBytes;
} else
{
if (numBytes!=1)
{
printf("InMemoryFileIO::fileRead Attempt to read beyond end of file\n");
}
}
}
}
return 0;
}
virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
{
return 0;
}
virtual void fileClose(int fileHandle)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
m_fileHandles[fileHandle].m_file = 0;
m_fileHandles[fileHandle].m_curPos = 0;
//printf("InMemoryFileIO fileClose %d\n", fileHandle);
}
}
}
virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)
{
InMemoryFile* f = getInMemoryFile(fileName);
int fileNameLen = strlen(fileName);
if (f && fileNameLen<(resourcePathMaxNumBytes-1))
{
memcpy(resourcePathOut, fileName, fileNameLen);
resourcePathOut[fileNameLen]=0;
return true;
}
return false;
}
virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
{
int numRead = 0;
int endOfFile = 0;
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES )
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
//return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]);
char c = 0;
do
{
int bytesRead = fileRead(fileHandle,&c,1);
if (bytesRead != 1)
{
endOfFile = 1;
c=0;
}
if (c && c!='\n')
{
if (c!=13)
{
destBuffer[numRead++]=c;
} else
{
destBuffer[numRead++]=0;
}
}
} while (c != 0 && c != '\n' && numRead<(numBytes-1));
}
}
if (numRead==0 && endOfFile)
{
return 0;
}
if (numRead<numBytes)
{
if (numRead >=0)
{
destBuffer[numRead]=0;
}
return &destBuffer[0];
} else
{
if (endOfFile==0)
{
printf("InMemoryFileIO::readLine readLine warning: numRead=%d, numBytes=%d\n", numRead, numBytes);
}
}
return 0;
}
virtual int getFileSize(int fileHandle)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES )
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
return f.m_file->m_fileSize;
}
}
return 0;
}
};
struct WrapperFileIO : public CommonFileIOInterface
{
CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES];
int m_numWrapperInterfaces;
WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES];
InMemoryFileIO m_cachedFiles;
WrapperFileIO()
:m_numWrapperInterfaces(0)
{
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
m_availableFileIOInterfaces[i]=0;
m_wrapperFileHandles[i].childFileIO=0;
m_wrapperFileHandles[i].m_childFileHandle=0;
}
//addFileIOInterface(&m_cachedFiles);
}
virtual ~WrapperFileIO()
{
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
removeFileIOInterface(i);
}
}
int addFileIOInterface(CommonFileIOInterface* fileIO)
{
int result = -1;
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
if (m_availableFileIOInterfaces[i]==0)
{
m_availableFileIOInterfaces[i]=fileIO;
result = i;
break;
}
}
return result;
}
void removeFileIOInterface(int fileIOIndex)
{
if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
{
if (m_availableFileIOInterfaces[fileIOIndex])
{
delete m_availableFileIOInterfaces[fileIOIndex];
m_availableFileIOInterfaces[fileIOIndex]=0;
}
}
}
virtual int fileOpen(const char* fileName, const char* mode)
{
//find an available wrapperFileHandle slot
int wrapperFileHandle=-1;
int slot = -1;
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
if (m_wrapperFileHandles[i].childFileIO==0)
{
slot=i;
break;
}
}
if (slot>=0)
{
//first check the cache
int cacheHandle = m_cachedFiles.fileOpen(fileName, mode);
if (cacheHandle>=0)
{
m_cachedFiles.fileClose(cacheHandle);
}
if (cacheHandle<0)
{
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i];
if (childFileIO)
{
int childHandle = childFileIO->fileOpen(fileName, mode);
if (childHandle>=0)
{
int fileSize = childFileIO->getFileSize(childHandle);
char* buffer = 0;
if (fileSize)
{
buffer = m_cachedFiles.allocateBuffer(fileSize);
if (buffer)
{
int readBytes = childFileIO->fileRead(childHandle, buffer, fileSize);
if (readBytes!=fileSize)
{
if (readBytes<fileSize)
{
fileSize = readBytes;
} else
{
printf("WrapperFileIO error: reading more bytes (%d) then reported file size (%d) of file %s.\n", readBytes, fileSize, fileName);
}
}
} else
{
fileSize=0;
}
}
//potentially register a zero byte file, or files that only can be read partially
m_cachedFiles.registerFile(fileName,buffer, fileSize);
childFileIO->fileClose(childHandle);
break;
}
}
}
}
{
int childHandle = m_cachedFiles.fileOpen(fileName, mode);
if (childHandle>=0)
{
wrapperFileHandle = slot;
m_wrapperFileHandles[slot].childFileIO = &m_cachedFiles;
m_wrapperFileHandles[slot].m_childFileHandle = childHandle;
} else
{
//figure out what wrapper interface to use
//use the first one that can open the file
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i];
if (childFileIO)
{
int childHandle = childFileIO->fileOpen(fileName, mode);
if (childHandle>=0)
{
wrapperFileHandle = slot;
m_wrapperFileHandles[slot].childFileIO = childFileIO;
m_wrapperFileHandles[slot].m_childFileHandle = childHandle;
break;
}
}
}
}
}
}
return wrapperFileHandle;
}
virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
{
int fileReadResult=-1;
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
fileReadResult = m_wrapperFileHandles[fileHandle].childFileIO->fileRead(
m_wrapperFileHandles[fileHandle].m_childFileHandle, destBuffer, numBytes);
}
}
return fileReadResult;
}
virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
{
//todo
return -1;
}
virtual void fileClose(int fileHandle)
{
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
m_wrapperFileHandles[fileHandle].childFileIO->fileClose(
m_wrapperFileHandles[fileHandle].m_childFileHandle);
m_wrapperFileHandles[fileHandle].childFileIO = 0;
m_wrapperFileHandles[fileHandle].m_childFileHandle = -1;
}
}
}
virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)
{
if (m_cachedFiles.findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes))
return true;
bool found = false;
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
if (m_availableFileIOInterfaces[i])
{
found = m_availableFileIOInterfaces[i]->findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes);
}
if (found)
break;
}
return found;
}
virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
{
char* result = 0;
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
result = m_wrapperFileHandles[fileHandle].childFileIO->readLine(
m_wrapperFileHandles[fileHandle].m_childFileHandle,
destBuffer, numBytes);
}
}
return result;
}
virtual int getFileSize(int fileHandle)
{
int numBytes = 0;
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
numBytes = m_wrapperFileHandles[fileHandle].childFileIO->getFileSize(
m_wrapperFileHandles[fileHandle].m_childFileHandle);
}
}
return numBytes;
}
};
struct FileIOClass
{
int m_testData;
WrapperFileIO m_fileIO;
FileIOClass()
: m_testData(42),
m_fileIO()
{
}
virtual ~FileIOClass()
{
}
};
B3_SHARED_API int initPlugin_fileIOPlugin(struct b3PluginContext* context)
{
FileIOClass* obj = new FileIOClass();
context->m_userPointer = obj;
#ifndef B3_EXCLUDE_DEFAULT_FILEIO
obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO());
#endif //B3_EXCLUDE_DEFAULT_FILEIO
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
int result=-1;
FileIOClass* obj = (FileIOClass*)context->m_userPointer;
printf("text argument:%s\n", arguments->m_text);
printf("int args: [");
//remove a fileIO type
if (arguments->m_numInts==1)
{
int fileIOIndex = arguments->m_ints[0];
obj->m_fileIO.removeFileIOInterface(fileIOIndex);
}
if (arguments->m_numInts==2)
{
int action = arguments->m_ints[0];
switch (action)
{
case eAddFileIOAction:
{
//create new fileIO interface
int fileIOType = arguments->m_ints[1];
switch (fileIOType)
{
case ePosixFileIO:
{
#ifdef B3_EXCLUDE_DEFAULT_FILEIO
printf("ePosixFileIO is not enabled in this build.\n");
#else
obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO());
#endif
break;
}
case eZipFileIO:
{
#ifdef B3_USE_ZIPFILE_FILEIO
if (arguments->m_text)
{
obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text, &obj->m_fileIO));
}
#else
printf("eZipFileIO is not enabled in this build.\n");
#endif
break;
}
case eCNSFileIO:
{
#ifdef B3_USE_CNS_FILEIO
if (strlen(arguments->m_text))
{
obj->m_fileIO.addFileIOInterface(new CNSFileIO(arguments->m_text));
}
else
{
obj->m_fileIO.addFileIOInterface(new CNSFileIO(""));
}
#else//B3_USE_CNS_FILEIO
printf("CNSFileIO is not enabled in this build.\n");
#endif //B3_USE_CNS_FILEIO
break;
}
default:
{
}
}
break;
}
case eRemoveFileIOAction:
{
//remove fileIO interface
int fileIOIndex = arguments->m_ints[1];
obj->m_fileIO.removeFileIOInterface(fileIOIndex);
break;
}
default:
{
printf("executePluginCommand_fileIOPlugin: unknown action\n");
}
}
}
return result;
}
B3_SHARED_API struct CommonFileIOInterface* getFileIOFunc_fileIOPlugin(struct b3PluginContext* context)
{
FileIOClass* obj = (FileIOClass*)context->m_userPointer;
return &obj->m_fileIO;
}
B3_SHARED_API void exitPlugin_fileIOPlugin(struct b3PluginContext* context)
{
FileIOClass* obj = (FileIOClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

24
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef FILE_IO_PLUGIN_H
#define FILE_IO_PLUGIN_H
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_fileIOPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_fileIOPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct CommonFileIOInterface* getFileIOFunc_fileIOPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif //#define FILE_IO_PLUGIN_H

254
examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h Normal file
View File

@@ -0,0 +1,254 @@
#include "minizip/unzip.h"
#define B3_ZIP_FILEIO_MAX_FILES 1024
struct ZipFileIO : public CommonFileIOInterface
{
std::string m_zipfileName;
unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ];
int m_numFileHandles;
ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
:m_zipfileName(zipfileName),
m_numFileHandles(0)
{
for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
{
m_fileHandles[i]=0;
}
}
static bool FileIOPluginFindFile(void* userPtr, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
ZipFileIO* fileIo = (ZipFileIO*) userPtr;
return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
}
virtual ~ZipFileIO()
{
for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES;i++)
{
fileClose(i);
}
}
virtual int fileOpen(const char* fileName, const char* mode)
{
//search a free slot
int slot = -1;
for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
{
if (m_fileHandles[i]==0)
{
slot=i;
break;
}
}
if (slot>=0)
{
unzFile zipfile;
unz_global_info m_global_info;
zipfile = unzOpen(m_zipfileName.c_str());
if (zipfile == NULL)
{
printf("%s: not found\n", m_zipfileName.c_str());
slot = -1;
} else
{
int result = 0;
result = unzGetGlobalInfo(zipfile, &m_global_info );
if (result != UNZ_OK)
{
printf("could not read file global info from %s\n", m_zipfileName.c_str());
unzClose(zipfile);
zipfile = 0;
slot = -1;
}
}
if (slot >=0)
{
int result = unzLocateFile(zipfile, fileName, 0);
if (result == UNZ_OK)
{
unz_file_info info;
result = unzGetCurrentFileInfo(zipfile, &info, NULL, 0, NULL, 0, NULL, 0);
if (result != UNZ_OK)
{
printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result);
slot=-1;
unzClose(zipfile);
zipfile = 0;
}
else
{
result = unzOpenCurrentFile(zipfile);
if (result == UNZ_OK)
{
printf("zipFile::fileOpen %s in mode %s in fileHandle %d\n", fileName, mode, slot);
m_fileHandles[slot] = zipfile;
} else
{
slot=-1;
unzClose(zipfile);
zipfile = 0;
}
}
} else
{
slot=-1;
unzClose(zipfile);
zipfile = 0;
}
}
}
return slot;
}
virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
{
int result = -1;
if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
{
unzFile f = m_fileHandles[fileHandle];
if (f)
{
result = unzReadCurrentFile(f, destBuffer,numBytes);
//::fread(destBuffer, 1, numBytes, f);
}
}
return result;
}
virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
{
#if 0
if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
{
FILE* f = m_fileHandles[fileHandle];
if (f)
{
return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]);
}
}
#endif
return -1;
}
virtual void fileClose(int fileHandle)
{
if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
{
unzFile f = m_fileHandles[fileHandle];
if (f)
{
printf("zipFile::fileClose slot %d\n", fileHandle);
unzClose(f);
m_fileHandles[fileHandle]=0;
}
}
}
virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes)
{
return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, ZipFileIO::FileIOPluginFindFile, this);
}
virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
int fileHandle = -1;
fileHandle = fileOpen(orgFileName, "rb");
if (fileHandle>=0)
{
//printf("original file found: [%s]\n", orgFileName);
sprintf(relativeFileName, "%s", orgFileName);
fileClose(fileHandle);
return true;
}
//printf("Trying various directories, relative to current working directory\n");
const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
int f = 0;
bool fileFound = false;
for (int i = 0; !f && i < numPrefixes; i++)
{
#ifdef _MSC_VER
sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
#else
sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
#endif
f = fileOpen(relativeFileName, "rb");
if (f>=0)
{
fileFound = true;
break;
}
}
if (f>=0)
{
fileClose(f);
}
return fileFound;
}
virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
{
int numRead = 0;
if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
{
unzFile f = m_fileHandles[fileHandle];
if (f)
{
//return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]);
char c = 0;
do
{
fileRead(fileHandle,&c,1);
if (c && c!='\n')
{
if (c!=13)
{
destBuffer[numRead++]=c;
} else
{
destBuffer[numRead++]=0;
}
}
} while (c != 0 && c != '\n' && numRead<(numBytes-1));
}
}
if (numRead<numBytes && numRead>0)
{
destBuffer[numRead]=0;
return &destBuffer[0];
}
return 0;
}
virtual int getFileSize(int fileHandle)
{
int size=0;
if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
{
unzFile f = m_fileHandles[fileHandle];
if (f)
{
unz_file_info info;
int result = unzGetCurrentFileInfo(f, &info, NULL, 0, NULL, 0, NULL, 0);
if (result == UNZ_OK)
{
size = info.uncompressed_size;
}
}
}
return size;
}
};

99
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View File

@@ -182,13 +182,14 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
m_data->m_hasLightSpecularCoeff = true;
}
void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO, int flags)
{
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
visualShapeOut.m_dimensions[0] = 0;
visualShapeOut.m_dimensions[1] = 0;
visualShapeOut.m_dimensions[2] = 0;
memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName));
#if 0
if (visual->m_geometry.m_hasLocalMaterial)
{
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0];
@@ -196,6 +197,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2];
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3];
}
#endif
GLInstanceGraphicsShape* glmesh = 0;
@@ -318,12 +320,34 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
switch (visual->m_geometry.m_meshFileType)
{
case UrdfGeometry::MEMORY_VERTICES:
{
break;
}
case UrdfGeometry::FILE_OBJ:
{
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO))
{
if (flags&URDF_USE_MATERIAL_COLORS_FROM_MTL)
{
if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR)
{
visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0];
visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1];
visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2];
if (flags&URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
{
visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3];
} else
{
visualShapeOut.m_rgbaColor[3] = 1;
}
}
}
if (meshData.m_textureImage1)
{
MyTexture2 texData;
@@ -338,7 +362,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
break;
}
case UrdfGeometry::FILE_STL:
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), fileIO);
break;
case UrdfGeometry::FILE_COLLADA:
{
@@ -354,7 +378,8 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
upAxis,
fileIO);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
@@ -421,8 +446,10 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
}
default:
// should never get here (findExistingMeshFile returns false if it doesn't recognize extension)
btAssert(0);
{
// should never get here (findExistingMeshFile returns false if it doesn't recognize extension)
btAssert(0);
}
}
if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices > 0))
@@ -437,7 +464,10 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
}
else
{
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str());
if (visual->m_geometry.m_meshFileType !=UrdfGeometry::MEMORY_VERTICES)
{
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str());
}
}
break;
} // case mesh
@@ -540,9 +570,10 @@ static btVector4 sColors[4] =
};
void TinyRendererVisualShapeConverter::convertVisualShapes(
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const UrdfLink* linkPtr, const UrdfModel* model,
int collisionObjectUniqueId, int bodyUniqueId)
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId,
int bodyUniqueId, struct CommonFileIOInterface* fileIO)
{
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
if (linkPtr)
@@ -663,9 +694,14 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
{
B3_PROFILE("convertURDFToVisualShape");
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape);
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags);
}
rgbaColor[0] = visualShape.m_rgbaColor[0];
rgbaColor[1] = visualShape.m_rgbaColor[1];
rgbaColor[2] = visualShape.m_rgbaColor[2];
rgbaColor[3] = visualShape.m_rgbaColor[3];
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex);
@@ -1132,7 +1168,7 @@ void TinyRendererVisualShapeConverter::resetAll()
void TinyRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
{
btAssert(textureUniqueId < m_data->m_textures.size());
if (textureUniqueId >= 0 && textureUniqueId < m_data->m_textures.size())
if (textureUniqueId >= -1 && textureUniqueId < m_data->m_textures.size())
{
for (int n = 0; n < m_data->m_swRenderInstances.size(); n++)
{
@@ -1149,7 +1185,13 @@ void TinyRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, in
if ((shapeIndex < 0) || (shapeIndex == v))
{
renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height);
if (textureUniqueId>=0)
{
renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height);
} else
{
renderObj->m_model->setDiffuseTextureFromData(0,0,0);
}
}
}
}
@@ -1168,12 +1210,39 @@ int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int
return m_data->m_textures.size() - 1;
}
int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO)
{
B3_PROFILE("loadTextureFile");
int width, height, n;
unsigned char* image = 0;
image = stbi_load(filename, &width, &height, &n, 3);
if (fileIO)
{
b3AlignedObjectArray<char> buffer;
buffer.reserve(1024);
int fileId = fileIO->fileOpen(filename,"rb");
if (fileId>=0)
{
int size = fileIO->getFileSize(fileId);
if (size>0)
{
buffer.resize(size);
int actual = fileIO->fileRead(fileId,&buffer[0],size);
if (actual != size)
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
}
}
}
if (buffer.size())
{
image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
}
} else
{
image = stbi_load(filename, &width, &height, &n, 3);
}
if (image && (width >= 0) && (height >= 0))
{
return registerTexture(image, width, height);

4
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h
View File

@@ -11,7 +11,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
virtual ~TinyRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex);
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
virtual int getNumVisualShapes(int bodyUniqueId);
@@ -47,7 +47,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void render();
virtual void render(const float viewMat[16], const float projMat[16]);
virtual int loadTextureFile(const char* filename);
virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO);
virtual int registerTexture(unsigned char* texels, int width, int height);
virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);

5
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp
View File

@@ -41,8 +41,7 @@ B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
printf("hi vrSyncPlugin!\n");
return SHARED_MEMORY_MAGIC_NUMBER;
}
@@ -192,6 +191,4 @@ B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context)
MyClass* obj = (MyClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
printf("bye vrSyncPlugin!\n");
}

84
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -22,13 +22,18 @@
#include <string>
#include <vector>
#include <map>
#ifdef USE_STREAM
#include <fstream>
#else
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#endif
#include <sstream>
#include "tiny_obj_loader.h"
#include <stdio.h>
namespace tinyobj
{
#ifdef USE_STREAM
//See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
std::istream& safeGetline(std::istream& is, std::string& t)
{
@@ -64,7 +69,7 @@ std::istream& safeGetline(std::istream& is, std::string& t)
}
}
}
#endif
struct vertex_index
{
int v_idx, vt_idx, vn_idx, dummy;
@@ -340,7 +345,8 @@ void InitMaterial(material_t& material)
std::string LoadMtl(
std::map<std::string, material_t>& material_map,
const char* filename,
const char* mtl_basepath)
const char* mtl_basepath,
CommonFileIOInterface* fileIO)
{
material_map.clear();
std::stringstream err;
@@ -355,22 +361,44 @@ std::string LoadMtl(
{
filepath = std::string(filename);
}
#ifdef USE_STREAM
std::ifstream ifs(filepath.c_str());
if (!ifs)
{
err << "Cannot open file [" << filepath << "]" << std::endl;
return err.str();
}
#else
int fileHandle = fileIO->fileOpen(filepath.c_str(),"r");
if (fileHandle<0)
{
err << "Cannot open file [" << filepath << "]" << std::endl;
return err.str();
}
#endif
material_t material;
int maxchars = 8192; // Alloc enough size.
std::vector<char> buf(maxchars); // Alloc enough size.
#ifdef USE_STREAM
while (ifs.peek() != -1)
#else
char* line = 0;
do
#endif
{
std::string linebuf;
#ifdef USE_STREAM
safeGetline(ifs, linebuf);
#else
char tmpBuf[1024];
line = fileIO->readLine(fileHandle, tmpBuf, 1024);
if (line)
{
linebuf=line;
}
#endif
// Trim newline '\r\n' or '\r'
if (linebuf.size() > 0)
@@ -379,7 +407,7 @@ std::string LoadMtl(
}
if (linebuf.size() > 0)
{
if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
if (linebuf[linebuf.size() - 1] == '\r') linebuf.erase(linebuf.size() - 1);
}
// Skip if empty line.
@@ -389,11 +417,10 @@ std::string LoadMtl(
}
linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1);
// Skip leading space.
const char* token = linebuf.c_str();
token += strspn(token, " \t");
assert(token);
if (token[0] == '\0') continue; // empty line
@@ -546,9 +573,16 @@ std::string LoadMtl(
material.unknown_parameter.insert(std::pair<std::string, std::string>(key, value));
}
}
#ifndef USE_STREAM
while (line);
#endif
// flush last material.
material_map.insert(std::pair<std::string, material_t>(material.name, material));
if (fileHandle)
{
fileIO->fileClose(fileHandle);
}
return err.str();
}
@@ -556,7 +590,8 @@ std::string
LoadObj(
std::vector<shape_t>& shapes,
const char* filename,
const char* mtl_basepath)
const char* mtl_basepath,
CommonFileIOInterface* fileIO)
{
std::string tmp = filename;
if (!mtl_basepath)
@@ -577,13 +612,21 @@ LoadObj(
MyIndices face;
std::stringstream err;
#ifdef USE_STREAM
std::ifstream ifs(filename);
if (!ifs)
{
err << "Cannot open file [" << filename << "]" << std::endl;
return err.str();
}
#else
int fileHandle = fileIO->fileOpen(filename,"r");
if (fileHandle<0)
{
err << "Cannot open file [" << filename << "]" << std::endl;
return err.str();
}
#endif
std::vector<float> v;
v.reserve(1024 * 1024);
@@ -606,10 +649,24 @@ LoadObj(
std::string linebuf;
linebuf.reserve(maxchars);
#ifdef USE_STREAM
while (ifs.peek() != -1)
#else
char* line = 0;
do
#endif
{
linebuf.resize(0);
#ifdef USE_STREAM
safeGetline(ifs, linebuf);
#else
char tmpBuf[1024];
line = fileIO->readLine(fileHandle, tmpBuf, 1024);
if (line)
{
linebuf=line;
}
#endif
// Trim newline '\r\n' or '\r'
if (linebuf.size() > 0)
@@ -720,7 +777,7 @@ LoadObj(
token += 7;
sscanf(token, "%s", namebuf);
std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath);
std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath,fileIO);
if (!err_mtl.empty())
{
//faceGroup.resize(0); // for safety
@@ -789,6 +846,9 @@ LoadObj(
// Ignore unknown command.
}
#ifndef USE_STREAM
while (line);
#endif
shape_t shape;
bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
@@ -798,6 +858,10 @@ LoadObj(
}
faceGroup.resize(0); // for safety
if (fileHandle)
{
fileIO->fileClose(fileHandle);
}
return err.str();
}

11
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
View File

@@ -10,6 +10,8 @@
#include <vector>
#include <map>
struct CommonFileIOInterface;
namespace tinyobj
{
typedef struct
@@ -51,10 +53,19 @@ typedef struct
/// The function returns error string.
/// Returns empty string when loading .obj success.
/// 'mtl_basepath' is optional, and used for base path for .mtl file.
#ifdef USE_STREAM
std::string LoadObj(
std::vector<shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath = NULL);
#else
std::string
LoadObj(
std::vector<shape_t>& shapes,
const char* filename,
const char* mtl_basepath,
CommonFileIOInterface* fileIO);
#endif
}; // namespace tinyobj

20
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -13,6 +13,8 @@
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
#include "Bullet3Common/b3Logging.h"
#include "../CommonInterfaces/CommonFileIOInterface.h"
#include "../Utils/b3BulletDefaultFileIO.h"
struct DepthShader : public IShader
{
@@ -264,11 +266,11 @@ TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedOb
m_lightSpecularCoeff = 0.05;
}
void TinyRenderObjectData::loadModel(const char* fileName)
void TinyRenderObjectData::loadModel(const char* fileName, CommonFileIOInterface* fileIO)
{
//todo(erwincoumans) move the file loading out of here
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
if (!fileIO->findResourcePath(fileName, relativeFileName, 1024))
{
printf("Cannot find file %s\n", fileName);
}
@@ -335,7 +337,7 @@ void TinyRenderObjectData::registerMeshShape(const float* vertices, int numVerti
}
}
void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices)
void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices, CommonFileIOInterface* fileIO)
{
if (0 == m_model)
{
@@ -344,7 +346,7 @@ void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertic
m_model = new Model();
char relativeFileName[1024];
if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
if (fileIO->findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
{
m_model->loadDiffuseTexture(relativeFileName);
}
@@ -368,12 +370,18 @@ void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertic
}
}
void TinyRenderObjectData::createCube(float halfExtentsX, float halfExtentsY, float halfExtentsZ)
void TinyRenderObjectData::createCube(float halfExtentsX, float halfExtentsY, float halfExtentsZ, CommonFileIOInterface* fileIO)
{
b3BulletDefaultFileIO defaultFileIO;
if (fileIO==0)
{
fileIO = &defaultFileIO;
}
m_model = new Model();
char relativeFileName[1024];
if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
if (fileIO->findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
{
m_model->loadDiffuseTexture(relativeFileName);
}

6
examples/TinyRenderer/TinyRenderer.h
View File

@@ -41,12 +41,12 @@ struct TinyRenderObjectData
TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedObjectArray<float>& depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex, int linkIndex);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);
void createCube(float HalfExtentsX, float HalfExtentsY, float HalfExtentsZ);
void loadModel(const char* fileName, struct CommonFileIOInterface* fileIO);
void createCube(float HalfExtentsX, float HalfExtentsY, float HalfExtentsZ, struct CommonFileIOInterface* fileIO=0);
void registerMeshShape(const float* vertices, int numVertices, const int* indices, int numIndices, const float rgbaColor[4],
unsigned char* textureImage = 0, int textureWidth = 0, int textureHeight = 0);
void registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices);
void registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals, btAlignedObjectArray<int>& indices, struct CommonFileIOInterface* fileIO);
void* m_userData;
int m_userIndex;

184
examples/Utils/b3BulletDefaultFileIO.h Normal file
View File

@@ -0,0 +1,184 @@
#ifndef B3_BULLET_DEFAULT_FILE_IO_H
#define B3_BULLET_DEFAULT_FILE_IO_H
#include "../CommonInterfaces/CommonFileIOInterface.h"
#include "b3ResourcePath.h"
#include <stdio.h>
#define B3_FILEIO_MAX_FILES 1024
struct b3BulletDefaultFileIO : public CommonFileIOInterface
{
static bool FileIOPluginFindFile(void* userPtr, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
b3BulletDefaultFileIO* fileIo = (b3BulletDefaultFileIO*) userPtr;
return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
}
FILE* m_fileHandles[B3_FILEIO_MAX_FILES];
int m_numFileHandles;
b3BulletDefaultFileIO()
:m_numFileHandles(0)
{
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
{
m_fileHandles[i]=0;
}
}
virtual ~b3BulletDefaultFileIO()
{
}
virtual int fileOpen(const char* fileName, const char* mode)
{
//search a free slot
int slot = -1;
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
{
if (m_fileHandles[i]==0)
{
slot=i;
break;
}
}
if (slot>=0)
{
FILE*f = ::fopen(fileName, mode);
if (f)
{
m_fileHandles[slot]=f;
} else
{
slot=-1;
}
}
return slot;
}
virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
FILE* f = m_fileHandles[fileHandle];
if (f)
{
int readBytes = ::fread(destBuffer, 1, numBytes, f);
return readBytes;
}
}
return -1;
}
virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
FILE* f = m_fileHandles[fileHandle];
if (f)
{
return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]);
}
}
return -1;
}
virtual void fileClose(int fileHandle)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
FILE* f = m_fileHandles[fileHandle];
if (f)
{
::fclose(f);
m_fileHandles[fileHandle]=0;
}
}
}
virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes)
{
return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this);
}
virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
FILE* f = 0;
f = fopen(orgFileName, "rb");
if (f)
{
//printf("original file found: [%s]\n", orgFileName);
sprintf(relativeFileName, "%s", orgFileName);
fclose(f);
return true;
}
//printf("Trying various directories, relative to current working directory\n");
const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
f = 0;
bool fileFound = false;
for (int i = 0; !f && i < numPrefixes; i++)
{
#ifdef _MSC_VER
sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
#else
sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
#endif
f = fopen(relativeFileName, "rb");
if (f)
{
fileFound = true;
break;
}
}
if (f)
{
fclose(f);
}
return fileFound;
}
virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
FILE* f = m_fileHandles[fileHandle];
if (f)
{
char* txt = ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]);
for (int i=0;i<numBytes;i++)
{
if (destBuffer[i]=='\r'||destBuffer[i]=='\n' || destBuffer[i]==0)
{
destBuffer[i] = 0;
break;
}
}
return txt;
}
}
return 0;
}
virtual int getFileSize(int fileHandle)
{
int size = 0;
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
FILE* f = m_fileHandles[fileHandle];
if (f)
{
if (fseek(f, 0, SEEK_END) || (size = ftell(f)) == EOF || fseek(f, 0, SEEK_SET))
{
printf("Error: Cannot access file to determine size\n");
}
}
}
return size;
}
};
#endif //B3_BULLET_DEFAULT_FILE_IO_H

21
examples/Utils/b3ResourcePath.cpp
View File

@@ -87,12 +87,21 @@ void b3ResourcePath::setAdditionalSearchPath(const char* path)
}
}
int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes)
bool b3MyFindFile(void* userPointer, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
return b3FileUtils::findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
}
int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes, PFN_FIND_FILE findFile, void* userPointer)
{
if (findFile==0)
{
findFile=b3MyFindFile;
}
//first find in a resource/<exeName> location, then in various folders within 'data' using b3FileUtils
char exePath[B3_MAX_EXE_PATH_LEN];
bool res = b3FileUtils::findFile(resourceName, resourcePathOut, resourcePathMaxNumBytes);
bool res = findFile(userPointer, resourceName, resourcePathOut, resourcePathMaxNumBytes);
if (res)
{
return strlen(resourcePathOut);
@@ -104,7 +113,7 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
char* resourcePathIn = tmpPath.m_path;
sprintf(resourcePathIn, "%s/%s", sAdditionalSearchPath, resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePathOut);
}
@@ -122,20 +131,20 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
char* resourcePathIn = tmpPath.m_path;
sprintf(resourcePathIn, "%s../data/%s", pathToExe, resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePathOut);
}
sprintf(resourcePathIn, "%s../resources/%s/%s", pathToExe, &exePath[exeNamePos], resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePathOut);
}
sprintf(resourcePathIn, "%s.runfiles/google3/third_party/bullet/data/%s", exePath, resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
if (findFile(userPointer, resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePathOut);
}

4
examples/Utils/b3ResourcePath.h
View File

@@ -3,11 +3,13 @@
#include <string>
typedef bool (* PFN_FIND_FILE)(void* userPointer, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen);
class b3ResourcePath
{
public:
static int getExePath(char* path, int maxPathLenInBytes);
static int findResourcePath(const char* sourceName, char* resourcePath, int maxResourcePathLenInBytes);
static int findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes, PFN_FIND_FILE findFile, void* userPointer=0);
static void setAdditionalSearchPath(const char* path);
};
#endif

34
examples/pybullet/examples/contactFriction.py Normal file
View File

@@ -0,0 +1,34 @@
import pybullet as p
p.connect(p.GUI)
useMaximalCoordinates = False
p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates )
#p.loadURDF("sphere2.urdf",[0,0,1])
p.loadURDF("cube.urdf",[0,0,1], useMaximalCoordinates=useMaximalCoordinates )
p.setGravity(0,3,-10)
while(1):
p.stepSimulation()
pts = p.getContactPoints()
print("num pts=",len(pts))
totalNormalForce = 0
totalFrictionForce = [0,0,0]
totalLateralFrictionForce=[0,0,0]
for pt in pts:
#print("pt.normal=",pt[7])
#print("pt.normalForce=",pt[9])
totalNormalForce += pt[9]
#print("pt.lateralFrictionA=",pt[10])
#print("pt.lateralFrictionADir=",pt[11])
#print("pt.lateralFrictionB=",pt[12])
#print("pt.lateralFrictionBDir=",pt[13])
totalLateralFrictionForce[0]+=pt[11][0]*pt[10]+pt[13][0]*pt[12]
totalLateralFrictionForce[1]+=pt[11][1]*pt[10]+pt[13][1]*pt[12]
totalLateralFrictionForce[2]+=pt[11][2]*pt[10]+pt[13][2]*pt[12]
print("totalNormalForce=",totalNormalForce)
print("totalLateralFrictionForce=",totalLateralFrictionForce)

35
examples/pybullet/examples/createObstacleCourse.py
View File

@@ -12,10 +12,39 @@ p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]);
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
#a few different ways to create a mesh:
#convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj")
#concave mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
verts=[[-0.246350, -0.246483, -0.000624],
[ -0.151407, -0.176325, 0.172867],
[ -0.246350, 0.249205, -0.000624],
[ -0.151407, 0.129477, 0.172867],
[ 0.249338, -0.246483, -0.000624],
[ 0.154395, -0.176325, 0.172867],
[ 0.249338, 0.249205, -0.000624],
[ 0.154395, 0.129477, 0.172867]]
#convex mesh from vertices
stoneConvexId = p.createCollisionShape(p.GEOM_MESH,vertices=verts)
indices=[0,3,2,3,6,2,7,4,6,5,0,4,6,0,2,3,5,7,0,1,3,3,7,6,7,5,4,5,1,0,6,4,0,3,1,5]
#concave mesh from vertices+indices
stoneConcaveId = p.createCollisionShape(p.GEOM_MESH,vertices=verts, indices=indices)
stoneId = stoneConvexId
#stoneId = stoneConcaveId
boxHalfLength = 0.5
boxHalfWidth = 2.5
boxHalfHeight = 0.1
@@ -72,10 +101,10 @@ for i in range (segmentLength):
p.changeDynamics(boxId,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range (p.getNumJoints(boxId)):
targetVelocity = 1
targetVelocity = 10
if (i%2):
targetVelocity =-1
p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=10)
targetVelocity =-10
p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100)
segmentStart=segmentStart-1.1

100
examples/pybullet/examples/getCameraImageTest.py Normal file
View File

@@ -0,0 +1,100 @@
import matplotlib.pyplot as plt
import numpy as np
import pybullet as p
import time
direct = p.connect(p.GUI)#, options="--window_backend=2 --render_device=0")
#egl = p.loadPlugin("eglRendererPlugin")
p.loadURDF('plane.urdf')
p.loadURDF("r2d2.urdf",[0,0,1])
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
p.changeVisualShape(cube_trans,-1,rgbaColor=[1,1,1,0.1])
width = 128
height = 128
fov = 60
aspect = width / height
near = 0.02
far = 1
view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
# Get depth values using the OpenGL renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True,renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl= np.reshape(images[2], (height, width, 4))*1./255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
seg_opengl = np.reshape(images[4], [width, height])*1./255.
time.sleep(1)
# Get depth values using Tiny renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_TINY_RENDERER)
depth_buffer_tiny = np.reshape(images[3], [width, height])
depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
rgb_tiny= np.reshape(images[2], (height, width, 4))*1./255.
seg_tiny = np.reshape(images[4],[width,height])*1./255.
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
for b in range (p.getNumBodies()):
p.changeVisualShape(b,linkIndex=-1,textureUniqueId=textureId)
for j in range(p.getNumJoints(b)):
p.changeVisualShape(b,linkIndex=j,textureUniqueId=textureId)
viewMat = [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0]
projMat = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
proj_opengl= np.reshape(images[2], (height, width, 4))*1./255.
time.sleep(1)
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_TINY_RENDERER, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
proj_tiny= np.reshape(images[2], (height, width, 4))*1./255.
# Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane
plt.subplot(4, 2, 1)
plt.imshow(depth_opengl, cmap='gray', vmin=0, vmax=1)
plt.title('Depth OpenGL3')
plt.subplot(4, 2, 2)
plt.imshow(depth_tiny, cmap='gray', vmin=0, vmax=1)
plt.title('Depth TinyRenderer')
plt.subplot(4,2,3)
plt.imshow(rgb_opengl)
plt.title('RGB OpenGL3')
plt.subplot(4,2,4)
plt.imshow(rgb_tiny)
plt.title('RGB Tiny')
plt.subplot(4,2,5)
plt.imshow(seg_opengl)
plt.title('Seg OpenGL3')
plt.subplot(4,2,6)
plt.imshow(seg_tiny)
plt.title('Seg Tiny')
plt.subplot(4,2,7)
plt.imshow(proj_opengl)
plt.title('Proj OpenGL')
plt.subplot(4,2,8)
plt.imshow(proj_tiny)
plt.title('Proj Tiny')
plt.subplots_adjust(hspace=0.7)
plt.show()

1
examples/pybullet/examples/inverse_kinematics.py
View File

@@ -48,6 +48,7 @@ p.setRealTimeSimulation(useRealTimeSimulation)
trailDuration = 15
while 1:
p.getCameraImage(320,200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_BULLET_HARDWARE_OPENGL)
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second/60.)*2.*math.pi

2
examples/pybullet/examples/testrender_egl.py
View File

@@ -54,7 +54,7 @@ while (1):
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, shadow=1,flags=pybullet.ER_SEGMENTATION_MASK, lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
stop = time.time()
print ("renderImage %f" % (stop - start))

3
examples/pybullet/examples/transparent.py
View File

@@ -15,4 +15,5 @@ while (1):
blue = p.readUserDebugParameter(blueSlider)
alpha = p.readUserDebugParameter(alphaSlider)
p.changeVisualShape(sphereUid,-1,rgbaColor=[red,green,blue,alpha])
time.sleep(0.01)
p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_BULLET_HARDWARE_OPENGL)
time.sleep(0.01)

2
examples/pybullet/examples/userData.py
View File

@@ -3,7 +3,7 @@ import time
from pybullet_utils import bullet_client
server = bullet_client.BulletClient(connection_mode=pb.GUI_SERVER)
server = bullet_client.BulletClient(connection_mode=pb.SHARED_MEMORY_SERVER)
print ("Connecting to bullet server")

10
examples/pybullet/gym/pybullet_data/laikago/chassis.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

9330
examples/pybullet/gym/pybullet_data/laikago/chassis.obj Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
examples/pybullet/gym/pybullet_data/laikago/chassis.stl Normal file
View File

Binary file not shown.

11
examples/pybullet/gym/pybullet_data/laikago/chassis_mod.mtl Normal file
View File

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

8167
examples/pybullet/gym/pybullet_data/laikago/chassis_mod.obj Normal file
View File

File diff suppressed because it is too large Load Diff

1227
examples/pybullet/gym/pybullet_data/laikago/chassis_vhacd.obj Normal file
View File

File diff suppressed because it is too large Load Diff

1644
examples/pybullet/gym/pybullet_data/laikago/chassis_vhacd_mod.obj Normal file
View File

File diff suppressed because it is too large Load Diff

2499
examples/pybullet/gym/pybullet_data/laikago/data1.txt Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
examples/pybullet/gym/pybullet_data/laikago/hip_motor.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/laikago/hip_motor_mirror.stl Normal file
View File

Binary file not shown.

100
examples/pybullet/gym/pybullet_data/laikago/laikago.py Normal file
View File

@@ -0,0 +1,100 @@
import pybullet as p
import time
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
p.setGravity(0,0,-9.8)
p.setTimeStep(1./500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False)
#enable collision between lower legs
for j in range (p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped,j))
#2,5,8 and 11 are the lower legs
lower_legs = [2,5,8,11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1>l0):
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
jointIds=[]
paramIds=[]
jointOffsets=[]
jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
for i in range (4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
for j in range (p.getNumJoints(quadruped)):
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480,320)
p.setRealTimeSimulation(0)
joints=[]
with open("data1.txt","r") as filestream:
for line in filestream:
print("line=",line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints=currentline[2:14]
#print("joints=",joints)
for j in range (12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1./500.)
for j in range (p.getNumJoints(quadruped)):
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped,j)
js = p.getJointState(quadruped,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,(js[0]-jointOffsets[j])/jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce)

477
examples/pybullet/gym/pybullet_data/laikago/laikago.urdf Normal file
View File

@@ -0,0 +1,477 @@
<?xml version="1.0" ?>
<robot name="plane">
<link name="chassis">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0.03 0.043794"/>
<mass value="13.715"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.043794"/>
<geometry>
<mesh filename="chassis.stl" scale="1 1 1"/>
</geometry>
<material name="yellow">
<color rgba="0.95 0.75 0.05 1"/>
</material>
</visual>
<collision>
<origin rpy="-1.57 0 0" xyz="0 0 0.043794"/>
<geometry>
<mesh filename="chassis_vhacd_mod.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<link name="FR_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0.02 0 0"/>
<mass value="0.241"/>
<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="hip_motor_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor_mirror.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FR_hip_motor_2_chassis_joint" type="continuous">
<axis xyz="0 0 -1"/>
<parent link="chassis"/>
<child link="FR_hip_motor"/>
<origin rpy="0 0 0" xyz="-0.0817145 0 0.242889"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FR_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<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="upper_leg_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_right_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FR_upper_leg_2_hip_motor_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="FR_hip_motor"/>
<child link="FR_upper_leg"/>
<origin rpy="0 0 0" xyz="-0.053565 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FR_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0.0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0.0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FR_lower_leg_2_upper_leg_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="FR_upper_leg"/>
<child link="FR_lower_leg"/>
<origin rpy="0 0 0" xyz="0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FL_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="-.02 0 0"/>
<mass value="1.095"/>
<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="hip_motor.stl" scale="1 1 1"/>
</geometry>
<material name="green">
<color rgba="0.23 0.73 0.33 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FL_hip_motor_2_chassis_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis"/>
<child link="FL_hip_motor"/>
<origin rpy="0 0 0" xyz="0.0817145 0 0.242889"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FL_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<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="upper_leg.stl" scale="1 1 1"/>
</geometry>
<material name="blue">
<color rgba="0.28 0.52 0.93 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_left_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FL_upper_leg_2_hip_motor_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="FL_hip_motor"/>
<child link="FL_upper_leg"/>
<origin rpy="0 0 0" xyz="0.055855 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="FL_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
<material name="red">
<color rgba="0.85 0.19 0.21 1"/>
</material>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="FL_lower_leg_2_upper_leg_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="FL_upper_leg"/>
<child link="FL_lower_leg"/>
<origin rpy="0 0 0" xyz="-0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RR_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0.02 0 0"/>
<mass value="0.241"/>
<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="hip_motor_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor_mirror.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RR_hip_motor_2_chassis_joint" type="continuous">
<axis xyz="0 0 -1"/>
<parent link="chassis"/>
<child link="RR_hip_motor"/>
<origin rpy="0 0 0" xyz="-0.0817145 0 -0.194401"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RR_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<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="upper_leg_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_right_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RR_upper_leg_2_hip_motor_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="RR_hip_motor"/>
<child link="RR_upper_leg"/>
<origin rpy="0 0 0" xyz="-0.053565 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RR_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RR_lower_leg_2_upper_leg_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="RR_upper_leg"/>
<child link="RR_lower_leg"/>
<origin rpy="0 0 0" xyz="0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RL_hip_motor">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="-.02 0 0"/>
<mass value="1.095"/>
<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="hip_motor.stl" scale="1 1 1"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="hip_motor.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RL_hip_motor_2_chassis_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis"/>
<child link="RL_hip_motor"/>
<origin rpy="0 0 0" xyz="0.0817145 0 -0.194401"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RL_upper_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
<mass value="1.527"/>
<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="upper_leg.stl" scale="1 1 1"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="upper_leg_left_vhacd.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RL_upper_leg_2_hip_motor_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="RL_hip_motor"/>
<child link="RL_upper_leg"/>
<origin rpy="0 0 0" xyz="0.055855 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="RL_lower_leg">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
<mass value="0.241"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
<geometry>
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="RL_lower_leg_2_upper_leg_joint" type="continuous">
<axis xyz="1 0 0"/>
<parent link="RL_upper_leg"/>
<child link="RL_lower_leg"/>
<origin rpy="0 0 0" xyz="-0.02069 -0.20833 -0.1422"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
</robot>

13
examples/pybullet/gym/pybullet_data/laikago/license.txt Normal file
View File

@@ -0,0 +1,13 @@
URDF model created by Erwin Coumans.
Meshes derived from Laikago cad model, used by permission of Unitree.
If you use the model, add a citation to PyBullet:
@MISC{coumans2018,
author = {Erwin Coumans and Yunfei Bai},
title = {PyBullet, a Python module for physics simulation for games, robotics and machine learning},
howpublished = {\url{http://pybullet.org}},
year = {2016--2018}
}

BIN
examples/pybullet/gym/pybullet_data/laikago/lower_leg_3.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/laikago/upper_leg.stl Normal file
View File

Binary file not shown.

10
examples/pybullet/gym/pybullet_data/laikago/upper_leg_left.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

3394
examples/pybullet/gym/pybullet_data/laikago/upper_leg_left.obj Normal file
View File

File diff suppressed because it is too large Load Diff

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