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

Merge remote-tracking branch 'origin/physx' into physx_clean

Browse Source
This commit is contained in:
erwincoumans
2019-02-28 17:52:59 -08:00
parent 208b38c51f a9996088c8
commit 4d711ed411
17 changed files with 930 additions and 1780 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,7 +5,7 @@
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include <string.h> //memcpy
struct MySTLTriangle
{
float normal[3];

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

@@ -95,6 +95,13 @@ struct UrdfRenderingInterface
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) {}
virtual void setProjectiveTexture(bool useProjectiveTexture) {}
virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const
{
return false;
}
};
#endif //LINK_VISUAL_SHAPES_CONVERTER_H

6
examples/SharedMemory/PhysicsClientSharedMemory2.h
View File

@@ -1,5 +1,5 @@
#ifndef PHYSICS_CLIENT_SHARED_MEMORY2_H
#define PHYSICS_CLIENT_SHARED_MEMORY2_H
#ifndef PHYSICS_CLIENT_SHARED_MEMORY3_H
#define PHYSICS_CLIENT_SHARED_MEMORY3_H
#include "PhysicsDirect.h"
@@ -14,4 +14,4 @@ public:
void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
};
#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H
#endif //PHYSICS_CLIENT_SHARED_MEMORY3_H

9
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -658,6 +658,7 @@ public:
}
GUIHelperInterface* m_childGuiHelper;
btHashMap<btHashPtr, int> m_cachedTextureIds;
int m_uidGenerator;
const unsigned char* m_texels;
int m_textureWidth;
@@ -856,6 +857,11 @@ public:
virtual int registerTexture(const unsigned char* texels, int width, int height)
{
int* cachedTexture = m_cachedTextureIds[texels];
if (cachedTexture)
{
return *cachedTexture;
}
m_texels = texels;
m_textureWidth = width;
m_textureHeight = height;
@@ -864,7 +870,7 @@ public:
m_cs->setSharedParam(1, eGUIHelperRegisterTexture);
workerThreadWait();
m_cachedTextureIds.insert(texels, m_textureId);
return m_textureId;
}
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId)
@@ -918,6 +924,7 @@ public:
virtual void removeAllGraphicsInstances()
{
m_cachedTextureIds.clear();
m_cs->lock();
m_cs->setSharedParam(1, eGUIHelperRemoveAllGraphicsInstances);
workerThreadWait();

9
examples/SharedMemory/physx/PhysXC_API.cpp
View File

@@ -1,13 +1,14 @@
#ifdef BT_ENABLE_PHYSX
#include "PhysXC_API.h"
#include "../PhysicsDirect.h"
#include "PhysXServerCommandProcessor.h"
#include "PhysXClient.h"
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX()
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX(int argc, char* argv[])
{
PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor;
PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor(argc,argv);
PhysXClient* direct = new PhysXClient(sdk, true);
PhysicsDirect* direct = new PhysicsDirect(sdk, true);
bool connected;
connected = direct->connect();
return (b3PhysicsClientHandle)direct;

2
examples/SharedMemory/physx/PhysXC_API.h
View File

@@ -10,7 +10,7 @@ extern "C"
{
#endif
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX();
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX(int argc, char* argv[]);
#ifdef __cplusplus
}

1409
examples/SharedMemory/physx/PhysXClient.cpp
View File

File diff suppressed because it is too large Load Diff

135
examples/SharedMemory/physx/PhysXClient.h
View File

@@ -1,135 +0,0 @@
#ifndef PHYSX_CLIENT_H
#define PHYSX_CLIENT_H
#include "../PhysicsClient.h"
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class PhysXClient : public PhysicsClient
{
protected:
struct PhysXDirectInternalData* m_data;
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
public:
PhysXClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
virtual ~PhysXClient();
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len);
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
virtual int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);
virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData);
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
//the following APIs are for internal use for visualization:
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();
virtual void debugDraw(int debugDrawMode);
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const
{
return false;
}
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const
{
return -1;
}
virtual int getNumUserData(int bodyUniqueId) const
{
return 0;
}
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //PHYSX_CLIENT_H

762
examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
View File

@@ -9,6 +9,7 @@
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "LinearMath/btMinMax.h"
#include "Bullet3Common/b3FileUtils.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "../../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3ResizablePool.h"
#include "PxPhysicsAPI.h"
@@ -20,6 +21,7 @@
#include "URDF2PhysX.h"
#include "../b3PluginManager.h"
#include "PxRigidActorExt.h"
#include "LinearMath/btThreads.h"
#define STATIC_EGLRENDERER_PLUGIN
#ifdef STATIC_EGLRENDERER_PLUGIN
@@ -48,6 +50,27 @@ public:
}
};
class CustomProfilerCallback : public physx::PxProfilerCallback
{
public:
virtual ~CustomProfilerCallback() {}
virtual void* zoneStart(const char* eventName, bool detached, uint64_t contextId)
{
b3EnterProfileZone(eventName);
return 0;
}
virtual void zoneEnd(void* profilerData, const char* eventName, bool detached, uint64_t contextId)
{
b3LeaveProfileZone();
}
};
static CustomProfilerCallback gCustomProfilerCallback;
struct InternalPhysXBodyData
{
physx::PxArticulationReducedCoordinate* mArticulation;
@@ -76,13 +99,118 @@ struct InternalPhysXBodyData
typedef b3PoolBodyHandle<InternalPhysXBodyData> InternalPhysXBodyHandle;
struct PhysXServerCommandProcessorInternalData
struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEventCallback, public physx::PxContactModifyCallback
{
bool m_isConnected;
bool m_verboseOutput;
double m_physicsDeltaTime;
int m_numSimulationSubSteps;
btSpinMutex m_taskLock;
btAlignedObjectArray<b3ContactPointData> m_contactPoints;
void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count)
{
for (physx::PxU32 i = 0; i<count; i++)
{
//...
}
}
void onContact(const physx::PxContactPairHeader& pairHeader, const physx::PxContactPair* pairs, physx::PxU32 nbPairs)
{
B3_PROFILE("onContact");
//todo: are there really multiple threads calling 'onContact'?
m_taskLock.lock();
btAlignedObjectArray<physx::PxContactPairPoint> contacts;
for (physx::PxU32 i = 0; i < nbPairs; i++)
{
physx::PxU32 contactCount = pairs[i].contactCount;
if (contactCount)
{
contacts.resize(contactCount);
pairs[i].extractContacts(&contacts[0], contactCount);
for (physx::PxU32 j = 0; j < contactCount; j++)
{
const physx::PxContactPairPoint& contact = contacts[i];
b3ContactPointData srcPt;
MyPhysXUserData* udA = (MyPhysXUserData*)pairHeader.actors[0]->userData;
MyPhysXUserData* udB = (MyPhysXUserData*)pairHeader.actors[1]->userData;
srcPt.m_bodyUniqueIdA = udA->m_bodyUniqueId;
srcPt.m_linkIndexA = udA->m_linkIndex;
srcPt.m_bodyUniqueIdB = udB->m_bodyUniqueId;
srcPt.m_linkIndexB = udB->m_linkIndex;
srcPt.m_positionOnAInWS[0] = contact.position.x + contact.separation*contact.normal.x;
srcPt.m_positionOnAInWS[1] = contact.position.y + contact.separation*contact.normal.y;
srcPt.m_positionOnAInWS[2] = contact.position.z + contact.separation*contact.normal.z;
srcPt.m_positionOnBInWS[0] = contact.position.x - contact.separation*contact.normal.x;
srcPt.m_positionOnBInWS[1] = contact.position.y - contact.separation*contact.normal.y;
srcPt.m_positionOnBInWS[2] = contact.position.z - contact.separation*contact.normal.z;
srcPt.m_contactNormalOnBInWS[0] = contact.normal.x;
srcPt.m_contactNormalOnBInWS[1] = contact.normal.y;
srcPt.m_contactNormalOnBInWS[2] = contact.normal.z;
srcPt.m_contactDistance = contact.separation;
srcPt.m_contactFlags = 0;
srcPt.m_linearFrictionDirection1[0] = 0;
srcPt.m_linearFrictionDirection1[1] = 0;
srcPt.m_linearFrictionDirection1[2] = 0;
srcPt.m_linearFrictionDirection2[0] = 0;
srcPt.m_linearFrictionDirection2[1] = 0;
srcPt.m_linearFrictionDirection2[2] = 0;
srcPt.m_linearFrictionForce2 = 0;
srcPt.m_normalForce = contact.impulse.dot(contact.normal);
//compute friction direction from impulse projected in contact plane using contact normal.
physx::PxVec3 fric = contact.impulse - contact.normal*srcPt.m_normalForce;
double fricForce = fric.normalizeSafe();
if (fricForce)
{
srcPt.m_linearFrictionDirection1[0] = fric.x;
srcPt.m_linearFrictionDirection1[1] = fric.y;
srcPt.m_linearFrictionDirection1[2] = fric.z;
srcPt.m_linearFrictionForce1 = fricForce;
}
m_contactPoints.push_back(srcPt);
// std::cout << "Contact: bw " << pairHeader.actors[0]->getName() << " and " << pairHeader.actors[1]->getName() << " | " << contacts[j].position.x << "," << contacts[j].position.y << ","
// << contacts[j].position.z << std::endl;
}
}
}
m_taskLock.unlock();
}
void onConstraintBreak(physx::PxConstraintInfo* constraints, physx::PxU32 count)
{
PX_UNUSED(constraints);
PX_UNUSED(count);
}
void onWake(physx::PxActor** actors, physx::PxU32 count)
{
PX_UNUSED(actors);
PX_UNUSED(count);
}
void onSleep(physx::PxActor** actors, physx::PxU32 count)
{
PX_UNUSED(actors);
PX_UNUSED(count);
}
void onTrigger(physx::PxTriggerPair* pairs, physx::PxU32 count)
{
PX_UNUSED(pairs);
PX_UNUSED(count);
}
void onAdvance(const physx::PxRigidBody* const*, const physx::PxTransform*, const physx::PxU32)
{
}
b3PluginManager m_pluginManager;
@@ -108,16 +236,23 @@ struct PhysXServerCommandProcessorInternalData
int m_profileTimingLoggingUid;
int m_stateLoggersUniqueId;
std::string m_profileTimingFileName;
b3CommandLineArgs m_commandLineArgs;
int m_userDebugParametersUid;
btHashMap<btHashInt, double> m_userDebugParameters;
btAlignedObjectArray<int> m_graphicsIndexToSegmentationMask;
PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk)
PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[])
: m_isConnected(false),
m_verboseOutput(false),
m_physicsDeltaTime(1. / 240.),
m_numSimulationSubSteps(0),
m_pluginManager(sdk),
m_profileTimingLoggingUid(-1),
m_stateLoggersUniqueId(1)
m_stateLoggersUniqueId(1),
m_commandLineArgs(argc,argv),
m_userDebugParametersUid(0)
{
m_foundation = NULL;
m_physics = NULL;
m_cooking = NULL;
@@ -138,9 +273,10 @@ struct PhysXServerCommandProcessorInternalData
}
};
PhysXServerCommandProcessor::PhysXServerCommandProcessor()
PhysXServerCommandProcessor::PhysXServerCommandProcessor(int argc, char* argv[])
{
m_data = new PhysXServerCommandProcessorInternalData(this);
m_data = new PhysXServerCommandProcessorInternalData(this, argc, argv);
}
PhysXServerCommandProcessor::~PhysXServerCommandProcessor()
@@ -158,9 +294,10 @@ physx::PxFilterFlags MyPhysXFilter(physx::PxFilterObjectAttributes attributes0,
PX_UNUSED(attributes1);
PX_UNUSED(constantBlock);
PX_UNUSED(constantBlockSize);
if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2)
return physx::PxFilterFlag::eKILL;
pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT;
// if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2)
// return physx::PxFilterFlag::eKILL;
pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT | physx::PxPairFlag::eNOTIFY_TOUCH_FOUND
| physx::PxPairFlag::eDETECT_DISCRETE_CONTACT | physx::PxPairFlag::eNOTIFY_CONTACT_POINTS | physx::PxPairFlag::eMODIFY_CONTACTS;
return physx::PxFilterFlag::eDEFAULT;
}
@@ -178,21 +315,57 @@ bool PhysXServerCommandProcessor::connect()
{
m_data->m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_data->m_allocator, m_data->m_errorCallback);
// This call should be performed after PVD is initialized, otherwise it will have no effect.
PxSetProfilerCallback(&gCustomProfilerCallback);
m_data->m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxTolerancesScale(), true, 0);
m_data->m_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxCookingParams(physx::PxTolerancesScale()));
physx::PxU32 numCores = 1;//
physx::PxU32 numCores = 1;
m_data->m_commandLineArgs.GetCmdLineArgument("numCores", numCores);
printf("PhysX numCores=%d\n", numCores);
m_data->m_dispatcher = physx::PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1);
physx::PxSceneDesc sceneDesc(m_data->m_physics->getTolerancesScale());
sceneDesc.gravity = physx::PxVec3(0.0f, -9.81f, 0.0f);
sceneDesc.solverType = physx::PxSolverType::eTGS;
//sceneDesc.solverType = physx::PxSolverType::ePGS;
sceneDesc.solverType = physx::PxSolverType::ePGS;
std::string solver;
m_data->m_commandLineArgs.GetCmdLineArgument("solver", solver);
if (solver=="tgs")
{
sceneDesc.solverType = physx::PxSolverType::eTGS;
printf("PhysX using TGS\n");
}
else
{
printf("PhysX using PGS\n");
}
sceneDesc.cpuDispatcher = m_data->m_dispatcher;
//sceneDesc.filterShader = MyPhysXFilter;
sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader;
//todo: add some boolean, to allow enable/disable of this contact filtering
bool enableContactCallback = false;
m_data->m_commandLineArgs.GetCmdLineArgument("enableContactCallback", enableContactCallback);
if (enableContactCallback)
{
sceneDesc.filterShader = MyPhysXFilter;
sceneDesc.simulationEventCallback = this->m_data;
sceneDesc.contactModifyCallback = this->m_data;
printf("PhysX enableContactCallback\n");
}
else
{
sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader;
}
m_data->m_scene = m_data->m_physics->createScene(sceneDesc);
m_data->m_material = m_data->m_physics->createMaterial(0.5f, 0.5f, 0.f);
@@ -588,13 +761,14 @@ bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct Sh
//find the joint motors and apply the desired velocity and maximum force/torque
{
int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base
int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for (int link = 1; link < numLinks2; link++)
{
int dofs = physxLinks[link]->getInboundJointDof();
physx::PxReal stiffness = 10.f;
physx::PxReal damping = 0.1f;
physx::PxReal forceLimit = PX_MAX_F32;
int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
if (dofs == 1)
{
@@ -614,33 +788,34 @@ bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct Sh
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
{
desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
}
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
{
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
}
physx::PxReal damping = kd;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
{
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex];
stiffness = kp;
}
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
{
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
}
physx::PxReal damping = kd;
joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity);
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
{
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex];
stiffness = kp;
}
physx::PxReal forceLimit = 1000000.f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
{
forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex];
joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity);
physx::PxReal forceLimit = 1000000.f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
{
forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex];
}
bool isAcceleration = false;
joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition);
joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration);
}
bool isAcceleration = false;
joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration);
joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition);
}
dofIndex += dofs;
@@ -1049,6 +1224,91 @@ bool PhysXServerCommandProcessor::processRequestPhysicsSimulationParametersComma
return hasStatus;
}
bool PhysXServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
int totalBytesPerContact = sizeof(b3ContactPointData);
int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1;
b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;
int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
int numContactPointBatch = btMin(int(m_data->m_contactPoints.size()), contactPointStorage);
int endContactPointIndex = startContactPointIndex + numContactPointBatch;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
for (int i = startContactPointIndex; i < endContactPointIndex; i++)
{
const b3ContactPointData& srcPt = m_data->m_contactPoints[i];
b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
destPt = srcPt;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
}
serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = startContactPointIndex;
serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = m_data->m_contactPoints.size() - startContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED;
return true;
}
bool PhysXServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
return false;
}
bool PhysXServerCommandProcessor::processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH");
b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path);
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
return hasStatus;
}
bool PhysXServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
///dummy, so commands don't fail
bool hasStatus = true;
BT_PROFILE("CMD_USER_DEBUG_DRAW");
SharedMemoryStatus& serverCmd = serverStatusOut;
int uid = 0;
serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER)
{
int uid = m_data->m_userDebugParametersUid++;
double value = clientCmd.m_userDebugDrawArgs.m_startValue;
m_data->m_userDebugParameters.insert(uid, value);
serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
if (clientCmd.m_updateFlags & USER_DEBUG_READ_PARAMETER)
{
double* valPtr = m_data->m_userDebugParameters[clientCmd.m_userDebugDrawArgs.m_itemUniqueId];
if (valPtr)
{
serverCmd.m_userDebugDrawArgs.m_parameterValue = *valPtr;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED;
}
}
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL)
{
m_data->m_userDebugParameters.clear();
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
{
m_data->m_userDebugParameters.remove(clientCmd.m_userDebugDrawArgs.m_itemUniqueId);
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
return hasStatus;
}
bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
@@ -1057,7 +1317,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
int sz = sizeof(SharedMemoryStatus);
int sz2 = sizeof(SharedMemoryCommand);
bool hasStatus = false;
bool hasStatus = true;
serverStatusOut.m_type = CMD_INVALID_STATUS;
serverStatusOut.m_numDataStreamBytes = 0;
@@ -1111,15 +1371,6 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
break;
}
default:
{
BT_PROFILE("CMD_UNKNOWN");
printf("Unknown command encountered: %d", clientCmd.m_type);
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
hasStatus = true;
}
case CMD_LOAD_URDF:
{
hasStatus = processLoadURDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
@@ -1160,6 +1411,35 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
hasStatus = processRequestContactpointInformationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_COLLISION_SHAPE:
{
hasStatus = processCreateCollisionShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SET_ADDITIONAL_SEARCH_PATH:
{
hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_USER_DEBUG_DRAW:
{
hasStatus = processUserDebugDrawCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
hasStatus = processRequestCameraImageCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
#if 0
case CMD_SET_VR_CAMERA_STATE:
@@ -1195,11 +1475,6 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
break;
}
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_BODY_INFO:
{
@@ -1216,11 +1491,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_COLLISION_SHAPE:
{
hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_VISUAL_SHAPE:
{
hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
@@ -1231,11 +1502,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SET_ADDITIONAL_SEARCH_PATH:
{
hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_MJCF:
{
@@ -1319,11 +1586,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
@@ -1345,45 +1608,45 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
break;
}
case CMD_REMOVE_BODY:
{
hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processRemoveBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_USER_CONSTRAINT:
{
hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processCreateUserConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CALCULATE_INVERSE_KINEMATICS:
{
hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO:
{
hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processRequestVisualShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_COLLISION_SHAPE_INFO:
{
hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_UPDATE_VISUAL_SHAPE:
{
hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processUpdateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CHANGE_TEXTURE:
{
hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processChangeTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_TEXTURE:
{
hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processLoadTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_RESTORE_STATE:
{
hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
@@ -1397,47 +1660,291 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
}
case CMD_LOAD_BULLET:
{
hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processLoadBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SAVE_BULLET:
{
hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processSaveBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_MJCF:
{
hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_USER_DEBUG_DRAW:
{
hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_USER_DATA:
{
hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_ADD_USER_DATA:
{
hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REMOVE_USER_DATA:
{
hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
{
hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
#endif
default:
{
BT_PROFILE("CMD_UNKNOWN");
printf("Unknown command encountered: %d", clientCmd.m_type);
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
hasStatus = true;
}
};
return hasStatus;
}
bool PhysXServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA");
int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
int numPixelsCopied = 0;
int oldWidth;
int oldHeight;
m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(oldWidth, oldHeight);
serverStatusOut.m_type = CMD_CAMERA_IMAGE_FAILED;
if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) &&
(clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT) != 0)
{
if (m_data->m_pluginManager.getRenderInterface())
{
m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
clientCmd.m_requestPixelDataArguments.m_pixelHeight);
}
}
int flags = 0;
if (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_FLAGS)
{
flags = clientCmd.m_requestPixelDataArguments.m_flags;
}
if (m_data->m_pluginManager.getRenderInterface())
{
m_data->m_pluginManager.getRenderInterface()->setFlags(flags);
}
int numTotalPixels = width * height;
int numRemainingPixels = numTotalPixels - startPixelIndex;
if (numRemainingPixels > 0)
{
int totalBytesPerPixel = 4 + 4 + 4; //4 for rgb, 4 for depth, 4 for segmentation mask
int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1;
unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
int numRequestedPixels = btMin(maxNumPixels, numRemainingPixels);
float* depthBuffer = (float*)(bufferServerToClient + numRequestedPixels * 4);
int* segmentationMaskBuffer = (int*)(bufferServerToClient + numRequestedPixels * 8);
serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel;
float viewMat[16];
float projMat[16];
float projTextureViewMat[16];
float projTextureProjMat[16];
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) == 0)
{
b3OpenGLVisualizerCameraInfo tmpCamResult;
bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo(
&tmpCamResult.m_width,
&tmpCamResult.m_height,
tmpCamResult.m_viewMatrix,
tmpCamResult.m_projectionMatrix,
tmpCamResult.m_camUp,
tmpCamResult.m_camForward,
tmpCamResult.m_horizontal,
tmpCamResult.m_vertical,
&tmpCamResult.m_yaw,
&tmpCamResult.m_pitch,
&tmpCamResult.m_dist,
tmpCamResult.m_target);
if (result)
{
for (int i = 0; i < 16; i++)
{
viewMat[i] = tmpCamResult.m_viewMatrix[i];
projMat[i] = tmpCamResult.m_projectionMatrix[i];
}
}
else
{
//failed
m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight);
return hasStatus;
}
}
else
{
for (int i = 0; i < 16; i++)
{
viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i];
projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i];
}
}
{
if (m_data->m_pluginManager.getRenderInterface())
{
if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0)
{
// printf("-------------------------------\nRendering\n");
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow != 0));
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0)
{
m_data->m_pluginManager.getRenderInterface()->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff);
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0)
{
m_data->m_pluginManager.getRenderInterface()->render(
clientCmd.m_requestPixelDataArguments.m_viewMatrix,
clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
}
else
{
b3OpenGLVisualizerCameraInfo tmpCamResult;
bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo(
&tmpCamResult.m_width,
&tmpCamResult.m_height,
tmpCamResult.m_viewMatrix,
tmpCamResult.m_projectionMatrix,
tmpCamResult.m_camUp,
tmpCamResult.m_camForward,
tmpCamResult.m_horizontal,
tmpCamResult.m_vertical,
&tmpCamResult.m_yaw,
&tmpCamResult.m_pitch,
&tmpCamResult.m_dist,
tmpCamResult.m_target);
if (result)
{
m_data->m_pluginManager.getRenderInterface()->render(tmpCamResult.m_viewMatrix, tmpCamResult.m_projectionMatrix);
}
else
{
m_data->m_pluginManager.getRenderInterface()->render();
}
}
}
}
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);
}
#if 0
m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
clientCmd.m_requestPixelDataArguments.m_projectionMatrix, pixelRGBA, numRequestedPixels,
depthBuffer, numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex, width, height, &numPixelsCopied);
#endif
}
//each pixel takes 4 RGBA values and 1 float = 8 bytes
}
else
{
}
m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight);
serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;
serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied;
serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied;
serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex;
serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width;
serverStatusOut.m_sendPixelDataArguments.m_imageHeight = height;
return hasStatus;
}
bool PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -1767,8 +2274,13 @@ bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct Sha
int numArt = m_data->m_scene->getNbArticulations();
{
B3_PROFILE("clear Contacts");
m_data->m_contactPoints.clear();
}
{
B3_PROFILE("PhysX_simulate_fetchResults");
m_data->m_scene->simulate(m_data->m_physicsDeltaTime);
m_data->m_scene->fetchResults(true);
}

9
examples/SharedMemory/physx/PhysXServerCommandProcessor.h
View File

@@ -20,14 +20,19 @@ class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface
bool processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
void resetSimulation();
bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
public:
PhysXServerCommandProcessor();
PhysXServerCommandProcessor(int argc, char* argv[]);
virtual ~PhysXServerCommandProcessor();

2
examples/SharedMemory/physx/PhysXUserData.h
View File

@@ -4,5 +4,7 @@
struct MyPhysXUserData
{
int m_graphicsUniqueId;
int m_bodyUniqueId;
int m_linkIndex;
};
#endif //PHYSX_USER_DATA_H

35
examples/SharedMemory/physx/URDF2PhysX.cpp
View File

@@ -281,11 +281,21 @@ int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedDat
{
btVector3 planeNormal = col.m_geometry.m_planeNormal;
btScalar planeConstant = 0; //not available?
//PxPlane(1, 0, 0, 0).
btVector3 planeRefAxis(1, 0, 0);
btQuaternion diffQuat = shortestArcQuat(planeRefAxis, planeNormal);
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material);
//todo: compensate for plane axis
//physx::PxTransform localPose = tr*physx::PxTransform
//shape->setLocalPose(localPose);
btTransform diffTr;
diffTr.setIdentity();
diffTr.setRotation(diffQuat);
btTransform localTrans = localInertiaFrame.inverse()*childTrans*diffTr;
physx::PxTransform tr;
tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
physx::PxTransform localPose = tr;
shape->setLocalPose(localPose);
numShapes++;
break;
}
@@ -294,6 +304,13 @@ int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedDat
btScalar radius = collision->m_geometry.m_capsuleRadius;
btScalar height = collision->m_geometry.m_capsuleHeight;
//static PxShape* createExclusiveShape(PxRigidActor& actor, const PxGeometry& geometry, PxMaterial*const* materials, PxU16 materialCount,
// PxShapeFlags shapeFlags = PxShapeFlag::eVISUALIZATION | PxShapeFlag::eSCENE_QUERY_SHAPE | PxShapeFlag::eSIMULATION_SHAPE)
//{
physx::PxShapeFlags shapeFlags = physx::PxShapeFlag::eVISUALIZATION | physx::PxShapeFlag::eSCENE_QUERY_SHAPE | physx::PxShapeFlag::eSIMULATION_SHAPE;
//shape = PxGetPhysics().createShape(physx::PxCapsuleGeometry(radius, 0.5*height), &material, 1, false, shapeFlags);
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxCapsuleGeometry(radius, 0.5*height), *material);
btTransform childTrans = col.m_linkLocalFrame;
@@ -691,6 +708,7 @@ btTransform ConvertURDF2PhysXInternal(
//Now create the slider and fixed joints...
//cache.m_articulation->setSolverIterationCounts(4);//todo: API?
cache.m_articulation->setSolverIterationCounts(32);//todo: API?
cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED);
@@ -796,6 +814,8 @@ btTransform ConvertURDF2PhysXInternal(
//todo: mem leaks
MyPhysXUserData* userData = new MyPhysXUserData();
userData->m_graphicsUniqueId = graphicsIndex;
userData->m_bodyUniqueId = u2b.getBodyUniqueId();
userData->m_linkIndex = mbLinkIndex;
linkPtr->userData = userData;
}
@@ -805,8 +825,10 @@ btTransform ConvertURDF2PhysXInternal(
convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr);
physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass);
if (rbLinkPtr && mass)
{
physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass);
}
//base->setMass(massOut);
//base->setMassSpaceInertiaTensor(diagTensor);
@@ -1003,6 +1025,7 @@ physx::PxBase* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* phy
if (cache.m_rigidStatic)
{
scene->addActor(*cache.m_rigidStatic);
return cache.m_rigidStatic;
}

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

@@ -72,7 +72,7 @@ struct MyTexture3
struct EGLRendererObjectArray
{
btAlignedObjectArray<TinyRenderObjectData*> m_renderObjects;
btAlignedObjectArray<int> m_graphicsInstanceIds;
int m_objectUniqueId;
int m_linkIndex;
@@ -92,6 +92,33 @@ struct EGLRendererObjectArray
#define START_WIDTH 1024
#define START_HEIGHT 768
struct btHashVisual
{
UrdfShape m_vis;
btTransform m_tr;
int getHash() const
{
if (m_vis.m_geometry.m_meshFileName.length())
{
btHashString s = m_vis.m_geometry.m_meshFileName.c_str();
return s.getHash();
}
return 0;
}
bool equals(const btHashVisual& other) const
{
if ((m_vis.m_geometry.m_type == URDF_GEOM_MESH) &&
(other.m_vis.m_geometry.m_type == URDF_GEOM_MESH))
{
bool sameTr = m_tr == other.m_tr;
bool sameVis = m_vis.m_geometry.m_meshFileName == other.m_vis.m_geometry.m_meshFileName;
bool sameLocalFrame = m_vis.m_linkLocalFrame == other.m_vis.m_linkLocalFrame;
return sameTr&&sameVis&&sameLocalFrame;
}
return false;
}
};
struct EGLRendererVisualShapeConverterInternalData
{
@@ -105,6 +132,8 @@ struct EGLRendererVisualShapeConverterInternalData
btAlignedObjectArray<int> m_graphicsIndexToSegmentationMask;
btHashMap<btHashInt, EGLRendererObjectArray*> m_swRenderInstances;
btHashMap<btHashPtr, int> m_cachedTextureIds;
btHashMap<btHashVisual, int> m_cachedVisualShapes;
btAlignedObjectArray<b3VisualShapeData> m_visualShapes;
@@ -942,7 +971,7 @@ int EGLRendererVisualShapeConverter::convertVisualShapes(
colorIndex &= 3;
btVector4 color;
color = sColors[colorIndex];
float rgbaColor[4] = {(float)color[0], (float)color[1], (float)color[2], (float)color[3]};
float rgbaColor[4] = { (float)color[0], (float)color[1], (float)color[2], (float)color[3] };
//if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE)
//{
// color.setValue(1,1,1,1);
@@ -1012,64 +1041,91 @@ int EGLRendererVisualShapeConverter::convertVisualShapes(
visualShape.m_rgbaColor[1] = rgbaColor[1];
visualShape.m_rgbaColor[2] = rgbaColor[2];
visualShape.m_rgbaColor[3] = rgbaColor[3];
int shapeIndex = -1;
btHashVisual tmp;
{
B3_PROFILE("convertURDFToVisualShape2");
convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags);
btTransform tr = localInertiaFrame.inverse() * childTrans;
tmp.m_vis = *vis;
tmp.m_tr = tr;
int* bla = m_data->m_cachedVisualShapes[tmp];
if (bla)
{
shapeIndex = *bla;
}
else
{
convertURDFToVisualShape2(vis, pathPrefix, tr, vertices, indices, textures, visualShape, fileIO, m_data->m_flags);
}
}
m_data->m_visualShapes.push_back(visualShape);
if (vertices.size() && indices.size())
int textureIndex = -1;
if (shapeIndex < 0)
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex);
unsigned char* textureImage1 = 0;
int textureWidth = 0;
int textureHeight = 0;
bool isCached = false;
int textureIndex = -1;
if (textures.size())
if (vertices.size() && indices.size())
{
textureImage1 = textures[0].textureData1;
textureWidth = textures[0].m_width;
textureHeight = textures[0].m_height;
isCached = textures[0].m_isCached;
textureIndex = m_data->m_instancingRenderer->registerTexture(textureImage1, textureWidth, textureHeight);
}
unsigned char* textureImage1 = 0;
int textureWidth = 0;
int textureHeight = 0;
bool isCached = false;
{
B3_PROFILE("registerMeshShape");
tinyObj->registerMeshShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), rgbaColor,
textureImage1, textureWidth, textureHeight);
}
visuals->m_renderObjects.push_back(tinyObj);
{
B3_PROFILE("m_instancingRenderer register");
// register mesh to m_instancingRenderer too.
int shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
double scaling[3] = {1, 1, 1};
int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling);
int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24);
if (textures.size())
{
if (graphicsIndex >= 0)
textureImage1 = textures[0].textureData1;
textureWidth = textures[0].m_width;
textureHeight = textures[0].m_height;
isCached = textures[0].m_isCached;
int* bla = m_data->m_cachedTextureIds[textureImage1];
if (bla)
{
visuals->m_graphicsInstanceIds.push_back(graphicsIndex);
if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1))
{
m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1);
}
m_data->m_graphicsIndexToSegmentationMask[graphicsIndex] = segmentationMask;
textureIndex = *bla;
}
else
{
textureIndex = m_data->m_instancingRenderer->registerTexture(textureImage1, textureWidth, textureHeight);
m_data->m_cachedTextureIds.insert(textureImage1, textureIndex);
}
}
m_data->m_instancingRenderer->writeTransforms();
}
}
{
B3_PROFILE("m_instancingRenderer register");
// register mesh to m_instancingRenderer too.
if (shapeIndex < 0)
{
shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
m_data->m_cachedVisualShapes.insert(tmp, shapeIndex);
}
double scaling[3] = { 1, 1, 1 };
int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling);
int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24);
{
if (graphicsIndex >= 0)
{
visuals->m_graphicsInstanceIds.push_back(graphicsIndex);
if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1))
{
m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1);
}
m_data->m_graphicsIndexToSegmentationMask[graphicsIndex] = segmentationMask;
}
}
m_data->m_instancingRenderer->writeTransforms();
}
for (int i = 0; i < textures.size(); i++)
{
if (!textures[i].m_isCached)
@@ -1078,6 +1134,7 @@ int EGLRendererVisualShapeConverter::convertVisualShapes(
}
}
}
}
return orgGraphicsUniqueId;
}
@@ -1163,13 +1220,7 @@ void EGLRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int link
EGLRendererObjectArray* visuals = *ptrptr;
if ((bodyUniqueId == visuals->m_objectUniqueId) && (linkIndex == visuals->m_linkIndex))
{
for (int q = 0; q < visuals->m_renderObjects.size(); q++)
{
if (shapeIndex < 0 || q == shapeIndex)
{
visuals->m_renderObjects[q]->m_model->setColorRGBA(rgba);
}
}
}
}
}
@@ -1225,6 +1276,8 @@ void EGLRendererVisualShapeConverter::render(const float viewMat[16], const floa
render();
m_data->m_camera.disableVRCamera();
//cout<<viewMat[4*0 + 0]<<" "<<viewMat[4*0+1]<<" "<<viewMat[4*0+2]<<" "<<viewMat[4*0+3] << endl;
//cout<<viewMat[4*1 + 0]<<" "<<viewMat[4*1+1]<<" "<<viewMat[4*1+2]<<" "<<viewMat[4*1+3] << endl;
//cout<<viewMat[4*2 + 0]<<" "<<viewMat[4*2+1]<<" "<<viewMat[4*2+2]<<" "<<viewMat[4*2+3] << endl;
@@ -1503,15 +1556,11 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu
EGLRendererObjectArray* ptr = *ptrptr;
if (ptr)
{
for (int o = 0; o < ptr->m_renderObjects.size(); o++)
for (int i = 0; i < ptr->m_graphicsInstanceIds.size(); i++)
{
for (int i = 0; i < ptr->m_graphicsInstanceIds.size(); i++)
{
m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceIds[i]);
}
delete ptr->m_renderObjects[o];
m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceIds[i]);
}
}
delete ptr;
m_data->m_swRenderInstances.remove(collisionObjectUniqueId);
@@ -1520,6 +1569,8 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu
void EGLRendererVisualShapeConverter::resetAll()
{
m_data->m_cachedTextureIds.clear();
for (int i = 0; i < m_data->m_swRenderInstances.size(); i++)
{
EGLRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i);
@@ -1528,10 +1579,7 @@ void EGLRendererVisualShapeConverter::resetAll()
EGLRendererObjectArray* ptr = *ptrptr;
if (ptr)
{
for (int o = 0; o < ptr->m_renderObjects.size(); o++)
{
delete ptr->m_renderObjects[o];
}
}
delete ptr;
}
@@ -1556,25 +1604,7 @@ void EGLRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, int
btAssert(textureUniqueId < m_data->m_textures.size());
if (textureUniqueId >= 0 && textureUniqueId < m_data->m_textures.size())
{
for (int n = 0; n < m_data->m_swRenderInstances.size(); n++)
{
EGLRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(n);
if (0 == visualArrayPtr)
continue; //can this ever happen?
EGLRendererObjectArray* visualArray = *visualArrayPtr;
if (visualArray->m_objectUniqueId == objectUniqueId && visualArray->m_linkIndex == jointIndex)
{
for (int v = 0; v < visualArray->m_renderObjects.size(); v++)
{
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
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);
}
}
}
}
}
}
@@ -1651,3 +1681,59 @@ void EGLRendererVisualShapeConverter::syncTransform(int collisionObjectUniqueId,
}
}
}
bool EGLRendererVisualShapeConverter::getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const
{
if (m_data->m_instancingRenderer && m_data->m_instancingRenderer->getActiveCamera())
{
*width = m_data->m_window->getWidth() * m_data->m_window->getRetinaScale();
*height = m_data->m_window->getHeight() * m_data->m_window->getRetinaScale();
m_data->m_instancingRenderer->getActiveCamera()->getCameraViewMatrix(viewMatrix);
m_data->m_instancingRenderer->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_data->m_instancingRenderer->getActiveCamera()->getCameraUpVector(camUp);
m_data->m_instancingRenderer->getActiveCamera()->getCameraForwardVector(camForward);
float top = 1.f;
float bottom = -1.f;
float tanFov = (top - bottom) * 0.5f / 1;
float fov = btScalar(2.0) * btAtan(tanFov);
btVector3 camPos, camTarget;
m_data->m_instancingRenderer->getActiveCamera()->getCameraPosition(camPos);
m_data->m_instancingRenderer->getActiveCamera()->getCameraTargetPosition(camTarget);
btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget - camPos);
rayForward.normalize();
float farPlane = 10000.f;
rayForward *= farPlane;
btVector3 rightOffset;
btVector3 cameraUp = btVector3(camUp[0], camUp[1], camUp[2]);
btVector3 vertical = cameraUp;
btVector3 hori;
hori = rayForward.cross(vertical);
hori.normalize();
vertical = hori.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f * fov);
hori *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btScalar aspect = float(*width) / float(*height);
hori *= aspect;
//compute 'hor' and 'vert' vectors, useful to generate raytracer rays
hor[0] = hori[0] * m_data->m_window->getRetinaScale();
hor[1] = hori[1] * m_data->m_window->getRetinaScale();
hor[2] = hori[2] * m_data->m_window->getRetinaScale();
vert[0] = vertical[0] * m_data->m_window->getRetinaScale();
vert[1] = vertical[1] * m_data->m_window->getRetinaScale();
vert[2] = vertical[2] * m_data->m_window->getRetinaScale();
*yaw = m_data->m_instancingRenderer->getActiveCamera()->getCameraYaw();
*pitch = m_data->m_instancingRenderer->getActiveCamera()->getCameraPitch();
*camDist = m_data->m_instancingRenderer->getActiveCamera()->getCameraDistance();
cameraTarget[0] = camTarget[0];
cameraTarget[1] = camTarget[1];
cameraTarget[2] = camTarget[2];
return true;
}
return false;
}

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

@@ -59,6 +59,9 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void mouseMoveCallback(float x, float y);
virtual void mouseButtonCallback(int button, int state, float x, float y);
virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const;
};
#endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H

66
examples/pybullet/examples/otherPhysicsEngine.py
View File

@@ -1,21 +1,64 @@
import pybullet as p
import pybullet_data as pd
import time
import math
usePhysX = True
useMaximalCoordinates = True
if usePhysX:
p.connect(p.PhysX)
p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.loadURDF("plane.urdf")
p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
for i in range (10):
sphere = p.loadURDF("marble_cube.urdf",[0,-1,1+i*1], useMaximalCoordinates=False)
p.changeDynamics(sphere ,-1, mass=1000)
p.setAdditionalSearchPath(pd.getDataPath())
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
door = p.loadURDF("door.urdf",[0,0,1])
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100
num=64
radius=0.1
numDominoes=0
for i in range (int(num*50)):
num=(radius*2*math.pi)/0.08
radius += 0.05/float(num)
orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)])
pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03]
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
numDominoes+=1
pos=[pos[0],pos[1],pos[2]+0.3]
orn = p.getQuaternionFromEuler([0,0,-math.pi/4.])
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
print("numDominoes=",numDominoes)
#for j in range (20):
# for i in range (100):
# if (i<99):
# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
# else:
# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
print("loaded!")
#p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf",[0,0,-11])
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door))
@@ -31,8 +74,13 @@ prevTime = time.time()
angle = math.pi*0.5
count=0
while (1):
count+=1
if (count==12):
p.stopStateLogging(logId)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
curTime = time.time()
diff = curTime - prevTime
@@ -45,5 +93,7 @@ while (1):
p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001)
else:
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
#contacts = p.getContactPoints()
#print("contacts=",contacts)
p.stepSimulation()
time.sleep(1./240.)
#time.sleep(1./240.)

2
examples/pybullet/premake4.lua
View File

@@ -223,12 +223,10 @@ if not _OPTIONS["no-enet"] then
"../../examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp",
"../../examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h",
"../../examples/SharedMemory/physx/PhysXC_API.cpp",
"../../examples/SharedMemory/physx/PhysXClient.cpp",
"../../examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp",
"../../examples/SharedMemory/physx/PhysXUrdfImporter.cpp",
"../../examples/SharedMemory/physx/URDF2PhysX.cpp",
"../../examples/SharedMemory/physx/PhysXC_API.h",
"../../examples/SharedMemory/physx/PhysXClient.h",
"../../examples/SharedMemory/physx/PhysXServerCommandProcessor.h",
"../../examples/SharedMemory/physx/PhysXUrdfImporter.h",
"../../examples/SharedMemory/physx/URDF2PhysX.h",

2
examples/pybullet/pybullet.c
View File

@@ -456,7 +456,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
#ifdef BT_ENABLE_PHYSX
case eCONNECT_PHYSX:
{
sm = b3ConnectPhysX();
sm = b3ConnectPhysX(argc, argv);
break;
}
#endif