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

Merge pull request #1929 from erwincoumans/master

Browse Source 
allow to provide rayCastBatch in local 'from'/'to' with a parent/link…
This commit is contained in:
erwincoumans
2018-10-11 18:14:20 -07:00
committed by GitHub
parent d6b50d3bcc c441a9469c
commit 11ef1bf0cd
17 changed files with 698 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -189,7 +189,12 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
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;
}

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

@@ -87,8 +87,9 @@ 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;

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2898,6 +2898,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 +2949,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;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -564,6 +564,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);

85
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -77,7 +77,8 @@
#include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
#endif
#ifndef B3_ENABLE_FILEIO_PLUGIN
#ifdef B3_ENABLE_FILEIO_PLUGIN
#include "plugins/fileIOPlugin/fileIOPlugin.h"
#endif//B3_DISABLE_FILEIO_PLUGIN
@@ -5050,6 +5051,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);
@@ -9864,7 +9912,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)
{
@@ -9873,8 +9921,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);

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

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -293,6 +293,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 )
};

14
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -909,4 +909,18 @@ enum eConstraintSolverTypes
eConstraintSolverLCP_BLOCK_PGS,
};
enum eFileIOActions
{
eAddFileIOAction = 1024,//avoid collision with eFileIOTypes
eRemoveFileIOAction,
};
enum eFileIOTypes
{
ePosixFileIO = 1,
eZipFileIO,
eCNSFileIO,
};
#endif //SHARED_MEMORY_PUBLIC_H

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

@@ -1317,12 +1317,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);

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

@@ -48,7 +48,7 @@ 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]);

403
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
View File

@@ -6,48 +6,92 @@
#include <stdio.h>
#include "../../../CommonInterfaces/CommonFileIOInterface.h"
#include "../../../Utils/b3ResourcePath.h"
#ifndef B3_EXCLUDE_DEFAULT_FILEIO
#include "../../../Utils/b3BulletDefaultFileIO.h"
#endif //B3_EXCLUDE_DEFAULT_FILEIO
//#define B3_USE_ZLIB
#ifdef B3_USE_ZLIB
#ifdef B3_USE_ZIPFILE_FILEIO
#include "zipFileIO.h"
#endif //B3_USE_ZIPFILE_FILEIO
#include "minizip/unzip.h"
struct MyFileIO : public CommonFileIOInterface
#ifdef B3_USE_CNS_FILEIO
#include "CNSFileIO.h"
#endif //B3_USE_CNS_FILEIO
#define B3_MAX_FILEIO_INTERFACES 1024
struct WrapperFileHandle
{
std::string m_zipfileName;
CommonFileIOInterface* childFileIO;
int m_childFileHandle;
};
unzFile m_fileHandles[FILEIO_MAX_FILES];
int m_numFileHandles;
struct WrapperFileIO : public CommonFileIOInterface
{
CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES];
int m_numWrapperInterfaces;
MyFileIO(const char* zipfileName)
:m_zipfileName(zipfileName),
m_numFileHandles(0)
WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES];
WrapperFileIO()
:m_numWrapperInterfaces(0)
{
for (int i=0;i<FILEIO_MAX_FILES;i++)
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
m_fileHandles[i]=0;
m_availableFileIOInterfaces[i]=0;
m_wrapperFileHandles[i].childFileIO=0;
m_wrapperFileHandles[i].m_childFileHandle=0;
}
}
static bool FileIOPluginFindFile(void* userPtr, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
virtual ~WrapperFileIO()
{
MyFileIO* fileIo = (MyFileIO*) userPtr;
return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
removeFileIOInterface(i);
}
}
virtual ~MyFileIO()
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)
{
//search a free slot
//find an available wrapperFileHandle slot
int wrapperFileHandle=-1;
int slot = -1;
for (int i=0;i<FILEIO_MAX_FILES;i++)
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
if (m_fileHandles[i]==0)
if (m_wrapperFileHandles[i].childFileIO==0)
{
slot=i;
break;
@@ -55,214 +99,118 @@ struct MyFileIO : public CommonFileIOInterface
}
if (slot>=0)
{
unzFile zipfile;
unz_global_info m_global_info;
zipfile = unzOpen(m_zipfileName.c_str());
if (zipfile == NULL)
//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++)
{
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)
CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i];
if (childFileIO)
{
printf("could not read file global info from %s\n", m_zipfileName.c_str());
unzClose(zipfile);
zipfile = 0;
slot = -1;
} else
{
m_fileHandles[slot] = zipfile;
}
}
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)
int childHandle = childFileIO->fileOpen(fileName, mode);
if (childHandle>=0)
{
printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result);
slot=-1;
}
else
{
result = unzOpenCurrentFile(zipfile);
if (result == UNZ_OK)
{
} else
{
slot=-1;
}
wrapperFileHandle = slot;
m_wrapperFileHandles[slot].childFileIO = childFileIO;
m_wrapperFileHandles[slot].m_childFileHandle = childHandle;
break;
}
}
}
}
return slot;
return wrapperFileHandle;
}
virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
{
int result = -1;
if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES)
int fileReadResult=-1;
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
unzFile f = m_fileHandles[fileHandle];
if (f)
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
result = unzReadCurrentFile(f, destBuffer,numBytes);
//::fread(destBuffer, 1, numBytes, f);
fileReadResult = m_wrapperFileHandles[fileHandle].childFileIO->fileRead(
m_wrapperFileHandles[fileHandle].m_childFileHandle, destBuffer, numBytes);
}
}
return result;
return fileReadResult;
}
virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
{
#if 0
if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES)
{
FILE* f = m_fileHandles[fileHandle];
if (f)
{
return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]);
}
}
#endif
//todo
return -1;
}
virtual void fileClose(int fileHandle)
{
if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES)
int fileReadResult=-1;
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
unzFile f = m_fileHandles[fileHandle];
if (f)
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
unzClose(f);
m_fileHandles[fileHandle]=0;
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* relativeFileName, int relativeFileNameSizeInBytes)
virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)
{
return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, MyFileIO::FileIOPluginFindFile, this);
}
virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
int fileHandle = -1;
fileHandle = fileOpen(orgFileName, "rb");
if (fileHandle>=0)
bool found = false;
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
//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)
if (m_availableFileIOInterfaces[i])
{
fileFound = true;
found = m_availableFileIOInterfaces[i]->findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes);
}
if (found)
break;
}
}
if (f>=0)
{
fileClose(f);
}
return fileFound;
return found;
}
virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
{
int numRead = 0;
if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES)
char* result = 0;
int fileReadResult=-1;
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
unzFile f = m_fileHandles[fileHandle];
if (f)
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
//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));
result = m_wrapperFileHandles[fileHandle].childFileIO->readLine(
m_wrapperFileHandles[fileHandle].m_childFileHandle,
destBuffer, numBytes);
}
}
if (numRead<numBytes && numRead>0)
{
destBuffer[numRead]=0;
return &destBuffer[0];
}
return 0;
return result;
}
virtual int getFileSize(int fileHandle)
{
int size=0;
int numBytes = 0;
if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES)
int fileReadResult=-1;
if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
{
unzFile f = m_fileHandles[fileHandle];
if (f)
if (m_wrapperFileHandles[fileHandle].childFileIO)
{
unz_file_info info;
int result = unzGetCurrentFileInfo(f, &info, NULL, 0, NULL, 0, NULL, 0);
if (result == UNZ_OK)
{
size = info.uncompressed_size;
}
numBytes = m_wrapperFileHandles[fileHandle].childFileIO->getFileSize(
m_wrapperFileHandles[fileHandle].m_childFileHandle);
}
}
return size;
return numBytes;
}
};
#else
typedef b3BulletDefaultFileIO MyFileIO;
#endif
struct FileIOClass
{
int m_testData;
MyFileIO m_fileIO;
WrapperFileIO m_fileIO;
FileIOClass()
: m_testData(42),
m_fileIO()//"e:/develop/bullet3/data/plane.zip")
m_fileIO()
{
}
virtual ~FileIOClass()
@@ -274,49 +222,98 @@ 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)
{
printf("text argument:%s\n", arguments->m_text);
printf("int args: [");
for (int i = 0; i < arguments->m_numInts; i++)
{
printf("%d", arguments->m_ints[i]);
if ((i + 1) < arguments->m_numInts)
{
printf(",");
}
}
printf("]\nfloat args: [");
for (int i = 0; i < arguments->m_numFloats; i++)
{
printf("%f", arguments->m_floats[i]);
if ((i + 1) < arguments->m_numFloats)
{
printf(",");
}
}
printf("]\n");
int result=-1;
FileIOClass* obj = (FileIOClass*)context->m_userPointer;
b3SharedMemoryStatusHandle statusHandle;
int statusType = -1;
int bodyUniqueId = -1;
b3SharedMemoryCommandHandle command =
b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_URDF_LOADING_COMPLETED)
printf("text argument:%s\n", arguments->m_text);
printf("int args: [");
//remove a fileIO type
if (arguments->m_numInts==1)
{
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
int fileIOIndex = arguments->m_ints[0];
obj->m_fileIO.removeFileIOInterface(fileIOIndex);
}
return bodyUniqueId;
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));
}
#else
printf("eZipFileIO is not enabled in this build.\n");
#endif
break;
}
case eCNSFileIO:
{
#ifdef B3_USE_CNS_FILEIO
B3_USE_ZIPFILE_FILEIO
if (arguments->m_text)
{
obj->m_fileIO.addFileIOInterface(new CNSFileIO(arguments->m_text));
}
#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)

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

@@ -0,0 +1,246 @@
#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)
: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;
} else
{
m_fileHandles[slot] = zipfile;
}
}
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;
}
else
{
result = unzOpenCurrentFile(zipfile);
if (result == UNZ_OK)
{
} else
{
slot=-1;
}
}
} else
{
slot=-1;
}
}
}
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)
{
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;
}
};

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

@@ -1170,12 +1170,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);

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

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

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

@@ -48,7 +48,7 @@ p.setRealTimeSimulation(useRealTimeSimulation)
trailDuration = 15
while 1:
p.getCameraImage(320,200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_HARDWARE_OPENGL_RENDERER)
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

22
examples/pybullet/pybullet.c
View File

@@ -4742,12 +4742,14 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
b3PhysicsClientHandle sm = 0;
int sizeFrom = 0;
int sizeTo = 0;
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "physicsClientId", NULL};
int parentObjectUniqueId = -1;
int parentLinkIndex = -1;
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|ii", kwlist,
&rayFromObjList, &rayToObjList, &numThreads, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiii", kwlist,
&rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -4831,6 +4833,11 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
}
}
if (parentObjectUniqueId>=0)
{
b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)
@@ -9904,6 +9911,13 @@ initpybullet(void)
PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES);
PyModule_AddIntConstant(m, "STATE_LOG_JOINT_TORQUES", STATE_LOG_JOINT_USER_TORQUES + STATE_LOG_JOINT_MOTOR_TORQUES);
PyModule_AddIntConstant(m, "AddFileIOAction", eAddFileIOAction);
PyModule_AddIntConstant(m, "RemoveFileIOAction", eRemoveFileIOAction);
PyModule_AddIntConstant(m, "PosixFileIO", ePosixFileIO );
PyModule_AddIntConstant(m, "ZipFileIO", eZipFileIO );
PyModule_AddIntConstant(m, "CNSFileIO", eCNSFileIO );
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);

24
setup.py
View File

@@ -43,6 +43,9 @@ CXX_FLAGS += '-DBT_ENABLE_ENET '
CXX_FLAGS += '-DBT_ENABLE_CLSOCKET '
CXX_FLAGS += '-DB3_DUMP_PYTHON_VERSION '
CXX_FLAGS += '-DEGL_ADD_PYTHON_INIT '
CXX_FLAGS += '-DB3_ENABLE_FILEIO_PLUGIN '
CXX_FLAGS += '-DB3_USE_ZIPFILE_FILEIO '
EGL_CXX_FLAGS = ''
@@ -73,6 +76,7 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/TinyRenderer/TinyRenderer.cpp"]\
+["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\
+["examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp"]\
+["examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp"]\
+["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\
+["examples/SharedMemory/IKTrajectoryHelper.cpp"]\
+["examples/SharedMemory/InProcessMemory.cpp"]\
@@ -106,6 +110,24 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp"]\
+["examples/ThirdPartyLibs/stb_image/stb_image.cpp"]\
+["examples/ThirdPartyLibs/stb_image/stb_image_write.cpp"]\
+["examples/ThirdPartyLibs/minizip/ioapi.c"]\
+["examples/ThirdPartyLibs/minizip/unzip.c"]\
+["examples/ThirdPartyLibs/minizip/zip.c"]\
+["examples/ThirdPartyLibs/zlib/adler32.c"]\
+["examples/ThirdPartyLibs/zlib/compress.c"]\
+["examples/ThirdPartyLibs/zlib/crc32.c"]\
+["examples/ThirdPartyLibs/zlib/deflate.c"]\
+["examples/ThirdPartyLibs/zlib/gzclose.c"]\
+["examples/ThirdPartyLibs/zlib/gzlib.c"]\
+["examples/ThirdPartyLibs/zlib/gzread.c"]\
+["examples/ThirdPartyLibs/zlib/gzwrite.c"]\
+["examples/ThirdPartyLibs/zlib/infback.c"]\
+["examples/ThirdPartyLibs/zlib/inffast.c"]\
+["examples/ThirdPartyLibs/zlib/inflate.c"]\
+["examples/ThirdPartyLibs/zlib/inftrees.c"]\
+["examples/ThirdPartyLibs/zlib/trees.c"]\
+["examples/ThirdPartyLibs/zlib/uncompr.c"]\
+["examples/ThirdPartyLibs/zlib/zutil.c"]\
+["examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp"]\
+["examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp"]\
+["examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp"]\
@@ -559,7 +581,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name = 'pybullet',
version='2.2.9',
version='2.3.0',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',