From 19295f2859c046e4aab6fa127c6ab1368cc5e94f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 17 May 2017 19:29:12 -0700 Subject: [PATCH] enable file caching, currently only for Wavefront .obj files. You can disable file caching using pybullet.setPhysicsEngineParameter(enableFileCaching=0) Allow VR camera tracking only using position tracking, no orientation tracking (use pybullet.setVRCamera([posX,posY,posZ],trackObjectFlag=0 or pybullet.VR_CAMERA_TRACK_OBJECT_ORIENTATION) --- .../ImportMeshUtility/b3ImportMeshUtility.cpp | 5 +- .../ImportObjDemo/LoadMeshFromObj.cpp | 49 ++++++++++++++++++- .../Importers/ImportObjDemo/LoadMeshFromObj.h | 8 +++ .../ImportURDFDemo/BulletUrdfImporter.cpp | 11 ++++- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +++++++++- examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 26 +++++++++- examples/SharedMemory/SharedMemoryCommands.h | 7 ++- examples/SharedMemory/SharedMemoryPublic.h | 5 ++ .../Wavefront/tiny_obj_loader.cpp | 5 +- examples/pybullet/pybullet.c | 25 ++++++++-- 11 files changed, 153 insertions(+), 15 deletions(-) diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 442a2f777..c14658ab9 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -7,7 +7,7 @@ #include "../../Utils/b3ResourcePath.h" #include "Bullet3Common/b3FileUtils.h" #include "../../ThirdPartyLibs/stb_image/stb_image.h" - +#include "../ImportObjDemo/LoadMeshFromObj.h" bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData) { B3_PROFILE("loadAndRegisterMeshFromFileInternal"); @@ -28,7 +28,8 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& std::vector shapes; { B3_PROFILE("tinyobj::LoadObj"); - std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix); + std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix); + //std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix); } GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp index 158d3456b..a7cf426cd 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp @@ -1,11 +1,56 @@ #include "LoadMeshFromObj.h" -#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" + #include "../../OpenGLWindow/GLInstanceGraphicsShape.h" #include //fopen #include "Bullet3Common/b3AlignedObjectArray.h" #include #include #include "Wavefront2GLInstanceGraphicsShape.h" +#include "Bullet3Common/b3HashMap.h" + +struct CachedObjResult +{ + std::string m_msg; + std::vector m_shapes; +}; + +static b3HashMap gCachedObjResults; +static int gEnableFileCaching = 1; + +void b3EnableFileCaching(int enable) +{ + gEnableFileCaching = enable; + if (enable==0) + { + gCachedObjResults.clear(); + } +} + + +std::string LoadFromCachedOrFromObj( + std::vector& shapes, // [output] + const char* filename, + const char* mtl_basepath) +{ + CachedObjResult* resultPtr = gCachedObjResults[filename]; + if (resultPtr) + { + const CachedObjResult& result = *resultPtr; + shapes = result.m_shapes; + return result.m_msg; + } + + std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath); + CachedObjResult result; + result.m_msg = err; + result.m_shapes = shapes; + if (gEnableFileCaching) + { + gCachedObjResults.insert(filename,result); + } + return err; +} + GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath) { @@ -13,7 +58,7 @@ GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const cha std::vector shapes; { B3_PROFILE("tinyobj::LoadObj2"); - std::string err = tinyobj::LoadObj(shapes, relativeFileName, materialPrefixPath); + std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath); } { diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h index 72533dfd8..4d23eea8d 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h @@ -4,6 +4,14 @@ struct GLInstanceGraphicsShape; +#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" + +void b3EnableFileCaching(int enable); + +std::string LoadFromCachedOrFromObj( + std::vector& shapes, // [output] + const char* filename, + const char* mtl_basepath); GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 9080f8954..2a43dc739 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -634,8 +634,15 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co case UrdfGeometry::FILE_OBJ: if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { - - glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0); + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (b3ResourcePath::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); } else { diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ca5ae4053..6a4023c58 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -381,6 +381,17 @@ int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHan } +int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + + command->m_physSimParamArgs.m_enableFileCaching= enableFileCaching; + command->m_updateFlags |= SIM_PARAM_ENABLE_FILE_CACHING ; + return 0; + +} + int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -1785,7 +1796,10 @@ void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandl b3Assert(command); b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); b3Assert(renderer>(1<<15)); - command->m_updateFlags |= renderer; + if (renderer>(1<<15)) + { + command->m_updateFlags |= renderer; + } } void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16]) @@ -2727,6 +2741,15 @@ int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int o return 0; } +int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE); + command->m_updateFlags |= VR_CAMERA_FLAG; + command->m_vrCameraStateArguments.m_trackingObjectFlag = flag; + return 0; +} b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b9802de36..cee3a9b41 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -215,6 +215,7 @@ int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold); int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold); int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms); +int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching); //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes //Use at own risk: magic things may or my not happen when calling this API @@ -376,6 +377,7 @@ b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]); int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]); int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); +int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag); b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient); void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9751fd1de..ea720de67 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -22,6 +22,7 @@ #include "../Utils/RobotLoggingUtil.h" #include "LinearMath/btTransform.h" #include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h" +#include "../Importers/ImportObjDemo/LoadMeshFromObj.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "LinearMath/btSerializer.h" @@ -54,6 +55,8 @@ bool gCreateDefaultRobotAssets = false; int gInternalSimFlags = 0; bool gResetSimulation = 0; int gVRTrackingObjectUniqueId = -1; +int gVRTrackingObjectFlag = VR_CAMERA_TRACK_OBJECT_ORIENTATION; + btTransform gVRTrackingObjectTr = btTransform::getIdentity(); int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads @@ -2440,6 +2443,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid) { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str()); m_data->m_profileTimingLoggingUid = -1; } @@ -2482,6 +2486,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId; } + if (clientCmd.m_updateFlags & VR_CAMERA_FLAG) + { + gVRTrackingObjectFlag = clientCmd.m_vrCameraStateArguments.m_trackingObjectFlag; + } + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; @@ -4059,6 +4068,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; } + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) + { + b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); + } + + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; @@ -6194,7 +6209,16 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(gVRTrackingObjectUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - gVRTrackingObjectTr = bodyHandle->m_multiBody->getBaseWorldTransform(); +// gVRTrackingObjectTr = bodyHandle->m_multiBody->getBaseWorldTransform(); + + if (gVRTrackingObjectUniqueId>=0) + { + gVRTrackingObjectTr.setOrigin(bodyHandle->m_multiBody->getBaseWorldTransform().getOrigin()); + } + if (gVRTrackingObjectFlag&VR_CAMERA_TRACK_OBJECT_ORIENTATION) + { + gVRTrackingObjectTr.setBasis(bodyHandle->m_multiBody->getBaseWorldTransform().getBasis()); + } } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8b1feed62..c9f1099a5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -353,6 +353,8 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512, SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024, SIM_PARAM_MAX_CMD_PER_1MS = 2048, + SIM_PARAM_ENABLE_FILE_CACHING = 4096, + }; enum EnumLoadBunnyUpdateFlags @@ -384,6 +386,7 @@ struct SendPhysicsSimulationParameters int m_internalSimFlags; double m_defaultContactERP; int m_collisionFilterMode; + int m_enableFileCaching; }; struct LoadBunnyArgs @@ -675,7 +678,8 @@ enum eVRCameraEnums { VR_CAMERA_ROOT_POSITION=1, VR_CAMERA_ROOT_ORIENTATION=2, - VR_CAMERA_ROOT_TRACKING_OBJECT=4 + VR_CAMERA_ROOT_TRACKING_OBJECT=4, + VR_CAMERA_FLAG = 8, }; enum eStateLoggingEnums @@ -696,6 +700,7 @@ struct VRCameraState double m_rootPosition[3]; double m_rootOrientation[4]; int m_trackingObjectUniqueId; + int m_trackingObjectFlag; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 25a8afc40..0f4bb31cd 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -320,6 +320,11 @@ enum eVRDeviceTypeEnums VR_DEVICE_GENERIC_TRACKER=4, }; +enum EVRCameraFlags +{ + VR_CAMERA_TRACK_OBJECT_ORIENTATION=1, +}; + struct b3VRControllerEvent { int m_controllerId;//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index f56de333a..d8e3102a8 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -560,9 +560,12 @@ LoadObj( int maxchars = 8192; // Alloc enough size. std::vector buf(maxchars); // Alloc enough size. + std::string linebuf; + linebuf.reserve(maxchars); + while (ifs.peek() != -1) { - std::string linebuf; + linebuf.resize(0); safeGetline(ifs,linebuf); // Trim newline '\r\n' or '\r' diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 91b1b5466..275d93976 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -724,13 +724,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int collisionFilterMode = -1; double contactBreakingThreshold = -1; int maxNumCmdPer1ms = -2; + int enableFileCaching = -1; + b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &physicsClientId)) { return NULL; } @@ -781,6 +783,11 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParamSetMaxNumCommandsPer1ms(command, maxNumCmdPer1ms); } + if (enableFileCaching>=0) + { + b3PhysicsParamSetEnableFileCaching(command, enableFileCaching); + } + //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -3703,11 +3710,12 @@ static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObj PyObject* rootPosObj = 0; PyObject* rootOrnObj = 0; int trackObjectUid = -2; + int trackObjectFlag = -1; double rootPos[3]; double rootOrn[4]; - static char* kwlist[] = {"rootPosition", "rootOrientation", "trackObject", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|OOii", kwlist, &rootPosObj, &rootOrnObj, &trackObjectUid, &physicsClientId)) + static char* kwlist[] = {"rootPosition", "rootOrientation", "trackObject", "trackObjectFlag","physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|OOiii", kwlist, &rootPosObj, &rootOrnObj, &trackObjectUid,&trackObjectFlag, &physicsClientId)) { return NULL; } @@ -3734,6 +3742,11 @@ static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObj b3SetVRCameraTrackingObject(commandHandle, trackObjectUid); } + if (trackObjectFlag>=-1) + { + b3SetVRCameraTrackingObjectFlag(commandHandle, trackObjectFlag); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -6355,6 +6368,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "VR_DEVICE_HMD", VR_DEVICE_HMD); PyModule_AddIntConstant(m, "VR_DEVICE_GENERIC_TRACKER", VR_DEVICE_GENERIC_TRACKER); + PyModule_AddIntConstant(m, "VR_CAMERA_TRACK_OBJECT_ORIENTATION", VR_CAMERA_TRACK_OBJECT_ORIENTATION); + PyModule_AddIntConstant(m, "KEY_IS_DOWN", eButtonIsDown); PyModule_AddIntConstant(m, "KEY_WAS_TRIGGERED", eButtonTriggered); PyModule_AddIntConstant(m, "KEY_WAS_RELEASED", eButtonReleased);